CN112276953A - 连续型超冗余机械臂的臂型协同规划方法、终端设备和存储介质 - Google Patents

Abstract

本发明提供一种连续型超冗余机械臂的臂型协同规划方法、终端设备和存储介质，方法包括：建立连续型超冗余机械臂运动学模型；构建虚拟引导管道；通过机器人逆运动学方程求解出虚拟引导管道的初始臂型；构建人工势场，人工势场包括虚拟引导管道产生的引导力势场；计算连续型超冗余机械臂的关键点在引导力势场中受到的力，根据关键点的受力情况按照单位规划周期内臂上关键点做功之和最大的策略来对连续型超冗余机械臂的运动进行规划，直到连续型超冗余机械臂抵达目标臂型。规划算法不需要依靠机械臂的逆运动学，所以其复杂度不会随着机械臂的自由度增加而显著增加。

Description

连续型超冗余机械臂的臂型协同规划方法、终端设备和存储 介质

技术领域

本发明涉及连续型超冗余机械臂的臂型技术领域，尤其涉及一种连续型超冗余机械臂的臂型协同规划方法、终端设备和存储介质。

背景技术

在科技的高速发展中，定期的监测和维护是确保航天器、大飞机以及核设施等设备安全工作的重要方式，但是由于此类设备往往具有复杂的结构，使得可供监测及维护作业的空间非常狭小。因此，探索机器人在狭小空间实施设备监测和维护的相关技术十分重要。为了顺利完成狭小空间中的监测和维护，机器人需要具备穿越狭窄环境的能力、同时回避障碍物、关节奇异和关节超限的能力以及足够大的灵巧操作空间。传统的刚性机器人，难以同时实现上述目标。而连续型超冗余机械臂不仅具有极好的环境适应性和极高的避障能力，还可以采用环绕等方式工作于非结构化环境及非合作目标的多设备环境。

由于具有大量冗余自由度，超冗余机械臂的运动学逆解十分复杂，一直是研究中的重点和难点，且绝大多数的超冗余机械臂规划算法都基于其运动学逆解。常见的机械臂逆运动学求解方法分为三类：广义逆法、人工神经网络法和脊线法。随着机器人自由度的增多，广义逆法和人工神经网络法的计算量都会变得非常宏大，不适用于超冗余机械臂。目前，脊线法被广泛用于柔性臂的规划，但该方法存在不易调整末端姿态，牺牲较多自由度和脊线不容易获取和调整等问题。总而言之，现有的基于逆运动学的机械臂规划方法都不能很好的解决超冗余机械臂的规划问题。

常见的机器人避障规划算法包括：A*算法和人工势场法。A*算法利于全局的信息进行规划，可以获得全局的最优解，但是随着规划的变量数量的增加，计算量会呈指数级数的增加，并不适用于超冗余机械臂的臂型规划。人工势场法具有很高的效率和通用性，但是它只考虑局部的环境信息，所以很容易陷入局部极小值点而无法抵达目标位置。而且传统的人工势场法往往只用于规划二维空间内单个质点的运动轨迹。之前并没有出现过将人工势场法用于三维空间内，多自由度同时避障规划的研究算法出现。

以上背景技术内容的公开仅用于辅助理解本发明的构思及技术方案，其并不必然属于本专利申请的现有技术，在没有明确的证据表明上述内容在本专利申请的申请日已经公开的情况下，上述背景技术不应当用于评价本申请的新颖性和创造性。

发明内容

本发明为了解决现有的问题，提供一种连续型超冗余机械臂的臂型协同规划方法、终端设备和存储介质。

为了解决上述问题，本发明采用的技术方案如下所述：

一种连续型超冗余机械臂的臂型协同规划方法，包括如下步骤：S1：建立连续型超冗余机械臂运动学模型；S2：构建虚拟引导管道；S3：通过机器人逆运动学方程求解出所述虚拟引导管道的初始臂型；S4：构建人工势场，所述人工势场包括所述虚拟引导管道产生的引导力势场；S5：计算所述连续型超冗余机械臂的关键点在所述引导力势场中受到的力，根据所述关键点的受力情况按照单位规划周期内臂上所述关键点做功之和最大的策略来对所述连续型超冗余机械臂的运动进行规划，直到所述连续型超冗余机械臂抵达目标臂型。

