CN105171744A - 五自由度旋转链式码垛机械臂的运动控制方法 - Google Patents

Abstract

本发明公开了一种五自由度旋转链式码垛机械臂的运动控制方法，适用于全旋转的链式机械臂，通过对给定的末端运动轨迹进行运动学反解，得到其对应的各驱动关节处的转动角度信息，并下发至运动控制卡，通过伺服驱动系统驱动各臂转动，进而完成给定的运动轨迹，通过传感器检测并反馈各臂转动角度，以提高末端控制器所走出轨迹的精度。本发明的方法针对五自由度链式码垛机械臂的工作特征，通过坐标系的移动，使得反解的过程变得简单。

Description

五自由度旋转链式码垛机械臂的运动控制方法

技术领域

本发明涉及机器人的运动控制领域，尤其涉及一种五自由度旋转链式码垛机械臂的运动控制方法。

背景技术

随着码垛机械臂在工业领域中的广泛应用，针对码垛机械臂控制系统的研究便具有重要意义，其中运动反解问题是控制码垛机械臂运动的先决条件。通过反解运算，可将机械臂空间位姿转换为各转动关节变量，进而通过控制转动关节的转动角度实现机构的运动，完成给定的运动轨迹。对于全旋转的链式机械臂，可用单个连杆变换矩阵左乘末端的齐次变换矩阵F，依次得到各臂的转动角度值，但如此会出现许多角度的耦合，对计算带来不便。

发明内容

本发明提供了一种五自由度链式码垛机械臂的运动控制方法，以克服反解过程中角度耦合给计算带来的困扰。

本发明的五自由度旋转链式码垛机械臂的运动控制方法，包括如下步骤：

1)获取机械臂末端运动轨迹曲线，在该运动轨迹曲线上，连续选取包含起始点和终止点的n个坐标点，记为P_i(x_i,y_i,z_i)，其中i＝1,…,n；

2)对选取的每一坐标点P_i(x_i,y_i,z_i)进行运动学反解计算，得到该坐标点对应的各驱动关节处的转动角度信息；从而获得运动轨迹曲线与各臂转动角度之间的对应关系；

3)将上述对应关系下发至运动控制卡，通过伺服驱动系统驱动各臂转动，通过传感器检测并反馈转动信号，各臂联动完成给定的运动轨迹。

对于五自由度旋转链式机械臂，采用D-H方法描述该机械臂，对所有的关节建立各自的坐标系，如图1所示。

其中，臂1可绕z轴转动，其转动角度记为θ₁，所在坐标系记为{1}；

臂2长度为l₂，与臂1间距离为d₂，可相对臂1转动，其转动角度记为θ₂，所在坐标系记为{2}；

臂3长度为l₃，与臂2间距离为d₃，可相对臂2转动，其转动角度记为θ₃，所在坐标系记为{3}；

臂4长度为l₄，与臂3间距离为d₄，可相对臂3转动，其转动角度记为θ₄，所在坐标系记为{4}；

末端执行器5可相对臂4绕z轴转动，其转动角度记为θ₅，所在坐标系记为{5}；

末端n、o、a分别为法向向量、方位向量和接近向量，用于描述末端执行器5的姿态。

根据图1得到的码垛机械臂的连杆参数如表1所示。

表1码垛机械臂的连杆参数

k	ak-1	αk-1	dk	θk	关节变量的范围
						1	0	0°	0	θ1(0°)	θ1∈(-180°,180°)

2	0	-90°	-d2	θ2(0°)	θ2∈(-180°,45°)
						3	l2	0°	d3	θ3(0°)	θ3∈(-90°,90°)
4	l3	0°	d4	θ4(0°)	θ4＝θ3-θ2
						5	l4	90°	0	θ5(90°)	θ5∈(-180°,180°)

码垛机械臂中连杆间变换的通式为：

$T_k^{k-1}\left({\mathrm{\θ}}_k\right)=\left[\begin{array}{cccc}{\mathrm{cos\θ}}_k& -{\mathrm{sin\θ}}_k& 0& a_{k-1}\\ {\mathrm{sin\θ}}_k{\mathrm{cos\α}}_{k-1}& {\mathrm{cos\θ}}_k{\mathrm{cos\α}}_{k-1}& -{\mathrm{sin\α}}_{k-1}& -d_k{\mathrm{sin\α}}_{k-1}\\ {\mathrm{sin\θ}}_k{\mathrm{sin\α}}_{k-1}& {\mathrm{cos\θ}}_k{\mathrm{sin\α}}_{k-1}& {\mathrm{cos\α}}_{k-1}& d_k{\mathrm{cos\α}}_{k-1}\\ 0& 0& 0& 1\end{array}\right]---\left(1\right)$

