CN108908331B - 超冗余柔性机器人的避障方法及***、计算机存储介质 - Google Patents

Abstract

本发明公开了一种超冗余柔性机器人的避障方法及***、计算机存储介质，将空间中的障碍物分别用空间超二次曲面方程和空间几何模型进行建模；以障碍物为中心，将障碍物周围的空间划分为安全区域、预警区域和危险区域，安全区域和预警区域之间的界限为由超二次函数表示的预警边界，预警区域和危险区域之间的界限为由几何函数表示的危险边界；实时检测超冗余柔性机器人与预警边界之间的最小伪距离是否小于或等于零，若是，判断超冗余柔性机器人到达预警区域；对超冗余柔性机器人进行避障处理；实时检测超冗余柔性机器人与危险边界之间的最小欧几里得距离是否大于零，若判断结果为是，判断超冗余柔性机器人避障成功；完成超冗余柔性机器人的避障规划。

Description

超冗余柔性机器人的避障方法及***、计算机存储介质

技术领域

本发明涉及机器人控制领域，尤其是一种超冗余柔性机器人的避障方法及***、计算机存储介质。

背景技术

超冗余机器人实际的工作环境往往比较复杂，在执行期望任务的同时还需要满足回避环境障碍物的要求。当工作环境已知或者工作环境可通过特定手段探测时，具有冗余自由度的机械臂在完成主任务的同时可以回避环境中的障碍物等附加任务。

首先，障碍物建模是避障规划算法的关键问题之一。为了减少计算量提高避障效率，通常采用简单的几何模型及其组合体来表征机械臂及其工作环境。Colbaugh等研究了四连杆平面机械臂的避障规划问题。在该方法中将平面障碍物和机械臂分别通过圆和直线来建模。这种简化建模方式满足了时变环境下机械臂的多障碍物避障规划的需求。Rahmanian-Shahri等提出了在线碰撞识别方法。该方法在平面中将冗余机器人的每个连杆和工作空间中的每个障碍物通过椭圆来建模。在三维工作空间中通常使用球体、椭圆体、凸多面体和圆柱体作为建模的几何单元。Bonner等提出了连续球面近似的方式来表示空间对象，这有助于无碰撞路径的规划。Choi等分别采用球和椭球体对障碍物和机械臂建模。Wang等还将工作空间中的对象建模为椭圆体，以简化数学表示并降低碰撞检测的计算复杂性。对于一些物体(例如圆柱臂杆)圆柱体模型是一种更合适的表示方式，因此Patel等提出了一种使用对偶向量和角度表达的距离计算和碰撞检测的方法。Saramago等针对移动障碍物的避障问题提出了一种通过添加惩罚函数来规划离线最优轨迹的方法。Perdereau等提出了一种使用超二次表面函数来表示障碍物的方法。该方法通过逆向迭代的求解几何模型来处理碰撞回避问题。Mu等采用超二次曲面方程建立了空间障碍物的数学模型，为冗余机械臂的避障规划提供了判据，实现了冗余机械臂的在三维环境下多障碍物的避障规划。

在避障规划方面国内外学者也开展了一定相关的研究。Glass等针对冗余机械臂的特点提出了实时避障规划方法。该方法在逆运动学层面利用阻尼最小二乘法实现了机械臂的构型控制。Yoshida等基于迭代的双阶段规划方法提出了机械臂在复杂三维环境下的避障规划方法。Chi等用通过顶点描述的多面体表示机械臂的连杆和障碍物。在最小距离算法中获得障碍物多面体及机械臂多面体之间的距离信息，基于距离信息提出了三种方法(梯度投影法、力障碍物回避方法、速度障碍物回避方法)实现了障碍物回避功能。Freund等提出了一种在线规划多障碍物环境下多机器人避障运动的方法。将避障优化问题转换为二次凸优化问题，为障碍物回避提供了数学表达方式。为了实现障碍物的回避，该方法采用了可以直接控制关节运动的加速度求解方式，并能同时考虑潜在的碰撞危险；由于是基于模型的规划方法，因此该方法具有一定的通用性。该方法已经应用于多机器人实验台中。V.Mayorga等提出了一种采用几何包围法在线避障的方法，该方法可以通过计算零空间、包围几何体及距离向量等方式简单有效的处理障碍物回避问题。Homayoun等提出了一种实时避障的方法。该方法将避障问题公式化为力位混合控制问题。在所建立的弹簧阻尼模型中用虚拟力的大小表示障碍物与机械臂的接近程度。Guo等提出了基于不等式的避障标准，在满足不等式标准的条件下通过对关节加速度的控制实现障碍物的回避。通过结合这种动态更新的不等式标准和关节物理约束(即关节角度限制，关节速度限制和关节加速度限制)，提出了最小加速度范数求解方案，并进行了冗余求解的研究。Tsoukalas等向传统的动态微机械臂模型引入范德华力。其中机械臂的每个连杆在运动过程中被分解成与相邻对象相互作用的一系列基本粒子。通过范德华力导出障碍物的近似位置，然后通过判断范德华力的大小获得无碰撞路径，从而实现障碍物的有效回避。Hu等开发了所谓的反向二次搜索算法解决障碍物回避中的逆运动学求解问题。反向二次搜索算法是根据二次函数根的属性来检测潜在的碰撞。二次函数的类别是通过椭圆包围的障碍物以及每个连杆端点的位置得到的。该算法从末端执行器到基座反向搜索与障碍物可能的碰撞位置，并通过使用混合逆运动学方案(包括阻尼最小二乘法，加权最小范数法和梯度投影方法)回避环境中的障碍物。