优选地，建立连续型超冗余机械臂运动学模型包括如下：确定所述连续型超冗余机械臂上任意一点的坐标系为：x指向该点圆弧的切向，y轴指向该点所处圆弧的圆心，由右手定则可得z轴方向垂直于圆弧所在平面；每个关节具有柔性关节的弯曲自由度θ和旋转自由度

两个自由度；起始坐标系{O₀}至臂杆上弧长为l处的坐标系{O₁}的变换分解为以下几个基本变换：绕x轴旋转φ角度、起始位置到目标位置的平移，绕z轴旋转θ角度；得到齐次变换矩阵：

其中，l为起始位置至目标位置的圆弧长度，s_*为sin(*)的缩写，c_*为cos(*)的缩写；

在基座固定的情况下，得到所述连续型超冗余机械臂第i段柔性关节上弧长为l处的坐标系与柔性臂关节角度之间的变换函数为：

其中，θ＝[θ₁，θ₂，......，θ_n]，φ＝[φ₁，φ₂......，φ_n]。

优选地，构建虚拟引导管道包括如下步骤：S21：根据任务要求以及障碍物分布情况确定所述目标臂型；S22：根据障碍物和所述目标臂型之间的空间关系确定所述虚拟引导管道的起始位置和横截半径；S23：在所述目标臂型上的起点等间隔的取起点p₀至所述目标臂型末端p_k的共k+1个点作为圆心，作以所述横截半径R为半径、垂直于所述点的x方向的圆，所有圆组成的集合，即为所述虚拟引导管道，即：

{P|P＝p_i+R*Rot(x_i,ρ)*y_i,i∈(0,1...,k),ρ∈[0,2π)}

其中，y_i为p_i处y方向单位向量。

优选地，所述虚拟引导管道上任意一点P₀位置为：

P₀＝p_i+R*Rot(x_i,ρ)*y_i

任意一点P₀产生的引导力场为：

其中，固定参数d意义为：当所述虚拟引导管道的位置与目标位置距离大于d时，依靠引导力前进；当所述虚拟引导管道位置与目标位置距离小于d时，不再需要引导力，依靠吸引力前进。

优选地，所述人工势场还包括：目标位置对当前位置产生与距离成正比的吸引力势场和障碍物和所述虚拟引导管道产生与距离平方成反比的排斥力势场。

优选地，目标位置对当前位置产生与距离成正比的吸引力通过如下公式计算：

障碍物和所述虚拟引导管道产生与距离平方成反比的排斥力通过如下公式计算：

其中，k_*和ρ为给定正系数，X为当前位置，X_tar为目标位置，X_o为障碍物位置，ρ和d为固定参数。

优选地，在所述单位规划周期内计算所述虚拟引导管道上的各关键点的受力、计算关键角增量，进而更新所述虚拟引导管道的臂型，判断所述臂型是否达到目标臂型，如是，则输出臂型序列，如不是，则重复计算所述虚拟引导管道上的各关键点的受力、计算关键角增量。

优选地，取所述虚拟引导管道上的m个关键点进行受力分析计算出各个关键点的受力：

F_i＝F_atti+F_repi+F_guidei,i∈(1,2,...,m)

计算出各所述关键点上受力在一个单位规划周期内做的总功为：

功函数的梯度为：

每个单位规划周期内的关键角增量可求得：

其中，

为第i个关键点受到的相应种类的力，F_i为第i个关键点受到的合力，dX_i为第i个关键点在一个单位规划周期内的位置改变量，

和

由所述连续型超冗余机械臂运动学模型求导算得，k是给定的正步长系数。

本发明还提供一种连续型超冗余机械臂的臂型协同规划终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如上任一所述方法的步骤。

本发明再提高一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如上任一所述方法的步骤。

本发明的有益效果为：提供一种连续型超冗余机械臂的臂型协同规划方法、终端设备和存储介质，通过虚拟引导管道和引导力的改进人工势场以及最大功规划策略，对连续型超冗余机械臂在狭窄环境中进行避障规划，规划算法不需要依靠机械臂的逆运动学，所以其复杂度不会随着机械臂的自由度增加而显著增加。

