CN112809682A - 机械臂避障路径规划方法、系统及存储介质 - Google Patents

Abstract

本申请适用于机械臂避障路径规划技术领域，提供了一种机械臂避障路径规划方法、系统及存储介质，方法包括：获取机械臂的初始参数；计算出执行末端的第一运动方向；计算出执行末端的当前位置；确定机械臂的当前碰撞状态；确定当前位置所处的当前区域；其中，当前区域包括安全区域；若当前区域为安全区域且碰撞状态为未发生碰撞状态，则将预设步进长度与预设步进增益因子的乘积作为预设步进长度，将当前位置作为初始位置，并执行根据初始位置、预设步进长度和第一运动方向计算出执行末端的当前位置；确定当前位置为目标位置，将得到的执行末端的轨迹作为规划路径。使得机械臂的执行末端能够快速且有效地到达目标点。

Description

机械臂避障路径规划方法、系统及存储介质

技术领域

本申请涉及机械臂避障路径规划技术领域，特别涉及一种机械臂避障路径规划方法、系统及存储介质。

背景技术

工业机械臂的避障路径规划是指在给定障碍物和目标点的前提下，规划处由起始点到目标点的最优无碰撞路径。最优是指时间最短、轨迹最稳定光滑、路径最短、结果最精确以及安全系数最高，这样才能使机械臂平稳地工作，降低关节间的磨损。传统的路径规划方法包括人工势场法和快速搜索随机树法。

人工势场法是指在空间内构造虚拟力的路径规划方法，把机械臂的运动视为一种合力驱动下的运动，目标点对其产生吸引力，力的大小随着距离的减小而减小，障碍物对其产生斥力，斥力的大小与距离成反比。该方法缺乏全局信息，易陷入局部最小值。例如，目标点的附近存在障碍物，存在障碍物对机械臂的斥力与目标点对机械臂的引力相同的情况，但是该点并未到达目标点，而此时机械臂停止运动，则会导致机械臂避障路径规划目标物体的失败。

快速搜索随机树是一种树形数据存储结构和算法，通过递增的方法建立，并快速减小随机选择点同树的距离，可以有效的搜索空间的目标。然而，这种方法在规划机械臂的路径时，由于其算法随机性较强、没有目的性的原因，使得在规划避障路径时效率较低。

发明内容

本申请旨在至少解决现有技术中存在的技术问题之一，为此，本申请提出一种机械臂避障路径规划方法、系统及存储介质，使得机械臂的执行末端能够快速且有效地到达目标点。

本申请的第一方面，提供了一种机械臂避障路径规划方法，包括：获取机械臂的初始参数；其中，初始参数包括机械臂的执行末端的初始位置；根据初始参数和目标位置计算出执行末端的第一运动方向；根据初始位置、预设步进长度和第一运动方向计算出执行末端的当前位置；根据预设的障碍物模型、机械臂模型和当前位置确定机械臂的当前碰撞状态；其中，当前碰撞状态包括发生碰撞状态和未发生碰撞状态；确定当前位置所处的当前区域；其中，当前区域包括安全区域；若当前区域为安全区域且碰撞状态为未发生碰撞状态，则将预设步进长度与预设步进增益因子的乘积作为预设步进长度，将当前位置作为初始位置，并执行根据初始位置、预设步进长度和第一运动方向计算出执行末端的当前位置；确定当前位置为目标位置，将得到的执行末端的轨迹作为规划路径。

根据本申请第一方面实施例的机械臂避障路径规划方法，至少具有如下有益效果：根据机械臂执行末端的初始位置和目标位置，可以得到执行末端的第一运动方向；而根据预设步进长度、执行末端的初始位置和第一运动方向，可以得到执行末端移动预设步进长度后的当前位置；而根据障碍物模型、机械臂模型和当前位置，可以确定机械臂在当前位置的碰撞状态；根据障碍物模型和执行末端的当前位置，可以确定执行末端当前位置所处的当前区域，在执行末端的当前位置所处的当前区域为安全区域且机械臂在当前位置的碰撞状态为未发生碰撞状态时，通过在安全区域设置预设步进增益因子，在安全区域，可以得到变化的步进长度，从而目的性更强，执行末端更加快速的到达目标位置，使得规划路径的效率更高。

