CN111421540A - 一种机械臂运动控制方法 - Google Patents

Abstract

本发明涉及机器人技术领域，具体公开了一种机械臂运动控制方法，包括以下步骤：针对六自由度机械臂建立机械臂运动D‑H模型；对六自由度机械臂建立机械臂四连杆简化模型；利用改进人工势场法对机械臂运动进行避障控制。本发明通过采用改进人工势场法在关节空间内、以机械臂所受的引力势能与斥力势能的合势能最速下降为标准，控制机械臂运动，能快速解决简单障碍环境下机械臂运动避障的问题，使得机械臂运动灵活精确，避障能力强，实时性佳，准确度高；通过以关节空间作为搜索空间使得到的机械臂运动路径是一定可达的，不会出现奇异点，从关节坐标系转换到世界坐标系的计算难度远远低于由世界坐标系转换到关节坐标系的难度。

Description

一种机械臂运动控制方法

技术领域

本发明涉及机器人技术领域，具体是一种机械臂运动控制方法。

背景技术

机械臂是模仿人的手臂而设计的自动控制装置，能够代替人类完成复杂、精细、危险的工作，机械臂被越来越多地应用在喷涂、包装、焊接、装配等生产环节，以便代替人工在恶劣环境下完成高强度的重复劳动，机械臂控制技术的进步对提高劳动生产率，实现工业生产的机械化和自动化具有重要意义。

现有技术中机械臂运动采用传统人工势场法进行避障：

将机械臂当前位置记为X＝[x，y]^Τ，目标点位置记为X_goal＝[x_goal，y_goal]^Τ，距离机械臂最近的障碍物位置记为X_obs＝[x_obs，y_obs]^Τ，则机械臂所受的引力场为：

式中ξ为正比例系数；ρ(X，X_goal)＝||X-X_goal||为机械臂从当前位置X到目标X_goal的距离。

机械臂所受的斥力场定义为：

式中η为正比例系数；ρ(X，X_obs)为机械臂到障碍物的距离；ρ₀代表不影响机械臂安全运动的最小安全距离。

然而，现有技术的机械臂各关节角度之间存在着复杂的耦合关系，使得解耦过程过于复杂，并且求解过程复杂，此时需要根据机械臂的结构特点确定位姿，不满足实时定位的要求；另一方面，机械臂各关节角度与机械臂末端的空间位置不是一对一的关系，而是多对一的关系，传统的解析法很难得到单一的优化解。这些缺陷导致机械臂运动不够灵活精确，避障能力差，实时性差，准确度不能满足实际应用的需要，同时，传统人工势场法导致机械臂容易陷入局部极小值点无法逃离。

发明内容

本发明的目的在于提供一种机械臂运动控制方法，以解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：

一种机械臂运动控制方法，包括以下步骤：

步骤一：针对六自由度机械臂建立机械臂运动D-H模型；

步骤二：对六自由度机械臂建立机械臂四连杆简化模型；

步骤三：利用改进人工势场法对机械臂运动进行避障控制。

作为本发明进一步的方案：步骤一中，所述机械臂运动D-H模型通过运动学正解的方法得到在当前关节角下所对应的末端位置，只考虑机械臂的末端位置，以机械臂关节1～关节5的关节角度作为控制目标，不考虑关节6角度的影响。

作为本发明进一步的方案：步骤二中，所述机械臂四连杆简化模型将机械臂的末端定义为关节7；

将基座、关节1和关节2构成的一条线段定义为直线一；

将关节2、关节3和关节4构成的一条线段定义为直线二；

将关节4和关节5构成的一条线段定义为直线三；

将关节5、关节6和关节7构成的一条线段定义为直线四。

作为本发明进一步的方案：根据所述机械臂四连杆简化模型，可由机械臂当前关节角组合S＝(θ_i1，θ_i2，…，θ_i6)求解出基座、关节2、关节4、关节5和关节7的空间位置，计算方法如下：

基座：

关节2：

关节4：

关节5：

关节7：

上式中，p_ix，p_iy，p_iz分别表示关节i在世界坐标系下x，y，z三个轴向上的分量，通过立体几何方法，由基座、关节2、关节4、关节5和关节7的空间位置求得直线一到直线四的空间解析方程，进一步求解得到空间障碍到各个连杆的距离。

作为本发明进一步的方案：步骤三中，所述改进人工势场法以机械臂各关节角度作为控制和运算的对象。

作为本发明进一步的方案：所述改进人工势场法采用机械臂关节空间作为搜索空间，以当前关节状态S＝(θ_i1，θ_i2，…，θ_i5)作为初始状态，遍历当前状态的相邻关节值，求取出各种关节组合所对应的合势能，选择合适的关节组合作为机械臂下一步的运动目标，设置搜索步长λ为0.5；

以机械臂六个关节角度最终位置为目标，求解得到机械臂所受的引力势能和斥力势能：

其中，所述引力势能：

所述斥力势能：

其中E_att、E_rep和E_i分别表示机械臂所受引力势能、斥力势能和各关节连杆所受的斥力势能，k_a和k_r分别表示所受引力和斥力的比例系数，d_i和d₀分别表示各关节到障碍物的距离和障碍物的斥力影响范围。

