预选择最小距离指标冗余机器人动态避障方法

摘要

本发明涉及障碍物随机运动环境下，预选择最小距离指标冗余机器人动态避障路径规划方法，包括步骤：利用OBB包围盒对冗余机器人进行建模，建立预选择最小距离指标；通过相交测试，在计算实时距离之前剔除安全杆件；在相应杆件的局部坐标系中，采用局部映射距离计算方法，利用目标点坐标值直接得出实时最小距离并确定碰撞点位置；在此基础上，建立和最小距离相关的避障增益和逃逸速度，利用冗余机器人零空间映射矩阵进行避障路径规划。本发明在动态避障规划前便剔除安全杆件，去除了不必要的繁琐计算并优化距离计算方法，提高了动态避障的规划效率，满足实时性的要求。

说明书

技术领域

本发明涉及障碍物随机运动的环境下，预选择最小距离指标冗余机器人动态避障路径规划方法，在动态避障规划前便剔除安全杆件，并采用局部映射距离计算方法，得出实时最小距离。本发明无需计算障碍物到所有杆件的最小距离，提高了动态避障的规划效率，满足实时性的要求。属于智能机器人技术领域。 

背景技术

冗余机器人，是指关节空间维数n大于任务空间维数m，可以在自运动而不影响末端位姿的情况下实现多种二次目标。由于任务环境越来越复杂，要求冗余机器人具有更高的智能性，其中动态避障是机器人自主路径规划中一个重要的组成部分，日益成为人们关注的焦点。 

目前，利用杆件与障碍物的距离信息完成避障规划是学者们的研究重点。Maciejiewski和Khatib应用人工势场法，通过距离信息建立吸引势场和排斥势场完成避障。LALIBERTE等人针对人工势场法容易发生局部极小的不足，提出了离散势场法，从而减少了局部极小发生的可能性。CHO等人基于流体力学的势流理论，提出了一种可以避免局部极小的新颖的速度排斥函数法。另外，Liegeois提出梯度投影法，利用最小距离指标，优化雅克比矩阵的零空间矢量，修改齐次解完成避障。IKADA等人提出一种潜在避障操作性能形状指标来控制机械臂达到易于避障的位形来进行避障，同时保持末端轨迹的跟踪。Kavraki等人提出了随机路标法，该方法通过环境距离检测，搜索出一些能够避开障碍物的关节坐标点来实现避障，但是随着机构的冗余度增加，计算量也在增加。 

避障指标选取的合适与否将直接影响到避障效果的好坏，特别对于动态避障，障碍物随机运动，选择合理的并能准确快速描述障碍物与机械臂位置关系的指标，对于保证避障规划的实时性尤为重要。 

传统的距离指标，需要计算障碍物到每个杆件的最小距离，然后再取最小值。当杆件处于某些特殊构形时，障碍物到杆件最小距离的垂足可能会落到杆件的延长线上，延长线并不属于机器人的一部分，对于该杆件计算的最小距离不准确，这时需要在杆件上取多个标志点并遍历各个标志点到障碍物的距离。可见，传统的距离避障指标，一是准确性不足，二是多个标志点的计算，耗费了大量的时间。对于传统的距离指标，可以完成静态避障规划，但是对于动态避障规划来说，障碍物随机运动，需要机器人在有限的时间内完成障碍物检测及关节的多种动作，对避障的效率提出了更高的要求。 

发明内容

基于上述，本发明提出在障碍物随机运动环境下，一种预选择距离指标冗余机器人动态避障路径规划方法，该方法无需计算障碍物到所有杆件的最小距离，并且采用局部映射距离计算方法，提高了冗余机器人动态避障的规划效率。 

为了实现此目的，本发明所采取的技术方案是： 

(1)对于冗余机器人各个杆件及随机运动障碍物建立相应的包围盒，减少基本几何元素的数目和参与相交测试的包围盒的数目。 

进一步的，本发明选用OBB方向包围盒建模，并设定包围盒的方向和杆件的方向重合，包围盒的长度和杆件的长度一致，宽度根据最小安全距离设定。面对全部关节都是旋转关节的冗余机器人，当杆件发生旋转运动后，只需让OBB盒的基底进行同样的旋转进行实时更新。对于形状不一的动态障碍物，采用球形包围盒。 

(2)实时检测运动障碍物与机器人杆件的相对位置关系，建立预选择最小距离指标H_d-min，其思想是：针对不同的机器人构型建立相应的包围盒，利用相交测试，在计算实时距离之前剔除其他安全杆件，只考虑相碰撞的杆件和障碍物的位置关系，并在杆件的局部坐标系中计算距离值。如果在相交测试中有多个杆件相交，则H_d-min取相交距离中最小的。在避障过程中，要求H_d-min越大越好，说明离障碍物越远，机器人越安全。 

