CN102646148B - 仿人机器人机械臂避碰运动轨迹规划方法 - Google Patents

Abstract

本发明提出了一种仿人机器人机械臂避碰运动轨迹规划方法，现有技术没有从根本上解决碰撞问题，本发明采用基于关节空间插补方法生成机械臂运动轨迹所带来的机械臂与桌子碰撞的问题，主要针对仿人机器人的拟人手臂面向桌面或类似桌面物体的作业，其中机械臂的自由度分布为肩部三个自由度，肘部两个自由度，腕部两个自由度。具体实现方法是，在能够保证固定时刻完成目标作业的前提下，在手臂运动过程中发生碰撞的时刻点，让肘部适当弯曲，避免碰撞。这种方案使机械臂实现了在具有较高的灵活性和较大作业空间的同时，无碰撞的运动。

Description

仿人机器人机械臂避碰运动轨迹规划方法

技术领域

本发明属于机器人领域，尤其是一种仿人机器人机械臂避碰运动轨迹规划方法。

背景技术

机械臂的轨迹规划是指，在满足一定约束的条件下，规划一条从起始位姿到目标位姿的无碰撞的运动路径。根据规划空间的不同，轨迹规划方法可分为基于关节空间的轨迹规划和基于任务空间（即笛卡尔空间）的轨迹规划两种。这两种方法均有一定的局限性，由于机械臂作业通常在笛卡尔空间描述，所以基于关节空间的轨迹规划方法对任务的描述不直观，而基于任务空间的轨迹规划则需要实现将笛卡尔空间的位姿进行逆运动学求解，转换为机械臂各个关节角，需要大量的运算，而且无法保证整条笛卡尔空间的轨迹中不存在奇异构型，所以基于笛卡尔空间的轨迹规划方法很难应用于实时控制。目前，轨迹规划方法主要有矩形加速度函数法，梯形加速度函数法，B样条曲线法，多项式插值法等。其中矩形加速度函数法计算简单，易于实现，可应用于实时应用场合。

目前，对机械臂无碰撞轨迹规划的研究已有很多，Lazona-Perze在Automaticplanning of manipulator transfer movement和Spacial planning:a configurationspace approach两篇文章中，提出了一种基于C空间规划方法的自由空间法，该方法能够规划出无碰撞的机械臂运动轨迹，但该方法中对障碍物的建模方法比较复杂，所以规划的效率比较低，不适于实时应用。Khabit在Real-time obstacleavoidance for manipulators and mobile robots一文中提出，在障碍处定义一个排斥势场，在目标点出定义一个吸引势场，机械臂的运动就由排斥势场和吸引势场的共同作用来决定，该方法在处理全局路径规划的动态避障问题时，非常有效，但易于陷入局部最小点处。申请号为201110037726.9的专利中，描述了一种用于工业机器人的无碰撞轨迹规划的方法，该方法将整个笛卡尔空间划分成三个部分，障碍物区域，无碰撞区域和发生碰撞的区域，每个区域用一个或多个圆柱形空间描述，机械臂的轨迹仅在无碰撞区域进行规划，可见这种方法没有从根本上解决碰撞问题。

发明内容

本发明的目的在于针对现有技术的不足，提供了一种仿人机器人机械臂避碰运动轨迹规划方法。

本发明适用于具有七个自由度的机械臂，其七个关节角分别为肩俯仰角、肩滚动角、肩偏摆角、肘滚动角、肘偏摆角、腕滚动角和腕俯仰角。并且，固定机械臂从初始位姿到目标位姿的运动时间为tf₁，从目标位姿返回初始位姿的时间为tf₂。

本发明一种仿人机器人机械臂避碰运动轨迹规划方法包括以下步骤：

步骤一：根据桌面与机器人的相对位置，对桌子进行建模。

在笛卡尔空间下，以机器人肩心为世界坐标系，将桌子模型建立为肩心所处水平面以下的一个区域。

步骤二：碰撞检测及分类。

