剛來新公司不久，部門給安排了新人作業，我被分到的任務是求標准6軸機器人的正反解，以及利用就近原則選擇最優解。從今天開始，逐步將這部分內容總結出來；
本文以及后續文章均使用改進DH法；
連桿坐標系：
坐標系的建立方法(摘自機器人學導論)：
- 找出各個關節軸，並標出這些軸線的延長線；
- 找出關節軸i和i+1之間的公垂線或者關鍵軸i和i+1的交點，以關節軸i和i+1的交點或者公垂線與關節軸i的交點作為連桿坐標系{i}的原點；
- 規定Z(i)軸沿關節軸i的指向；
- 規定X(i)軸沿公垂線的指向，如果關節軸i和i+1相交，則規定X(i)軸垂直於關節軸i和i+1所在的平面；
- 按照右手定則確定Y(i)軸；
- 當第一個關節變量為0時，規定坐標系{0}和{1}重合；對於坐標系{N}，其原點和X(n)的方向可以任意選取。但是在選取時，通常盡量使連桿參數為0；
建立連桿坐標系：
公司的工業機器人都是6軸，並且都是旋轉軸，基坐標系位於1軸軸線和2軸x負方向交點，4，5，6軸軸線交於一點，對其建立改進DH坐標系如下圖所示(不知道為什么變形了，后面再替換原圖)：
MDH參數表：
MDH的確認方法(摘自機器人學導論):
a(i) = 沿X(i)軸，從Z(i)移動到Z(i+1)的距離；
alpha(i) = 繞X(i)軸，從Z(i)移動到Z(i+1)的角度；
d(i) = 沿Z(i)軸，從X(i-1)移動到X(i)的距離；
theta(i) = 繞Z(i)軸，從X(i-1)移動到X(i)的角度；
MDH參數表：
按照上述規則，形成的MDH參數表如下：