具体步骤分为：2.1在机器人杆件的局部坐标系中，利用分离轴定理进行相交测试。2.2利用局部映射计算最小距离，优化传统距离计算方法。 

进一步的，所述2.1相交测试中，OBB包围盒采用分离轴检测方法，将障碍物的坐标转换到每个杆件的局部坐标系中，相交测试在局部坐标系下完成，分离轴减少到3个，即X、Y、z。如果在3个分离轴上，障碍物和机器人的投影全都重合，则判断相交。如果其中有一个分离轴上的投影分离，则不会发生碰撞，记为安全杆件，不作为计算实时距离的有效杆件。 

设在杆件AB的局部坐标系中，A端的坐标为(x_A’，y_A’，z_A’)，B端的坐标为(x_B’，y_B’，z_B’)包围盒长度和宽度为2*d，高度为杆件长度L；障碍物球心半径为R，中心坐标为D₁(x₁，y₁，z₁)，将杆件AB的包围盒和球体分别向三个坐标轴投影，当 

$\left(\left(\begin{array}{c}|{x_A}^{,}-x_1|\le R+d\\ {y_A}^{,}-R\le y_1\le {y_B}^{,}+R\\ |{z_A}^{,}-z_1|\le R+d\end{array}\right)\right)$

3个式子同时成立时，判断杆件AB和障碍物球体相碰。否则不相交，视为需要剔除的安全杆件。 

进一步的，所述2.2局部映射最小距离计算，在相交测试后，剔除不和障碍物发生碰撞的安全杆件，在此基础上，在碰撞杆件的局部坐标系中计算有效的最小距离。由于最小距离d_min是标量，所以在任意坐标系中它的值不变。碰撞点的求取，实质是为了求得碰撞点的雅可比矩阵，利用矢量积法计算雅可比矩阵J₀，则需要计算碰撞点到杆件末端的距离，也是标量。所以不用转换到基坐标系中，直接在局部坐标系中利用目标点的坐标值直接得出结果。 

设定相碰撞杆件A端坐标(x_A’，y_A’，z_A’)，B端坐标(x_B’，y_B’，z_B’)，障碍物中心坐标D₁(x₁，y₁，z₁)，碰撞点为P，障碍物距此杆件的最小距离为d_min，碰撞点距离末端关节A的长度为PA。则 

P(x₁，y_A’，z_A’) 

d_min＝|y₁-y_A’| 

PA＝|x₁-x_A’| 

(3)根据上述预选择距离指标的建立，进行逃逸速度冗余机器人动态避障运动规划，其具体步骤分为： 

3.1通过预选择最小距离指标的建立得出有效的最小距离d_min，并和预设定的最小安全距离做比较。如果大于最小安全距离，则不开启动态避障命令，继续执行末端规划的轨迹。反之则开启动态避障命令，并跟踪末端轨迹。 

3.2建立和最小距离d_min相关的避障增益和逃逸速度，根据最小距离d_min的不同，而取相应的避障增益和逃逸速度，已达到较好的避障效果。 

3.3计算机器人雅可比矩阵，碰撞点雅可比矩阵，明确逃逸方向。 

3.4通过机器人逆运动学，利用冗余机器人自运动零空间映射矩阵进行动态避障路径规划； 

3.5对角速度值积分，得到下一步关节角。 

3.6跟踪末端轨迹，判断是否到达终点。 

本发明的优点在于：通过预选择最小距离指标H_d-min的建立，在计算障碍物和机器人各个杆件距离之前，先进行相交测试，剔除安全杆件，并在局部坐标系中优化距离计算方法，去除了传统指标中大量繁琐的计算量，提高了冗余机器人动态避障的规划效率，更好的满足实时性的要求。 

附图说明

图1为杆件坐标系中相交检测附图。 

图2为局部坐标系中确定碰撞点与计算最小距离附图。 

图3为避障增益和逃逸速度与最小距离d_min的关系。 

图4为本发明整体的动态避障流程图。 

具体实施方式

本发明是一种预选择最小距离指标冗余机器人动态避障方法，通过预选择最小距离指标H_d-min的建立，设定和最小距离相关的避障增益和逃逸速度，利用冗余机器人自运动零空间映射矩阵实现动态避障。 

本发明中H_d-min的思想是：针对不同的机器人构型建立相应的包围盒，利用相交测试，在计算实时距离之前剔除其他安全杆件，只考虑相碰撞的杆件和障碍物的位置关系，并在杆件的局部坐标系中计算距离值。如果在相交测试中有多个杆件相交，则H_d-min取相交距离中最小的。采用H_d-min指标描述障碍物和机器人的位置关系，在避免障碍物到杆件的垂足落在杆件延长线上的情况下，剔除安全杆件，优化传统距离计算，去除不必要的计算量。提高了冗余机器人动态避障的规划效率，更好的满足实时性的要求。 

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

