[发明专利]预选择最小距离指标冗余机器人动态避障方法

权利要求书

1.一种预选择最小距离指标冗余机器人动态避障方法，其特征在于包括以下步骤：

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

(2)实时检测运动障碍物与机器人杆件的相对位置关系，将基坐标系中的障碍物映射到杆件的局部坐标系中，建立预选择最小距离指标H_d-min。

(3)设定和最小距离相关的避障增益和逃逸速度，利用冗余机器人自运动零空间映射矩阵实现逃逸速度动态避障运动规划。

2.如权利要求1所述的预选择最小距离指标冗余机器人动态避障方法，其特征在于：

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

所述步骤(2)中建立预选择最小距离指标H_d-min的具体步骤是：

2.1在机器人杆件的局部坐标系中，利用分离轴定理进行相交测试。

2.2利用局部映射计算最小距离，优化传统距离计算方法。

3.如权利要求3所述的预选择最小距离指标冗余机器人动态避障方法，其特征在于：

所述步骤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的包围盒和球体分别向三个坐标轴投影，当

|xA,-x1|≤R+dyA,-R≤y1≤yB,+R|zA,-z1|≤R+d]]>

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