CN104750116A - 一种双臂机器人协调操作的路径规划方法 - Google Patents

Abstract

本发明公开了一种双臂机器人协调操作的路径规划方法，双臂末端连接一个剪刀的两柄，首先获取双臂初始关节角、剪刀初始夹角以及剪刀终止夹角，并获取路径规划的总步数K_n；采用带抛物线过渡的速度梯形法对角度差θ_n规划获得剪刀左、右柄质心角速度的梯形速度曲线ω_c、ω_d；利用速度约束公式，根据剪刀柄质心的角速度ω_c、ω_d，求得剪刀柄质心的线速度曲线v_c、v_d；将ω_c、ω_d和v_c、v_d，代入双臂协调运动的约束方程，分别求得左臂末端的线速度曲线v_l和角速度曲线ω_l，右臂末端的线速度曲线v_r和角速度曲线ω_r；根据路径规划中的当前步数对应的时间，获取左右臂末端的角速度和线速度，并根据约束公式求解左右臂关节角速度；然后求解下一步数k+1的关节角度，由此顺次求解步数1～K_n的关节角度。

Description

一种双臂机器人协调操作的路径规划方法

技术领域

本发明涉及双臂机器人协调操作的路径规划方法，属于双臂路径规划技术领域。

背景技术

随着科技的不断发展，机械臂广泛应用于工厂车间的加工、装配以及空间站的组装、维护等工作，机械臂技术已经成为重要研究方向。近年来，双臂机器人已成为机器人领域中的研究热点之一。协调操作路径规划算法是双臂机器人控制中的关键技术，因此，对双臂机器人协调操作路径规划算法进行研究具有重要的意义。

目前，双臂机器人的主要研究内容为运动规划，协调控制算法和操作力(力矩)等方面。但这些方法的计算效率都比较低。

发明内容

有鉴于此，本发明提供了一种双臂机器人协调操作的路径规划方法，能够实现双臂的协调路径规划。

为了达到上述目的，本发明的技术方案为：双臂末端分别连接一个剪刀的两柄，双臂机器人的臂为二连杆结构，二连杆的连接处为一个关节，该方法包括以下步骤：

步骤1、将剪刀两柄分别记为C柄和D柄，以两柄连接处为原点建立两柄相连接处三维坐标系为{O_T}，以C柄质心为原点建立三维质心系{O_C}，以D柄的质心为原点建立三维质心系{O_D}，以左臂末端为原点建立三维坐标系{O_L}，以右臂末端为原点建立三维坐标系{O_R}；惯性系为{O₀}。

获取双臂关节的初始关节角、剪刀初始夹角以及剪刀终止夹角。

同时设置操作剪刀的整个运动规划过程中的总时间t_z、加速时间t_a和时间间隔t₀，由K_n＝t_a/t_o求得路径规划的总步数K_n。

步骤2、求解剪刀初始夹角与终止夹角的角度差θ_n。

步骤3、采用带抛物线过渡的速度梯形法对角度差θ_n的变化速度进行规划，获得剪刀左、右柄质心角速度的梯形速度曲线ω_c、ω_d；

