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

预选择最小距离指标冗余机器人动态避障方法专利名称：预选择最小距离指标冗余机器人动态避障方法技术领域：本发明涉及障碍物随机运动的环境下，预选择最小距离指标冗余机器人动态避障路径规划方法，在动态避障规划前便剔除安全杆件，并采用局部映射距离计算方法，得出实时最小距离本发明无需计算障碍物到所有杆件的最小距离，提高了动态避障的规划效率，满足实时性的要求属于智能机器人技术领域背景技术：冗余机器人，是指关节空间维数η大于任务空间维数m，可以在自运动而不影响末端位姿的情况下实现多种二次目标由于任务环境越来越复杂，要求冗余机器人具有更高的智能性，其中动态避障是机器人自主路径规划中一个重要的组成部分，日益成为人们关注的焦点 目前，利用杆件与障碍物的距离信息完成避障规划是学者们的研究重点Maciejiewski和Khatib应用人工势场法,通过距离信息建立吸引势场和排斥势场完成避障LALIBERTE等人针对人工势场法容易发生局部极小的不足，提出了离散势场法，从而减少了局部极小发生的可能性CHO等人基于流体力学的势流理论，提出了一种可以避免局部极小的新颖的速度排斥函数法另外，Liegeois提出梯度投影法，利用最小距离指标，优化雅克比矩阵的零空间矢量，修改齐次解完成避障。

IKADA等人提出一种潜在避障操作性能形状指标来控制机械臂达到易于避障的位形来进行避障，同时保持末端轨迹的跟踪Kavraki等人提出了随机路标法，该方法通过环境距离检测，搜索出一些能够避开障碍物的关节坐标点来实现避障，但是随着机构的冗余度增加，计算量也在增加避障指标选取的合适与否将直接影响到避障效果的好坏，特别对于动态避障，障碍物随机运动，选择合理的并能准确快速描述障碍物与机械臂位置关系的指标，对于保证避障规划的实时性尤为重要传统的距离指标，需要计算障碍物到每个杆件的最小距离，然后再取最小值当杆件处于某些特殊构形时，障碍物到杆件最小距离的垂足可能会落到杆件的延长线上，延长线并不属于机器人的一部分，对于该杆件计算的最小距离不准确，这时需要在杆件上取多个标志点并遍历各个标志点到障碍物的距离可见，传统的距离避障指标，一是准确性不足，二是多个标志点的计算，耗费了大量的时间对于传统的距离指标，可以完成静态避障规划，但是对于动态避障规划来说，障碍物随机运动，需要机器人在有限的时间内完成障碍物检测及关节的多种动作，对避障的效率提出了更高的要求发明内容基于上述，本发明提出在障碍物随机运动环境下，一种预选择距离指标冗余机器人动态避障路径规划方法，该方法无需计算障碍物到所有杆件的最小距离，并且采用局部映射距离计算方法，提高了冗余机器人动态避障的规划效率。

为了实现此目的，本发明所采取的技术方案是(I)对于冗余机器人各个杆件及随机运动障碍物建立相应的包围盒，减少基本几何元素的数目和参与相交测试的包围盒的数目进一步的，本发明选用OBB方向包围盒建模，并设定包围盒的方向和杆件的方向重合，包围盒的长度和杆件的长度一致，宽度根据最小安全距离设定面对全部关节都是旋转关节的冗余机器人，当杆件发生旋转运动后，只需让OBB盒的基底进行同样的旋转进行实时更新对于形状不一的动态障碍物，采用球形包围盒2)实时检测运动障碍物与机器人杆件的相对位置关系，建立预选择最小距离指标Hd_min，其思想是针对不同的机器人构型建立相应的包围盒，利用相交测试，在计算实时距离之前剔除其他安全杆件，只考虑相碰撞的杆件和障碍物的位置关系，并在杆件的局部坐标系中计算距离值如果在相交测试中有多个杆件相交，则Hd_min取相交距离中最小的在避障过程中，要求Hd_min越大越好，说明离障碍物越远，机器人越安全 具体步骤分为2. I在机器人杆件的局部坐标系中，利用分离轴定理进行相交测试2. 2利用局部映射计算最小距离，优化传统距离计算方法进一步的，所述2. I相交测试中，OBB包围盒采用分离轴检测方法，将障碍物的坐标转换到每个杆件的局部坐标系中，相交测试在局部坐标系下完成，分离轴减少到3个，即X、Y、z。