附图说明

图1是本发明实施例中连续型超冗余机械臂的臂型协同规划方法的示意图。

图2是本发明实施例中一种连续型超冗余机械臂的示意图。

图3是本发明实施例中一种单段柔性关节几何模型的示意图。

图4是本发明实施例中工作环境示例。

图5是本发明实施例中一种目标臂型以及虚拟引导管道的示意图。

图6是本发明实施例中一种连续型超冗余机械臂的臂型协同规划方法的流程示意图。

图7是本发明实施例中构建虚拟引导管道的方法示意图。

图8是本发明实施例中一种虚拟引导管道的示意图。

图9是本发明实施例中一种虚拟引导管道的初始臂型的示意图。

图10(a)是本发明实施例中第一种传统势场局部极小值点受力情况的示意图。

图10(b)是本发明实施例中第二种传统势场局部极小值点受力情况的示意图。

图11(a)和图11(b)分别是本发明实施例指向目标位置的吸引力和远离障碍物的斥力的示意图。

图12是本发明实施例中吸引力和排斥力合力分布图。

图13是本发明实施例中引导力分布的示意图。

图14是本发明实施例中改进的总力场分布示意图。

图15是本发明实施例中一种受力矛盾示意图。

图16是本发明实施例中第一种实施例中成功仿真结果时序图。

图17是本发明实施例中第二种实施例中成功仿真结果时序图。

图18是本发明实施例中第三种实施例中成功仿真结果时序图。

图19是本发明实施例中第四种实施例中成功仿真结果时序图。

图20是本发明实施例中第五种实施例中成功仿真结果时序图。

图21是本发明实施例中第六种实施例中成功仿真结果时序图。

具体实施方式

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

需要说明的是，当元件被称为“固定于”或“设置于”另一个元件，它可以直接在另一个元件上或者间接在该另一个元件上。当一个元件被称为是“连接于”另一个元件，它可以是直接连接到另一个元件或间接连接至该另一个元件上。另外，连接既可以是用于固定作用也可以是用于电路连通作用。

需要理解的是，术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明实施例和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多该特征。在本发明实施例的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

现有技术中常采用如下方法进行连续型超冗余机械臂的臂型协同规划方法：

1.基于广义逆解的逆运动学机械臂规划方法

2.基于神经网络的逆运动学机械臂规划方法

3.基于A*算法的机械臂规划方法

4.脊线法规划方法

其中，方法1、2、3的计算复杂度都会随着机械臂的自由度增加而显著上升，方法效率大大降低；方法4存在不易调整末端姿态，牺牲较多自由度和脊线不容易获取和调整等问题。

如图1所示，本发明提供一种连续型超冗余机械臂的臂型协同规划方法，包括如下步骤：

S1：建立连续型超冗余机械臂运动学模型；

S2：构建虚拟引导管道；

S3：通过机器人逆运动学方程求解出所述虚拟引导管道的初始臂型；

S4：构建人工势场，所述人工势场包括所述虚拟引导管道产生的引导力势场；

S5：计算所述连续型超冗余机械臂的关键点在所述引导力势场中受到的力，根据所述关键点的受力情况按照单位规划周期内臂上所述关键点做功之和最大的策略来对所述连续型超冗余机械臂的运动进行规划，直到所述连续型超冗余机械臂抵达目标臂型。

本发明提供的方法通过虚拟引导管道和引导力的改进人工势场以及最大功规划策略，对连续型超冗余机械臂在狭窄环境中进行避障规划，规划算法不需要依靠机械臂的逆运动学，所以其复杂度不会随着机械臂的自由度增加而显著增加。

如图2所示，是本发明实施例中一种连续型超冗余机械臂的示意图。可知，连续型超冗余机械臂是柔性臂。不同于传统刚性机械臂，柔性机械臂不具有刚性的旋转、平移关节，不能采用传统的D-H方法进行运动学分析。本发明采用一种几何上非常直观的方法来为柔性臂进行建模和分析。为方便建立柔性臂运动学模型，本发明规定柔性臂上任意一点的坐标系为：x指向该点圆弧的切向，y轴指向该点所处圆弧的圆心，由右手定则可得z轴方向垂直与圆弧所在平面。

