经典罗德里格斯参数（CRP）与DCM的转换、加法、微分运动方程

一、简介

经典罗德里格斯参数（CRP）由四元数衍生，是描述坐标系之间关系的方法之一。其与四元数转换关系如下：

由此推广，可以得到CRP与DCM的转换关系：

二、CRP转DCM

代入公式即可：

q=[0.1;0.2;0.3];
dcm=[1+q(1)^2-q(2)^2-q(3)^2, 2*(q(1)*q(2)+q(3)), 2*(q(1)*q(3)-q(2));
     2*(q(2)*q(1)-q(3)), 1-q(1)^2+q(2)^2-q(3)^2,2*(q(2)*q(3)+q(1));
     2*(q(3)*q(1)+q(2)), 2*(q(3)*q(2)-q(1)), 1-q(1)^2-q(2)^2+q(3)^2]/(1+q'*q);
disp(dcm)

但这个3x3的矩阵太复杂了，实际上我们采用另一个更简洁的公式：

括号内的部分可以与3x3矩阵进行代换，是q的反对称矩阵。例如q=[x1; x2; x3], 

而这样的转换函数还有一个性质：

这样就可以将一个逆矩阵/矩阵转置问题转换成一个加负号的问题。具体代码实现可以自己尝试，与上文直接生成3x3矩阵的代码进行比较。

三、DCM转CRP

前文已经提到了相关公式，这里再放一下：

dcm=[0.333333, -0.666667, 0.666667;
    0.871795, 0.487179, 0.0512821;
    -0.358974, 0.564103, 0.74359];
tr=dcm(1,1)+dcm(2,2)+dcm(3,3);
zeta=sqrt(tr+1);
q=[dcm(2,3)-dcm(3,2),dcm(3,1)-dcm(1,3), dcm(1,2)-dcm(2,1)]/zeta^2;
disp(q)

四、CRP加法

CRP与四元数类似，由两个加法公式，满足不同的需求。

①已知FB与BN求FN，使用左边的公式

②已知FN与BN求FB，使用右边的公式

根据计算需求，列公式，无需把目标参数单独放在等式一边。

此代码是一个已知BN、FN求BF的程序

FN=[0.1,0.2,0.3];
BN=[-0.3,0.3,0.1];
BF=(BN-FN+cross(BN,FN))/(1+BN*FN');
disp(BF)

五、CRP微分运动方程

与四元数类似，微分运动方程也是一个复杂的矩阵乘以一个列向量，不同的是CRP只有3个参数，因此向量也就变成3x3

当然也有简洁形式：

与四元数相关问题思路一致，不作赘述。

q=[0.4,0.2,-0.1]';

T=(0:0.00001:42)';

for i=1:length(T)

 w=(pi/60)*[sin(0.1*T(i)); 0.01; cos(0.1*T(i))];

 qk=[1+q(1)^2, q(1)*q(2)-q(3), q(1)*q(3)+q(2);
     q(2)*q(1)+q(3),1+q(2)^2, q(2)*q(3)-q(1);
     q(3)*q(1)-q(2),q(3)*q(2)+q(1), 1+q(3)^2];
 
 Bd=(1/2)*qk*w;

 q = q + Bd*0.00001;

end
answer= sqrt(q(1)^2 + q(2)^2 + q(3)^2);
disp(answer)

六、结语

经典罗德里格斯参数是一个重要方法。本文给出了几个经典问题的公式与代码，供读者学习参考。如有纰漏，敬请批评指正。