作为本发明进一步的方案：所述改进人工势场法在局部极小点附近添加空间虚拟障碍点以避免机械臂运动过程中易于陷入局部极小值点。

作为本发明进一步的方案：所述空间虚拟障碍点的设置满足以下条件：

所述空间虚拟障碍点与机械臂到实际障碍点最近位置点P的连线垂直于平面X；

平面X由当前角度下关节B、关节C所在位置点与目标角度下关节C所在的位置点确定；

关节B和关节C为从关节2、关节4、关节5和关节7中的其中两个关节组成的关节组合；

设定所述空间虚拟障碍点的斥力系数为实际障碍点的10～100倍、作用范围为实际障碍点的2～3倍；

设定所述空间虚拟障碍点到点P的距离为机械臂的安全距离d₀，设定斥力为实际障碍点的10倍，斥力范围为实际障碍点的2倍。

与现有技术相比，本发明的有益效果是：

1、该机械臂运动控制方法，采用改进人工势场法在关节空间内，以机械臂所受的引力势能与斥力势能的合势能最速下降为标准，控制机械臂运动，能快速解决简单障碍环境下机械臂运动避障的问题，使得机械臂运动灵活精确，避障能力强，实时性佳，准确度高；

2、通过以关节空间作为搜索空间使得到的机械臂运动路径是一定可达的，关节空间搜索是连续的，而且不会出现奇异点，从关节坐标系转换到世界坐标系的计算难度远远低于由世界坐标系转换到关节坐标系的难度。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例。

图1为机械臂运动控制方法中机械臂运动D-H模型。

图2为机械臂运动控制方法中机械臂运动D-H模型参数表。

图3为机械臂运动控制方法中机械臂四连杆简化模型。

具体实施方式

为了使本发明所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

实施例1

请参阅图1～3，本发明实施例中，一种机械臂运动控制方法，包括以下步骤：

步骤一：针对六自由度机械臂建立机械臂运动D-H模型；

步骤二：对六自由度机械臂建立机械臂四连杆简化模型；

步骤三：利用改进人工势场法对机械臂运动进行避障控制。

进一步的，所述机械臂运动D-H模型通过运动学正解的方法得到在当前关节角下所对应的末端位置，只考虑机械臂的末端位置，以机械臂关节1～关节5的关节角度作为控制目标，不考虑关节6角度的影响。

通过求解可知，机械臂末端位置是关节1～关节5的角度的函数，机械臂末端位置与关节6无关，关节6影响的只是机械臂末端的姿态。

如图3所示，本实施例中，所述机械臂四连杆简化模型将机械臂的末端定义为关节7；将基座、关节1和关节2构成的一条线段定义为直线一；将关节2、关节3和关节4构成的一条线段定义为直线二；将关节4和关节5构成的一条线段定义为直线三；将关节5、关节6和关节7构成的一条线段定义为直线四。

进一步的，根据所述机械臂四连杆简化模型，可由机械臂当前关节角组合S＝(θ_i1，θ_i2，…，θ_i6)求解出基座、关节2、关节4、关节5和关节7的空间位置，计算方法如下：

基座：

关节2：

关节4：

关节5：

关节7：

上式中，p_ix，p_iy，p_iz分别表示关节i在世界坐标系下x，y，z三个轴向上的分量，也就是世界坐标系下的三维直角坐标，通过立体几何方法，由基座、关节2、关节4、关节5和关节7的空间位置求得直线一到直线四的空间解析方程，进一步求解得到空间障碍到各个连杆的距离。

进一步的，所述改进人工势场法以机械臂各关节角度作为控制和运算的对象。

在机械臂由当前关节角度运动到目标关节角度的过程中，实现机械臂的避障路径规划，保证末端接近目标位置点时，机械臂避开障碍点。

进一步的，所述改进人工势场法采用机械臂关节空间作为搜索空间，以当前关节状态S＝(θ_i1，θ_i2，…，θ_i5)作为初始状态，遍历当前状态的相邻关节值，求取出各种关节组合所对应的合势能，选择合适的关节组合作为机械臂下一步的运动目标，设置搜索步长λ为0.5；

以机械臂六个关节角度最终位置为目标，求解得到机械臂所受的引力势能和斥力势能：

其中，所述引力势能：

所述斥力势能：

其中E_att、E_rep和E_i分别表示机械臂所受引力势能、斥力势能和各关节连杆所受的斥力势能，k_a和k_r分别表示所受引力和斥力的比例系数，d_i和d₀分别表示各关节到障碍物的距离和障碍物的斥力影响范围。

另外，由于人工势场法的缺陷在于容易陷入局部极小值点。在机械臂的使用过程中，以势能的变化为控制参考标准，机械臂运动到局部极小值点的表现为：

在机械臂进行关节空间搜索时，运动到的某个角度使得机械臂的引力势能与斥力势能合势能达到一个极小点，任何角度的变化都会使二者的和变大，这就导致机械臂不会再产生关节角度的变化，也就停止了运动。

若当前关节不是机械臂运动的目标角度值，则此时机械臂陷入了当前关节局部极小值点，无法逃离。