根据本申请的一些实施例，当前区域还包括威胁区域，还包括以下步骤：若当前区域为威胁区域且碰撞状态为未发生碰撞状态，则建立机械臂的执行末端在威胁区域的惩罚势函数和第二引力势函数，惩罚势函数与第二引力势函数之和为机械臂的执行末端在威胁区域的第一总势场强度；采用模拟退火法搜索势场强度小于第一总势场强度的第一位置；若第一位置处的惩罚势函数对应的值为0，则第一位置位于安全区域。在当前区域为威胁区域，则首先从威胁区域逃离，进入安全区域后再继续按变化的步进长度向目标位置前进，直到最终到达目标位置，完成路径规划。在威胁区域时，引入惩罚势函数取代人工势场法的斥力势函数，通过计算第一总势场强度代替虚拟力，使得搜索朝着第一总势场强度减小的方向进行，即威胁区内的路径的搜索不再受引力和斥力的控制，而是第一总势场强度的控制，采用模拟退火法搜索威胁区域内势场强度最小的位置，找到全局最小势场强度，以避免局部极小点的出现；在第一位置处的惩罚势函数对应的值为0时，执行末端成功从威胁区域内逃离。

根据本申请的一些实施例，若第一位置处的惩罚势函数对应的值大于0，则将所述第一位置处的势场强度作为第一总势场强度，并将第一位置作为当前位置，执行采用模拟退火法搜索势场强度小于第一总势场强度的第一位置。在第一位置处的惩罚势函数对应的值大于0时，则继续在威胁区域寻找势场强度比第一总势场强度小的第一位置，并把第一位置处的势场强度重新作为第一总势场强度，将第一位置作为当前位置，再执行采用模拟退火法搜索势场强度小于第一总势场强度的第一位置，直到在威胁区域找到最小势场强度的位置，即惩罚势函数为0的位置，执行末端则成功逃离威胁区域。

根据本申请的一些实施例，还包括构建障碍物模型，具体包括：在三维空间采用球体包络法对障碍物进行建模，并向外径向扩展第一距离，形成一个球体的范围，得到障碍物模型；将三维空间区域分为安全区域和威胁区域，威胁区域为球体包围的区域，安全区域为球体外部的区域。采用球体包络法建立障碍物模型，再径向向外扩展第一距离的余量，并将向外扩展第一距离的余量作为威胁区域，可以增加执行末端的容错率。采用变化的步进长度可以使执行末端在安全区域快速到达目标位置，而在威胁区域采用惩罚函数替代人工势场法的斥力势函数，可以找到全局最小势场强度的位置，避免采用人工势场法找到的是局部最小势场强度的位置。

根据本申请的一些实施例，还包括构建机械臂模型，机械臂模型包括连杆模型和关节模型，具体包括：在三维空间将机械臂的连杆采用圆柱体包络法进行建模，形成圆柱体区域，得到连杆模型；在空间将机械臂的关节采用球体包络法进行建模，形成球体区域，得到关节模型。将机械臂的连杆建立圆柱体模型，将关节建立球体模型，可以通过确定关节的球体模型的球心位置、半径大小以、障碍物的位置和球心与障碍物之间的距离，确定机械臂的关节是否与障碍物发送碰撞；通过确定机械臂连杆的中心线的位置、圆柱体的半径、障碍物的位置以及连杆与障碍物之间的距离确定连杆与障碍物之间是否发生碰撞。