(一)冗余机器人建模 

对于冗余机器人，特别是全部关节都是旋转关节的机器人，OBB方向包围盒不同于一般的AABB包围盒，它的紧密性好，能更好地适应杆件的位置和方向变化，不损失避障空间。OBB盒的建模，只需让包围盒的方向和杆件的方向重合，当杆件发生旋转运动后，OBB的基底进行同样的旋转，无需繁琐的变换方向计算，因此本文选用OBB方向包围盒建模。OBB的中心坐标分别为各个杆件的中间点坐标，长度为杆件的长度，宽、高分别设定为机器人的最小安全距离d。对于形状不一的动态障碍物，我们采用球形包围盒，设球体的中心为(x₀，y₀，z₀)，半径为R，则球体的方程可以描述为(x-x₀)²+(y-y₀)²+(z-z₀)²＝R²。 

(二)预选择最小指标H_d-min的建立 

本发明提出预选择最小距离指标H_d-min的思想是：针对不同的机器人构型建立相应的包围盒，利用相交测试，在计算实时距离之前剔除其他安全杆件，只考虑相碰撞的杆件和障碍物的位置关系，并在杆件的局部坐标系中计算距离值。如果在相交测试中有多个杆件相交，则H_d-min取相交距离中最小的。采用H_d-min指标描述障碍物和机器人的位置关系，在避免障碍物到杆件的垂足落在杆件延长线上的情况下，剔除安全杆件，优化传统距离计算，去除不必要的计算量。在避障过程中，要求H_d-min越大越好，说明离障碍物越远，机器人越安全。 

(1)相交测试 

OBB包围盒采用分离轴检测方法，即给定两个凸体集A和B，若两个集合不存在交集，则必存在一个分离超平面P，使得A和B分别位于该超平面的两侧，向直线分离轴L投影，投影区间不重叠。如果在基坐标下测试，OBB包围盒，最多有15个分离轴，在分离轴上的投影全部重叠，才能判断相交。测试OBB中15个分离轴显然很繁琐。 

如图1所示。将障碍物的坐标转换到每个杆件的局部坐标系中，相交测试在局部坐标系下完成，分离轴减少到3个，即X、Y、Z。冗余机器人中每个杆件都有相应的转换矩阵T，旋转矩阵R。若x₀为障碍物在基坐标系中的坐标，R_0n为杆件n相对于基坐标的旋转矩阵，x_n障碍物在杆件n的局部坐标系中的坐标，则x₀＝R_0n·x_n，故 

设在杆件AB的局部坐标系中，A端的坐标为(x_A，y_A，z_A’)，B端的坐标为(x_B’，y_B’，z_B’)包围盒长度和宽度为2*d，高度为杆件长度L；障碍物球心半径为R，中心坐标为D₁(x₁，y₁，z₁)，将杆件AB的包围盒和球体分别向三个坐标轴投影，当 

$\left(\left(\begin{array}{c}|{x_A}^{,}-x_1|\le R+d\\ {y_A}^{,}-R\le y_1\le {y_B}^{,}+R\\ |{z_A}^{,}-z_1|\le R+d\end{array}\right)\right)$

3个式子同时成立时，判断杆件AB和障碍物球体相碰。否则不相交，视为需要剔除的安全杆件。 

(2)局部映射距离计算 

在相交测试后，剔除不和障碍物发生碰撞的安全杆件，在此基础上，在碰撞杆件的局部 坐标系中计算有效的最小距离。如图2所示，杆件AB和障碍物球体检测相交，由于最小距离d_min是标量，所以在任意坐标系中它的值不变。碰撞点P的求取，实质是为了求得碰撞点的雅可比矩阵J₀，利用矢量积法计算雅可比矩阵J₀，则需要|PA|，也是标量。所以不用转换到基坐标系中，直接在局部坐标系中计算即可。在杆L局部坐标系中，从相交测试可知，A端坐标(x_A’，y_A’，z_A’)，B端坐标(x_B’，y_B’，z_B’)，障碍物中心坐标D₁(x₁，y₁，z₁)，碰撞点P。则 

P(x₁，y_A’，z_A’) 

d_min＝|y₁-y_A’| 

PA＝|x₁-x_A’| 

利用局部映射方法计算距离，直接利用目标点的坐标值就能直接得出结果，省去了传统距离计算中解大量方程组的繁琐计算。下面给出在基坐标系中传统的距离计算，作以比较。 

传统距离计算在基坐标系中完成，杆件AB的空间直线方程可写成： 

$\frac{x-x_A}{m}=\frac{y-y_A}{n}=\frac{z-z_A}{P}=t$