由通式(1)可知连杆变换依赖于四个参数a_k-1，α_k-1，d_k和θ_k，其中a_k-1表示关节偏移，α_k-1表示关节扭角，d_k表示臂k与k-1间的距离，θ_k表示臂k的旋转角度。对于转动关节k而言，是θ_k的函数。

将表1中的连杆参数代入通式(1)可得各连杆的变换矩阵：

${}_1^{0}T\left({\mathrm{\θ}}_1\right)=\left[\begin{array}{cccc}{\mathrm{cos\θ}}_1& -{\mathrm{sin\θ}}_1& 0& 0\\ {\mathrm{sin\θ}}_1& {\mathrm{cos\θ}}_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]{;}_2^{1}T\left({\mathrm{\θ}}_2\right)=\left[\begin{array}{cccc}{\mathrm{cos\θ}}_2& -{\mathrm{sin\θ}}_2& 0& 0\\ 0& 0& 1& -d_2\\ -{\mathrm{sin\θ}}_2& -{\mathrm{cos\θ}}_2& 0& 0\\ 0& 0& 0& 1\end{array}\right];$

${}_3^{2}T\left({\mathrm{\θ}}_3\right)=\left[\begin{array}{cccc}{\mathrm{cos\θ}}_3& -{\mathrm{sin\θ}}_3& 0& l_2\\ {\mathrm{sin\θ}}_3& {\mathrm{cos\θ}}_3& 0& 0\\ 0& 0& 1& d_3\\ 0& 0& 0& 1\end{array}\right]{;}_4^{3}T\left({\mathrm{\θ}}_4\right)=\left[\begin{array}{cccc}{\mathrm{cos\θ}}_4& -{\mathrm{sin\θ}}_4& 0& l_3\\ {\mathrm{sin\θ}}_4& {\mathrm{cos\θ}}_4& 0& 0\\ 0& 0& 1& d_4\\ 0& 0& 0& 1\end{array}\right];$

${}_5^{4}T\left({\mathrm{\θ}}_5\right)=\left[\begin{array}{cccc}{\mathrm{cos\θ}}_5& -{\mathrm{sin\θ}}_5& 0& l_4\\ 0& 0& -1& 0\\ {\mathrm{sin\θ}}_5& {\mathrm{cos\θ}}_5& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

步骤2)中所述的对坐标点P_i(x_i,y_i,z_i)进行运动学反解计算，具体包括如下步骤：

1)将选取的坐标点P_i(x_i,y_i,z_i)由末端执行器5所在的坐标系{5}移动到臂4所在的坐标系{4}中，得到运动轨迹曲线上的点在坐标系{4}中相应的坐标(x_i+l₄sinθ₁,y_i-l₄cosθ₁,z_i)；其中l₄为臂4的长度，θ₁为臂1的转动角度；

2)不考虑机械臂末端执行器转动角度时，坐标系{4}处的齐次变换矩阵可表示为：

$F^{\′}=\left[\begin{array}{cccc}{n}_x^{\′}& o_x^{\′}& a_x^{\′}& x_i+l_4{\mathrm{sin\θ}}_1\\ n_y^{\′}& o_y^{\′}& a_y^{\′}& y_i-l_4{\mathrm{cos\θ}}_1\\ n_z^{\′}& o_z^{\′}& a_z^{\′}& z_i\\ 0& 0& 0& 1\end{array}\right]$

其中，n′、o′、a′分别为法向向量、方位向量和接近向量，用于描述臂4的姿态，

由 ${}_1^{0}T{\left({\mathrm{\θ}}_1\right)}^{-1}{F}^{\′}{=}_2^{1}T{\left({\mathrm{\θ}}_2\right)}_3^{2}T{\left({\mathrm{\θ}}_3\right)}_4^{3}T\left({\mathrm{\θ}}_4\right),$ 其中为连杆k相对连杆k-1的变换矩阵，得：