根据本申请的一些实施例，根据预设的障碍物模型、机械臂模型和当前位置确定机械臂的当前碰撞状态，包括：根据预设的机器人模型获取机械臂的多个连杆之间的第一关系；建立机械臂的运动学方程，并根据当前位置分别得到运动学方程的多个逆解，逆解为机械臂关节的角度；根据当前位置、第一关系和多个机械臂关节的角度，得到连杆位置和关节位置；根据连杆位置、连杆模型、关节位置、关节模型和障碍物模型，确定机械臂与障碍物的当前碰撞状态。通过建立机械臂的运动学方程，在运动学方程中，由机械臂的关节变量，可以得到执行末端的位置，而对运动学方程求得逆解，可以由执行末端的位置反推出关节变量，其中变量可以包括各关节的位置以及关节角度。

根据本申请的一些实施例，还包括：若碰撞状态为发生碰撞状态，则将当前位置的预设区域内的任一位置作为目标位置，并执行根据初始参数和目标位置计算出执行末端的第一运动方向。在碰撞状态为发生碰撞状态时，则将当前位置的预设区域内的任一位置为目标位置，并根据当前位置和目标位置，重新确定运动方向，并把该方向作为第一方向，使执行末端在当前位置沿第一方向运动，直到执行末端运动到机械臂与障碍物不再处于发生碰撞状态的位置。

根据本申请的一些实施例，还包括：采用最小二乘法对规划路径进行拟合，用于得到平滑的规划路径。采用最小二乘法对规划路径进行拟合后得到更平滑的规划路径，便于机械臂的运动更加流畅。

本申请的第二方面，提供了一种机械臂避障路径规划系统，包括：至少一个存储器、至少一个处理器及至少一个程序指令，程序指令存储在存储器上并可在处理器上运行，处理器用于本申请第一方面的机械臂避障路径规划方法。

本申请的第三方面提供了一种存储介质，存储介质上存储有程序指令，程序指令用于执行本申请第一方面的机械臂避障路径规划方法。

本申请的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本申请的实践了解到。

附图说明

本申请的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为本申请实施例的机械臂避障路径规划方法的流程图；

图2为本申请实施例的机械臂避障路径规划方法的逻辑示意图。

具体实施方式

下面详细描述本申请的实施例，实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本申请，而不能理解为对本申请的限制。

在本申请的描述中，需要理解的是，涉及到方位描述，例如上、下、前、后、左、右等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。

在本申请的描述中，若干的含义是一个或者多个，多个的含义是两个以上，大于、小于、超过等理解为不包括本数，以上、以下、以内等理解为包括本数。如果有描述到第一、第二只是用于区分技术特征为目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量或者隐含指明所指示的技术特征的先后关系。

本申请的描述中，除非另有明确的限定，设置、安装、连接等词语应做广义理解，所属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本申请中的具体含义。上面结合附图对本申请实施例作了详细说明，但是本申请不限于上述实施例，在技术领域普通技术人员所具备的知识范围内，还可以在不脱离本申请宗旨的前提下作出各种变化。

参照图1，本申请的第一方面，提供了一种机械臂避障路径规划方法，包括：

S100、获取机械臂的初始参数；其中，初始参数包括机械臂的执行末端的初始位置；

S200、根据初始参数和目标位置计算出执行末端的第一运动方向；

S300、根据初始位置、预设步进长度和第一运动方向计算出执行末端的当前位置；

S400、根据预设的障碍物模型、机械臂模型和当前位置确定机械臂的当前碰撞状态；其中，当前碰撞状态包括发生碰撞状态和未发生碰撞状态；

S500、确定当前位置所处的当前区域；其中，当前区域包括安全区域；

S600、若当前区域为安全区域且碰撞状态为未发生碰撞状态，则将预设步进长度与预设步进增益因子的乘积作为预设步进长度，将当前位置作为初始位置，并执行根据初始位置、预设步进长度和第一运动方向计算出执行末端的当前位置；

