一、笛卡爾軌跡規劃需求
????????笛卡爾軌跡規劃本質就是我們對機械臂的末端位置和姿態進行規劃，其實也就是對末端坐標系的位姿進行規劃。我們清楚末端坐標系的位姿是可以用齊次變換矩陣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