在以往的研究中，空间障碍物建模问题已得到一定的研究。然而，在这些三维空间障碍物建模和回避的研究中鲜有兼顾了避障效率和避障准确性的方法。当避障效率是建模过程中的重点关注因素时，一般会采用简单的几何模型包络或者计算定性的距离(比如超二次曲面方程的伪距离)。这样将降低潜在碰撞的检测的精度而且安全工作空间被大大减小。相反，如果采用了相对精确的模型，则在障碍物回避过程中计算量总是很大，占用较多的计算机运算资源及设备的存储资源。

另外，现有的避障规划方法主要针对6自由度机械臂、7自由度或平面冗余机械臂，对于超冗余机器人的在三维空间的避障规划方法研究较少，并不能满足超冗余机器人在三维空间中避障规划的需求。为了解决超冗余机器人在多障碍空间避障轨迹规划的问题，该技术有必要进行改进。

发明内容

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。为此，本发明的一个目的是提供一种超冗余柔性机器人的避障方法及***、计算机存储介质，用于实现超冗余柔性机器人的避障处理。

本发明所采用的技术方案是：一种超冗余柔性机器人的避障方法，包括以下步骤：

将空间中的障碍物分别用空间超二次曲面方程和空间几何模型进行建模；

基于所建立的模型，以所述障碍物为中心，将所述障碍物周围的空间划分为安全区域、预警区域和危险区域，所述安全区域和预警区域之间的界限为由超二次函数表示的预警边界，所述预警区域和危险区域之间的界限为由几何函数表示的危险边界；

实时检测所述超冗余柔性机器人与所述预警边界之间的伪距离；

判断所述超冗余柔性机器人与所述预警边界之间的最小伪距离是否小于或等于零，若判断结果为是，判断所述超冗余柔性机器人到达预警区域；

对所述超冗余柔性机器人进行避障处理；

实时检测所述超冗余柔性机器人与所述危险边界之间的欧几里得距离；

判断所述超冗余柔性机器人与所述危险边界之间的最小欧几里得距离是否大于零，若判断结果为是，判断所述超冗余柔性机器人避障成功。

进一步地，判断所述超冗余柔性机器人与所述危险边界之间的最小欧几里得距离是否大于零，若判断结果为否，判断所述超冗余柔性机器人避障失败，控制所述超冗余柔性机器人停止运动。

进一步地，所述超冗余柔性机器人与所述危险边界之间的最小欧几里得距离为所述危险边界与超冗余柔性机器人的机械臂的公垂线的最小长度、或者为所述危险边界上的点到超冗余柔性机器人的机械臂的最小垂直距离、或者为超冗余柔性机器人的机械臂端点到所述危险边界的最小垂直距离。

进一步地，判断所述超冗余柔性机器人与所述预警边界之间的伪距离是否大于零，若判断结果为是，判断所述超冗余柔性机器人位于所述安全区域，以能量最优原则规划所述超冗余柔性机器人的机械臂的运动。

进一步地，通过调整改进模式函数法的避障参数实现所述超冗余柔性机器人的避障处理。

进一步地，所述避障参数包括臂型角参数和/或等效臂杆长度。

本发明所采用的另一技术方案是：一种超冗余柔性机器人的避障***，包括