S700、确定当前位置为目标位置，将得到的执行末端的轨迹作为规划路径。

具体的，在安全区域设置预设步进增益因子β，例如，预设步进长度为λ₀，则执行末端在初始位置x₀处沿第一运动方向，以λ₀为步进长度运动，运动后到达当前位置，则当前位置的坐标为x₀+λ₀，执行末端的步进长度则变成βλ₀，在当前位置以第一方向向下一位置运动，下一位置为x₀+λ₀+βλ₀，以此类推，执行末端可以以目标位置为目标，迅速到达目标位置，有效提升路径规划的效率。

根据机械臂执行末端的初始位置和目标位置，可以得到执行末端的第一运动方向；而根据预设步进长度、执行末端的初始位置和第一运动方向，可以得到执行末端移动预设步进长度后的当前位置；而根据障碍物模型、机械臂模型和当前位置，可以确定机械臂在当前位置的碰撞状态；根据障碍物模型和执行末端的当前位置，可以确定执行末端当前位置所处的当前区域，在执行末端的当前位置所处的当前区域为安全区域且机械臂在当前位置的碰撞状态为未发生碰撞状态时，通过在安全区域设置预设步进增益因子，在安全区域，可以得到变化的步进长度，从而目的性更强，执行末端更加快速的到达目标位置，使得规划路径的效率更高。

在本申请的一些实施例中，当前区域还包括威胁区域，还包括以下步骤：若当前区域为威胁区域且碰撞状态为未发生碰撞状态，则建立机械臂的执行末端在威胁区域的惩罚势函数和第二引力势函数，惩罚势函数与第二引力势函数之和为机械臂的执行末端在威胁区域的第一总势场强度；采用模拟退火法搜索势场强度小于第一总势场强度的第一位置；若第一位置处的惩罚势函数对应的值为0，则第一位置位于安全区域。在当前区域为威胁区域，则首先从威胁区域逃离，进入安全区域后再继续按变化的步进长度向目标位置前进，直到最终到达目标位置，完成路径规划。在威胁区域时，引入惩罚势函数取代人工势场法的斥力势函数，通过计算第一总势场强度代替虚拟力，使得搜索朝着第一总势场强度减小的方向进行，即威胁区内的路径的搜索不再受引力和斥力的控制，而是第一总势场强度的控制，采用模拟退火法搜索威胁区域内势场强度最小的位置，找到全局最小势场强度，以避免局部极小点的出现；在第一位置处的惩罚势函数对应的值为0时，执行末端成功从威胁区域内逃离。

具体的，进入威胁区域时，第二引力势函数为：

惩罚势函数为：

其中，k_att和k_p为增益系数，ρ_b为当前点到威胁区中心，也就是障碍物中心的距离，这样规划可以使得第二引力势函数与惩罚势函数具有相近的数量级，避免引力过大或斥力过大引起规划点不行。当路径点搜索进入威胁区域时，总势场强度为：U'(x)＝U_att*(x)+U_P(x)。很显然，总势场强度只有一个全局最小点，即目标点，从而避免找到的点为局部最小。而势函数U_p(x)＝0时，也是总势场强度的最小点，该点所在的位置亦从威胁区域逃离，进入安全区域。

在本申请的一些实施例中，若第一位置处的惩罚势函数对应的值大于0，则将所述第一位置处的势场强度作为第一总势场强度，并将第一位置作为当前位置，执行采用模拟退火法搜索势场强度小于第一总势场强度的第一位置。在第一位置处的惩罚势函数对应的值大于0时，则继续在威胁区域寻找势场强度比第一总势场强度小的第一位置，并把第一位置处的势场强度重新作为第一总势场强度，将第一位置作为当前位置，再执行采用模拟退火法搜索势场强度小于第一总势场强度的第一位置，直到在威胁区域找到最小势场强度的位置，即第一位置处的惩罚势函数对应的值为0的位置，执行末端则成功逃离威胁区域。

