四元数相乘

ROS TF坐标系变换时用四元数变换可以忽略旋转轴的转变顺序，使用更方便
一、多个三维坐标系变换时，四元数相乘
Qx1， Qy1 ， Qz1，Qw1 表示B in A下的四元数
Qx2， Qy2 ， Qz2，Qw2 表示C in B下的四元数

C in A 的四元数表示为Qx， Qy ， Qz，Qw

Qx = Qw2*Qx1+Qx2*Qw1+(Qz2*Qy1-Qy2*Qz1)
Qy = Qw2*Qy1+Qy2*Qw1+(Qx2*Qz1-Qz2*Qx1)
Qz = Qw2*Qz1+Qz2*Qw1+(Qy2*Qx1-Qx2*Qy1)
Qw=  Qw2*Qw1-(Qx2*Qx1+Qy2*Qy1+Qz2*Qz1)

二、已知末端坐标系在初始坐标系下的四元数，求解中间一段的四元数

#多元线性方程组如
# x-2y+3z=-10
# 2x+3y+4z=5
# 3x+5y+7z=7
import  numpy as np
def solve_quaternion(p0,p1,p2,p3,Qw,Qx,Qy,Qz):
    """该函数功能为:已知1->3的四元数和1->2的四元数,求解2->3的四元数q0,q1,q2,q3
        Q(1->2)* Q(2->3) =  Q(1->3)
        p0,p1,p2,p3表示1->2的四元数. Qw,Qx,Qy,Qz表示1->3的四元数.
    """
    A=np.array([[p0,-p1,-p2,-p3],[p1,p0,p3,-p2],[p2,-p3,p0,p1],[p3,p2,-p1,p0]])
    b=np.array([Qw,Qx,Qy,Qz])
    x=np.linalg.solve(A,b)
    print('四元一次方程组的解：\n',x)
    return x[0],x[1],x[2],x[3]