建模单元，用于将空间中的障碍物分别用空间超二次曲面方程和空间几何模型进行建模；

区域划分单元，用于基于所建立的模型，以所述障碍物为中心，将所述障碍物周围的空间划分为安全区域、预警区域和危险区域，所述安全区域和预警区域之间的界限为由超二次函数表示的预警边界，所述预警区域和危险区域之间的界限为由几何函数表示的危险边界；

距离检测单元，用于实时检测所述超冗余柔性机器人与所述预警边界之间的伪距离；

避障需求检测单元，用于判断所述超冗余柔性机器人与所述预警边界之间的最小伪距离是否小于或等于零，若判断结果为是，判断所述超冗余柔性机器人到达预警区域；

避障单元，用于对所述超冗余柔性机器人进行避障处理；

避障结果判断单元，用于实时检测所述超冗余柔性机器人与所述危险边界之间的欧几里得距离；判断所述超冗余柔性机器人与所述危险边界之间的最小欧几里得距离是否大于零，若判断结果为是，判断所述超冗余柔性机器人避障成功。

进一步地，所述超冗余柔性机器人与所述危险边界之间的最小欧几里得距离为所述危险边界与超冗余柔性机器人的机械臂的公垂线的最小长度、或者为所述危险边界上的点到超冗余柔性机器人的机械臂的最小垂直距离、或者为超冗余柔性机器人的机械臂端点到所述危险边界的最小垂直距离。

进一步地，通过调整改进模式函数法的避障参数实现所述超冗余柔性机器人的避障处理。

本发明所采用的另一技术方案是：一种计算机存储介质，其上存储有计算机程序，所述程序被处理器执行时实现以下步骤：

将空间中的障碍物分别用空间超二次曲面方程和空间几何模型进行建模；

基于所建立的模型，以所述障碍物为中心，将所述障碍物周围的空间划分为安全区域、预警区域和危险区域，所述安全区域和预警区域之间的界限为由超二次函数表示的预警边界，所述预警区域和危险区域之间的界限为由几何函数表示的危险边界；

实时检测所述超冗余柔性机器人与所述预警边界之间的伪距离；

判断所述超冗余柔性机器人与所述预警边界之间的最小伪距离是否小于或等于零，若判断结果为是，判断所述超冗余柔性机器人到达预警区域；

对所述超冗余柔性机器人进行避障处理；

实时检测所述超冗余柔性机器人与所述危险边界之间的欧几里得距离；

判断所述超冗余柔性机器人与所述危险边界之间的最小欧几里得距离是否大于零，若判断结果为是，判断所述超冗余柔性机器人避障成功。

本发明的有益效果是：

本发明一种超冗余柔性机器人的避障方法及***、计算机存储介质，将空间中的障碍物分别用空间超二次曲面方程和空间几何模型进行建模；基于所建立的模型，以所述障碍物为中心，将障碍物周围的空间划分为安全区域、预警区域和危险区域，安全区域和预警区域之间的界限为由超二次函数表示的预警边界，预警区域和危险区域之间的界限为由几何函数表示的危险边界；实时检测超冗余柔性机器人与预警边界之间的伪距离；判断超冗余柔性机器人与预警边界之间的最小伪距离是否小于或等于零，若判断结果为是，判断超冗余柔性机器人到达预警区域；对超冗余柔性机器人进行避障处理；实时检测超冗余柔性机器人与危险边界之间的欧几里得距离；判断超冗余柔性机器人与危险边界之间的最小欧几里得距离是否大于零，若判断结果为是，判断超冗余柔性机器人避障成功；建立混合障碍物模型，实现了对超冗余柔性机器人的避障需求检测、避障处理以及避障结果检测，完成超冗余柔性机器人的避障规划。

附图说明

下面结合附图对本发明的具体实施方式作进一步说明：

图1a和图1b是本发明中一种超冗余柔性机器人的避障方法的建模方法示意图；

图2是本发明中一种超冗余柔性机器人的避障方法中空间点与超二次曲面模型的一具体实施例相对关系示意图；

图3是本发明中一种超冗余柔性机器人的避障方法的一具体实施例混合模型示意图；

图4a、图4b和图4c是本发明中一种超冗余柔性机器人的避障方法的超冗余柔性机器人与几何模型的一具体实施例位置关系示意图；

图5是本发明中一种超冗余柔性机器人的避障方法中调整等效臂杆长度实现避障的一具体实施例示意图；

