四元数求逆实现方法 四元数求逆实现方法有哪些

1.RAPID对于位姿Pose运算，有矩阵乘法PoseMult（pose1，pose2）和矩阵求逆PoseInv（pose1）

2.对于PoseMult，解释如下

3.对于PoseInv，解释如下

4.对于Pose3：=PoseMult(Pose1,pose2)，已知Pose1和Pose3，要计算Pose2，可以利用矩阵左乘计算

Pose3= Pose1 * Pose2

(Pose1)-1*Pose3=Pose2

即Pose2=(Pose1)-1*Pose3

Pose2:=PoseMult(PoseInv(Pose1),Pose3)

5.姿态数据orient，可以直接使用*运算，形式同位姿

Orient1=orient2*orient3

Orient3=(orient2)-1*orient1

6.对于姿态求逆，RAPID未提供标准函数

7.四元数求逆，实质就是获取四元数的共轭四元数，即

也就是o(q1,q2,q3,q4)的逆为o(q1,-q2,-q3,-q4)

8.编写求四元数逆的代码如下：

FUNC orient OriInv(orient o)
        o.q2:=-o.q2;
        o.q3:=-o.q3;
        o.q4:=-o.q4;
        RETURN o;
    ENDFUNC