機器人運動學和動力學/串聯機械臂位置運動學

正向位置運動學

正向位置運動學問題可以表述為：給定不同的關節角度，末端執行器的位置是什麼？考慮到前面的章節，答案相當簡單：構造不同的變換矩陣，並以正確的方式組合它們，結果是 $_{bs}^{ee}T\,$，其中 $\{bs\}$ 是機器人機械臂的基座座標系。

解

假設相鄰連桿之間的相互方向矩陣是已知的。（由於每個連桿的固定引數是已知的，並且關節角度是給定問題，因此可以計算這些引數。一種可能的方法是使用 Denavit-Hartenberg 約定。）將最後一個座標系與第一個座標系聯絡起來的變換，因此，正向運動學問題的解，由複合齊次變換矩陣表示。軸是移動的，因此，複合齊次變換矩陣是透過預乘各個變換矩陣得到的

$_{bs}^{ee}T=\,_{0}^{N}T=\,_{0}^{1}T\,_{1}^{2}T\ldots \,_{N-2}^{N-1}T\,_{N-1}^{N}T$

例子

平面三連桿機械臂

一個平面三連桿機械臂。每個 $X_{i}$-軸沿著第 $i$ 個連桿。每個 $Y_{i}$-軸垂直於相應的 $X_{i}$-軸，使得正的 $\theta _{i}$ 對應於從 $X_{i}$ 到 $Y_{i}$ 的旋轉。

下面的方程使用 3 × 3 姿態矩陣，因為這只是一個二維情況（參見右邊的圖）。

第一個連桿相對於參考座標系的姿態由下式給出（回顧上一節中關於 z 軸的基本旋轉）

$T_{1}(\theta _{1})={\begin{pmatrix}\cos(\theta _{1})&-\sin(\theta _{1})&0\\\sin(\theta _{1})&\cos(\theta _{1})&0\\0&0&1\end{pmatrix}}$

相對於第一杆件，第二杆件的位置由下式給出：

$T_{2}(\theta _{2})={\begin{pmatrix}\cos(\theta _{2})&-\sin(\theta _{2})&l_{1}\\\sin(\theta _{2})&\cos(\theta _{2})&0\\0&0&1\end{pmatrix}}$

這對應於一個角度為 $\theta _{2}$ 的旋轉和距離為 $l_{1}$ 的平移，其中 $l_{1}$ 是第一杆件的長度。

相對於第二杆件，第三杆件的位置由下式給出：

$T_{3}(\theta _{3})={\begin{pmatrix}\cos(\theta _{3})&-\sin(\theta _{3})&l_{2}\\\sin(\theta _{3})&\cos(\theta _{3})&0\\0&0&1\end{pmatrix}}$

相對於第三杆件，末端執行器的位置由下式給出：

$T_{4}={\begin{pmatrix}1&0&l_{3}\\0&1&0\\0&0&1\end{pmatrix}}$

正向運動學問題的解法如下：

$_{bs}^{ee}T=\,_{0}^{4}T(\theta _{1},\theta _{2},\theta _{3})={\begin{pmatrix}c_{123}&-s_{123}&l_{1}c_{1}+l_{2}c_{12}+l_{3}c_{123}\\s_{123}&c_{123}&l_{1}s_{1}+l_{2}s_{12}+l_{3}s_{123}\\0&0&1\end{pmatrix}}$

因此

${\begin{pmatrix}x\\y\\1\end{pmatrix}}=T{\begin{pmatrix}0\\0\\1\end{pmatrix}}$

最終得到的 **運動學方程** 為

${\begin{array}{l}x=l_{1}\cos \theta _{1}+l_{2}\cos(\theta _{1}+\theta _{2})+l_{3}\cos(\theta _{1}+\theta _{2}+\theta _{3})\\y=l_{1}\sin \theta _{1}+l_{2}\sin(\theta _{1}+\theta _{2})+l_{3}\sin(\theta _{1}+\theta _{2}+\theta _{3})\end{array}}$

逆運動學

逆運動學問題是正運動學問題的反面，可以概括為：給定末端執行器的期望位置，可以使用哪些關節角組合來實現該位置？

逆運動學問題導致相同末端執行器位置和方向的兩種不同解決方案的示例。