如果在3个分离轴上，障碍物和机器人的投影全都重合，则判断相交如果其中有一个分离轴上的投影分离，则不会发生碰撞，记为安全杆件，不作为计算实时距离的有效杆件设在杆件AB的局部坐标系中，A端的坐标为(xA’，yA’，zA’)，B端的坐标为(xB’，yB’，zB’ )包围盒长度和宽度为2*d，高度为杆件长度L ;障碍物球心半径为R，中心坐标为D1 (x1； y1； Z1),将杆件AB的包围盒和球体分别向三个坐标轴投影，当权利要求1.一种预选择最小距离指标冗余机器人动态避障方法，其特征在于包括以下步骤 (1)对于冗余机器人各个杆件及随机运动障碍物建立相应的OBB包围盒，减少基本几何元素的数目和参与相交测试的包围盒的数目2)实时检测运动障碍物与机器人杆件的相对位置关系，将基坐标系中的障碍物映射到杆件的局部坐标系中，建立预选择最小距离指标Hd_min3)设定和最小距离相关的避障增益和逃逸速度，利用冗余机器人自运动零空间映射矩阵实现逃逸速度动态避障运动规划2.如权利要求I所述的预选择最小距离指标冗余机器人动态避障方法，其特征在于 预选择最小距离指标Hd_min的思想是针对不同的机器人构型建立相应的包围盒，利用相交测试，在计算实时距离之前剔除安全杆件，只考虑相碰撞的杆件和障碍物的位置关系，并在杆件的局部坐标系中计算距离值。

如果在相交测试中有多个杆件相交，则Hd_min取相交距离中最小的在避障过程中，要求Hd_min越大越好，说明离障碍物越远，机器人越安全所述步骤(2)中建立预选择最小距离指标Hd_min的具体步骤是 ·2.I在机器人杆件的局部坐标系中，利用分离轴定理进行相交测试 · 2.2利用局部映射计算最小距离，优化传统距离计算方法3.如权利要求3所述的预选择最小距离指标冗余机器人动态避障方法，其特征在于 所述步骤2. I相交测试的依据是0ΒΒ包围盒采用分离轴检测方法，将障碍物的坐标转换到每个杆件的局部坐标系中，相交测试在局部坐标系下完成，分离轴减少到3个，即X、Y、Ζ如果在3个分离轴上，障碍物和机器人的投影全都重合，则判断相交如果其中有一个分离轴上的投影分离，则不会发生碰撞，记为安全杆件，不作为计算实时距离的有效杆件设在杆件AB的局部坐标系中，A端的坐标为(V，yA，，zA’)，B端的坐标为(xB，，yB，，zB’)包围盒长度和宽度为2*d，高度为杆件长度L ;障碍物球心半径为R，中心坐标为D1U1,Y1, Z1),将杆件AB的包围盒和球体分别向三个坐标轴投影，当4.如权利要求3所述的预选择最小距离指标冗余机器人动态避障方法，其特征在于 所述步骤2. 2局部映射最小距离计算的依据是在相交测试后，剔除不和障碍物发生碰撞的安全杆件，在此基础上，在碰撞杆件的局部坐标系中计算有效的最小距离。

由于最小距离dmin是标量，所以在任意坐标系中它的值不变碰撞点的求取，实质是为了求得碰撞点的雅可比矩阵，利用矢量积法计算雅可比矩阵Jtl，则需要计算碰撞点到杆件末端的距离，也是标量所以不用转换到基坐标系中，直接在局部坐标系中利用目标点的坐标值直接得出结果设定相碰撞杆件A端坐标(xA’，yA’，zA’)，B端坐标(xB’，yB’，zB’)，障碍物中心坐标D1U1, y1； Z1)，碰撞点为P，障碍物距此杆件的最小距离为dmin，碰撞点距离末端关节A的长度为PA则P (χι，Ya，zA )dmin = ΥΓΥα^PA = I X1-X/5.如权利要求I所述的预选择最小距离指标冗余机器人动态避障方法，其特征在于所述步骤(3)中根据预选择最小距离指标Hd_min的建立，进行逃逸速度冗余机器人动态避障运动规划具体步骤是.3.I通过预选择最小距离指标的建立得出有效的最小距离dmin，并和预设定的最小安全距离做比较如果大于最小安全距离，则不开启动态避障命令，继续执行末端规划的轨迹反之则开启动态避障命令，并跟踪末端轨迹3.2建立和最小距离dmin相关的避障增益和逃逸速度，根据最小距离dmin的不同，而取相应的避障增益和逃逸速度，已达到较好的避障效果。

3.3计算机器人雅可比矩阵，碰撞点雅可比矩阵，明确逃逸方向3.4通过机器人逆运动学，利用冗余机器人自运动零空间映射矩阵进行动态避障路径规划； .3.5对角速度值积分,得到下一步关节角3.6跟踪末端轨迹，判断是否到达终点全文摘要本发明涉及障碍物随机运动环境下，预选择最小距离指标冗余机器人动态避障路径规划方法，包括步骤利用OBB包围盒对冗余机器人进行建模，建立预选择最小距离指标；通过相交测试，在计算实时距离之前剔除安全杆件；在相应杆件的局部坐标系中，采用局部映射距离计算方法，利用目标点坐标值直接得出实时最小距离并确定碰撞点位置；在此基础上，建立和最小距离相关的避障增益和逃逸速度，利用冗余机器人零空间映射矩阵进行避障路径规划本发明在动态避障规划前便剔除安全杆件，去除了不必要的繁琐计算并优化距离计算方法，提高了动态避障的规划效率，满足实时性的要求。