式中，(x_A，y_A，z_A)为杆件A端点在基坐标中的坐标，(m，n，p)为杆件的方向矢量，t为方程参数。最小距离的计算，一般是向直线做垂线，垂足为最近点，即碰撞点，过球心点向杆件直线做垂面，如图2所示。垂面的法线为杆件的方向矢量，过球心点(x₀，y₀，z₀)并垂直于杆件直线的平面方程为： 

m(x-x₀)+n(y-y₀)+p(z-z₀)＝0 

将式(3)改写成如下形式 

$\left(\begin{array}{c}x=x_A+\mathrm{mt}\\ y=y_A+\mathrm{nt}\\ z=z_A+\mathrm{pt}\end{array}\right)$

式(3)与式(4)联立方程组，解得 

$t=\frac{m(x_0-x_A)+n(y_0-y_A)+p(z_0-z_A)}{m^2+n^2+p^2}$

将t值代入式(5)，得到杆件直线和垂面的交点，即碰撞点B(x_B，y_B，z_B)。 

$d_{\min }=\sqrt{{(x_B-x_0)}^2+{(y_B-y_0)}^2+{(z_B-z_0)}^2}$

通过以上分析，预选择最小距离指标H_d-min在冗余机器人动态避障运用中，节省了避障处理中环境检测的时间，更好的满足动态避障实时性的要求，主要体现在以下两个方面： 

①冗余机器人杆件数量多，在计算障碍物和各个杆件距离之前，先进行相交测试，剔除不相交杆件，即安全杆件，只考虑可能与障碍物发生碰撞的杆件，排除不必要的计算量。 

②OBB包围盒的方向和杆件的方向一致，完成相交测试后，由于最小距离以及碰撞点到杆件端点的距离都是标量，不会随着坐标系的改变而改变，所以采用局部映射距离计算方法，舍去了传统方法中解大量方程组的繁琐计算。 

(三)逃逸速度动态避障路径规划 

实时最小距离d_min计算得出后，利用逃逸速度动态避障方法进行运动规划。建立和最小距离相关的避障增益a_n和逃逸速度a₀，设置3个阈值距离d₁，d₂，d₃，如图3所示。 

当d_min＞d₃时，安全区域，障碍物与杆件满足无碰撞条件，a₀，a_n均为零，可以不改变由伪逆解所得出的机器人位姿，即用最小范数求解。OBB包围盒的宽、高，由d₃决定。 

当d₂＜d_min＜d₃时，预警区域，a_n均匀增大，开启避障项，a₀为零，此时可以不引入逃逸速度，得以保证障碍物远离杆件。 

当d₁＜d_min＜d₂时，危险区域，随着d_min的减小，a₀迅速增大，在连杆上增加一个远离障碍物的逃逸速度，利用冗余机器人零空间映射矩阵进行避障路径规划，当d_min到d₁时，逃逸速度达到最大值。 

若机器人在运动过程中某一时刻，通过碰撞检测，机械手与一连杆相交，记该连杆与障碍物的碰撞点为x₀，碰撞点的雅可比矩阵J₀。 

利用冗余机器人的自运动进行避障，在关节的零空间选择最安全避障解，其逆运动学表 

逃逸速度动态避障逆运动学表示为 

$\stackrel{·}{q}=J^{+}\stackrel{·}{x}+a_n{\left[J_0(I-J^{+}J)\right]}^{+}(a_0{\stackrel{·}{x}}_0-J_0{J}^{+}\stackrel{·}{x})$

式中，x₀′躲避障碍物的速度方向(单位矢量)，设定为障碍物运动速度的反方向。由于避障增益和逃逸速度的平滑变化，也使机器人在避障过程中，关节运动连续，避免抖动现象的发生。 

图4是本发明预选择最小距离指标动态避障运动规划的整体流程图，其步骤分为：对于冗余机器人各个杆件及随机运动障碍物建立OBB包围盒；实时检测运动障碍物与机器人杆件的相对位置关系，将基坐标系中的障碍物映射到杆件的局部坐标系中，建立预选择最小距离指标H_d-min；利用分离轴定理进行相交测试，剔除安全杆件；利用局部映射计算最小距离d_min，并和预设定的最小安全距离做比较，如果大于最小安全距离，则不开启动态避障命令，继续执行末端规划的轨迹，反之则开启动态避障命令，并跟踪末端轨迹；建立和最小距离d_min相关的避障增益和逃逸速度，根据最小距离d_min的不同，而取相应的避障增益和逃逸速度，已达到较好的避障效果；计算机器人雅可比矩阵，碰撞点雅可比矩阵，明确逃逸方向；通过机器人逆运动学，利用冗余机器人自运动零空间映射矩阵进行动态避障路径规划；对角速度值积分，得到下一步关节角；跟踪末端轨迹，判断是否到达终点。