图6是本发明中一种超冗余柔性机器人的避障方法中调整臂型角大小实现避障的一具体实施例示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

针对超冗余柔性机器人在工作环境中的典型障碍物建模及其回避问题，提出基于混合障碍物模型的建模及避障策略，提供了一种超冗余柔性机器人的避障方法，包括以下步骤：

将空间中的障碍物分别用空间超二次曲面方程和空间几何模型进行建模；

参考图1a和图1b，图1a和图1b是本发明中一种超冗余柔性机器人的避障方法的建模方法示意图；基于所建立的模型，以障碍物为中心，将障碍物周围的空间划分为安全区域、预警区域和危险区域，安全区域和预警区域之间的界限为由超二次函数表示的预警边界，超二次函数可以表达任意障碍物的形状；预警区域和危险区域之间的界限为由几何函数表示的危险边界；

参考图1a，实时检测超冗余柔性机器人即其机械臂与预警边界之间的伪距离；判断超冗余柔性机器人与预警边界之间的最小伪距离是否小于或等于零，若判断结果为是，判断超冗余柔性机器人到达或者跨过预警边界位于预警区域；

开始对超冗余柔性机器人进行避障处理；

参考图1b，判断超冗余柔性机器人到达预警区域的同时，开始实时检测超冗余柔性机器人与危险边界之间的欧几里得距离；判断超冗余柔性机器人与危险边界之间的最小欧几里得距离是否大于零，若判断结果为是，判断超冗余柔性机器人避障成功。

本发明通过建立混合障碍物模型，通过计算机器人与预警边界的伪距离实现了对超冗余柔性机器人的避障需求检测，当到达预警区域时进行避障处理，再通过计算机器人与危险边界的距离判断避障结果，完成了对超冗余柔性机器人的避障规划。另外，判断超冗余柔性机器人与危险边界之间的最小欧几里得距离是否大于零，若判断结果为否，判断超冗余柔性机器人避障失败，此时，表示机器人到达危险区域，需要控制超冗余柔性机器人停止运动，进入人工干预。而判断超冗余柔性机器人与预警边界之间的伪距离是否大于零，若判断结果为是，判断超冗余柔性机器人位于安全区域，以能量最优原则规划超冗余柔性机器人的机械臂的运动，能量最优原则是保证机械臂的运动变化量最小，以节省机械臂的能量损耗。

参考图1a和图1b，利用几何模型和超二次曲面模型对障碍物进行建模以对障碍物所在空间进行划分，参考图2，图2是本发明中一种超冗余柔性机器人的避障方法中空间点与超二次曲面模型的一具体实施例相对关系示意图；空间点在超二次曲面模型中可以分为三类：曲面内部、在曲面上及曲面外部。事实上，用实际几何模型来模拟障碍物得到的危险边界为障碍物实际本体加上一定的安全阈值。参考图3，图3是本发明中一种超冗余柔性机器人的避障方法的一具体实施例混合模型示意图；空间桁架是由杆件组成的狭小空间，超冗余柔性机器人需要穿过空间桁架中的狭小空间开展作业任务，本实施例以空间站桁架为例，分别建立了其混合模型，图3中外层预警边界采用超二次曲面模型建立，提供障碍物回避时的伪距离判据；内层的危险边界采用空间几何模型建立，提供障碍回避时的欧几里得距离判据。基于对超二次曲面模型及空间几何模型的分析可以看出：第一，超二次曲面模型适合对障碍物进行宏观包络，提供定性的避障规划准则即伪距离，伪距离的计算方式是将需要判断的点坐标直接代入统一的超二次曲面方程，因此伪距离的计算量远低于欧几里得距离的计算量。第二，空间几何模型适合对障碍物进行精细包络，提供定量的避障规划准则即欧几里得距离，这种准则特点是计算量大，但可以精确判断超冗余柔性机器人与障碍物之间的空间关系。

作为技术方案的进一步改进，参考图4a、图4b和图4c，图4a、图4b和图4c是本发明中一种超冗余柔性机器人的避障方法的超冗余柔性机器人与几何模型的一具体实施例位置关系示意图；超冗余柔性机器人的机械臂臂杆到危险边界的最小距离通常出现在下面三个地方之一：(1)危险边界与超冗余柔性机器人的机械臂的公垂线的最小长度、(2)危险边界上的点到超冗余柔性机器人的机械臂的最小垂直距离、(3)超冗余柔性机器人的机械臂端点到危险边界的最小垂直距离。下面具体分析超冗余柔性机器人的机械臂臂杆到危险边界的最小距离的计算过程：