具体的，在未到达总势场强度的最小点的位置时，即惩罚势函数仍大于0时，执行末端仍处于威胁区域。在威胁区域内采用模拟退火算法，寻找比当前位置处的第一总势场强度的势场强度小的位置，若找到比第一总势场强度的势场强度小的点，则将该点的总势场强度作为第一势场强度，并将该点所在位置作为第一位置，重新搜索总势场强度更小的点，直到找到最小势场强度的点，即第一位置处的惩罚势函数对应的值为0的位置，执行末端将成功逃离威胁区域，进入安全区域。此时再按安全区域的步进方法继续搜索执行末端的运动轨迹。

在本申请的一些实施例中，还包括构建障碍物模型，具体包括：在三维空间采用球体包络法对障碍物进行建模，并向外径向扩展第一距离，形成一个球体的范围，得到障碍物模型；将三维空间区域分为安全区域和威胁区域，威胁区域为球体包围的区域，安全区域为球体外部的区域。采用球体包络法建立障碍物模型，再径向向外扩展第一距离的余量，并将向外扩展第一距离的余量作为威胁区域，可以增加执行末端的容错率。采用变化的步进长度可以使执行末端在安全区域快速到达目标位置，而在威胁区域采用惩罚函数替代人工势场法的斥力势函数，可以找到全局最小势场强度的位置，避免采用人工势场法找到的是局部最小势场强度的位置。

在本申请的一些实施例中，还包括构建机械臂模型，机械臂模型包括连杆模型和关节模型，具体包括：在三维空间将机械臂的连杆采用圆柱体包络法进行建模，形成圆柱体区域，得到连杆模型；在空间将机械臂的关节采用球体包络法进行建模，形成球体区域，得到关节模型。将机械臂的连杆建立圆柱体模型，将关节建立球体模型，可以通过确定关节的球体模型的球心位置、半径大小以、障碍物的位置和球心与障碍物之间的距离，确定机械臂的关节是否与障碍物发送碰撞；通过确定机械臂连杆的中心线的位置、圆柱体的半径、障碍物的位置以及连杆与障碍物之间的距离确定连杆与障碍物之间是否发生碰撞。

具体的，根据D-H(Denavit-Hartenberg)法确定连杆i和连杆i+1之间的变换矩阵：

i表示连杆号，θ表示关节角度，ɑ表示扭角，a表示连杆长度，d表示连杆偏距。

建立机械臂运动学方程：

将已知的关节变量θ、ɑ、a、d带入，可以得到机械臂末端执行器的位姿，定义为基坐标。然后计算相对于基座标的雅可比矩阵J的伪逆解J+

J⁺＝J^T(JJ^T)^-1

再利用修正施密特QR分解方法计算(JJ^T)^-1

通过QR分解将JJT分解成一个正交矩阵和一个上三角矩阵，正交矩阵通过转置得到逆解，上三角矩阵通过初等变换得到逆解：

JJ^T＝QR(JJ^T)^-1＝R^-1Q^-1＝R^-1Q^T

最后通过矩阵相乘从而求得J+再乘上末端速度得到关节角度，完成机械臂的逆解。

执行末端的当前位置处于安全区域时，只受引力作用：

第一引力势函数：

式中k_a为引力增益系数，x_goal为目标点的位置

斥力势函数：

k_r为斥力常数，d_i为机械臂各关节与障碍物的距离，d₀为斥力场半径与障碍物包络后加上裕度的半径相同。相应的吸引力与斥力是各自势函数的负梯度：

其中第一引力势函数的负梯度为矢量函数，可以根据该矢量函数确定机械臂执行末端的运动方向。