为了满足实时性要求，将笛卡尔空间描述的初始位姿和目标任务位姿转换成关节空间的初始关节角序列和目标关节角序列，在关节空间下采用矩形函数加速度法对各个关节角进行插补，生成关节空间下的轨迹，然后对这条轨迹进行正运动学运算得到笛卡尔空间的轨迹，如果机械臂采用这条轨迹完成动作的过程中，整条机械臂都未经过桌子所在的区域，说明机械臂此次运动没有与桌子发生碰撞，则采用该关节空间的轨迹控制机械臂完成任务。否则若机械臂的轨迹与桌面模型有交集，则可确定机械臂在此次任务中与桌面发生了碰撞，需要对机械臂的轨迹进行处理，然后再完成动作。此时需要根据发生碰撞的时刻T区分碰撞的三种情况：（1）T≤tf₁，初始位姿到目标位姿的运动过程发生碰撞，返回过程未发生碰撞；（2）tf₁≤T≤tf₁+tf₂，初始位姿到目标位姿的运动过程未发生碰撞，返回初始位姿的过程发生碰撞；（3）以上两种碰撞都发生了，即初始位姿到目标位姿的运动过程和返回过程都发生碰撞。

步骤三：计算肘滚动角的“临界值”，并根据该临界值调整肘滚动角的轨迹，避免发生碰撞。

针对发生碰撞的情况，在碰撞的时刻使机械臂的肘部适当的弯曲直至刚好不与桌面发生碰撞，即在碰撞时刻对造成机械臂与桌面碰撞影响最显著的位于肘部的肘滚动角的值进行修改，使得修改后得到的肘滚动角的“临界值”，结合肩俯仰角、肩滚动角、肩偏摆角、肘偏摆角、腕滚动角和腕俯仰角的值形成的机械臂的构型，刚好不与桌面碰撞。肘滚动角临界值的求解步骤：第一步，根据碰撞时刻肩俯仰角、肩滚动角、肩偏摆角、肘偏摆角、腕滚动角和腕俯仰角的值以及假设一个肘滚动角的初始值0，计算机械臂的正运动学即可得到肘关节方向向量的两个顶点A、B及机械臂上的一个检测点C的坐标分别为A(x₁,y₁,z₁)，B(x₂,y₂,z₂)，C(x₃,y₃,z₃)，并计算此时肘关节的方向向量AB；第二步，计算过点C且垂直于肘关节方向向量AB的平面L，可得其方程为(x-x₃)(x₂-x₁)+(y-y₃)(y₂-y₁)+(z-z₃)(z₂-z₁)＝0；第三步，计算肘关节方向向量AB与上一步得到的平面L的交点O（x₀,y₀,z₀），以此交点O为球心，以点O到点C的距离||OC||为半径的球面方程ψ，其表达式为(x-x₀)²+(y-y₀)²+(z-z₀)²＝(x₃-x₀)²+(y₃-y₀)²+(z₃-z₀)²；第四步，求该球面ψ、第二步的平面L以及桌面的交点D（x₄,y₄,z₄）；第五步，计算∠COD，并用右手定则判断∠COD正负，得到的值设为Δq₄，然后计算机械臂与桌面发生碰撞时肘滚动角的值，设为q4bound，则q4bound＝90+Δq₄，最后在q4bound基础上增加一定余量5，即可得到肘滚动角的临界值。

取该碰撞时刻和该时刻肘滚动角的临界值作为一个中间状态，以及机械臂在初始时刻的肘滚动角为其初始状态和在目标时刻的肘滚动角为目标状态，将肘滚动角的轨迹进行分段生成。则如果发生第（1）种碰撞，三段轨迹分别为：初始状态到中间状态，中间状态到目标状态，目标状态返回初始状态；如果发生第（2）种碰撞，三段轨迹为：初始状态到目标状态，目标状态到中间状态，中间状态返回初始状态；如果发生第（3）种碰撞，说明机械臂运动过程中，共需要处理与桌面发生的两次碰撞，三段轨迹为：初始状态到第一次避碰的中间状态，第一次碰撞的中间状态到第二次碰撞的中间状态，第二次碰撞的中间状态返回初始状态。

本发明的有益效果是：本方案采用关节空间的轨迹规划方法，对手臂各个关节分别进行轨迹插补，在充分发挥各电机的性能、满足关节限幅的要求、避免出现奇异位形的同时，实现机械臂与桌面的避碰。达到提高机械臂完成任务的成功率和扩大作业空间的目的。

附图说明

图1为机械臂各关节的轴向方向及相关坐标系的建立方法；