如图3所示，是本发明实施例中一种单段柔性关节几何模型的示意图。每个关节具有柔性关节的弯曲自由度θ和旋转自由度

两个自由度；起始坐标系{O₀}至臂杆上弧长为l处的坐标系{O₁}的变换分解为以下几个基本变换：绕x轴旋转φ角度、起始位置到目标位置的平移，绕z轴旋转θ角度；得到齐次变换矩阵：

其中，l为起始位置至目标位置的圆弧长度，s_*为sin(*)的缩写，c_*为cos(*)的缩写；

在基座固定的情况下，柔性关节齐次变换矩阵的简单累乘便可得到各柔性关节的串联之后得到的柔性臂的齐次变换矩阵，得到所述连续型超冗余机械臂第i段柔性关节上弧长为l处的坐标系与柔性臂关节角度之间的变换函数为：

其中，θ＝[θ₁，θ₂，......，θ_n]，φ＝[φ₁，φ₂......，φ_n]。

如图4所示，是本发明一种实施例中工作环境示例。其中，黑色线条为障碍物区域，五角星为目标末端位置。

本发明的方法适用于连续型超冗余机械臂穿越狭窄环境完成任务过程中的路径规划。首先需要根据任务要求以及障碍物分布情况确定目标臂型，根据障碍物和目标臂型之间的空间关系确定虚拟引导管道的起始位置和横截半径以保证虚拟引导管道之内没有障碍物。为虚拟引导管道赋予人工势场，计算机械臂关键点在势场中受到的力，根据受力情况按照单位规划周期内臂上关键点做功之和最大的策略来对机械臂运动进行规划，直到机械臂抵达目标臂型。

如图5所示，是本发明实施例中一种目标臂型以及虚拟引导管道的示意图。

如图6所示，是本发明实施例中一种连续型超冗余机械臂的臂型协同规划方法的流程示意图。图中右侧完成一个循环即为一个单位规划周期。在单位规划周期内计算所述虚拟引导管道上的各关键点的受力、计算关键角增量，进而更新所述虚拟引导管道的臂型，判断所述臂型是否达到目标臂型，如是，则输出臂型序列，如不是，则重复计算所述虚拟引导管道上的各关键点的受力、计算关键角增量。

虚拟引导管道的使用，虽然增加了限制条件，但可以使得本发明的方法不需要详细讨论各种复杂障碍物环境，仅需关注管道本身即可进行规划，适用于非常多的场景，具有很广泛的运用前景。

本发明采用等径圆柱形虚拟引导管道，优势如下：

适用于各种情景。任何障碍物及目标臂型已知的情景下，只需要知道障碍物和目标臂型的最近距离，都可以构造出不包含障碍物的圆柱形管道。

方便人工势场构建。圆柱形管道具有很好的对称性，使得构造人工势场的难度不高。

如图7所示，构建虚拟引导管道包括如下步骤：

S21：根据任务要求以及障碍物分布情况确定所述目标臂型；

S22：根据障碍物和所述目标臂型之间的空间关系确定所述虚拟引导管道的起始位置和横截半径；

S23：在所述目标臂型上的起点等间隔的取起点p₀至所述目标臂型末端p_k的共k+1个点作为圆心，作以所述横截半径R为半径、垂直于所述点的x方向的圆，所有圆组成的集合，即为所述虚拟引导管道，即：

{P|P＝p_i+R*Rot(x_i,ρ)*y_i,i∈(0,1...,k),ρ∈[0,2π)} (3)

其中，y_i为p_i处y方向单位向量。

如图8所示，是本发明实施例中一种虚拟引导管道的示意图。即构建以目标臂型上弧线p₀p_k为轴线的圆柱作为管道，弧线p₀p_k上任意一点距离管道的最小距离均为R。

完成虚拟引导管道的构建之后，通过机器人逆运动学方程求解出虚拟引导管道的初始臂型。初始臂型的求解遵循以下原则：

连续型超冗余机械臂末端位置位于出虚拟引导管道的起点处，规划从出虚拟引导管道起点开始，到出虚拟引导管道末端结束。