在本申请的一些实施例中，根据预设的障碍物模型、机械臂模型和当前位置确定机械臂的当前碰撞状态，包括：根据预设的机器人模型获取机械臂的多个连杆之间的第一关系；建立机械臂的运动学方程，并根据当前位置分别得到运动学方程的多个逆解，逆解为机械臂关节的角度；根据当前位置、第一关系和多个机械臂关节的角度，得到连杆位置和关节位置；根据连杆位置、连杆模型、关节位置、关节模型和障碍物模型，确定机械臂与障碍物的当前碰撞状态。通过建立机械臂的运动学方程，在运动学方程中，由机械臂的关节变量，可以得到执行末端的位置，而对运动学方程求得逆解，可以由执行末端的位置反推出关节变量，其中变量可以包括各关节的位置以及关节角度。

在本申请的一些实施例中，还包括：若碰撞状态为发生碰撞状态，则将当前位置的预设区域内的任一位置作为目标位置，并执行根据初始参数和目标位置计算出执行末端的第一运动方向。在碰撞状态为发生碰撞状态时，则将当前位置的预设区域内的任一位置为目标位置，并根据当前位置和目标位置，重新确定运动方向，并把该方向作为第一方向，使执行末端在当前位置沿第一方向运动，直到执行末端运动到机械臂与障碍物不再处于发生碰撞状态的位置。

具体的，将机械臂连杆用圆柱体包络法进行建模中心线段为a半径为R1，关节用球体包络法进行建模圆心为O1半径为R2，前面已经将障碍物用球体建模好了圆心为O2半径为R3，于是关节与障碍物之间的距离可以用两圆心之间的距离来表示于是：

如果L(O₁,O₂)≤R₃+R₂那么关节与障碍物就发生了碰撞。

如果L(a,O₂)≤(R₁+R₃)那么连杆与障碍物就发生了碰撞。

在本申请的一些实施例中，还包括：采用最小二乘法对规划路径进行拟合，用于得到平滑的规划路径。采用最小二乘法对规划路径进行拟合后得到更平滑的规划路径，便于机械臂的运动更加流畅。

本申请的第二方面，提供了一种机械臂避障路径规划系统，包括：至少一个存储器、至少一个处理器及至少一个程序指令，程序指令存储在存储器上并可在处理器上运行，处理器用于本申请第一方面的机械臂避障路径规划方法。

本申请的第三方面，提供了一种存储介质，存储介质上存储有程序指令，程序指令用于执行本申请第一方面的机械臂避障路径规划方法。

上面结合附图对本申请实施例作了详细说明，但是本申请不限于上述实施例，在技术领域普通技术人员所具备的知识范围内，还可以在不脱离本申请宗旨的前提下作出各种变化。

下面参考图1和图2，以一个具体的实施例详细描述根据本申请实施例的机械臂避障路径规划方法。值得理解的是，下述描述仅是示例性说明，而不是对本申请的具体限制。

参照图1至图2，本申请实施例提供了一种机械臂避障路径规划方法，包括：

在三维空间通过球体包络法对障碍物进行建模，并向外径向扩展第一距离，形成一个球体的范围，得到障碍物模型；将障碍物模型形成的球体范围作为威胁区域，将球体外部的区域称为安全区域；

在三维空间将机械臂的连杆采用圆柱体包络法进行建模，形成圆柱体区域，得到连杆模型；在空间将机械臂的关节采用球体包络法进行建模，形成球体区域，得到关节模型；根据D-H(Denavit-Hartenberg)法确定连杆i和连杆i+1之间的关系；

获取机械臂的初始参数；其中，初始参数包括机械臂的执行末端的初始位置；

在安全区域，执行末端只受目标位置处的引力作用：

通过第一引力势函数可以得到第一引力势函数的负梯度，而第一引力势函数的负梯度为矢量函数，可以作为执行末端的第一运动方向，即根据执行末端的初始位置和目标位置，计算出执行末端的第一运动方向；设置预设步进增益因子β，例如，预设步进长度为λ₀，则执行末端在初始位置x₀处沿第一运动方向，以λ₀为步进长度运动，运动后到达当前位置，则当前位置的坐标为x₀+λ₀，执行末端的步进长度则变成βλ₀，在当前位置以第一方向向下一位置运动，下一位置为x₀+λ₀+βλ₀，以此类推，直到执行末端运动到目标位置点。