参考图4a、图4b和图4c，本实施例中，危险边界以圆柱模型(如图4a、图4b、图4c中的圆柱形)为例做介绍，将机器人的基座及各个关节点(即万向节)的坐标位置转换到圆柱模型坐标系中表示，机械臂上的关节点P_i坐标在圆柱模型坐标系{O_cy}中，如式(1)所示：

图4a、图4b和图4c中，

为圆柱体体心坐标，

为圆柱上顶面(up)圆心坐标，

为圆柱下底面(below)圆心坐标。

(1)危险边界与超冗余柔性机器人的机械臂的公垂线的最小长度

参考图4a，危险边界与超冗余柔性机器人的机械臂的公垂线的两个垂足分别在机械臂的臂杆和圆柱的内部，其中，圆柱的几何方程为式(2)。

在{O_cyX_cY_cZ_c}坐标系中O_bO_u直线方程如式(3)所示：

为方向向量，

为机械臂臂杆的方向矢量。可知垂线

方程为：

方向向量为

其中r₀为机械臂的半径，R为圆柱半径，2H为圆柱高度。

由垂足的几何性质可知：

进一步可得：

由

分别属于

可知：

联立方程可得：

则最小距离为：

(2)危险边界上的点到超冗余柔性机器人的机械臂的最小垂直距离

参考图4b，危险边界上的点到超冗余柔性机器人的机械臂的垂线的一垂足在臂杆内部，另一垂足在圆柱外部，当臂杆上的垂足在当前臂杆上，而在圆柱的垂足落在圆柱外部时，臂杆与危险边界即圆柱的最小距离不再是公垂线

的长度。空间三点P₁P₂O_cy在空间形成一个平面，而此平面与圆柱顶面相交于Cr点(Cross)，沿Cr点向

作垂线，垂足为

则将

的长度定义为P₁P₂即臂杆到危险边界的最小距离。

设定求解点为Cr(x_Cr,y_Cr,z_Cr)、

则在此种条件下臂杆到危险边界的最小距离为

(3)超冗余柔性机器人的机械臂端点到危险边界的最小垂直距离

参考图4c，超冗余柔性机器人的机械臂端点到危险边界的垂线的一垂足在臂杆外部，另一垂足在圆柱内部；当圆柱上的垂足在圆柱上，而在臂杆的垂足落在臂杆外部时，臂杆与危险边界的最小距离不再是公垂线

的长度，而变成了端点P₁或P₂到圆柱的距离，例如

则此时，臂杆到危险边界的最小距离为

通过计算超冗余柔性机器人与危险边界之间的最小欧几里得距离是否大于零，以判断超冗余柔性机器人是否避障成功，通过计算上述三种可能出现最小距离的情形，降低所需的计算量，提高判断效率。

作为技术方案的进一步改进，可以通过调整改进模式函数法的避障参数实现超冗余柔性机器人的避障处理。利用改进模式函数法可以控制机械臂的运动，有关改进函数模式函数法的论文信息如下：《Acta Astronautica》-2017-A modified modal method forsolving the mission-oriented inverse kinematics of hyper-redundant spacemanipulators for on-orbit servicing；或者参阅已公开的专利申请中“基于模式函数的超冗余机械臂混合逆向求解方法及***”中的描述。其中，避障参数包括臂型角参数和/或等效臂杆长度。

参照图5，图5是本发明中一种超冗余柔性机器人的避障方法中调整等效臂杆长度实现避障的一具体实施例示意图；使用改进模式函数法它们都是基于等效臂杆参数

的调整。定义如下参数：

为基于等效臂杆长度的万向节位置；F_i为最短距离垂线的垂足；

为依赖等效臂杆参数垂线的垂足。

等效臂杆是自变量，实际的连杆表示为

等效连杆表示为

参考面表示为

为了避开空间中的危险边界需要迭代搜索最优的等效臂杆参数值。通过优化等效臂杆参数，保证超冗余柔性机器人在预警区域的运动过程中能够回避空间障碍物。

参照图6，图6是本发明中一种超冗余柔性机器人的避障方法中调整臂型角大小实现避障的一具体实施例示意图。当第i个关节组的等效臂杆参数确定后，臂型角

成为调整偶数编号万向节的可控变量。定义如下参数：