图2为矩形加速度函数轨迹规划法中加速度曲线图；

图3为矩形加速度函数轨迹规划法中速度曲线图；

图4为矩形加速度函数轨迹规划法中位移曲线图；

图5为机械臂的一组击球位姿下的构型；

图6为求击球过程中间点的流程图；

图7求解返回过程中间点的流程图。

具体实施方式

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

步骤一：根据桌面与机器人的相对位置，对桌面进行建模。

如图1所示，为一个仿人机器人右机械臂的简单模型，其中a_i,i＝1～7为其各个关节的轴向方向向量，(X,Y,Z)为世界坐标系，(x_i,y_i,z_i),i＝1～7为各关节的局部坐标系。在世界坐标系下，将桌子建立成一个平面以下的区域。即根据桌面高度和肩心高度，可以计算桌面所在平面在世界坐标系下的描述为z＝cons tant。

步骤二：碰撞检测及分类。

将笛卡尔空间的初始位姿及目标位姿转换成关节空间下的初始关节角序列和目标关节角序列，采用矩形加速度函数规划法生成机械臂在关节空间的轨迹，顾名思义，矩形加速度函数规划法即加速度函数的曲线为矩形，如图2所示，然后对加速度进行积分得到速度曲线，如图3所示，在对速度函数积分得到位移曲线，如图4所示；然后进行正运动学计算，将该轨迹转换成笛卡尔空间的轨迹。若机械臂沿该运动轨迹运动时，机械臂上的检测点的轨迹与步骤一中所建立的桌子模型没有交集，说明机械臂未与桌面发生碰撞，则采用关节空间的轨迹控制机械臂完成任务。否则，根据发生碰撞的时刻T，区分发生碰撞的类型：（1）T≤tf₁，初始位姿到目标位姿的运动过程发生碰撞，返回过程未发生碰撞；（2）tf₁≤T≤tf₁+tf₂，初始位姿到目标位姿的运动过程未发生碰撞，返回初始位姿的过程发生碰撞；（3）以上两种碰撞都发生了，即两个发生碰撞的时刻为T₁和T₂，且T₁≤tf₁，tf₁≤T₂≤tf₁+tf₂，初始位姿到目标位姿的运动过程和返回过程都发生碰撞。

步骤三：调整肘滚动角的轨迹，避免发生碰撞。

针对上述步骤二中提到的发生碰撞的三种情况，对造成机械臂与桌面碰撞影响最显著的肘滚动角的轨迹进行处理后再控制机械臂的运动。具体处理方法如下：

①发生情况（1）的碰撞，记录碰撞发生的时刻T。此时已知条件为由矩形加速度函数法得到的其余六个关节角在该时刻的值以及机械臂与桌面的相对位置，将这些已知条件计算该时刻肘滚动角的一个临界值，这个“临界值”是指肘滚动角取该值时，结合由一般轨迹插补方法得到的T时刻其余六个关节角的值，组成的七个关节角序列，使得此时刻采用此关节序列得到的机械臂构型，不与桌面发生碰撞。肘滚动角的临界值的计算过程如下：

如图5所示，点A’是机械臂竖直状态下肘关节点，点B’是机械臂竖直状态下肘滚动角的轴向向量的顶点，假设一个机械臂姿态：肘滚动角初值选为90度，且其余六个关节角的值分别取碰撞时刻的六个值。点A是该姿态下肘关节点，点B是该姿态下肘滚动角的轴向向量的顶点，点C为所取的取得机械臂末端执行器的边沿检测点之一，平面L为过点C且垂直于向量AB的平面，D点为肘关节在90度基础上转动一定角度（绕a₄逆时针转为正，顺时针转为负）后与桌面发生碰撞的碰撞点。可见，肘关节角取值为90+sign(∠COD)时，机械臂刚好与桌子发生碰撞，从而可在90+sign(∠COD)基础上增大肘关节角的取值，即可避免碰撞。

根据肘滚动角的初值90度，以及插补得到的T时刻其余六个关节角的值，计算机械臂的正运动学即可得到A、B、C三点的坐标A(x₁,y₁,z₁)，B(x₂,y₂,z₂)，C(x₃,y₃,z₃)。

从而可得平面L的描述方程为：