在威胁区域，引入惩罚势函数取代人工势场法的斥力势函数：

惩罚势函数与第二引力势函数之和为机械臂的执行末端在威胁区域的第一总势场强度；采用模拟退火法搜索势场强度小于第一总势场强度的第一位置；若第一位置处的惩罚势函数对应的值为0，则第一位置位于安全区域；若第一位置处的惩罚势函数对应的值大于0，则将所述第一位置处的势场强度作为第一总势场强度，并将第一位置作为当前位置，返回并执行采用模拟退火法搜索势场强度小于第一总势场强度的第一位置。

规划执行末端在安全区域和威胁区域的运动轨迹时，同时需要确定机械臂的当前碰撞状态，其中，当前碰撞状态包括发生碰撞状态和未发生碰撞状态：

将机械臂连杆用圆柱体包络法进行建模中心线段为a半径为R1，关节用球体包络法进行建模圆心为O1半径为R2，前面已经将障碍物用球体建模好了圆心为O2半径为R3，于是关节与障碍物之间的距离可以用两圆心之间的距离来表示于是：

如果L(O₁,O₂)≤R₃+R₂那么关节与障碍物就发生了碰撞。

如果L(a,O₂)≤(R₁+R₃)那么连杆与障碍物就发生了碰撞。

以上，将执行末端到达目标位置的轨迹通过最小二乘法进行拟合，得到较平滑的轨迹，即为机械臂的规划路径。

初温T0:根据需要设定的值(充分大)，如1000、10000、100000。

降温：T＝T0*γ，γ为0到1之间的常数为降温速度参数。

Metropolis准则：以一定的概率接受较差的解，这个概率为

从而使算法具有逃脱局部极值和避免过早收敛的全局优化能力，k为波尔兹曼常数，exp表示自然指数，rand(0，1)指的是0到1之间的随机数。

上述对本说明书特定实施例进行了描述。其他实施例在所附权利要求的范围内。在一些情况下，在权利要求书中记载的动作或步骤可以按照不同于实施例中的顺序来执行并且仍然可以实现期望的结果。另外，在附图中描绘的过程不一定要求示出的特定顺序或者连续顺序才能实现期望的结果。在某些实施方式中，多任务处理和并行处理也是可以的或者可能是有利的。

程序指令包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或者某些中间形式等。存储介质包括：能够携带计算机程序代码的任何实体或装置、记录介质、计算机存储器、只读存储器(ROM)、随机存取存储器(RAM)、电载波信号、电信信号以及软件分发介质等。需要说明的是，存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，存储介质不包括电载波信号和电信信号。

需要说明的是，对于前述的各方法实施例，为了简便描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本申请并不受所描述的动作顺序的限制，因为依据本申请，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定是本申请所必须的。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

上面结合附图对本申请实施例作了详细说明，但是本申请不限于上述实施例，在技术领域普通技术人员所具备的知识范围内，还可以在不脱离本申请宗旨的前提下作出各种变化。

Claims (10)

1.一种机械臂避障路径规划方法，其特征在于，包括以下步骤：

获取机械臂的初始参数；其中，所述初始参数包括所述机械臂的执行末端的初始位置；

根据所述初始参数和目标位置计算出所述执行末端的第一运动方向；

根据所述初始位置、预设步进长度和所述第一运动方向计算出所述执行末端的当前位置；

根据预设的障碍物模型、机械臂模型和所述当前位置确定所述机械臂的当前碰撞状态；其中，所述当前碰撞状态包括发生碰撞状态和未发生碰撞状态；

确定所述当前位置所处的当前区域；其中，所述当前区域包括安全区域；