为基于臂型角ψ_i垂线的垂足，

为基于臂型角ψ_i的万向节构型，ΔU_2i-1U_2i+1U_2N为臂型角ψ_i的参考面。

当超冗余柔性机器人的臂型角ψ＝[0,π)(当ψ＝[π,2π)时，超冗余柔性机器人在结构上形成另一半对称构型)，可以求一系列对应的构型，但并不是每种构型都能满足机械臂的要求。由于每种机械臂在设计完成后，都会有运动极限，当其中某个关节的解超过运动极限时，当前关节构型是不能实现的。因此，需要选择关节角度都满足运动极限要求的臂型角作为求解参数。通过调整及优化臂型角ψ_i的值，保证危险边界与机械臂的最小距离满足避障要求，即它们的最小距离大于零。

进一步，为了保证机械臂的安全运行，判定碰撞的目标函数通过预测的方式获得，基于预测的避障规划，总是用t+1时刻的数据去计算，判断是否碰撞，是否有能力避开。可选择的避障方式如下：

避障模式1：调节臂型角ψ_i的值，通过臂杆自运动的方式避障；

避障模式2：调节等效臂杆ρ_i长度，通过关节节点沿脊线移动避障；

避障模式3：通过协同操作臂型角ψ_i和等效臂杆ρ_i回避障碍物；

避障模式4：协同临近组的避障参数(即ψ_i,ρ_i,ψ_i+1,ρ_i+1)回避障碍物；即同时调节相临近的机械臂的参数实现避障；

避障模式5：根据需求确定能够实现避障的基本单元；即通过调节臂型角、等效臂杆长度等多个避障参数实现避障。

基于上述方法，本发明还提供一种超冗余柔性机器人的避障***，包括

建模单元，用于将空间中的障碍物分别用空间超二次曲面方程和空间几何模型进行建模；

区域划分单元，用于基于所建立的模型，以障碍物为中心，将障碍物周围的空间划分为安全区域、预警区域和危险区域，安全区域和预警区域之间的界限为由超二次函数表示的预警边界，预警区域和危险区域之间的界限为由几何函数表示的危险边界；

距离检测单元，用于实时检测超冗余柔性机器人与预警边界之间的伪距离；

避障需求检测单元，用于判断超冗余柔性机器人与预警边界之间的最小伪距离是否小于或等于零，若判断结果为是，判断超冗余柔性机器人进入预警区域；

避障单元，用于对超冗余柔性机器人进行避障处理；

避障结果判断单元，用于实时检测超冗余柔性机器人与危险边界之间的欧几里得距离；判断超冗余柔性机器人与危险边界之间的最小欧几里得距离是否大于零，若判断结果为是，判断超冗余柔性机器人避障成功。

其中，超冗余柔性机器人与危险边界之间的最小欧几里得距离为危险边界与超冗余柔性机器人的机械臂的公垂线的最小长度、或者为危险边界上的点到超冗余柔性机器人的机械臂的最小垂直距离、或者为超冗余柔性机器人的机械臂端点到危险边界的最小垂直距离。通过调整改进模式函数法的避障参数实现超冗余柔性机器人的避障处理。一种超冗余柔性机器人的避障***的具体工作过程描述可参照上述一种超冗余柔性机器人的避障方法的描述，在此不再赘述。

另外，本发明还提供一种计算机存储介质，其上存储有计算机程序，程序被处理器执行时实现以下步骤：

将空间中的障碍物分别用空间超二次曲面方程和空间几何模型进行建模；

基于所建立的模型，以障碍物为中心，将障碍物周围的空间划分为安全区域、预警区域和危险区域，安全区域和预警区域之间的界限为由超二次函数表示的预警边界，预警区域和危险区域之间的界限为由几何函数表示的危险边界；

实时检测超冗余柔性机器人与预警边界之间的伪距离；判断超冗余柔性机器人与预警边界之间的最小伪距离是否小于或等于零，若判断结果为是，判断超冗余柔性机器人进入预警区域；

对超冗余柔性机器人进行避障处理；

实时检测超冗余柔性机器人与危险边界之间的欧几里得距离；判断超冗余柔性机器人与危险边界之间的最小欧几里得距离是否大于零，若判断结果为是，判断超冗余柔性机器人避障成功。

计算机存储介质上存储的计算机程序的工作过程可参照上述一种超冗余柔性机器人的避障方法的具体描述，不再赘述。