连续型超冗余机械臂末端x方向与管道起点处x方向一致；连续型超冗余机械臂姿态和虚拟引导管道轴向方向偏离大不利于规划。

各柔性关节弯曲角θ∈[0，π)，柔性关节弯曲角过大时，连续型超冗余机械臂几何构型会出现“打结”现象。

最后一段柔性关节的弯曲角θ_n接近0；柔性关节弯曲度越小可调节性越好。

如图9所示，是本发明实施例中一种虚拟引导管道的初始臂型的示意图。

传统的人工势场一般只包含两类势场：指向目标位置的吸引力势场和远离障碍物的斥力势场，当连续型超冗余机械臂位于这两种力反向共线的位置，机械臂在该位置受力平衡无法依靠势场继续向目标位置移动，陷入局部极小值点。

如图10(a)所示，是本发明实施例中第一种传统势场局部极小值点受力情况的示意图。图中表明了只有一个障碍物2时，物体在当前位置1处的受力情况，包括目标位置3对当前位置1产生与距离成正比的吸引力F_att；障碍物2和虚拟引导管道产生与距离平方成反比的排斥力F_rep。

如图10(b)所示,是本发明实施例中第二种传统势场局部极小值点受力的示意图。图中表明了有两个障碍物21/22时，物体在当前位置1处的受力情况。

目标位置对当前位置产生与距离成正比的吸引力通过如下公式计算：

障碍物和所述虚拟引导管道产生与距离平方成反比的排斥力通过如下公式计算：

其中，k_*和ρ为给定正系数，X为当前位置，X_tar为目标位置，X_o为障碍物位置，ρ和d为固定参数。

如图11(a)和图11(b)所示，分别是本发明的实施例中指向目标位置的吸引力和远离障碍物的斥力的示意图。

如图12所示，是本发明实施例中吸引力和排斥力合力分布图。图中黑点为目标位置，虚拟引导管道内部势场分布杂乱，没有明显导向，不能引导连续型超冗余机械臂沿着管道抵达目标位置。

为解决上述问题，本算法创新性的加入了虚拟引导管道所产生的引导力势场，可以有效规避传统势场的局部极小值点问题引导机械臂沿管道抵达目标位置，改进的人工势场法的构建方法如下：

虚拟引导管道上一点P₀位置由公式(3)可求得：

P₀＝p_i+R*Rot(x_i,ρ)*y_i

任意一点P₀产生的引导力场类似于为将虚拟引导管道视作电流螺线管，螺线管产生的磁场：

其中，固定参数d意义为：当所述虚拟引导管道的位置与目标位置距离大于d时，依靠引导力前进；当所述虚拟引导管道位置与目标位置距离小于d时，不再需要引导力，依靠吸引力前进。

如图13所示，是本发明实施例中引导力的分布示意图。图中因为管道由散点组成，会出现一些奇异点，可通过加密管道取样点解决。

如图14所示，是本发明实施例中改进的总力场分布图。取势场参数d为管道半径，离目标位置距离大于d时，虚拟引导管道内部的场为斥力场和引导力场，两力互相垂直，不会出现反向共线平衡的情况，合力始终指向引导管道的轴向和远离管道边缘的方向。选取合适的d，可以使距离目标位置小于d时，当前位置与目标位置连线上不存在障碍物，同样也不存在合力为零的情形，从而解决了传统人工势场方法的局部极小值点的问题。

传统用于机械臂规划的势场算法往往只对规划机械臂的末端进行单自由度规划，得到末端轨迹之后再通过机械臂逆运动学求解各关节轨迹，而柔性臂因为具有较多冗余自由度，逆运动学十分复杂，上述方法复杂、效率低，难以保证除末端外的机械臂其他位置不与障碍物发生碰撞，很难适用于柔性臂穿越狭窄空间的情况。

本发明的方法则希望用各个关节角多个自由度同时对多个点的运动进行规划，与常规的多点规划不同，对机械臂进行整臂协同规划需要面临的问题是，机械臂上各点具有几何约束关系，但各点在势场中受力不一致，很可能会无法使得各个点都朝受力方向移动。