$\left\{\begin{array}{c}(x_i+l_{4}sin{\mathrm{\θ}}_1)cos{\mathrm{\θ}}_1+(y_i-l_{4}cos{\mathrm{\θ}}_1)sin{\mathrm{\θ}}_1=l_{2}cos{\mathrm{\θ}}_2+l_{3}cos({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)---\left(2\right)\\ (x_i+l_4\sin {\mathrm{\θ}}_1)\sin {\mathrm{\θ}}_1-(y_i-l_4\cos {\mathrm{\θ}}_1)\cos {\mathrm{\θ}}_1=d_2-d_3-d_4---\left(3\right)\\ z_i=-l_2\sin {\mathrm{\θ}}_2-l_3\sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)---\left(4\right)\end{array}\right.$

其中，l₂、l₃分别为臂2和臂3的长度，θ₂、θ₃分别为臂2、臂3的转动角度，d₂、d₃、d₄分别为臂2与1间的距离、臂3与2间的距离、臂4与3间的距离，

由(3)式可得，

$tan({\mathrm{\θ}}_1-\mathrm{\α})=(d_2-d_3-d_4-l_4)/\sqrt{{x_i}^2+{y_i}^2-{(d_2-d_3-d_4-l_4)}^2},$ 其中tanα＝y_i/x_i，求解得到坐标点P_i(x_i,y_i,z_i)相应的臂1的转动角度θ₁；3)由步骤2)中(2)、(4)式可得，

${\mathrm{tan\θ}}_3=\frac{\sqrt{{\left(2l_2{l}_3\right)}^2-{\left(\right({\left(x_i{\mathrm{cos\θ}}_1+y_i{\mathrm{sin\θ}}_1\right)}^2+{z_i}^2)-({l_2}^2+{l_3}^2\left)\right)}^2}}{({\left(x_i{\mathrm{cos\θ}}_1+y_i{\mathrm{sin\θ}}_1\right)}^2+{z_i}^2)-({l_2}^2+{l_3}^2)},$ 求解得到坐标点P_i(x_i,y_i,z_i)相应的臂3的转动角度θ₃；

4)由步骤2)中(4)式可得，

$tan({\mathrm{\θ}}_2+\mathrm{\β})=\frac{-z_i}{\sqrt{-{l_2}^2+{l_3}^2+2l_2{l}_3{\mathrm{cos\θ}}_3-{z_i}^2}},$

其中 $tan\mathrm{\β}=\frac{l_2{\mathrm{sin\θ}}_3}{(l_2+l_3{\mathrm{cos\θ}}_3)},$

求解得到坐标点P_i(x_i,y_i,z_i)相应的臂2的转动角度θ₂；

5)根据θ₄＝θ₃-θ₂，得到坐标点P_i(x_i,y_i,z_i)相应的臂4的转动角度θ₄；

6)考虑机械臂末端执行器的转动角度时，码垛机械臂末端执行器位姿的齐次变换矩阵可表示为：

$F=\left[\begin{array}{cccc}{n}_x& o_x& a_x& x_i\\ n_y& o_y& a_y& y_i\\ n_z& o_z& a_z& z_i\\ 0& 0& 0& 1\end{array}\right],$

其中n、o、a分别为法向向量、方位向量和接近向量，用于描述末端执行器5的姿态；

根据与连杆变换方程联立，码垛机械臂的运动学方程可表示为 $F{=}_1^{0}T{\left({\mathrm{\θ}}_1\right)}_2^{1}T{\left({\mathrm{\θ}}_2\right)}_3^{2}T{\left({\mathrm{\θ}}_3\right)}_4^{3}T{\left({\mathrm{\θ}}_4\right)}_5^{4}T\left({\mathrm{\θ}}_5\right),$

得到：

$tan({\mathrm{\θ}}_5+\mathrm{\γ})=\frac{x_i}{\sqrt{{{\mathrm{cos\θ}}_1}^2+cos{({\mathrm{\θ}}_2+{\mathrm{\θ}}_3+{\mathrm{\θ}}_4)}^2+{{\mathrm{sin\θ}}_1}^2-{o_x}^2}},$ 其中 $tan\mathrm{\γ}=\frac{{\mathrm{sin\θ}}_1}{{\mathrm{cos\θ}}_{1}cos({\mathrm{\θ}}_2+{\mathrm{\θ}}_3+{\mathrm{\θ}}_4)},$

求解得到坐标点P_i(x_i,y_i,z_i)相应的臂5的转动角度θ₅。