本发明中，提供一种基于环境障碍物混合建模的超冗余柔性机器人避障轨迹规划的方法及***，针对超冗余柔性机器人工作环境中的典型障碍物建模及其回避问题，建立基于混合障碍物模型的建模及避障策略，采用改进模式函数法实现避障。基于混合障碍物建模方法，机器人在安全区域内以能量最优原则规划机械臂运动。当在安全区域时，可以通过超二次曲面方程获得的伪距离定性判断机械臂与障碍物之间的相对关系。当判断机器人进入预警区域后可以准确计算机械臂与危险边界之间的欧式距离及具体的最小距离点坐标。通过调整改进模式函数法的避障参数(臂型角参数以及等效臂杆长度等)可以实现超冗余柔性机器人在障碍物环境中的作业任务；同时，本发明的方法除了对超冗余柔性机器人适用外，也适用于其余多种超冗余构型。

以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims (5)

1.一种超冗余柔性机器人的避障方法，其特征在于，包括以下步骤：

将空间中的障碍物分别用空间超二次曲面方程和空间几何模型进行建模；

基于所建立的模型，以所述障碍物为中心，将所述障碍物周围的空间划分为安全区域、预警区域和危险区域，所述安全区域和预警区域之间的界限为由超二次函数表示的预警边界，所述预警区域和危险区域之间的界限为由几何函数表示的危险边界；

实时检测所述超冗余柔性机器人与所述预警边界之间的伪距离；

判断所述超冗余柔性机器人与所述预警边界之间的最小伪距离是否小于或等于零，若判断结果为是，判断所述超冗余柔性机器人到达预警区域；

通过调整改进模式函数法的避障参数实现所述超冗余柔性机器人的避障处理，其中所述避障参数包括臂型角参数和等效臂杆长度；

实时检测所述超冗余柔性机器人与所述危险边界之间的欧几里得距离；

判断所述超冗余柔性机器人与所述危险边界之间的最小欧几里得距离是否大于零，若判断结果为是，判断所述超冗余柔性机器人避障成功；

其中，所述超冗余柔性机器人与所述危险边界之间的最小欧几里得距离为所述危险边界与超冗余柔性机器人的机械臂的公垂线的最小长度、或者为所述危险边界上的点到超冗余柔性机器人的机械臂的最小垂直距离、或者为超冗余柔性机器人的机械臂端点到所述危险边界的最小垂直距离；

避障处理的方式如下：

避障模式1：调节臂型角ψ_i的值，通过臂杆自运动的方式避障；

避障模式2：调节等效臂杆ρ_i长度，通过关节节点沿脊线移动避障；

避障模式3：通过协同操作臂型角ψ_i和等效臂杆ρ_i回避障碍物；

避障模式4：协同临近组的避障参数,即ψ_i,ρ_i,ψ_i+1,ρ_i+1回避障碍物；即同时调节相临近的机械臂的参数实现避障；

避障模式5：根据需求确定能够实现避障的基本单元；即通过调节臂型角、等效臂杆长度实现避障；

其中，ψ_i+1和ψ_i+1为臂型角，ρ_i为所述超冗余柔性机器人两个相邻奇数编号万向节之间的距离，ρ_i+1为所述超冗余柔性机器人下一组两个相邻奇数编号万向节之间的距离。

2.根据权利要求1所述的超冗余柔性机器人的避障方法，其特征在于，

判断所述超冗余柔性机器人与所述危险边界之间的最小欧几里得距离是否大于零，若判断结果为否，判断所述超冗余柔性机器人避障失败，控制所述超冗余柔性机器人停止运动。

3.根据权利要求1至2任一项所述的超冗余柔性机器人的避障方法，其特征在于，判断所述超冗余柔性机器人与所述预警边界之间的伪距离是否大于零，若判断结果为是，判断所述超冗余柔性机器人位于所述安全区域，以能量最优原则规划所述超冗余柔性机器人的机械臂的运动。

4.一种超冗余柔性机器人的避障***，其特征在于，包括

建模单元，用于将空间中的障碍物分别用空间超二次曲面方程和空间几何模型进行建模；

区域划分单元，用于基于所建立的模型，以所述障碍物为中心，将所述障碍物周围的空间划分为安全区域、预警区域和危险区域，所述安全区域和预警区域之间的界限为由超二次函数表示的预警边界，所述预警区域和危险区域之间的界限为由几何函数表示的危险边界；

