笛卡尔轨迹规划之齐次变换矩阵与欧拉角、四元数的转化

一、笛卡尔轨迹规划需求

        笛卡尔轨迹规划本质就是我们对机械臂的末端位置和姿态进行规划，其实也就是对末端坐标系的位姿进行规划。我们清楚末端坐标系的位姿是可以用齐次变换矩阵T来表示的，但这样表示的话，并不利于我们去做规划，所以在进行轨迹规划之前，我们需要先将对应的齐次变化矩阵转化成位姿向量去表示，也就是转化成：

        其中px，py和pz就是末端的位置，这个是比较好处理的，就是原点的移动，规划的思路就是插值，求解就可以了。

        但φx，φy，φz表示的是末端的姿态，这个相对难处理一点。一般而言，我们有两种计算方法，一种是转化成φx，φy，φz计算，也就是欧拉角。另一种就是转化成【w,x,y,z】的四元数计算。两种方法各有特点，目前我也只是了解了这些方法，但具体还没有做应用和比较。

        因为我们一般而言已知的就是起点和终点的齐次变化矩阵，可以用以下这个式子表示：

        后面我们要计算的姿态就是用标红的框框里面的数据去计算。

二、齐次变换矩阵与欧拉角

        欧拉角的表示方法就是让坐标系先绕x轴转一个φx，再绕y轴转一个φy，最后绕z轴转一个φz，进而得到