如图15所示，是本发明实施例中一种受力矛盾示意图。如果一个柔性关节的两个不同位置在势场中受到F₁和F₂两个力，F₁驱使θ增大，而F₂驱使θ减小，故两个力对与柔性关节的运动产生了互相矛盾的影响，不能用现有办法进行规划。

为了实现多自由度多点同时规划，并且由于本发明中提出了了环形力场分布，无法使用传统的沿势场梯度下降的策略。而功作为能量转换的度量，当力做功最多，臂的势能下降的最多，故本发明提出了使关节角朝让每个规划周期内柔性臂上关键点受力做功之和最多的方向运动的策略来进行整臂协同规划。直观的理解是，当柔性臂上各点做功之和最大时，柔性臂就尽可能地朝着受力的方向进行移动，当出现受力矛盾情况时，做功能力小的点会倾向于服从做功能力更大的点去进行移动。

具体实现方法为，取所述虚拟引导管道上的m个关键点进行受力分析计算出各个关键点的受力：

计算出各所述关键点上受力在一个单位规划周期内做的总功为：

功函数的梯度为：

每个单位规划周期内的关键角增量可求得：

其中，

为第i个关键点受到的相应种类的力，F_i为第i个关键点受到的合力，dX_i为第i个关键点在一个单位规划周期内的位置改变量，

和

由所述连续型超冗余机械臂运动学模型即公式(2)求导算得，k是给定的正步长系数。

需要注意的是，在本方法中，因为F_i和

与当前臂型有关，实际上是一个连续模型，采用离散的模型近似替代，为了减少模型误差，需要限制步长，使得一个规划周期内各关节角增量△θ_j和

较小。

在此策略下，即体现了势场力对于柔性臂的引导作用，又调和了受力以及臂的几何约束之间的矛盾，使得柔性臂尽可能的朝着做功更多的方向移动，实现了多自由度多点的协同规划。

为了验证算法的效果，在如下的条件下进行了仿真：柔性臂基座固定不动，柔性臂由5段长度为1的柔性关节组成，目标臂型已知，管道横截半径为R。

虚拟引导管道起点是决定任务难度的一个重要因素，起点越靠近基座，管道越长，任务也越发困难，在后续的仿真中，为了使仿真难度适中，又具有一定的代表性，将管道起点选取在第4段柔性关节起点处。

对每段柔性关节关键点数量m的选取进行了仿真，当m较小时，不足以体现柔性臂的几何特性，会出现柔性臂从管道边缘穿过的情况。而随着m的增大，算法计算量升高，效率降低。经过尝试，选取了较为适中的m＝18的设定。

本部分实验在不同的关节空间θ∈[0，θ_max]，φ∈[0，360°)和R的情况下，随机生成目标臂型，进行了100次仿真，成功得到可行解的率如下：

表格1仿真成功率表

当θ_max＞60°时，目前的初始臂型算法效果比较差，故不继续进行实验。通过调试观察，发现部分失败的案例是由于初始臂型不够理想，失败的案例经过对初始臂型的手动调整之后，可以优化得到如下结果：

表格2改进仿真成功率表

由表可知，本算法在关节空间θ∈[0，60°]，φ∈[0，360°)中且R≥0.08时，本算法的规划成功率都在97％以上。并且在仿真中发现，通过减小步长，可以稳定提高算法表现，但会花费更多运行时间，降低运行效率。

如图16所示，是本发明的一种实施例中成功仿真结果时序图。

如图17所示，是本发明的又一种实施例中成功仿真结果时序图。

接下来，固定虚拟引导管道弯曲度最小R仿真，在管道弯曲度固定的情况下，以0.01为步长不断缩小R的取值，求出能成功抵达目标臂型的最小的R取值。结果如下：

表格3固定虚拟引导管道弯曲度R_min表

图18-图21分别是本发明实施例中R＝0.05，φ₅＝0°，θ_4-5＝[0°，0°]、R＝0.07，φ₅＝0°，θ_4-5＝[90°，90°]、R＝0.08，φ₅＝90°，θ_4-5＝[90°，90°]、R＝0.11，φ₅＝180°，θ_4-5＝[90°，90°]的仿真结果时序图。