判断当前关节值与n次之前的角度值绝对值之差，若两者的差值在一个很小的范围内(该范围的阈值可以根据实际情况设定)，则表示机械臂在某个关节角度停止运动或进行震荡式运动，则认为机械臂陷入了局部极小值点无法逃离。

针对机械臂运动过程中易于陷入局部极小值点的问题，通过采用在局部极小点附近添加空间虚拟障碍点的方法，利用空间虚拟障碍点产生的斥力势能将机械臂推出局部极小值点。

进一步的，所述改进人工势场法在局部极小点附近添加空间虚拟障碍点以避免机械臂运动过程中易于陷入局部极小值点。

具体的，所述空间虚拟障碍点的设置满足以下条件：

所述空间虚拟障碍点与机械臂到实际障碍点最近位置点P的连线垂直于平面X；平面X由当前角度下关节B、关节C所在位置点与目标角度下关节C所在的位置点确定；关节B和关节C为从关节2、关节4、关节5和关节7中的其中两个关节组成的关节组合；

设定所述空间虚拟障碍点的斥力系数为实际障碍点的10～100倍、作用范围为实际障碍点的2～3倍，可以使得空间虚拟障碍点在局部极小值点附近成为主要作用场，保证机械臂逃离局部极小值点；

设定所述空间虚拟障碍点到点P的距离为机械臂的安全距离d₀，设定斥力为实际障碍点的10倍，斥力范围为实际障碍点的2倍。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种机械臂运动控制方法，其特征在于，包括以下步骤：

步骤一：针对六自由度机械臂建立机械臂运动D-H模型；

步骤二：对六自由度机械臂建立机械臂四连杆简化模型；

步骤三：利用改进人工势场法对机械臂运动进行避障控制。

2.根据权利要求1所述的机械臂运动控制方法，其特征在于，步骤一中，所述机械臂运动D-H模型通过运动学正解的方法得到在当前关节角下所对应的末端位置，以机械臂关节1～关节5的关节角度作为控制目标，不考虑关节6角度的影响。

3.根据权利要求1所述的机械臂运动控制方法，其特征在于，步骤二中，所述机械臂四连杆简化模型将机械臂的末端定义为关节7；

将基座、关节1和关节2构成的一条线段定义为直线一；

将关节2、关节3和关节4构成的一条线段定义为直线二；

将关节4和关节5构成的一条线段定义为直线三；

将关节5、关节6和关节7构成的一条线段定义为直线四。

4.根据权利要求3所述的机械臂运动控制方法，其特征在于，根据所述机械臂四连杆简化模型，可由机械臂当前关节角组合S＝(θ_i1，θ_i2，…，θ_i6)求解出基座、关节2、关节4、关节5和关节7的空间位置，计算方法如下：

基座：

关节2：

关节4：

关节5：

关节7：

上式中，p_ix，p_iy，p_iz分别表示关节i在世界坐标系下x，y，z三个轴向上的分量，通过立体几何方法，由基座、关节2、关节4、关节5和关节7的空间位置求得直线一到直线四的空间解析方程，进一步求解得到空间障碍到各个连杆的距离。

5.根据权利要求1所述的机械臂运动控制方法，其特征在于，步骤三中，所述改进人工势场法以机械臂各关节角度作为控制和运算的对象。

6.根据权利要求5所述的机械臂运动控制方法，其特征在于，所述改进人工势场法采用机械臂关节空间作为搜索空间，以当前关节状态S＝(θ_i1，θ_i2，…，θ_i5)作为初始状态，遍历当前状态的相邻关节值，求取出各种关节组合所对应的合势能，选择合适的关节组合作为机械臂下一步的运动目标，设置搜索步长λ为0.5；

以机械臂六个关节角度最终位置为目标，求解得到机械臂所受的引力势能和斥力势能：

其中，所述引力势能：

所述斥力势能：

其中E_att、E_rep和E_i分别表示机械臂所受引力势能、斥力势能和各关节连杆所受的斥力势能，k_a和k_r分别表示所受引力和斥力的比例系数，d_i和d₀分别表示各关节到障碍物的距离和障碍物的斥力影响范围。

7.根据权利要求6所述的机械臂运动控制方法，其特征在于，所述改进人工势场法在局部极小点附近添加空间虚拟障碍点以避免机械臂运动过程中易于陷入局部极小值点。

8.根据权利要求7所述的机械臂运动控制方法，其特征在于，所述空间虚拟障碍点的设置满足以下条件：

所述空间虚拟障碍点与机械臂到实际障碍点最近位置点P的连线垂直于平面X；

平面X由当前角度下关节B、关节C所在位置点与目标角度下关节C所在的位置点确定；

关节B和关节C为从关节2、关节4、关节5和关节7中的其中两个关节组成的关节组合；

设定所述空间虚拟障碍点的斥力系数为实际障碍点的10～100倍、作用范围为实际障碍点的2～3倍；

设定所述空间虚拟障碍点到点P的距离为机械臂的安全距离d₀，设定斥力为实际障碍点的10倍，斥力范围为实际障碍点的2倍。

Images