本发明方法的有益效果：对于全旋转的链式机械臂，可用单个连杆变换矩阵左乘末端的齐次变换矩阵F，依次得到各臂的转动角度值，但如此会出现许多角度的耦合，对计算带来不便。本发明的方法针对五自由度链式码垛机械臂的工作特征，即臂4始终处于水平工作状态，通过坐标系的移动，使得反解的过程变得简单。

附图说明

图1是五自由度链式机械臂的D-H模型；

图2是本发明的方法的流程。

图3是两种具体的运动轨迹曲线，其中a)为向低处放置物品，b)为向高处放置物品。

图4是对运动轨迹进行运动学反解的流程。

具体实施方式

下面结合附图对本发明作进一步说明。

本发明的对五自由度旋转链式机械臂的运动控制方法，其流程如图2所示，码垛机械臂的运动控制流程包括获取曲线段运动轨迹坐标、反解得到各臂转动角度、下发转动角度信号至运动控制卡、伺服驱动系统、传感器检测、各臂联动完成运动轨迹等。

获取曲线段运动轨迹坐标和反解得到各臂转动角度的过程由工控机完成，工控机与运动控制卡通过PCI总线进行通讯。

具体过程如下：

1)获取机械臂末端运动轨迹曲线，如图3给出了两种运动轨迹曲线，a)为向低处放置物品，b)为向高处放置物品；在该运动轨迹曲线上，连续选取包含起始点和终止点的n个坐标点，记为P_i(x_i,y_i,z_i)，其中i＝1,…,n；

2)对选取的每一坐标点P_i(x_i,y_i,z_i)进行运动学反解计算，得到该坐标点对应的各驱动关节处的转动角度信息；从而获得运动轨迹曲线与各臂转动角度之间的对应关系；

码垛机械臂的运动反解依托于经规划得到的曲线段运动轨迹。对得到的曲线段运动轨迹进行运动学反解计算，得到各驱动关节处的转动角度。如图4所示，具体过程如下：

考虑到码垛机械臂的工作特征，即臂4在工作过程中始终保持与地面平行，故臂4的坐标系{4}和末端执行器5的坐标系{5}始终在同一平面上，因此坐标系{4}的坐标(x_i4,y_i4,z_i4)和坐标系{5}的坐标(x_i5,y_i5,z_i5)之间存在如下的对应关系：

$\left\{\begin{array}{c}{x}_{i4}=x_{i5}+l_4{\mathrm{sin\θ}}_1\\ y_{i4}=y_{i5}-l_4{\mathrm{cos\θ}}_1\\ z_{i4}=z_{i5}\end{array}\right.---\left(5\right)$

如此，可先将末端运动轨迹由坐标系{5}移动到坐标系{4}，在此基础上进行运动学分析。

对末端轨迹坐标点P_i(x_i,y_i,z_i)，即坐标系{5}的坐标为(x_i,y_i,z_i)的点，根据(5)式可知，其在坐标系{4}中的坐标为

(x_i+l₄sinθ₁,y_i-l₄cosθ₁,z_i)。

由 $tan({\mathrm{\θ}}_1-\mathrm{\α})=(d_2-d_3-d_4-l_4)/\sqrt{{x_i}^2+{y_i}^2-{(d_2-d_3-d_4-l_4)}^2},$ 其中tanα＝y_i/x_i，求解得到坐标点P_i(x_i,y_i,z_i)相应的臂1的转动角度θ₁；

由 ${\mathrm{tan\θ}}_3=\frac{\sqrt{{\left(2l_2{l}_3\right)}^2-{\left(\right({\left(x_i{\mathrm{cos\θ}}_1+y_i{\mathrm{sin\θ}}_1\right)}^2+{z_i}^2)-({l_2}^2+{l_3}^2\left)\right)}^2}}{({\left(x_i{\mathrm{cos\θ}}_1+y_i{\mathrm{sin\θ}}_1\right)}^2+{z_i}^2)-({l_2}^2+{l_3}^2)},$ 求解得到坐标点P_i(x_i,y_i,z_i)相应的臂3的转动角度θ₃；

由 $tan({\mathrm{\θ}}_2+\mathrm{\β})=\frac{-z_i}{\sqrt{-{l_2}^2+{l_3}^2+2l_2{l}_3{\mathrm{cos\θ}}_3-{z_i}^2}},$

其中求解得到坐标点P_i(x_i,y_i,z_i)相应的臂2的转动角度θ₂；

根据θ₄＝θ₃-θ₂，得到坐标点P_i(x_i,y_i,z_i)相应的臂4的转动角度θ₄；由 $tan({\mathrm{\θ}}_5+\mathrm{\γ})=\frac{x_i}{\sqrt{{{\mathrm{cos\θ}}_1}^2+cos{({\mathrm{\θ}}_2+{\mathrm{\θ}}_3+{\mathrm{\θ}}_4)}^2+{{\mathrm{sin\θ}}_1}^2-{o_x}^2}},$ 其中 $tan\mathrm{\γ}=\frac{{\mathrm{sin\θ}}_1}{{\mathrm{cos\θ}}_{1}cos({\mathrm{\θ}}_2+{\mathrm{\θ}}_3+{\mathrm{\θ}}_4)},$ 求解得到坐标点P_i(x_i,y_i,z_i)相应的臂5的转动角度θ₅。

对每个选取的点做运动学反解，获得运动轨迹曲线与各臂转动角度之间的对应关系。

3)将上述对应关系下发至运动控制卡，通过伺服驱动系统驱动各臂转动，通过传感器检测并反馈转动信号，各臂联动完成给定的运动轨迹。

