修正罗德里格斯参数（MRP）与DCM转换、加法、微分方程

一、简介

修正罗德里格斯参数MRP与CRP类似，但有些许区别。MRP的定义如下：

可以发现，它与CRP的定义不同之处在于分母变成1+β0，这样可以规避β0=0的时候产生的奇异性问题。因此实际上，MRP方法更受欢迎。

MRP与CRP转换：

二、MRP转DCM

MRP转DCM的转换矩阵就相当复杂了：

因此，我们需要使用简便的向量方法进行计算与编程。

sigma=[0.2;0.2;-0.1];
sigma1=[0, -sigma(3), sigma(2);
        sigma(3), 0, -sigma(1);
       -sigma(2), sigma(1), 0];
dcm=eye(3)+(8*sigma1^2-4*(1-sigma'*sigma)*sigma1)/(1+sigma'*sigma)^2;
disp(dcm)

这个函数和CRP具有相似的性质，可以将逆矩阵、转置矩阵问题转换为简单的负号问题：

三、DCM转MRP

与CRP类似，不多赘述

dcm=[0.763314, 0.0946746, -0.639053;
    -0.568047, -0.372781, -0.733728;
    -0.307692, 0.923077, -0.230769];
tr=dcm(1,1)+dcm(2,2)+dcm(3,3);
zeta= sqrt(tr+1);
sigma=[dcm(2,3)-dcm(3,2); dcm(3,1)-dcm(1,3); dcm(1,2)-dcm(2,1)]/(zeta*(zeta+2));
disp(sigma)

四、MRP加法

与CRP类似，有两个计算公式，方法一样。可以根据需求选择计算公式。

已知RB、BN求RN：

BN=[0.1; 0.2; 0.3];
RB=[-0.1; 0.3; 0.1];
sigma=((1-BN'*BN)*RB+(1-RB'*RB)*BN-2*cross(RB,BN))/(1+(BN'*BN)*(RB'*RB)-2*dot(BN,RB));
disp(sigma)

已知BN、RN求BR

RN=[0.5; 0.3; 0.1];
BN=[0.1; 0.2; 0.3];
sigma=((1-RN'*RN)*BN-(1-BN'*BN)*RN+2*cross(BN,RN))/(1+(RN'*RN)*(BN'*BN)+2*dot(RN,BN));
disp(sigma)

五、MRP微分方程

给出微分运动方程公式，可以自行选择：

代码如下：

sigma=[0.4;0.2;-0.1];

T=(0:0.00001:42)';

for i=1:length(T)
 w=(20*pi/180)*[sin(0.1*T(i)); 0.01; cos(0.1*T(i))];
 B=[1-sigma'*sigma+2*sigma(1)^2, 2*(sigma(1)*sigma(2)-sigma(3)),2*(sigma(1)*sigma(3)+sigma(2));
     2*(sigma(2)*sigma(1)+sigma(3)), 1-sigma'*sigma+2*sigma(2)^2, 2*(sigma(2)*sigma(3)-sigma(1));
     2*(sigma(3)*sigma(1)-sigma(2)), 2*(sigma(3)*sigma(2)+sigma(1)), 1-sigma'*sigma+2*sigma(3)^2];
 Bd=(1/4)*B*w;
 sigma = sigma + Bd*0.00001;
 if sigma'*sigma>1
     sigma=(-sigma)/(sigma'*sigma);
 end
end
answer= sqrt(sigma(1)^2 + sigma(2)^2 + sigma(3)^2);
disp(answer)

与前文做法类似，但应该注意到在for循环最后有一个if条件判断。实际上这是因为MRP的投影

MRP与PRV有这样的关系：

MRP投影计算公式如下：

判定条件：

为什么选取1为判定条件？β从x轴出发只有正转90°和反转90°才有σ值，由图中旋转角为可分出180°的临界条件。≤180°时，投影在y负半轴；≥180°时，等价于≥-180°投影在y正半轴

因此，在for循环最后要进行判定，进行投影计算。

六、结语

MRP相较于CRP有了更丰富的性质，并且从四元数转换过来的时候规避了奇异性，在研究中广泛使用。如果文章有纰漏，尤其对于投影的解释部分，请读者不吝赐教，批评指正，谢谢。