三维旋转沿轴分解

将旋转分解为固定参考坐标系的绕轴n\bold{n}n的旋转和正交轴旋转:
R=R⊥R∥ \bold{R}= \bold{R}{\perp} \bold{R}{\parallel} R=R⊥R∥
R∥=cos⁡θI+(1−cos⁡θ)nnT+sin⁡θn∧ \bold{R}_{\parallel}=\cos \theta \bold{I}+\left( 1-\cos \theta \right) \bold{n}\bold{n}^{T}+\sin \theta \bold{n}^{\wedge } R∥=cosθI+(1−cosθ)nnT+sinθn∧

使用最小二乘法估计绕轴旋转,目标函数是使正交轴旋转的角度最小
min⁡θarccos⁡(tr(RR∥−1)−12) \min_{\theta}\arccos\left(\dfrac{\rm{tr}\left(\bold{RR}_{\parallel}^{-1}\right)-1}{2}\right) θminarccos 2tr(RR∥−1)−1

上面的式子使用了旋转矩阵转欧拉角 的公式

由于arccos⁡\arccosarccos函数单调递减,原问题变为

max⁡θtr(RR∥−1)=max⁡θtr(R(cos⁡θI+(1−cos⁡θ)nnT−sin⁡θn∧))=max⁡θ(cos⁡θtr(R−RnnT)−sin⁡θtr(Rn∧)) \begin{aligned} \max_{\theta} \rm{tr}\left(\bold{RR}{\parallel}^{-1}\right) &=\max{\theta}\rm{tr}\left(\bold{R}\left(\cos \theta \bold{I}+\left( 1-\cos \theta \right) \bold{n}\bold{n}^{T}-\sin \theta \bold{n}^{\wedge } \right)\right)\\ &=\max_{\theta}\left(\cos \theta \rm{tr}\left(\bold{R} - \bold{R}\bold{n}\bold{n}^{T}\right)-\sin\theta\rm{tr}\left(\bold{R}\bold{n}^{\wedge }\right)\right) \end{aligned} θmaxtr(RR∥−1)=θmaxtr(R(cosθI+(1−cosθ)nnT−sinθn∧))=θmax(cosθtr(R−RnnT)−sinθtr(Rn∧))

对θ\thetaθ求导得
−sin⁡θtr(R−RnnT)−cos⁡θtr(Rn∧)=0tan⁡θ=tr(Rn∧)tr(nTRn)−tr(R)θ=arctan⁡(tr(Rn∧)tr(nTRn)−tr(R)) \begin{aligned} -\sin\theta\rm{tr}\left(\bold{R} - \bold{R}\bold{n}\bold{n}^{T}\right)-\cos\theta\rm{tr}\left(\bold{R}\bold{n}^{\wedge }\right)&=0\\ \tan\theta&=\dfrac{\rm{tr}\left(\bold{R}\bold{n}^{\wedge }\right)}{\rm{tr}\left(\bold{n}^{T}\bold{R}\bold{n}\right)-\rm{tr}\left(\bold{R}\right) }\\ \theta&=\arctan\left(\dfrac{\rm{tr}\left(\bold{R}\bold{n}^{\wedge }\right)}{\rm{tr}\left(\bold{n}^{T}\bold{R}\bold{n}\right)-\rm{tr}\left(\bold{R}\right) }\right) \end{aligned} −sinθtr(R−RnnT)−cosθtr(Rn∧)tanθθ=0=tr(nTRn)−tr(R)tr(Rn∧)=arctan(tr(nTRn)−tr(R)tr(Rn∧))

相关推荐
stbomei1 分钟前
基于 MATLAB 的信号处理实战:滤波、傅里叶变换与频谱分析
算法·matlab·信号处理
2401_8762213425 分钟前
Reachability Query(Union-Find)
c++·算法
国科安芯41 分钟前
高速CANFD收发器ASM1042在割草机器人轮毂电机通信系统中的适配性研究
网络·单片机·嵌入式硬件·性能优化·机器人·硬件工程
德先生&赛先生1 小时前
LeetCode-542. 01 矩阵
算法·leetcode·矩阵
HAH-HAH1 小时前
【洛谷】P2197【模板】Nim 游戏
算法·游戏
lichkingyang2 小时前
最近遇到的几个JVM问题
java·jvm·算法
feifeigo1232 小时前
matlab中随机森林算法的实现
算法·随机森林·matlab
躲着人群3 小时前
次短路&&P2865 [USACO06NOV] Roadblocks G题解
c语言·数据结构·c++·算法·dijkstra·次短路
心动啊1214 小时前
支持向量机
算法·机器学习·支持向量机
小欣加油4 小时前
leetcode 1493 删掉一个元素以后全为1的最长子数组
c++·算法·leetcode