具体可采用六轴运动控制卡，实现对五台伺服直流电机的位置控制。运动控制卡与工控机间采用PCI总线进行数据传输。工控机完成运动轨迹坐标反解后，将得到的各臂转动角度信号下发给运动控制卡，经运动控制卡处理后，生成伺服直流电机驱动信号，通过控制伺服驱动器进而控制电机的转动角度和速度。

伺服驱动器接受来自运动控制卡的控制信号，并按照控制指令完成对伺服直流电机转动角度和速度的控制。

传感器用于检测各臂转动角度信号，并将信号反馈给运动控制卡，实现对轨迹的精确控制。

在伺服直流电机的作用下，各臂联动，使末端走出给定的运动轨迹。

Claims (2)

1.一种五自由度旋转链式码垛机械臂的运动控制方法，其特征在于，包括如下步骤：

1)获取机械臂末端运动轨迹曲线，在该运动轨迹曲线上，连续选取包含起始点和终止点的n个坐标点，记为P_i(x_i,y_i,z_i)，其中i＝1,...,n；

2)对选取的每一坐标点P_i(x_i,y_i,z_i)进行运动学反解计算，得到该坐标点对应的各驱动关节处的转动角度信息；从而获得运动轨迹曲线与各臂转动角度之间的对应关系；

3)将上述对应关系下发至运动控制卡，通过伺服驱动系统驱动各臂转动，通过传感器检测并反馈转动信号，各臂联动完成给定的运动轨迹。

2.根据权利要求1所述的五自由度旋转链式码垛机械臂的运动控制方法，其特征在于，步骤2)中所述的对坐标点P_i(x_i,y_i,z_i)进行运动学反解计算，具体包括如下步骤：

1)将选取的坐标点P_i(x_i,y_i,z_i)由末端执行器5所在的坐标系{5}移动到臂4所在的坐标系{4}中，得到运动轨迹曲线上的点在坐标系{4}中相应的坐标(x_i+l₄sinθ₁,y_i-l₄cosθ₁,z_i)；其中l₄为臂4的长度，θ₁为臂1的转动角度；

2)不考虑机械臂末端执行器转动角度时，坐标系{4}处的齐次变换矩阵可表示为：

$F^{\′}=\left[\begin{array}{cccc}{n}_x^{\′}& o_x^{\′}& a_x^{\′}& x_i+l_4{\mathrm{sin\θ}}_1\\ n_y^{\′}& o_y^{\′}& a_y^{\′}& y_i-l_4{\mathrm{cos\θ}}_1\\ n_z^{\′}& o_z^{\′}& a_z^{\′}& z_i\\ 0& 0& 0& 1\end{array}\right]$

其中，n′、o′、a′分别为法向向量、方位向量和接近向量，用于描述臂4的姿态，

由 ${}_1^{0}T{\left({\mathrm{\θ}}_1\right)}^{-1}{F}^{\′}{=}_2^{1}T{\left({\mathrm{\θ}}_2\right)}_3^{2}T{\left({\mathrm{\θ}}_3\right)}_4^{3}T\left({\mathrm{\θ}}_4\right),$ 其中为连杆k相对连杆k-1的变换矩阵，得：

