一、笛卡尔轨迹规划需求

        笛卡尔轨迹规划本质就是我们对机械臂的末端位置和姿态进行规划，其实也就是对末端坐标系的位姿进行规划。我们清楚末端坐标系的位姿是可以用齐次变换矩阵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)/2X=(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 # 此时算出来的是4XX=s/4Y=(T[0][1]+T[1][0])/s # 消元ZWZ=(T[0][2]+T[2][0])/s # 消元YWW=(T[2][1]-T[1][2])/s # 消YZelif (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 # 此时算出来的是4YY=s/4W=(T[0][2]-T[2][0])/s # 消XZX=(T[0][1]+T[1][0])/s # 消ZWZ=(T[1][2]+T[2][1])/s # 消XWelse: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 # 此时算出的是4zZ=s/4X=(T[0][2]+T[2][0])/s #消YWY=(T[1][2]+T[2][1])/s #消XWW=(T[1][0]-T[0][1])/s #消XY   return W,X,Y,Z