若所述当前区域为所述安全区域且所述碰撞状态为未发生碰撞状态，则将预设步进长度与预设步进增益因子的乘积作为预设步进长度，将所述当前位置作为所述初始位置，并执行所述根据所述初始位置、预设步进长度和所述第一运动方向计算出所述执行末端的当前位置；

确定所述当前位置为目标位置，将得到的所述执行末端的轨迹作为规划路径。

2.根据权利要求1所述的机械臂避障路径规划方法，其特征在于，所述当前区域还包括威胁区域，还包括以下步骤：

若所述当前区域为所述威胁区域且所述碰撞状态为未发生碰撞状态，则建立所述机械臂的执行末端在所述威胁区域的惩罚势函数和第二引力势函数，所述惩罚势函数与所述第二引力势函数之和为所述机械臂的执行末端在所述威胁区域的第一总势场强度；

采用模拟退火法搜索势场强度小于所述第一总势场强度的第一位置；

若所述第一位置处的惩罚势函数对应的值0，则所述第一位置位于所述安全区域。

3.根据权利要求2所述的机械臂避障路径规划方法，其特征在于，还包括：

若所述第一位置处的惩罚势函数对应的值大于0，则将所述第一位置处的势场强度作为第一总势场强度，并将所述第一位置作为当前位置，执行所述采用模拟退火法搜索势场强度小于所述第一总势场强度的第一位置。

4.根据权利要求1所述的机械臂避障路径规划方法，其特征在于，还包括构建所述障碍物模型，具体包括：

在三维空间采用球体包络法对所述障碍物进行建模，并向外径向扩展第一距离，形成一个球体的范围，得到障碍物模型；

将三维空间区域划分为安全区域和威胁区域，所述威胁区域为所述球体包围的区域，所述安全区域为所述球体外部的区域。

5.根据权利要求1所述的机械臂避障路径规划方法，其特征在于，还包括构建所述机械臂模型，所述机械臂模型包括连杆模型和关节模型，具体包括：

在三维空间将所述机械臂的连杆采用圆柱体包络法进行建模，形成圆柱体区域，得到连杆模型；

在空间将所述机械臂的关节采用球体包络法进行建模，形成球体区域，得到关节模型。

6.根据权利要求5所述的机械臂避障路径规划方法，其特征在于，所述根据预设的障碍物模型、机械臂模型和所述当前位置确定所述机械臂的当前碰撞状态，包括：

根据预设的机器人模型获取所述机械臂的多个连杆之间的第一关系；

建立所述机械臂的运动学方程，并根据所述当前位置分别得到所述运动学方程的多个逆解，所述逆解为所述关节的角度；

根据所述当前位置、所述第一关系和多个所述关节的角度，得到所述连杆的位置和所述关节的位置；

根据所述连杆的位置、所述连杆模型、所述关节的位置、所述关节模型和所述障碍物模型，确定所述机械臂与所述障碍物的当前碰撞状态。

7.根据权利要求1所述的机械臂避障路径规划方法，其特征在于，还包括：若所述碰撞状态为发生碰撞状态，则将所述当前位置的预设区域内的任意一位置作为目标位置，并执行所述根据所述初始参数和目标位置计算出所述执行末端的第一运动方向。

8.根据权利要求1至7任一项所述的机械臂避障路径规划方法，其特征在于，还包括：

采用最小二乘法对所述规划路径进行拟合，用于得到平滑的规划路径。

9.一种机械臂避障路径规划系统，其特征在于，包括：至少一个存储器、至少一个处理器及至少一个程序指令，所述程序指令存储在所述存储器上并可在所述处理器上运行，所述处理器用于执行权利要求1至8中任一项所述的机械臂避障路径规划方法。

10.一种存储介质，其特征在于：所述存储介质上存储有程序指令，所述程序指令用于执行权利要求1至8中任一项所述的机械臂避障路径规划方法。

Images