$\left\{\begin{array}{c}(x_i+l_{4}sin{\mathrm{\θ}}_1)cos{\mathrm{\θ}}_1+(y_i-l_{4}cos{\mathrm{\θ}}_1)sin{\mathrm{\θ}}_1=l_{2}cos{\mathrm{\θ}}_2+l_{3}cos({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)---\left(1\right)\\ (x_i+l_4\sin {\mathrm{\θ}}_1)\sin {\mathrm{\θ}}_1-(y_i-l_4\cos {\mathrm{\θ}}_1)\cos {\mathrm{\θ}}_1=d_2-d_3-d_4---\left(2\right)\\ z_i=-l_2\sin {\mathrm{\θ}}_2-l_3\sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)---\left(3\right)\end{array}\right.$

其中，l₂、l₃分别为臂2和臂3的长度，θ₂、θ₃分别为臂2、臂3的转动角度，d₂、d₃、d₄分别为臂2与1间的距离、臂3与2间的距离、臂4与3间的距离，

由(2)式可得，

$tan({\mathrm{\θ}}_1-\mathrm{\α})=(d_2-d_3-d_4-l_4)/\sqrt{{x_i}^2+{y_i}^2-{(d_2-d_3-d_4-l_4)}^2},$ 其中tanα＝y_i/x_i，求解得到坐标点P_i(x_i,y_i,z_i)相应的臂1的转动角度θ₁；3)由步骤2)中(1)、(3)式可得，

${\mathrm{tan\θ}}_3=\frac{\sqrt{{\left(2l_2{l}_3\right)}^2-{\left(\right({\left(x_i{\mathrm{cos\θ}}_1+y_i{\mathrm{sin\θ}}_1\right)}^2+{z_i}^2)-({l_2}^2+{l_3}^2\left)\right)}^2}}{({\left(x_i{\mathrm{cos\θ}}_1+y_i{\mathrm{sin\θ}}_1\right)}^2+{z_i}^2)-({l_2}^2+{l_3}^2)},$ 求解得到坐标点P_i(x_i,y_i,z_i)相应的臂3的转动角度θ₃；

4)由步骤2)中(3)式可得，

$tan({\mathrm{\θ}}_2+\mathrm{\β})=\frac{-z_i}{\sqrt{{l_2}^2+{l_3}^2+2l_2{l}_3{\mathrm{cos\θ}}_3-{z_i}^2}},$

其中tan $\mathrm{\β}=\frac{l_2{\mathrm{sin\θ}}_3}{(l_2+l_3{\mathrm{cos\θ}}_3)},$

求解得到坐标点P_i(x_i,y_i,z_i)相应的臂2的转动角度θ₂；

5)根据θ₄＝θ₃-θ₂，得到坐标点P_i(x_i,y_i,z_i)相应的臂4的转动角度θ₄；

6)考虑机械臂末端执行器的转动角度时，码垛机械臂末端执行器位姿的齐次变换矩阵可表示为：

$F=\left[\begin{array}{cccc}{n}_x& o_x& a_x& x_i\\ n_y& o_y& a_y& y_i\\ n_z& o_z& a_z& z_i\\ 0& 0& 0& 1\end{array}\right],$

其中n、o、a分别为法向向量、方位向量和接近向量，用于描述末端执行器5的姿态；

根据与连杆变换方程联立，码垛机械臂的运动学方程可表示为

$F{=}_1^{0}T{\left({\mathrm{\θ}}_1\right)}_2^{1}T{\left({\mathrm{\θ}}_2\right)}_3^{2}T{\left({\mathrm{\θ}}_3\right)}_4^{3}T{\left({\mathrm{\θ}}_4\right)}_5^{4}T\left({\mathrm{\θ}}_5\right),$

得到：

$tan({\mathrm{\θ}}_5+\mathrm{\γ})=\frac{x_i}{\sqrt{{{\mathrm{cos\θ}}_1}^2+cos{({\mathrm{\θ}}_2+{\mathrm{\θ}}_3+{\mathrm{\θ}}_4)}^2+{{\mathrm{sin\θ}}_1}^2-{o_x}^2}},$ 其中 $tan\mathrm{\γ}=\frac{{\mathrm{sin\θ}}_1}{{\mathrm{cos\θ}}_{1}cos({\mathrm{\θ}}_2+{\mathrm{\θ}}_3+{\mathrm{\θ}}_4)},$

求解得到坐标点P_i(x_i,y_i,z_i)相应的臂5的转动角度θ₅。