技術特征：

1.一種五自由度串聯(lián)機器人的逆運動學通用求解方法，其特征在于，包括如下步驟：

步驟1：求解機器人的肩部關節(jié)角度θ₁

根據(jù)指數(shù)積模型，機器人運動學方程可表示為：

$g=e^{{\mathrm{\θ}}_1{\widehat{\mathrm{\ξ}}}_1}{e}^{{\mathrm{\θ}}_2{\widehat{\mathrm{\ξ}}}_2}{e}^{{\mathrm{\θ}}_3{\widehat{\mathrm{\ξ}}}_3}{e}^{{\mathrm{\θ}}_4{\widehat{\mathrm{\ξ}}}_4}{e}^{{\mathrm{\θ}}_5{\widehat{\mathrm{\ξ}}}_5},g=g_{wt}\left(\mathrm{\θ}\right)g_{wt}{\left(0\right)}^{-1}---\left(1\right);$

且

$e^{{\mathrm{\θ}}_i{\widehat{\mathrm{\ξ}}}_i}=\left[\begin{array}{cc}{e}^{{\mathrm{\θ}}_i{\widehat{\mathrm{\ω}}}_i}& (I_{3\×3}-e^{{\mathrm{\θ}}_i{\widehat{\mathrm{\ω}}}_i})r_i\\ 0& 1\end{array}\right],i=1,...,5---\left(2\right);$

其中，下標t和w分別表示末端工具坐標系與世界坐標系，θ是各關節(jié)的旋轉角度向量θ＝[θ₁,...,θ₅]，g_wt(0)和g_wt(θ)分別表示在初始狀態(tài)下和θ狀態(tài)下末端工具坐標系相對世界坐標系的變換關系，為第i關節(jié)的運動旋量，包括關節(jié)軸的單位方向向量ω_i和軸上的任意一點r_i，ω_i和r_i被稱為旋量參數(shù)，為第i關節(jié)坐標變換的指數(shù)表達形式，為是旋轉矩陣的指數(shù)表達形式，其Rodrigues表達形式為：

$e^{{\mathrm{\θ}}_i{\widehat{\mathrm{\ω}}}_i}=I+{\mathrm{sin\θ}}_i{\widehat{\mathrm{\ω}}}_i+(1-{\mathrm{cos\θ}}_i){\widehat{\mathrm{\ω}}}_i^2,i=1,...,5---\left(3\right);$

空間任一向量p的齊次坐標用表示；

利用消元法消去機器人的腕部關節(jié)，設r₃是腕部關節(jié)的交點，將公式(1)兩邊同乘以可得：

$\tilde{q}=e^{{\mathrm{\θ}}_1{\widehat{\mathrm{\ξ}}}_1}{e}^{{\mathrm{\θ}}_2{\widehat{\mathrm{\ξ}}}_2}\tilde{p}---\left(5\right);$

其中根據(jù)旋量理論中的距離相等原則可知：

||c-r₂||＝||p-r₂|| (6)；

將帶入公式(6)，兩邊平方后進行整理，并利用的Rodrigues旋轉表達式將其化簡為關于θ₁的三角函數(shù)公式：

x₁sinθ₁+y₁cosθ₁＝z₁ (11)；

其中為已知參數(shù)，則根據(jù)公式(11)可解得θ₁為：

${\mathrm{\θ}}_1=arc\tan \left(\frac{\±z_1}{\sqrt{x_1^2+y_1^2-z_1^2}}\right)-arctan\left(\frac{y_1}{x_1}\right)---\left(14\right);$

其中

步驟2：求解機器人的肘部關節(jié)角度θ₂

將θ₁的值帶入中可獲得c的值，而c還可表示為：

$c=e^{{\mathrm{\θ}}_2{\widehat{\mathrm{\ω}}}_2}(p-r_2)+r_2---\left(15\right);$

將的Rodrigues表達式(3)帶入式(15)，整理可得：

x₂sinθ₂+y₂cosθ₂＝z₂ (16)；

其中均已知，則根據(jù)公式(16)可解得θ₂的表達式為：

${\mathrm{\θ}}_2=arctan\left(\frac{x_2^T{z}_2}{y_2^T{z}_2}\right)---\left(19\right);$

θ₂角度的具體象限由和的符號決定；

步驟3：求解機器人的腕部前兩個關節(jié)角度θ₃和θ₄

將θ₁和θ₂帶入公式(1)，并將已知項移到公式(1)的左邊，可得：

$e^{-{\mathrm{\θ}}_2{\widehat{\mathrm{\ξ}}}_2}{e}^{-{\mathrm{\θ}}_1{\widehat{\mathrm{\ξ}}}_1}g=e^{{\mathrm{\θ}}_3{\widehat{\mathrm{\ξ}}}_3}{e}^{{\mathrm{\θ}}_4{\widehat{\mathrm{\ξ}}}_4}{e}^{{\mathrm{\θ}}_5{\widehat{\mathrm{\ξ}}}_5}---\left(20\right);$

將式(20)兩邊同乘以且r₅≠r₃，易知可得：

${\tilde{q}}_1=e^{{\mathrm{\θ}}_3{\widehat{\mathrm{\ξ}}}_3}{e}^{{\mathrm{\θ}}_4{\widehat{\mathrm{\ξ}}}_4}{\tilde{p}}_1---\left(21\right);$

其中，可見公式(21)與公式(5)的形式相同，則根據(jù)θ₁和θ₂的表達式可給出θ₃和θ₄的表達式；

${\mathrm{\θ}}_3=arctan\left(\frac{\±z_3}{\sqrt{x_3^2+y_3^2-z_3^2}}\right)-arctan\left(\frac{y_3}{x_3}\right)---\left(22\right);$

${\mathrm{\θ}}_4=arctan\left(\frac{x_4^T{z}_4}{y_4^T{z}_4}\right)---\left(23\right);$

其中，且θ₄所在象限由和的符號來決定；

步驟4：求解機器人的腕部末端關節(jié)角度θ₅

將θ₁、θ₂、θ₃和θ₄帶入公式(1)，并將已知項移到公式(1)的左邊，可得：

$e^{-{\mathrm{\θ}}_4{\widehat{\mathrm{\ξ}}}_4}{e}^{-{\mathrm{\θ}}_3{\widehat{\mathrm{\ξ}}}_3}{e}^{-{\mathrm{\θ}}_2{\widehat{\mathrm{\ξ}}}_2}{e}^{-{\mathrm{\θ}}_1{\widehat{\mathrm{\ξ}}}_1}g=e^{{\mathrm{\θ}}_5{\widehat{\mathrm{\ξ}}}_5}---\left(24\right);$

將式(24)兩邊同乘以除r₅以外的點，這里取點p₂，可得：

${\tilde{q}}_2=e^{{\mathrm{\θ}}_5{\widehat{\mathrm{\ξ}}}_5}{\tilde{p}}_2---\left(25\right);$

其中，易得：

$q_2=e^{{\mathrm{\θ}}_5{\widehat{\mathrm{\ω}}}_5}(p_2-r_5)+r_5---\left(26\right);$

公式(26)與公式(15)形式相同，則可直接得出角度θ₅的表達式

${\mathrm{\θ}}_5=arctan\left(\frac{x_5^T{z}_5}{y_5^T{z}_5}\right)---\left(27\right);$

其中，均已知，且θ₅所在象限由和的符號來決定。