距离检测单元，用于实时检测所述超冗余柔性机器人与所述预警边界之间的伪距离；

避障需求检测单元，用于判断所述超冗余柔性机器人与所述预警边界之间的最小伪距离是否小于或等于零，若判断结果为是，判断所述超冗余柔性机器人到达预警区域；

避障单元，通过调整改进模式函数法的避障参数实现所述超冗余柔性机器人的避障处理，其中所述避障参数包括臂型角参数和等效臂杆长度；

避障结果判断单元，用于实时检测所述超冗余柔性机器人与所述危险边界之间的欧几里得距离；判断所述超冗余柔性机器人与所述危险边界之间的最小欧几里得距离是否大于零，若判断结果为是，判断所述超冗余柔性机器人避障成功；

其中，所述超冗余柔性机器人与所述危险边界之间的最小欧几里得距离为所述危险边界与超冗余柔性机器人的机械臂的公垂线的最小长度、或者为所述危险边界上的点到超冗余柔性机器人的机械臂的最小垂直距离、或者为超冗余柔性机器人的机械臂端点到所述危险边界的最小垂直距离；

避障处理的方式如下：

避障模式1：调节臂型角ψ_i的值，通过臂杆自运动的方式避障；

避障模式2：调节等效臂杆ρ_i长度，通过关节节点沿脊线移动避障；

避障模式3：通过协同操作臂型角ψ_i和等效臂杆ρ_i回避障碍物；

避障模式4：协同临近组的避障参数，即ψ_i,ρ_i,ψ_i+1,ρ_i+1回避障碍物；即同时调节相临近的机械臂的参数实现避障；

避障模式5：根据需求确定能够实现避障的基本单元；即通过调节臂型角、等效臂杆长度实现避障；

其中，ψ_i+1和ψ_i+1为臂型角，ρ_i为所述超冗余柔性机器人两个相邻奇数编号万向节之间的距离，ρ_i+1为所述超冗余柔性机器人下一组两个相邻奇数编号万向节之间的距离。

5.一种计算机存储介质，其特征在于，其上存储有计算机程序，所述程序被处理器执行时实现以下步骤：

将空间中的障碍物分别用空间超二次曲面方程和空间几何模型进行建模；

基于所建立的模型，以所述障碍物为中心，将所述障碍物周围的空间划分为安全区域、预警区域和危险区域，所述安全区域和预警区域之间的界限为由超二次函数表示的预警边界，所述预警区域和危险区域之间的界限为由几何函数表示的危险边界；

实时检测超冗余柔性机器人与所述预警边界之间的伪距离；

判断所述超冗余柔性机器人与所述预警边界之间的最小伪距离是否小于或等于零，若判断结果为是，判断所述超冗余柔性机器人到达预警区域；

通过调整改进模式函数法的避障参数实现所述超冗余柔性机器人的避障处理，其中所述避障参数包括臂型角参数和等效臂杆长度；

实时检测所述超冗余柔性机器人与所述危险边界之间的欧几里得距离；

判断所述超冗余柔性机器人与所述危险边界之间的最小欧几里得距离是否大于零，若判断结果为是，判断所述超冗余柔性机器人避障成功；

其中，所述超冗余柔性机器人与所述危险边界之间的最小欧几里得距离为所述危险边界与超冗余柔性机器人的机械臂的公垂线的最小长度、或者为所述危险边界上的点到超冗余柔性机器人的机械臂的最小垂直距离、或者为超冗余柔性机器人的机械臂端点到所述危险边界的最小垂直距离；

避障处理的方式如下：

避障模式1：调节臂型角ψ_i的值，通过臂杆自运动的方式避障；

避障模式2：调节等效臂杆ρ_i长度，通过关节节点沿脊线移动避障；

避障模式3：通过协同操作臂型角ψ_i和等效臂杆ρ_i回避障碍物；

避障模式4：协同临近组的避障参数，即ψ_i,ρ_i,ψ_i+1,ρ_i+1回避障碍物；即同时调节相临近的机械臂的参数实现避障；

避障模式5：根据需求确定能够实现避障的基本单元；即通过调节臂型角、等效臂杆长度实现避障；

其中，ψ_i+1和ψ_i+1为臂型角，ρ_i为所述超冗余柔性机器人两个相邻奇数编号万向节之间的距离，ρ_i+1为所述超冗余柔性机器人下一组两个相邻奇数编号万向节之间的距离。

Images