笛卡尔轨迹规划之齐次变换矩阵与欧拉角、四元数的转化
一、笛卡尔轨迹规划需求
笛卡尔轨迹规划本质就是我们对机械臂的末端位置和姿态进行规划，其实也就是对末端坐标系的位姿进行规划。我们清楚末端坐标系的位姿是可以用齐次变换矩阵T来表示的，但这样表示的话，并不利于我们去做规划，所以在进行轨迹规划之前，我们需要先将对应的齐次变化矩阵转化成位姿向量去表示，也就是转化成：
其中px，py和pz就是末端的位置，这个是比较好处理的，就是原点的移动，规划的思路就是插值，求解就可以了。
但φx，φy，φz表示的是末端的姿态，这个相对难处理一点。一般而言，我们有两种计算方法，一种是转化成φx，φy，φz计算，也就是欧拉角。另一种就是转化成【w,x,y,z】的四元数计算。两种方法各有特点，目前我也只是了解了这些方法，但具体还没有做应用和比较。
因为我们一般而言已知的就是起点和终点的齐次变化矩阵，可以用以下这个式子表示：
后面我们要计算的姿态就是用标红的框框里面的数据去计算。
二、齐次变换矩阵与欧拉角
欧拉角的表示方法就是让坐标系先绕x轴转一个φx，再绕y轴转一个φy，最后绕z轴转一个φz，进而得到旋转矩阵R，也就是上面红色框出来的部分。注意，这里先绕哪个轴，后绕哪个轴都是有顺序的，顺序不同，计算也不同。
对应的绕各轴旋转的矩阵可以表示如下：
因为我们这里是先绕x轴转一个φx，再绕y轴转一个φy，最后绕z轴转一个φz，旋转矩阵R就等于以下这个式子：
（因为是对固定坐标系，所以先转的放右边）
分析一下这个旋转矩阵R，就得以得到各个转角的计算公式
但用欧拉角也会有些不太方便的地方就是万向死锁，就是中间这个转角转了90°的时候，我们会发现cosφy=0。
为了避免这个现象，常采用的方式就是用四元数去代替欧拉角。
三、齐次变换矩阵与四元数
四元数的表示：
如果我们要用四元数描述旋转，那么就可以调整成以下这个式子：
其中θ是旋转角度，u是旋转轴，也是一个单位向量。
那怎么从旋转矩阵得到四元数呢？
我们知道在进行旋转变换时，都可以等效为绕一个轴f旋转θ（可以参见《机器人学》的P31），也就是下面这个式子：
其中：
分析可得：
同理，其他也一样可以做转化，进而得到以下这个式子：
然后我们就可以利用旋转矩阵来将他转化成四元数啦：
四元数计算代码，里面补充了一个迹小于零的处理方法，其实就是选出最大值，然后变换下计算的顺序，仅此而已，这样我们就可以完成四元数的计算了。后面轨迹规划的时候就算出了四元数，就只剩插值，求逆运动学了：）
def count_quaternion(T):
'''
利用旋转矩阵计算四元数
'''
if ((T[0][0]+T[1][1]+T[2][2])>0:
W=np.sqrt(T[0][0]+T[1][1]+T[2][2]+1)/2
X=(T[2][1]-T[1][2])/(4*W)
Y=(T[0][2]-T[2][0])/(4*W)
Z=(T[1][0]-T[0][1])/(4*W)
else:
# 迹小于零的处理方法
if (T[0][0]>T[1][1]) and (T[0][0]>T[2][2]):
s=np.sqrt(T[0][0]-T[1][1]-T[2][2]+1)*2 # 此时算出来的是4X
X=s/4
Y=(T[0][1]+T[1][0])/s # 消元ZW
Z=(T[0][2]+T[2][0])/s # 消元YW
W=(T[2][1]-T[1][2])/s # 消YZ
elif (T[1][1]>T[0][0]) and (T[1][1]>T[2][2]):
s=np.sqrt(T[1][1]-T[0][0]-T[2][2]+1)*2 # 此时算出来的是4Y
Y=s/4
W=(T[0][2]-T[2][0])/s # 消XZ
X=(T[0][1]+T[1][0])/s # 消ZW
Z=(T[1][2]+T[2][1])/s # 消XW
else:
if (T[2][2]>T[0][0]) and (T[2][2]>T[1][1]):
s=np.sqrt(T[2][2]-T[0][0]-T[1][1]+1)*2 # 此时算出的是4z
Z=s/4
X=(T[0][2]+T[2][0])/s #消YW
Y=(T[1][2]+T[2][1])/s #消XW
W=(T[1][0]-T[0][1])/s #消XY
return W,X,Y,Z