步骤4、利用公式 $\left\{\begin{array}{c}{v}_c={\mathrm{\ω}}_c\×P_{\mathrm{TC}}^{\left(0\right)}\\ v_d={\mathrm{\ω}}_c\×P_{\mathrm{TD}}^{\left(0\right)}\end{array}\right.,$ 根据剪刀柄质心的角速度ω_c、ω_d，求得剪刀柄质心的线速度曲线v_c、v_d。

其中和分别为P_TC和P_TD在惯性系下的表示，P_TC为{O_C}相对于{O_T}的相对位置矢量，P_TD为{O_D}相对于{O_T}的相对位置矢量，R_OT为{O_T}相对于{O₀}的姿态变换矩阵。

步骤5、将上述求解得到的剪刀柄质心角速度曲线ω_c、ω_d和线速度曲线v_c、v_d，代入双臂协调运动的约束方程 $\left\{\begin{array}{cc}{\mathrm{\ω}}_l={\mathrm{\ω}}_c,& v_l=v_c+{\hat{P}}_C^{\left(0\right)}\·{\mathrm{\ω}}_c\\ {\mathrm{\ω}}_r={\mathrm{\ω}}_d,& v_r=v_d+{\hat{P}}_{\mathrm{RD}}^{\left(0\right)}\·{\mathrm{\ω}}_c\end{array}\right.,$ 分别求得左臂末端的线速度曲线v_l和角速度曲线ω_l，右臂末端的线速度曲线v_r和角速度曲线ω_r。

式中，分别为和的反对称矩阵，和分别为P_LC和P_RD在惯性系下的表示，R_OL为{O_L}相对于{O₀}的姿态变换矩阵，P_LC为{O_C}相对于{O_L}的相对位置矢量，R_OR为{O_R}相对于{O₀}的姿态变换矩阵，P_RD为{O_C}相对于{O_R}的相对位置矢量。

步骤6、针对路径规划中的当前步数k，求解当前步数对应的时间t_k，利用t_k在左臂末端的线速度曲线v_l和角速度曲线ω_l，右臂末端的线速度曲线v_r和角速度曲线ω_r中找到当前步数的左臂末端线速度v_l(k)和角速度ω_l(k)、右臂末端线速度v_r(k)和角速度ω_r(k)；其中k的初始值为1。

利用公式 $\left\{\begin{array}{c}{\stackrel{\·}{q}}_{L\left(k\right)}=J_{L_{0}L}^{-1}{\stackrel{\·}{X}}_{L\left(k\right)}\\ {\stackrel{\·}{q}}_{R\left(k\right)}=J_{R_{0}R}^{-1}{\stackrel{\·}{X}}_{R\left(k\right)}\end{array}\right.,$ 解算获得路径规划中当前步数k下左臂关节角速度以及右臂关节角速度

分别为左、右臂的雅可比矩阵， ${\stackrel{\·}{X}}_{L\left(k\right)}={[v_{l\left(k\right)},{\mathrm{\ω}}_{l\left(k\right)}]}^T,{\stackrel{\·}{X}}_{l\left(k\right)}={[v_{l\left(k\right)},{\mathrm{\ω}}_{l\left(k\right)}]}^T.$

步骤7、利用求解路径规划中下一步数k+1的关节角度，下一步数k+1下左臂关节角度以及右臂关节角度其中θ_l(k)为当前步数k下左臂关节角度，θ_r(k)为当前步数k下右臂关节角度；其中，其中双臂初始关节角已知。

步骤8、判断当前步数k是否达到总步数K_n，若已达到总步数，则路径规划结束，若没有达到总步数则k自增1，返回步骤6。

有益效果：

本发明主要涉及一种双臂机器人协调操作的路径规划算法，其优势在于可实现双臂的协调路径规划，通过姿态约束和速度约束的方式对双臂进行协调操作。

附图说明

图1是双臂操作剪刀示意图；

图2是剪刀夹断路径规划算法流程图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明提供了一种双臂机器人协调操作的路径规划算法，下面结合附图对本发明作进一步说明。操作物的局部结构能够活动，即至少具有一个自由度，此时双臂末端与操作物之间有相对运动，如剪刀、钳子等，如图1所示。

实施例1、运动学约束方程

双臂末端位姿约束：剪刀工件的两柄记为C和D，两柄相连接处建立坐标系{O_T}，分别在C和D的质心c和d处建立坐标系{O_C}和{O_D}，惯性坐标系为{O₀}，左、右臂的末端分别建立坐标系{O_L}和{O_R}。由此可建立各坐标系之间的齐次变换矩阵为：左臂末端系相对于惯性系T_0L，右臂末端系相对于惯性系T_0R，柄C质心系相对于左臂末端系T_LC，柄D质心系相对于右臂末端系T_RD，柄C和D质心系相对于连接处坐标系分别为T_TC和T_TD。各齐次矩阵分别对应各自的相对位置矢量和姿态变换矩阵，记为T_0L(P_0L,R_0L)，T_0R(P_0R,R_0R)，T_LC(P_LC,R_LC)，T_RD(P_RD,R_RD)，T_TC(P_TC,R_TC)和T_TD(P_TD,R_TD)。

根据图1形成的运动链，两臂之间存在的齐次变换矩阵约束方程为：

T_0LT_LCT_CT＝T_0RT_RDT_DT   (1)

由此可建立协调运动过程中的位置和姿态应满足约束方程为：

$\left\{\begin{array}{c}{P}_{0L}+R_{0L}{P}_{\mathrm{LC}}+R_{0C}{P}_{\mathrm{CT}}=P_{0R}+R_{0R}{P}_{\mathrm{RD}}+R_{0D}{P}_{\mathrm{DT}}=P_{0T}\\ R_{0L}{R}_{\mathrm{LC}}{R}_{\mathrm{CT}}=R_{0R}{R}_{\mathrm{RD}}{R}_{\mathrm{DT}}=R_{0T}\end{array}\right.---\left(2\right)$

一般情况下，两柄的运动是预先规划的，并可通过两柄连接处的运动来体现，即P_0T和R_0T是已知的，故可由式(2)分别得出左、右臂的末端位姿关系式为：

$M_{1P}:\left\{\begin{array}{c}{R}_{0L}=R_{0T}{R}_{\mathrm{CT}}^{-1}{R}_{\mathrm{LC}}^{-1}\\ P_{0L}=P_{0T}-R_{0L}{P}_{\mathrm{LC}}-R_{0C}{P}_{\mathrm{CT}}=P_{0T}-R_{0T}{R}_{\mathrm{CT}}^{-1}{R}_{\mathrm{LC}}^{-1}(P_{\mathrm{LC}}+P_{\mathrm{LC}}{P}_{\mathrm{CT}})\end{array}\right.---\left(3\right)$

$M_{2P}:\left\{\begin{array}{c}{R}_{0R}=R_{0T}{R}_{\mathrm{DT}}^{-1}{R}_{\mathrm{RD}}^{-1}\\ P_{0R}=P_{0T}-R_{0R}{P}_{\mathrm{RD}}-R_{0D}{P}_{\mathrm{DT}}=P_{0T}-R_{0T}{R}_{\mathrm{DT}}^{-1}{R}_{\mathrm{RD}}^{-1}(P_{\mathrm{RD}}+P_{\mathrm{RD}}{P}_{\mathrm{DT}})\end{array}\right.---\left(4\right)$

其中，P_LC和P_RD分别是柄C和D质心相对左、右臂末端的位置，R_LC和R_RD分别是柄C和D质心系相对左、右臂末端系的姿态变换矩阵，一旦抓持位置确定，P_LC、P_RD、R_LC和R_RD均为已知量，同时根据剪刀工件自身的结构，柄C和D质心系与两者连接系之间的相对位置P_CT和P_DT亦为已知量，同时，姿态变换矩阵R_CT和R_DT可根据柄C和D绕连接点的相对转动关系确定。于是通过式(3)和(4)即可得出左、右臂位姿，进而可进行双臂末端位姿规划。

1.2、双臂末端速度约束

令两柄C和D的质心速度分别为两柄连接点速度为 v_c,ω_c、v_d,ω_d及v_t,ω_t分别为各自的线速度和角速度分量。一般来说，v_t,ω_t由运动规划预先确定。

则两柄质心与连接点速度之间的关系为:

上式中，ω_tc和ω_td是C和D质心基于连接处的相对角速度，由两柄操作时绕连接轴的旋转关系确定，C和D质心基于连接处无相对平动速度，P_TC和P_TD由剪刀结构尺寸确定，和分别为P_TC和P_TD在惯性系下的表示， 于是由式(5)即可确定两柄质心处的速度和

左臂末端与柄C之间，右臂末端与柄D之间无相对运动，由此可建立速度关系，并可得左、右臂末端速度为：

$\left\{\begin{array}{cc}{\mathrm{\ω}}_1={\mathrm{\ω}}_c,& v_l=v_c+{\hat{P}}_{\mathrm{LC}}^{\left(0\right)}\·{\mathrm{\ω}}_c\\ {\mathrm{\ω}}_r={\mathrm{\ω}}_d,& v_r=v_d+{\hat{P}}_{\mathrm{RD}}^{\left(0\right)}\·{\mathrm{\ω}}_d\end{array}\right.---\left(6\right)$

式中，分别为和的反对称矩阵。其中和分别为P_LC和P_RD在惯性系下的表示，反对称矩阵可按以下方法求得：

设P＝[p_x p_y p_z]，P的反对称矩阵为则有 $\hat{P}=\left[\begin{array}{ccc}0& p_z& -p_y\\ -p_z& 0& p_x\\ p_y& -p_x& 0\end{array}\right],$ 于是式(6)可改写成矩阵形式：

$\left\{\begin{array}{c}{M}_{1V}:{\stackrel{\·}{X}}_L={\stackrel{\·}{X}}_C+\left[\begin{array}{cc}{0}_{3\×3}& {\hat{P}}_{\mathrm{LC}}^{\left(0\right)}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]{\stackrel{\·}{X}}_C=\left[\begin{array}{cc}{I}_{3\×3}& {\hat{P}}_{\mathrm{LC}}^{\left(0\right)}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]{\stackrel{\·}{X}}_C=J_{\mathrm{LC}}{\stackrel{\·}{X}}_C\\ M_{2V}:{\stackrel{\·}{X}}_R={\stackrel{\·}{X}}_D+\left[\begin{array}{cc}{0}_{3\×3}& {{\hat{P}}_{\mathrm{RD}}^{\left(0\right)}}_{}^{}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]{\stackrel{\·}{X}}_D=\left[\begin{array}{cc}{I}_{3\×3}& {\hat{P}}_{\mathrm{RD}}^{\left(0\right)}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]{\stackrel{\·}{X}}_D=J_{\mathrm{RD}}{\stackrel{\·}{X}}_D\end{array}\right.---\left(7\right)$

式中，I_3×3为3×3阶单位阵，J_LC为左臂末端速度与柄C质心速度之间的雅可比矩阵，J_RD为右臂末端速度与柄D质心速度之间的雅可比矩阵。

左、右臂末端速度与关节速度之间所确定的速度关系如下：

$\left\{\begin{array}{c}{\stackrel{\·}{X}}_L=J_{L_{0}L}{\stackrel{\·}{q}}_L\\ {\stackrel{\·}{X}}_R=J_{R_{0}R}{\stackrel{\·}{q}}_R\end{array}\right.---\left(8\right)$

J_L0L，J_R0R分别为左、右臂的雅可比矩阵。由式(7)、(8)可以得到左、右臂的关节速度和刚体质心速度的关系：

$\left\{\begin{array}{c}{M}_{1J}:{\stackrel{\·}{q}}_L=J_{L_{0}L}^{-1}{J}_{\mathrm{LC}}{\stackrel{\·}{X}}_C\\ M_{2J}:{\stackrel{\·}{q}}_R=J_{R_{0}R}^{-1}{J}_{\mathrm{RD}}{\stackrel{\·}{X}}_D\end{array}\right.---\left(9\right)$

由于P_LC和P_RD分别是柄C和D质心相对左、右臂末端的位置，由抓持位置确定，R_0L和R_0R可由姿态约束式(3)和(4)求得，由此得到J_LC和J_RD；两柄质心处速度和由式(5)得到，于是可根据方程(7)求出左、右臂末端速度，由式(9)可以求出左、右臂的关节速度。

实施例2、操作物为剪刀，假设两柄连接处固定不动，且在惯性坐标系{O₀}下的P_0T和R_0T已知。剪刀左、右柄质心X_C,X_D以两柄连接处为中心进行旋转运动，剪刀初始夹角和终止夹角已知，采用带抛物线过度圆弧梯形法对左、右柄质心的旋转运动进行规划，得到左、右柄质心的角速度。对剪刀进行夹断规划的具体流程图如图2所示。

本方法的流程如图2所示，双臂机器人的臂为二连杆结构，二连杆的连接处为一个关节，双臂末端连接一个剪刀的两柄，包括以下步骤：

步骤1、将剪刀两柄分别记为C柄和D柄，以两柄连接处为原点建立两柄相连接处三维坐标系为{O_T}，以C柄质心为原点建立三维质心系{O_C}，以D柄的质心为原点建立三维质心系{O_D}，以左臂末端为原点建立三维坐标系{O_L}，以右臂末端为原点建立三维坐标系{O_R}；惯性系为{O₀}。

在建立坐标系的时候，坐标系的轴向的选取与本方法最终计算结果无关。

获取双臂初始关节角、剪刀初始夹角以及剪刀终止夹角；

同时设置操作剪刀的整个运动规划过程中的总时间t_z、加速时间t_a和时间间隔t₀，由K_n＝t_a/t_o求得路径规划的总步数K_n；

步骤2、求解剪刀初始夹角与终止夹角的角度差θ_n；

步骤3、采用带抛物线过渡的速度梯形法对角度差θ_n的变化速度进行规划，获得剪刀左、右柄质心角速度的梯形速度曲线ω_c、ω_d；

步骤4、利用公式 $\left\{\begin{array}{c}{v}_c={\mathrm{\ω}}_c\×P_{\mathrm{TC}}^{\left(0\right)}\\ v_d={\mathrm{\ω}}_c\×P_{\mathrm{TD}}^{\left(0\right)}\end{array}\right.,$ 根据剪刀柄质心的角速度ω_c、ω_d，求得剪刀柄质心的线速度曲线v_c、v_d；

其中和分别为P_TC和P_TD在惯性系下的表示，P_TC为{O_C}相对于{O_T}的相对位置矢量，P_TD为{O_D}相对于{O_T}的相对位置矢量，R_OT为{O_T}相对于{O₀}的姿态变换矩阵；

步骤5、将上述求解得到的剪刀柄质心角速度曲线ω_c、ω_d和线速度曲线v_c、v_d，代入双臂协调运动的约束方程 $\left\{\begin{array}{cc}{\mathrm{\ω}}_l={\mathrm{\ω}}_c,& v_l=v_c+{\hat{P}}_C^{\left(0\right)}\·{\mathrm{\ω}}_c\\ {\mathrm{\ω}}_r={\mathrm{\ω}}_d,& v_r=v_d+{\hat{P}}_{\mathrm{RD}}^{\left(0\right)}\·{\mathrm{\ω}}_c\end{array}\right.,$ 分别求得左臂末端的线速度曲线v_l和角速度曲线ω_l，右臂末端的线速度曲线v_r和角速度曲线ω_r；

式中，分别为和的反对称矩阵，和分别为P_LC和P_RD在惯性系下的表示，R_OL为{O_L}相对于{O₀}的姿态变换矩阵，P_LC为{O_C}相对于{O_L}的相对位置矢量，R_OR为{O_R}相对于{O₀}的姿态变换矩阵，P_RD为{O_C}相对于{O_R}的相对位置矢量；

步骤6、针对路径规划中的当前步数k，k的初始值为1，求解当前步数对应的时间t_k，利用t_k在左臂末端的线速度曲线v_l和角速度曲线ω_l，右臂末端的线速度曲线v_r和角速度曲线ω_r中找到当前步数的左臂末端线速度v_l(k)和角速度ω_l(k)、右臂末端线速度v_r(k)和角速度ω_r(k)；

利用公式 $\left\{\begin{array}{c}{\stackrel{\·}{q}}_{L\left(k\right)}=J_{L_{0}L}^{-1}{\stackrel{\·}{X}}_{L\left(k\right)}\\ {\stackrel{\·}{q}}_{R\left(k\right)}=J_{R_{0}R}^{-1}{\stackrel{\·}{X}}_{R\left(k\right)}\end{array}\right.,$ 解算获得路径规划中当前步数k下左臂关节角速度以及右臂关节角速度

分别为左、右臂的雅可比矩阵， ${\stackrel{\·}{X}}_{L\left(k\right)}={[v_{l\left(k\right)},{\mathrm{\ω}}_{l\left(k\right)}]}^T,{\stackrel{\·}{X}}_{l\left(k\right)}={[v_{l\left(k\right)},{\mathrm{\ω}}_{l\left(k\right)}]}^T;$

步骤7、利用求解路径规划中下一步数k+1的关节角度，下一步数k+1下左臂关节角度以及右臂关节角度其中θ_l(k)为当前步数k下左臂关节角度，θ_r(k)为当前步数k下右臂关节角度；其中，其中双臂初始关节角已知；

步骤8、判断当前步数k是否达到总步数K_n，若已达到总步数，则路径规划结束，若没有达到总步数则k自增1，返回步骤6。

综上，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种双臂机器人协调操作的路径规划方法，双臂末端分别连接一个剪刀的两柄，所述双臂机器人的臂为二连杆结构，二连杆的连接处为一个关节，其特征在于，包括以下步骤：

步骤1、将所述剪刀两柄分别记为C柄和D柄，以两柄连接处为原点建立两柄相连接处三维坐标系为{O_T}，以C柄质心为原点建立三维质心系{O_C}，以D柄的质心为原点建立三维质心系{O_D}，以左臂末端为原点建立三维坐标系{O_L}，以右臂末端为原点建立三维坐标系{O_R}；惯性系为{O₀}；

获取双臂关节的初始关节角、剪刀初始夹角以及剪刀终止夹角；

同时设置操作剪刀的整个运动规划过程中的总时间t_z、加速时间t_a和时间间隔t₀，由K_n＝t_a/t_o求得路径规划的总步数K_n；

步骤2、求解剪刀初始夹角与终止夹角的角度差θ_n；

步骤3、采用带抛物线过渡的速度梯形法对角度差θ_n的变化速度进行规划，获得剪刀左、右柄质心角速度的梯形速度曲线ω_c、ω_d；

步骤4、利用公式 $\left\{\begin{array}{c}{v}_c={\mathrm{\ω}}_c\×P_{\mathrm{TC}}^{\left(0\right)}\\ v_d={\mathrm{\ω}}_c\×P_{\mathrm{TD}}^{\left(0\right)}\end{array}\right.,$ 根据剪刀柄质心的角速度ω_c、ω_d，求得剪刀柄质心的线速度曲线v_c、v_d；

其中和分别为P_TC和P_TD在惯性系下的表示，P_TC为{O_C}相对于{O_T}的相对位置矢量，P_TD为{O_D}相对于{O_T}的相对位置矢量，所述R_OT为{O_T}相对于{O₀}的姿态变换矩阵；

步骤5、将上述求解得到的剪刀柄质心角速度曲线ω_c、ω_d和线速度曲线v_c、v_d，代入双臂协调运动的约束方程 $\left\{\begin{array}{c}{\mathrm{\ω}}_l={\mathrm{\ω}}_c,v_l=v_c+{\hat{P}}_{\mathrm{LC}}^{\left(0\right)}\·{\mathrm{\ω}}_c\\ {\mathrm{\ω}}_r={\mathrm{\ω}}_d,v_r=v_d+{\hat{P}}_{\mathrm{RD}}^{\left(0\right)}\·{\mathrm{\ω}}_d\end{array}\right.,$ 分别求得左臂末端的线速度曲线v_l和角速度曲线ω_l，右臂末端的线速度曲线v_r和角速度曲线ω_r；

式中，分别为和的反对称矩阵，和分别为P_LC和P_RD在惯性系下的表示，R_OL为{O_L}相对于{O₀}的姿态变换矩阵，P_LC为{O_C}相对于{O_L}的相对位置矢量，R_OR为{O_R}相对于{O₀}的姿态变换矩阵，P_RD为{O_C}相对于{O_R}的相对位置矢量；

步骤6、针对路径规划中的当前步数k，求解当前步数对应的时间t_k，利用t_k在左臂末端的线速度曲线v_l和角速度曲线ω_l，右臂末端的线速度曲线v_r和角速度曲线ω_r中找到当前步数的左臂末端线速度v_l(k)和角速度ω_l(k)、右臂末端线速度v_r(k)和角速度ω_r(k)；其中k的初始值为1；

利用公式 $\left\{\begin{array}{c}{\stackrel{.}{q}}_{L\left(k\right)}=J_{L_{0}L}^{-1}{\stackrel{.}{X}}_{L\left(k\right)}\\ {\stackrel{.}{q}}_{R\left(k\right)}=J_{R_{0}R}^{-1}{\stackrel{.}{X}}_{R\left(k\right)}\end{array}\right.,$ 解算获得路径规划中当前步数k下左臂关节角速度以及右臂关节角速度

分别为左、右臂的雅可比矩阵，

步骤7、利用求解路径规划中下一步数k+1的关节角度，下一步数k+1下左臂关节角度以及右臂关节角度其中θ_l(k)为当前步数k下左臂关节角度，θ_r(k)为当前步数k下右臂关节角度；其中，其中双臂初始关节角已知；

步骤8、判断当前步数k是否达到总步数K_n，若已达到总步数，则路径规划结束，若没有达到总步数则k自增1，返回步骤6。