(x-x₃)(x₂-x₁)+(y-y₃)(y₂-y₁)+(z-z₃)(z₂-z₁)＝0

AB所在直线描述为：

$\left\{\begin{array}{c}x=x_1+(x_2-x_1)k\\ y=y_1+(y_2-y_1)k\\ z=z_1+(z_2-z_1)k\end{array}\right.$

则可知AB所在直线在平面L上的垂足O的坐标（x₀,y₀,z₀）满足下述方程组：

$\left\{\begin{array}{c}(x-x_3)(x_2-x_1)+(y-y_3)(y_2-y_1)+(z-z_3)(z_2-z_1)=0\\ x=x_1+(x_2-x_1)k\\ y=y_1+(y_2-y_1)k\\ z=z_1+(z_2-z_1)k\end{array}\right.$

求解该方程组得，

$k=\frac{(x_3-x_1)(x_2-x_1)+(y_3-y_1)(y_2-y_1)+(z_3-z_1)(z_2-z_1)}{{(x_2-x_1)}^2+{(y_2-y_1)}^2+{(z_2-z_1)}^2}$

从而可得垂足O的坐标为，

$\left\{\begin{array}{c}{x}_0=x_1+(x_2-x_1)k\\ y_0=y_1+(y_2-y_1)k\\ z_0=z_1+(z_2-z_1)k\end{array}\right.$

计算以O为球心，||OC||为半径的球面ψ的方程为：

(x-x₀)²+(y-y₀)²+(z-z₀)²＝(x₃-x₀)²+(y₃-y₀)²+(z₃-z₀)²

求与z＝constant的平面相撞点D的坐标（x₄,y₄,z₄），其满足下面所示方程组：

$\left\{\begin{array}{c}{(x-x_0)}^2+{(y-y_0)}^2+{(z-z_0)}^2={(x_3-x_0)}^2+{(y_3-y_0)}^2+{(z_3-z_0)}^2\\ (x-x_3)(x_2-x_1)+(y-y_3)(y_2-y_1)+(z-z_3)(z_2-z_1)=0\\ z=\mathrm{cons}\tan t\end{array}\right.$

得， $y=y_3-\frac{(x-x_3)(x_2-x_1)+(-0.49-z_3)(z_2-z_1)}{y_2-y_1},$ 从而有，

$\begin{array}{c}{(x-x_0)}^2+{(y_3-\frac{(x-x_3)(x_2-x_1)+(-0.49-z_3)(z_2-z_1)}{y_2-y_1}-y_0)}^2+{(\mathrm{cons}\tan t-z_0)}^2=\\ {(x_3-x_0)}^2+{(y_3-y_0)}^2+{(z_3-z_0)}^2\end{array}$

可得，

$\begin{array}{c}(1+{\left(\frac{x_2-x_1}{y_2-y_1}\right)}^2)x^2-2(x_0+\frac{(y_3-y_0)(y_2-y_1)-(\mathrm{cons}\tan t-z_3)(z_2-z_1)+x_3(x_2-x_1)}{y_2-y_1}\frac{x_2-x_1}{y_2-y_1})x+x_0^2+{(\mathrm{cons}\tan t-z_0)}^2+\\ {\left(\frac{(y_3-y_0)(y_0-y_1)-(\mathrm{cons}\tan t-z_3)(z_2-z_1)+x_3(x_2-x_1)}{y_2-y_1}\right)}^2-{(x_3-x_0)}^2-{(y_3-y_0)}^2-{(z_3-z_0)}^2=0\end{array}$

设 $a=(1+{\left(\frac{x_2-x_1}{y_2-y_1}\right)}^2)$

$b=-2(x_0+\frac{(y_3-y_0)(y_2-y_1)-(-0.49-z_3)(z_2-z_1)+x_3(x_2-x_1)}{y_2-y_1}\frac{x_2-x_1}{y_2-y_1})$

$\begin{array}{c}c=x_0^2+{(\mathrm{cons}\tan t-z_0)}^2+{\left(\frac{(y_3-y_0)(y_2-y_1)-(\mathrm{cons}\tan t-z_3)(z_2-z_1)+x_3(x_2-x_1)}{y_2-y_1}\right)}^2-{(x_3-x_0)}^2-\\ {(y_3-y_0)}^2-{(z_3-z_0)}^2\end{array}$

判别式Δ＝b²-4ac

（1）Δ＜0，Δq4＝90-(q_4min)。此时说明未发生碰撞。

（2）Δ≥0， $x_4=\frac{-b+\sqrt{\mathrm{\Δ}}}{2a}$ 或 $x_4=\frac{-b-\sqrt{\mathrm{\Δ}}}{2a}$ （舍去），

$y_4=y_3-\frac{(x_4-x_3)(x_2-x_1)+(\mathrm{cons}\tan t-z_3)(z_2-z_1)}{y_2-y_1},z_4=\mathrm{cons}\tan t.$

$\∠\mathrm{COD}=a\cos \left(\frac{{\left|\right|\mathrm{CO}\left|\right|}^2+{\left|\right|\mathrm{DO}\left|\right|}^2-{\left|\right|\mathrm{CD}\left|\right|}^2}{2\left|\right|\mathrm{CO}\left|\right|\left|\right|\mathrm{DO}\left|\right|}\right)=a\cos (1-\frac{\left|\right|\mathrm{CD}\left|\right|}{2\left|\right|\mathrm{OC}\left|\right|}),$

$\left|\right|\mathrm{CO}\left|\right|=\sqrt{{(x_3-x_0)}^2+{(y_3-y_0)}^2+{(z_3-z_0)}^2}$

$\left|\right|\mathrm{CO}\left|\right|=\sqrt{{(x_4-x_0)}^2+{(y_4-y_0)}^2+{(z_4-z_0)}^2}=\left|\right|\mathrm{CD}\left|\right|$

$\left|\right|\mathrm{CD}\left|\right|=\sqrt{{(x_4-x_3)}^2+{(y_4-y_3)}^2+{(z_4-z_3)}^2}$

设q4bound＝90+Δq4

可见，当肘滚动角取得q4bound值时，机械臂刚好与球桌碰撞，所以肘滚动角的临界值为在q4bound留有余量的一个值，即比稍q4bound大的一个值，如q4bound+Δθ,Δθ＞0。其中Δq4为∠COD用右手定则判断正负后得到的值。

得到肘关节角的临界值后，用时刻T作为插补的一个中间时刻，这个中间时刻的肘滚动角的值为计算得到的“临界值”，此时肘滚动角的轨迹分为三段：由初始状态到T时刻的中间状态，由T时刻的中间状态到目标状态，由目标状态返回到初始状态，三段的轨迹插补均按一般的矩形加速度函数法进行规划。而其余六个关节的运动轨迹不变。

对上述第一种碰撞的处理过程的计算流程如图6所示。

②发生情况（2）的碰撞，找出碰撞发生的时刻T，并计算该时刻使球拍不与桌面碰撞的肘滚动角的一个“临界值”（方法同上），用该时刻作为一个插补的中间时刻，这个中间时刻的肘滚动角的值取计算得到的“临界值”。该时刻的其余六个关节的轨迹不变，而肘滚动角的运动轨迹分为三段：由初始状态到目标状态，由目标状态到T时刻的中间状态，由T时刻的中间状态返回到初始状态，三段轨迹的生成方法仍采用矩形加速度函数法进行规划。上述过程的计算流程如图7所示。

③发生情况（3）的碰撞，找出从初始位姿到目标位姿的运动过程中的碰撞时刻T₁和返回过程的碰撞时刻T₂，以及两个时刻使机械臂不与桌面碰撞的肘滚动角的两个“临界值”。用这两个时刻作为插补的中间时刻，两个中间时刻的肘滚动角的值分别为计算得到的两个“临界值”，其余6个关节角的值为一般轨迹插补得到的值。此时肘滚动角运动轨迹分为三段：由初始状态到T₁时刻的状态，由T₁时刻的状态到T₂时刻的状态，由T₂时刻的状态返回到初始状态。

Claims (1)

1.仿人机器人机械臂避碰运动轨迹规划方法，其特征在于该方法包括以下步骤：

步骤一：根据桌面与机器人的相对位置，对桌子进行建模

在笛卡尔空间下，以机器人肩心为世界坐标系，将桌子模型建立为肩心所处水平面以下的一个区域；

步骤二：碰撞检测及分类

为了满足实时性要求，将笛卡尔空间描述的初始位姿和目标任务位姿转换成关节空间的初始关节角序列和目标关节角序列，在关节空间下采用矩形函数加速度法对各个关节角进行插补，生成关节空间下的轨迹，然后对这条轨迹进行正运动学运算得到笛卡尔空间的轨迹，如果机械臂采用这条轨迹完成动作的过程中，整条机械臂都未经过桌子所在的区域，说明机械臂此次运动没有与桌子发生碰撞，则采用该关节空间的轨迹控制机械臂完成任务，否则若机械臂的轨迹与桌面模型有交集，则可确定机械臂在此次任务中与桌面发生了碰撞，需要对机械臂的轨迹进行处理，然后再完成动作，此时需要根据发生碰撞的时刻T区分碰撞的三种情况：（1）T≤tf₁，初始位姿到目标位姿的运动过程发生碰撞，返回过程未发生碰撞；（2）tf₁≤T≤tf₁+tf₂，初始位姿到目标位姿的运动过程未发生碰撞，返回初始位姿的过程发生碰撞；（3）以上两种碰撞都发生了，即初始位姿到目标位姿的运动过程和返回过程都发生碰撞；固定机械臂从初始位姿到目标位姿的运动时间为tf₁，从目标位姿返回初始位姿的时间为tf₂；

步骤三：计算肘滚动角的临界值，并根据该临界值调整肘滚动角的轨迹，避免发生碰撞

针对发生碰撞的情况，在碰撞的时刻使机械臂的肘部适当的弯曲直至刚好不与桌面发生碰撞，即在碰撞时刻对造成机械臂与桌面碰撞影响最显著的位于肘部的肘滚动角的值进行修改，使得修改后得到的肘滚动角的“临界值”，结合肩俯仰角、肩滚动角、肩偏摆角、肘偏摆角、腕滚动角和腕俯仰角的值形成的机械臂的构型，刚好不与桌面碰撞，肘滚动角临界值的求解步骤：第一步，根据碰撞时刻肩俯仰角、肩滚动角、肩偏摆角、肘偏摆角、腕滚动角和腕俯仰角的值以及肘滚动角的初始值0，计算机械臂的正运动学即可得到肘关节方向向量的两个顶点A、B及机械臂上的一个检测点C的坐标分别为A(x₁,y₁,z₁)，B(x₂,y₂,z₂)，C(x₃,y₃,z₃)，并计算此时肘关节的方向向量AB；第二步，计算过点C且垂直于肘关节方向向量AB的平面L，可得其方程为(x-x₃)(x₂-x₁)+(y-y₃)(y₂-y₁)+(z-z₃)(z₂-z₁)＝0；第三步，计算肘关节方向向量AB与上一步得到的平面L的交点O（x₀,y₀,z₀），以此交点O为球心，以点O到点C的距离OC为半径的球面方程ψ，其表达式为(x-x₀)²+(y-y₀)²+(z-z₀)²＝(x₃-x₀)²+(y₃-y₀)²+(z₃-z₀)²；第四步，求该球面ψ、第二步的平面L以及桌面的交点D（x₄,y₄,z₄）；第五步，计算∠COD，并用右手定则判断∠COD正负得到的值设为Δq₄，然后计算机械臂与桌面发生碰撞时肘滚动角的值，设为q4bound，则_q4bound＝90+Δq₄，最后在q4bound基础上增加一定余量5，即可得到肘滚动角的临界值；

取该碰撞时刻和该时刻肘滚动角的临界值作为一个中间状态，以及机械臂在初始时刻的肘滚动角为其初始状态和在目标时刻的肘滚动角为目标状态，将肘滚动角的轨迹进行分段生成，则如果发生第（1）种碰撞，三段轨迹分别为：初始状态到中间状态，中间状态到目标状态，目标状态返回初始状态；如果发生第（2）种碰撞，三段轨迹为：初始状态到目标状态，目标状态到中间状态，中间状态返回初始状态；如果发生第（3）种碰撞，说明机械臂运动过程中，共需要处理与桌面发生的两次碰撞，三段轨迹为：初始状态到第一次避碰的中间状态，第一次碰撞的中间状态到第二次碰撞的中间状态，第二次碰撞的中间状态返回初始状态。