可以考慮兩種型別的解決方案：閉式解和數值解。閉式解或解析解是一組完全描述末端執行器位置和關節角之間關係的方程。數值解是透過使用數值演算法找到的，即使沒有閉式解也可以存在。也可能存在多個解，或者根本沒有解。

示例：平面三連桿機械臂

這個二維機械臂的逆運動學問題可以用代數方法很容易地解決。

從前面的結果（為簡單起見，這裡將忽略$l_{3}$上的位移）

$_{0}^{3}T={\begin{pmatrix}c_{123}&-s_{123}&0&l_{1}c_{1}+l_{2}c_{12}\\s_{123}&c_{123}&0&l_{1}s_{1}+l_{2}s_{12}\\0&0&1&0\\0&0&0&1\\\end{pmatrix}}$

現在假設給定的末端執行器方向如下所示

$_{bs}^{ee}T={\begin{pmatrix}c_{\phi }&-s_{\phi }&0&x\\s_{\phi }&c_{\phi }&0&y\\0&0&1&0\\0&0&0&1\\\end{pmatrix}}$

將這兩個表示式相等得到

${\begin{array}{lll}c_{\phi }&=&c_{123}\\s_{\phi }&=&s_{123}\\x&=&l_{1}c_{1}+l_{2}c_{12}\\y&=&l_{1}s_{1}+l_{2}s_{12}\\\end{array}}$

如

${\begin{array}{lll}c_{12}&=&c_{1}c_{2}-s_{1}s_{2}\\s_{12}&=&c_{1}s_{2}+s_{1}c_{2}\\\end{array}}$,

將 $x$ 和 $y$ 的表示式分別平方，然後將它們相加，得到

$x^{2}+y^{2}=l_{1}^{2}+l_{2}^{2}+2l_{1}l_{2}c_{2}$

解出 $c_{2}$，得到

$c_{2}={\frac {x^{2}+y^{2}-l_{1}^{2}-l_{2}^{2}}{2l_{1}l_{2}}}$,

而 $s_{2}$ 等於

$s_{2}=\pm {\sqrt {1-c_{2}^{2}}}$,

最後，$\theta _{2}$

$\theta _{2}={\mbox{Atan2}}(s_{2},c_{2})$

注意: $s_{2}$ 符號的選擇對應於上圖中的兩個解之一。

現在可以根據 $\theta _{1}$ 來解出 $x$ 和 $y$ 的表示式。為此，將它們寫成如下形式

${\begin{array}{lll}x&=&k_{1}c_{1}-k_{2}s_{1}\\y&=&k_{1}s_{1}+k_{2}c_{1}\\\end{array}}$

其中 $k_{1}=l_{1}+l_{2}c_{2}$，以及 $k_{2}=l_{2}s_{2}$.

令

${\begin{array}{lll}r&=&{\sqrt {k_{1}^{2}+k_{2}^{2}}}\\\gamma &=&{\mbox{Atan2}}(k2,k1)\\\end{array}}$

然後

${\begin{array}{lll}k_{1}&=&r\cos \gamma \\k_{2}&=&r\sin \gamma \\\end{array}}$

將這些應用到上面關於 $x$ 和 $y$ 的等式中

${\begin{array}{lll}x/r&=&\cos \gamma \,c_{1}+\sin \gamma \,s_{1}\\y/r&=&\cos \gamma \,s_{1}+\sin \gamma \,c_{1}\\\end{array}}$,

或

${\begin{array}{lll}\cos(\gamma +\theta _{1})&=&{\frac {x}{r}}\\\sin(\gamma +\theta _{1})&=&{\frac {y}{r}}\\\end{array}}$

因此

$\gamma +\theta _{1}={\mbox{Atan2}}(y,x)$

因此

$\theta _{1}={\mbox{Atan2}}(y,x)-{\mbox{Atan2}}(k_{2},k_{1})$

注意：如果 $x=y=0$，$\theta _{1}$ 實際上變得任意。

$\theta _{3}$ 現在可以透過前兩個方程求解 $s_{\phi }$ 和 $c_{\phi }$

$\theta _{3}=\phi -\theta _{1}-\theta _{2}={\mbox{Atan2}}(s_{\phi },c_{\phi })-\theta _{1}-\theta _{2}$