本申请实施例还提供一种控制装置，包括处理器和用于存储计算机程序的存储介质；其中，处理器用于执行所述计算机程序时至少执行如上所述的方法。

本申请实施例还提供一种存储介质，用于存储计算机程序，该计算机程序被执行时至少执行如上所述的方法。

本申请实施例还提供一种处理器，所述处理器执行计算机程序，至少执行如上所述的方法。

所述存储介质可以由任何类型的易失性或非易失性存储设备、或者它们的组合来实现。其中，非易失性存储器可以是只读存储器(ROM，Read Only Memory)、可编程只读存储器(PROM，Programmable Read-Only Memory)、可擦除可编程只读存储器(EPROM，ErasableProgrammable Read-Only Memory)、电可擦除可编程只读存储器(EEPROM，ElectricallyErasable Programmable Read-Only Memory)、磁性随机存取存储器(FRAM，FerromagneticRandomAccess Memory)、快闪存储器(Flash Memory)、磁表面存储器、光盘、或只读光盘(CD-ROM，Compact Disc Read-Only Memory)；磁表面存储器可以是磁盘存储器或磁带存储器。易失性存储器可以是随机存取存储器(RAM，Random Access Memory)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器(SRAM，Static Random Access Memory)、同步静态随机存取存储器(SSRAM，SynchronousStatic Random Access Memory)、动态随机存取存储器(DRAM，DynamicRandom AccessMemory)、同步动态随机存取存储器(SDRAM，Synchronous Dynamic RandomAccessMemory)、双倍数据速率同步动态随机存取存储器(DDRSDRAM，Double DataRateSynchronous Dynamic Random Access Memory)、增强型同步动态随机存取存储器(ESDRAM，Enhanced Synchronous Dynamic RandomAccess Memory)、同步连接动态随机存取存储器(SLDRAM，SyncLink Dynamic Random Access Memory)、直接内存总线随机存取存储器(DRRAM，Direct Rambus Random Access Memory)。本发明实施例描述的存储介质旨在包括但不限于这些和任意其它适合类型的存储器。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统和方法，可以通过其它的方式实现。以上所描述的设备实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，如：多个单元或组件可以结合，或可以集成到另一个系统，或一些特征可以忽略，或不执行。另外，所显示或讨论的各组成部分相互之间的耦合、或直接耦合、或通信连接可以是通过一些接口，设备或单元的间接耦合或通信连接，可以是电性的、机械的或其它形式的。

上述作为分离部件说明的单元可以是、或也可以不是物理上分开的，作为单元显示的部件可以是、或也可以不是物理单元，即可以位于一个地方，也可以分布到多个网络单元上；可以根据实际的需要选择其中的部分或全部单元来实现本实施例方案的目的。

另外，在本发明各实施例中的各功能单元可以全部集成在一个处理单元中，也可以是各单元分别单独作为一个单元，也可以两个或两个以上单元集成在一个单元中；上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。

本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

或者，本发明上述集成的单元如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分。而前述的存储介质包括：移动存储设备、ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。

本申请所提供的几个方法实施例中所揭露的方法，在不冲突的情况下可以任意组合，得到新的方法实施例。

本申请所提供的几个产品实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的产品实施例。

本申请所提供的几个方法或设备实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的方法实施例或设备实施例。

以上内容是结合具体的优选实施方式对本发明所做的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。

Claims (10)

1.一种连续型超冗余机械臂的臂型协同规划方法，其特征在于，包括如下步骤：

S1：建立连续型超冗余机械臂运动学模型；

S2：构建虚拟引导管道；

S3：通过机器人逆运动学方程求解出所述虚拟引导管道的初始臂型；

S4：构建人工势场，所述人工势场包括所述虚拟引导管道产生的引导力势场；

S5：计算所述连续型超冗余机械臂的关键点在所述引导力势场中受到的力，根据所述关键点的受力情况按照单位规划周期内臂上所述关键点做功之和最大的策略来对所述连续型超冗余机械臂的运动进行规划，直到所述连续型超冗余机械臂抵达目标臂型。

2.如权利要求1所述的连续型超冗余机械臂的臂型协同规划方法，其特征在于，建立连续型超冗余机械臂运动学模型包括如下：

确定所述连续型超冗余机械臂上任意一点的坐标系为：x指向该点圆弧的切向，y轴指向该点所处圆弧的圆心，由右手定则可得z轴方向垂直于圆弧所在平面；每个关节具有柔性关节的弯曲自由度θ和旋转自由度

两个自由度；

起始坐标系{O₀}至臂杆上弧长为l处的坐标系{O₁}的变换分解为以下几个基本变换：绕x轴旋转φ角度、起始位置到目标位置的平移，绕z轴旋转θ角度；得到齐次变换矩阵：

其中，l为起始位置至目标位置的圆弧长度，s_*为sin(*)的缩写，c_*为cos(*)的缩写；

在基座固定的情况下，得到所述连续型超冗余机械臂第i段柔性关节上弧长为l处的坐标系与柔性臂关节角度之间的变换函数为：

其中，θ＝[θ₁，θ₂，......，θ_n]，φ＝[φ₁，φ₂......，φ_n]。

3.如权利要求2所述的连续型超冗余机械臂的臂型协同规划方法，其特征在于，构建虚拟引导管道包括如下步骤：

S21：根据任务要求以及障碍物分布情况确定所述目标臂型；

S22：根据障碍物和所述目标臂型之间的空间关系确定所述虚拟引导管道的起始位置和横截半径；

S23：在所述目标臂型上的起点等间隔的取起点p₀至所述目标臂型末端p_k的共k+1个点作为圆心，作以所述横截半径R为半径、垂直于所述点的x方向的圆，所有圆组成的集合，即为所述虚拟引导管道，即：

{P|P＝p_i+R*Rot(x_i，ρ)*y_i，i∈(0，1...，k)，ρ∈[0，2π)}

其中，y_i为p_i处y方向单位向量。

4.如权利要求3所述的连续型超冗余机械臂的臂型协同规划方法，其特征在于，所述虚拟引导管道上任意一点P₀位置为：

P₀＝p_i+R*Rot(x_i，ρ)*y_i

任意一点P₀产生的引导力场为：

其中，固定参数d意义为：当所述虚拟引导管道的位置与目标位置距离大于d时，依靠引导力前进；当所述虚拟引导管道位置与目标位置距离小于d时，不再需要引导力，依靠吸引力前进。

5.如权利要求4所述的连续型超冗余机械臂的臂型协同规划方法，其特征在于，所述人工势场还包括：目标位置对当前位置产生与距离成正比的吸引力势场和障碍物和所述虚拟引导管道产生与距离平方成反比的排斥力势场。

6.如权利要求5所述的连续型超冗余机械臂的臂型协同规划方法，其特征在于，目标位置对当前位置产生与距离成正比的吸引力通过如下公式计算：

障碍物和所述虚拟引导管道产生与距离平方成反比的排斥力通过如下公式计算：

其中，k_*和ρ为给定正系数，X为当前位置，X_tar为目标位置，X_o为障碍物位置，ρ和d为固定参数。

7.如权利要求6所述的连续型超冗余机械臂的臂型协同规划方法，其特征在于，在所述单位规划周期内计算所述虚拟引导管道上的各关键点的受力、计算关键角增量，进而更新所述虚拟引导管道的臂型，判断所述臂型是否达到目标臂型，如是，则输出臂型序列，如不是，则重复计算所述虚拟引导管道上的各关键点的受力、计算关键角增量。

8.如权利要求7所述的连续型超冗余机械臂的臂型协同规划方法，其特征在于，取所述虚拟引导管道上的m个关键点进行受力分析计算出各个关键点的受力：

计算出各所述关键点上受力在一个单位规划周期内做的总功为：

功函数的梯度为：

每个单位规划周期内的关键角增量可求得：

其中，

为第i个关键点受到的相应种类的力，F_i为第i个关键点受到的合力，dX_i为第i个关键点在一个单位规划周期内的位置改变量，

和

由所述连续型超冗余机械臂运动学模型求导算得，k是给定的正步长系数。

9.一种连续型超冗余机械臂的臂型协同规划终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1-8任一所述方法的步骤。

10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-8任一所述方法的步骤。

Images