CN110561426B - 一种超冗余机械臂的路径规划方法、装置及控制系统 - Google Patents

Abstract

本发明公开了一种超冗余机械臂的路径规划方法、装置及控制系统，方法包括：对机械臂配置后获取臂形描述参数并规划具有多个期望节点的期望路径，以第一个期望节点为目标期望节点，获取目标期望节点的参数，通过臂形描述参数获得等效臂形的末端当前参数和雅可比矩阵；结合末端当前参数及目标期望节点获得末端期望速度，基于雅克比矩阵迭代末端期望速度获得各个臂段的期望臂形参数，根据获得的期望臂形参数得到关节驱动参数，根据关节驱动参数将机械臂的末端向目标期望节点驱动，依次遍历以期望节点为目标期望节点完成机械臂的路径规划；解决了现有技术中超冗余机械臂的路径规划方法计算繁杂的技术问题，提供了一种高效的超冗余机械臂路径规划方法。

Description

一种超冗余机械臂的路径规划方法、装置及控制系统

技术领域

本发明涉及机械臂路径规划技术领域，尤其是涉及一种超冗余机械臂的路径规划方法、装置及控制系统。

背景技术

随着科学技术的不断发展，许多的大型设备经研发生产后广泛应用于各个领域中。而由于大型设备往往都具有非常复杂的内部结构，因此造成了在对这些大型设备进行维修或检测时的操作空间都非常狭小。为了能够在狭小的操作空间内实现对这些大型设备的维修或检测工作，往往通过控制超冗余机械臂进入大型设备的内部中进行相应操作。超冗余机械臂具有穿越狭窄空间、回避障碍物、关节奇异和关节超限的能力，且超冗余机械臂具有较大的灵巧操作空间。

但是，由于超冗余机械臂的自由度较多，导致其运动学方程非常复杂，进而导致超冗余机械臂逆解的计算量急剧增加，从而降低了超冗余机械臂实时在线路径规划效率。因此，如何解决由于超冗余机械臂的应用带来的路径规划效率低下的问题，成为本领域技术人员需克服的技术难题。

发明内容

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。为此，本发明的一个目的是提供一种高效的超冗余机械臂的路径规划方法。

本发明所采用的技术方案是：

第一方面，本发明提供一种超冗余机械臂的路径规划方法，所述超冗余机械臂包括多个臂段，多个所述臂段均包括多个小节，多个所述小节间依次通过关节连接，其包括步骤：

S10、获取所述臂段的臂形描述参数并规划所述超冗余机械臂的期望路径，在所述期望路径上取多个期望节点，且分别设有一个所述期望节点处于所述期望路径的首端和末端，并以第一个所述期望节点为目标期望节点；

S20、根据所述臂形描述参数获得所述机械臂的当前等效臂形参数；

S30、获取所述目标期望节点的目标期望位置信息和目标期望指向信息，并根据所述当前等效臂形参数递推获得所述超冗余机械臂的等效臂形的末端当前位置信息、末端当前指向信息和雅可比矩阵；

S40、根据所述末端当前位置信息、所述末端当前指向信息、所述雅可比矩阵、所述目标期望位置信息和所述目标期望指向信息获得末端期望线速度和末端期望角速度；

S50、基于所述雅可比矩阵不断迭代所述末端期望速度和所述末端期望角速度获得各个所述臂段的期望臂形参数；

S60、根据所述期望臂形参数获得关节驱动参数，并根据所述关节驱动参数将所述超冗余机械臂向所述目标期望节点驱动；

S70、以下一个所述期望节点为所述目标期望节点，重复所述步骤S20至所述步骤S60直至遍历完所有所述期望节点。

进一步地，所述步骤S40具体包括子步骤：

S401、根据所述目标期望位置信息、所述目标期望指向信息、所述末端当前位置信息和所述末端当前指向信息获得位置信息差和指向信息差；

S402、根据所述位置信息差、所述指向信息差及所述雅可比矩阵计算得出所述末端期望线速度和所述末端期望角速度。

进一步地，所述步骤S50之后所述步骤S60之前还包括步骤：

S510、根据所述期望臂形参数获得末端期望位置信息和末端期望指向信息；

S511、设定误差范围要求，当所述末端期望位置信息与所述目标期望位置信息的误差满足所述误差范围要求，且当所述末端期望指向信息与所述目标期望指向信息的误差满足所述误差范围要求时，执行所述步骤S60，否则返回执行所述步骤S20。

进一步地，分别对多个所述臂段内的多个所述关节按顺序进行标号，且将所述标号为奇数的所述关节设置为Yaw-Pitch型，所述标号为偶数的所述关节设置为Pitch-Yaw型。

第二方面，本发明提供一种超冗余机械臂的路径规划装置，所述超冗余机械臂包括多个臂段，多个所述臂段均包括多个小节，多个所述小节两两之间通过关节连接，所述臂段内的多个所述关节按顺序进行标号，且将所述标号为奇数的所述关节设置为Pitch-Yaw型，所述标号为偶数的所述关节设置为Yaw-Pitch型，其包括：

臂形描述参数获取及期望路径规划模块，用于获取所述臂段的臂形描述参数并规划所述超冗余机械臂的期望路径，在所述期望路径上取多个期望节点，且分别设有一个所述期望节点处于所述期望路径的首端和末端，并以第一个所述期望节点为目标期望节点；

当前等效臂形参数获取模块，用于根据所述臂形描述参数获得所述机械臂的当前等效臂形参数；

参数信息计算获取模块，用于获取所述目标期望节点的目标期望位置信息和目标期望指向信息，并根据所述前等效臂形参数推获得所述超冗余机械臂的等效臂形的末端当前位置信息、末端当前指向信息和雅可比矩阵；

末端期望速度获取模块，用于根据所述末端当前位置信息、所述末端当前指向信息、所述雅可比矩阵、所述目标期望位置信息和所述目标期望指向信息获得末端期望速度；

期望臂形参数获取模块，用于基于所述雅可比矩阵不断迭代所述末端期望速度获得各个所述臂段的期望臂形参数；

驱动控制模块，根据所述期望臂形参数获得关节驱动参数，并根据所述关节驱动参数将所述超冗余机械臂向所述目标期望节点驱动；

期望节点遍历模块，用于以下一个所述期望节点为所述目标期望节点，重复所述当前等效臂形参数获取模块、所述参数信息计算获取模块、末端期望速度获取模块、所述期望臂形参数获取模块和所述驱动控制模块的工作直至遍历完所有所述期望节点。

进一步地，所述末端期望速度获取模块具体包括：

信息误差计算单元，用于根据所述目标期望位置信息、所述目标期望指向信息、所述末端当前位置信息和所述末端当前指向信息获得位置信息差和指向信息差；

末端期望速度计算单元，用于根据所述位置信息差、所述指向信息差及所述雅可比矩阵计算得出所述末端期望速度。

进一步地，所述超冗余机械臂的路径规划装置还包括条件判断模块；所述条件判断模块具体包括：

末端参数信息获取单元，根据所述期望臂形参数获得末端期望位置信息和末端期望指向信息；

判断选择执行单元，用于设定误差范围要求，当所述末端期望位置信息与所述期望位置信息的误差满足所述误差范围要求，且当所述末端驱动指向信息与所述期望指向信息的误差满足所述误差范围要求时，执行所述驱动控制模块的工作，否则返回执行所述当前等效臂形参数获取模块的工作。

第三方面，本发明提供一种超冗余机械臂的路径控制系统，其包括超冗余机械臂本体和用于控制所述超冗余机械臂运动的驱动器，所述驱动器用于执行所述的超冗余机械臂的路径规划方法控制所述机械臂本体的工作。

本发明的有益效果是：

本发明中一种超冗余机械臂的路径规划方法，其通过对超冗余机械臂进行配置后，获取臂段的臂形描述参数并规划期望路径，在期望路径上取多个期望节点，并以第一个期望节点为目标期望节点后，根据臂形描述参数获取超冗余机械臂的当前等效臂形参数，通过当前等效臂形参数获得超冗余机械臂的等效臂形的末端当前位置信息、末端当前指向信息和雅可比矩阵后，结合目标期望节点的目标期望位置信息和目标期望指向信息计算出末端期望速度，再基于雅可比矩阵不断迭代末端期望速度获得各个臂段的期望臂形参数，进一步通过臂形参数获得关节驱动参数后，根据关节驱动参数将超冗余机械臂向目标期望节点驱动，最后依次遍历以期望节点为目标期望节点完成超冗余机械臂的路径规划；解决了现有技术中超冗余机械臂的路径规划方法计算繁杂、效率低的技术问题，提供了一种高效的超冗余机械臂路径规划方法。

附图说明

图1是本发明申请中超冗余机械臂的一具体实施例结构示意图；

图2是本发明申请中超冗余机械臂的在参考惯性系O_I下各个臂段中小节间的一具体实施例关节配置示意图；

图3是本发明申请中超冗余机械臂的路径规划方法的一具体实施例流程图；

图4是本发明申请中超冗余机械臂的第一臂段的一具体实施例臂形结构示意图；

图5是本发明申请中超冗余机械臂的路径规划装置的一具体实施例模块框图。

具体实施方式

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对照附图说明本发明的具体实施方式。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图，并获得其他的实施方式。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。

实施例一：

本发明实施例提供一种超冗余机械臂的路径规划方法，如图1所示，图1为本发明实施例中超冗余机械臂的结构示意图；其中，超冗余机械臂共包括N个臂段，分别标记为S₁至S_N，其中S₁表示离基座B₀最近的第一臂段，S_N表示离基座B₀最远的臂段；具体的，n表示超冗余机械臂各个臂段包含的小节数，且每个小节一一对应一个关节，两个小节之间依次通过关节连接，则B_i,j超冗余机械臂第i臂段的第j小节，J_i,j表示超冗余机械臂第i臂段的第j关节，E表示超冗余机械臂的末端。参照图2，图2是本发明实施例中在参考惯性系O_I下各个臂段中小节间的关节配置示意图，其中，各个臂段内的多个关节按照顺序进行标号，并将标号为奇数的关节设置为Yaw-Pitch型，Yaw是围绕惯性系的Y_I轴旋转，Pitch是围绕惯性系的X_I轴旋转，Yaw-Pitch即关节先绕Y_I轴旋转，再绕X_I轴旋转；将标号为偶数的关节设置为Pitch-Yaw型，则Pitch-Yaw为关节先绕X_I轴旋转，再绕Y_I轴旋转。其中，θ_i,j,k表示第i大段第j关节的第k个旋转轴，本发明实施例中，每个关节仅有2个旋转轴，因此k的取值为1或2，1表示该关节的第一个旋转轴，2表示该关节的第二个旋转轴。其中，若n为奇数(n为超冗余机械臂中臂段包含的小节数)，则该臂段内第一个关节的配置与最后一个关节的配置一致；若n为偶数，则该臂段内第一个关节的配置与最后一个关节的配置相反。而由于超冗余机械臂的联动作用，使得超冗余机械臂的每个臂段中各关节处的旋转角度时一致的，同时使得各个小节处于同一个平面内，即：

参照图3，图3是本发明实施例一种超冗余机械臂的路径规划方法的流程示意图，其包括步骤：

S10、获取臂段的臂形描述参数并规划超冗余机械臂的期望路径，在期望路径上取多个期望节点，且分别设有一个期望节点处于期望路径的首端和末端，并以第一个期望节点为目标期望节点；

S20、根据臂形描述参数获得机械臂的当前等效臂形参数；

S30、获取目标期望节点的目标期望位置信息和目标期望指向信息，并根据当前等效臂形参数递推获得超冗余机械臂的等效臂形的末端当前位置信息、末端当前指向信息和雅可比矩阵；

S40、根据末端当前位置信息、末端当前指向信息、雅可比矩阵、目标期望位置信息和目标期望指向信息获得末端期望速度；

S50、基于雅可比矩阵不断迭代末端期望速度获得各个所述臂段的期望臂形参数；

S60、根据期望臂形参数获得关节驱动参数，并根据关节驱动参数将超冗余机械臂向目标期望节点驱动；

S70、以下一个期望节点为目标期望节点，重复步骤S20至步骤S60直至遍历完所有期望节点。

具体的，步骤S10中，臂段的臂形描述参数由臂段所在的平面与参考平面的夹角α和该臂段中各个小节之间的弯曲角度φ决定，参照图4，图4是本发明实施例中超冗余机械臂第一臂段的臂形结构示意图，并结合图1所述内容和图2所述内容可知，超冗余机械臂的每个臂段的自由度为n+1，其中的1个自由度表示该臂段所在的平面相对于参考平面(超冗余机械臂本体坐标系的X_iO_iZ_i平面)的旋转方向(该旋转方向与臂形描述参数α相关)，其余n个自由度表示臂段中n个小节的弯曲方向(与臂形描述参数φ相关)，从而，本发明实施例的超冗余机械臂的路径规划方法降低了每一个臂段内需要计算的自由度数，减少了超冗余机械臂正、逆运动学方程的递推次数，进而使得误差变小、路径规划效率得到提高。

步骤S10中，超冗余机械臂到达目标探测位置的路径有多种情况，较为常用的为直线路径和圆弧路径等。规划好期望路径后，在期望路径上取多个期望节点，并且以第一个期望节点为目标期望节点，则期望路径可表示如下：

其中，P_di∈R^3×1表示期望路径中第i个期望节点的期望位置信息，t_di∈R^3×1表示期望路径中第i个期望节点的期望指向信息，则s_di记为第i个期望路径上的第i个节点(当中包含有i个节点的期望位置信息和期望指向信息)。其中，i的取值范围为1至m，m表示期望节点的总个数。

在规划好超冗余机械臂的期望路径并在期望路径上取多个期望节点，且分别设有一个期望节点处于期望路径的首端和末端后，将第一期望节点(记为s_d1，即期望路径的首端)设为目标期望节点。

步骤S20中，获取超冗余机械臂的等效臂形的雅可比矩阵记为：J∈R^6×2N，根据臂形描述参数获得机械臂的当前等效臂形参数具体为：获得臂形描述参数(包括α和φ)后，则可知臂形等效的机械臂的第i臂段的起始关节的等效臂形位置信息在惯性系下表示为：

其中，n表示各个臂段包含的小节数，a_i,j表示第i臂段中第j小节的关节到该小节质心的位置矢量，b_i,j表示第i臂段中第j小节质心到第j+1小节的关节处的位置矢量。此外，臂形等效的机械臂的第i臂段的起始关节的等效臂形指向信息相对于惯性系的旋转变换矩阵为：

其中，E_3×3为3行3列的单位矩阵，n表示每个臂段内的小节数，且：

则由等效臂形位置信息和等效臂形指向信息构成当前等效臂形参数信息。

步骤S30中，获得有(3)式和(4)式表示的当前等效臂形参数信息后，则可知机械臂的等效臂形的末端当前位置信息在惯性系下表示为：

末端当前指向信息相对于惯性系的旋转变换矩阵为：

A_e＝A_N·R_x(α_N)·R_y(n·φ_N) (8)

步骤S40中获得末端期望速度和末端期望角速度具体为：将(3)式两边微分求导可得超冗余机械臂的臂形等效的第i臂段起始关节关于当前等效臂形参数的一阶导数关系，即第i臂段起始关节的期望线速度为：

其中，z_i1表示第i臂段所在的平面的旋转方向轴，z_i2表示第i臂段中各个小节的弯曲方向轴。用ω_i表示第i臂段的末端期望角速度，其计算方程如下：

其中，ω₀＝[0 0 0]^T。

根据(8)式和(9)式可得超冗余机械臂的等效臂形的末端期望线速度v_e和末端期望角速度ω_e为：

则将(11)式改写为：

其中，J∈R^6×2N表示超冗余机械臂的雅可比矩阵，Θ∈R^2N×1表示超冗余机械臂的当前等效臂形参数的集合，即：

Θ＝[α₁ φ₁ α₂ φ₂ … α_N φ_N]^T (13)

为的Θ一阶导数，则雅克比矩阵J的具体表达式如下：

其中，

在(15)式中，J_v表示雅可比矩阵与线速度有关的部分，J_ω表示雅可比矩阵与角速度有关的部分。

步骤S40中，其具体包括子步骤：

S401、根据目标期望位置信息、目标期望指向信息、末端当前位置信息和末端当前指向信息获得位置信息差和指向信息差；

具体的，位置信息差记为：

ΔP＝P_d1-P_e (16)

指向信息差记为：

ΔO＝e_n·δ (17)

其中，

e_n＝X_e×X_d1 (18)

δ＝arccos(X_e·X_d1) (19)

S402、根据位置信息差、指向信息差及雅可比矩阵计算得出末端期望线速度和末端期望角速度；

具体的，根据位置信息差和指向信息差，得到超冗余机械臂的等效臂形的末端线期望速度和末端期望角速度为：

其中，v_d为末端期望速度的线速度，ω_d为末端期望速度的角速度，K_p表示比例系数矩阵，ΔP₁表示超冗余机械臂的等效臂形的末端当前位置信心到达第一期望节点的位置信息差，ΔO₁表示超冗余机械臂的等效臂形的末端当前指向信息到达第一期望节点的指向信息差。

步骤S50中，基于雅可比矩阵不断迭代末端期望线速度和所述末端期望角速度获得各个臂段的期望臂形参数，具体为：

根据(14)式和(20)式可计算末端驱动臂形参数Θ_d：

其中，k_v表示微分比例系数。

步骤S60中，根据期望臂形参数获得关节驱动参数，并根据关节驱动参数将超冗余机械臂向目标期望节点驱动具体包括：

将超冗余机械臂向目标期望节点驱动即是将当前超冗余机械臂的当前等效臂形参数α_i和φ_i映射到超冗余机械臂实际的关节驱动参数β_i1和β_i2，其中β_i1表示第i个臂段的第一个实际的关节驱动参数，β_i2表示第i个臂段的第二个实际的关节驱动参数。

以上述超冗余机械臂的末端到达第一期望节点(当前的目标期望节点)为例进行说明，当i＝1时，即已知α₁和φ₁，则可知第一臂段(最远离超冗余机械臂的末端的臂段)所在的平面法向量相对于超冗余机械臂本体坐标系表示为：

则第一臂段在平面内弯曲角度的余弦值为：

根据第一臂段的实际驱动参数β₁₁和β₁₂可得第一臂段所在平面的法向量相对于本体坐标系表示为：

根据第一臂段的实际驱动参数θ₁₁和θ₁₂可得第一臂段所在平面内弯曲角度的余弦值为：

其中，

则根据(23)、(24)、(25)和(26)式可得：

因此，可进一步求得第一臂段向第一期望节点的实际关节驱动参数：

获取到第一臂段向目标期望节点的实际驱动参数后(即i＝1)，而当i＝2、3……N时，可得第i臂段起始关节相对于惯性系的实际旋转变换矩阵为：

其中，

根据(4)式可得，采用当前等效臂形参数表示第i臂段起始关节相对于惯性系的实际旋转变换矩阵为：

从而可得，第i臂段所在平面的法向量相对于超冗余机械臂本体坐标系表示为：

n_i＝(R_i)^T·(A_i·[0 1 0]^T) (33)

另外，第i臂段所在平面的法向量相对于超冗余机械臂本体坐标系还可表示为：

且第i臂段在其所在的平面内弯曲角度的余弦值为：

联立方程(33)、(34)和(35)可得：

进而求得：

其中，n_i(2)和n_i(3)表示第i臂段所在平面法向量n_i中第2个元素和第3个元素。

步骤S70中，以下一个期望节点为目标期望节点，并重复上述超冗余机械臂的等效臂形的末端期望线速度和末端期望角速度求解计算过程、各个关节驱动参数的求解计算过程、关节驱动参数求解过程和超冗余机械臂驱动过程，直至遍历完所有期望路径上的期望节点，最终将超冗余机械臂的末端向目标探测空间驱动。

综上所述，本发明实施例中一种超冗余机械臂的路径规划方法，其解决了现有技术中超冗余机械臂的路径规划方法计算繁杂、效率低的技术问题，提供了一种高效的超冗余机械臂路径规划方法。

实施例二：

本发明实施例中，基于实施例一中所述的超冗余机械臂路径规划方法，其在步骤S50之后步骤S60之前还包括：

S510、根据期望臂形参数获得末端期望位置信息和末端期望指向信息；

S511、设定误差范围要求，当末端期望位置信息与目标期望位置信息的误差满足误差范围要求，且当末端期望指向信息与目标期望指向信息的误差满足误差范围要求时，执行步骤S60，否则返回执行步骤S20。

具体的，在获得当前等效臂形参数后，获得超冗余机械臂的等效臂形的末端当前位置信息和末端当前指向信息，并且设定超冗余机械臂的等效臂形的末端与目标期望节点的位置信息差和指向信息差的误差范围要求，两者的差处于误差范围要求中时，进入下一步骤S60；否则，需返回步骤20重新获取超冗余机械臂的等效臂形的当前等效臂形参数，进而得到新的末端当前等效臂形参数直至满足末端期望位置信息与目标期望位置信息的差以及末端期望指向信息与目标期望指向信息的差满足误差范围要求才可进入下一步骤S60。综上，通过对每个期望节点进行判断是否满足误差要求范围，保证了超冗余机械臂的路径规划的精确性。

实施例三：

参照图5，本发明实施例提供了一种超冗余机械臂的路径规划装置，其中，超冗余机械臂包括多个臂段，多个臂段均包括多个小节，多个小节两两之间通过关节连接，所述臂段内的多个关节按顺序进行标号，且将标号为奇数的关节设置为Pitch-Yaw型，标号为偶数的关节设置为Yaw-Pitch型，其包括：

臂形描述参数获取及期望路径规划模块，用于获取臂段的臂形描述参数并规划超冗余机械臂的期望路径，在期望路径上取多个期望节点，且分别设有一个期望节点处于期望路径的首端和末端，并以第一个期望节点为目标期望节点；

当前等效臂形参数获取模块，用于根据臂形描述参数获得机械臂的当前等效臂形参数；

参数信息计算获取模块，用于获取目标期望节点的目标期望位置信息和目标期望指向信息，并根据前等效臂形参数推获得超冗余机械臂的等效臂形的末端当前位置信息、末端当前指向信息和雅可比矩阵；

末端期望速度获取模块，用于根据末端当前位置信息、末端当前指向信息、雅可比矩阵、目标期望位置信息和目标期望指向信息获得末端期望线速度和末端期望角速度；

期望臂形参数获取模块，用于基于雅可比矩阵不断迭代末端期望线速度速度和末端期望角速度获得各个臂段的期望臂形参数；

驱动控制模块，根据期望臂形参数获得关节驱动参数，并根据关节驱动参数将超冗余机械臂向目标期望节点驱动；

期望节点遍历模块，用于以下一个期望节点为目标期望节点，重复当前等效臂形参数获取模块、参数信息计算获取模块、末端期望速度获取模块、期望臂形参数获取模块和驱动控制模块的工作直至遍历完所有期望节点。

其中，末端期望速度获取模块具体包括：

信息误差计算单元，用于根据目标期望位置信息、目标期望指向信息、末端当前位置信息和末端当前指向信息获得位置信息差和指向信息差；

末端期望速度计算单元，用于根据位置信息差、指向信息差及雅可比矩阵计算得出末端期望速度。

本发明实施例中提供的一种超冗余机械臂的路径规划装置，其实现的过程原理可与实施例一中的超冗余机械臂的路径规划方法实现的过程原理相互参照对应，在此不做赘述。

本发明实施中一种超冗余机械臂的路径规划装置，其解决了现有技术中超冗余机械臂的路径规划装置计算量繁杂、低效的技术问题，提供了一种高效的超冗余机械臂的路径规划装置。

实施例四：

本发明实施中，超冗余机械臂的路径规划装置还包括：还包括条件判断模块；条件判断模块具体包括：

末端参数信息获取单元，根据期望臂形参数获得末端期望位置信息和末端期望指向信息；

判断选择执行单元，用于设定误差范围要求，当末端期望位置信息与期望位置信息的误差满足所述误差范围要求，且当末端期望指向信息与期望指向信息的误差满足误差范围要求时，执行驱动控制模块的工作，否则返回执行当前等效臂形参数获取模块的工作。

本发明实施例中通过设置有条件判断模块，其提高了超冗余机械臂的路径规划装置的路径规划精度，使得超冗余机械臂在超冗余机械臂的路径规划装置规划的路径下能够精确到达目标探测空间。

实施例五：

本发明实施例提供了一种超冗余机械臂的路径控制系统，其包括超冗余机械臂本体和用于控制该超冗余机械臂本体的驱动器，该驱动器用于执行如实施例一或实施例二所述的超冗余机械臂的路径规划方法，其解决了现有技术中超冗余机械臂的路径控制系统在进行狭窄环境探测时，其计算量大或路径规划精度不高的技术问题，提供了一种高效且精度高的超冗余机械臂的路径控制系统。

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

Claims (7)

1.一种超冗余机械臂的路径规划方法，所述超冗余机械臂包括多个臂段，多个所述臂段均包括多个小节，多个所述小节间依次通过关节连接，其特征在于，包括步骤：

S10、获取所述臂段的臂形描述参数并规划所述超冗余机械臂的期望路径，在所述期望路径上取多个期望节点，且分别设有一个所述期望节点处于所述期望路径的首端和末端，并以第一个所述期望节点为目标期望节点；

S20、根据所述臂形描述参数获得所述机械臂的当前等效臂形参数；

S30、获取所述目标期望节点的目标期望位置信息和目标期望指向信息，并根据所述当前等效臂形参数递推获得所述超冗余机械臂的等效臂形的末端当前位置信息、末端当前指向信息和雅可比矩阵；

S40、根据所述末端当前位置信息、所述末端当前指向信息、所述雅可比矩阵、所述目标期望位置信息和所述目标期望指向信息获得末端期望线速度和末端期望角速度；

S50、基于所述雅可比矩阵不断迭代所述末端期望线速度和所述末端期望角速度获得各个所述臂段的期望臂形参数；

S60、根据所述期望臂形参数获得关节驱动参数，并根据所述关节驱动参数将所述超冗余机械臂向所述目标期望节点驱动；

S70、以下一个所述期望节点为所述目标期望节点，重复所述步骤S20至所述步骤S60直至遍历完所有所述期望节点；

所述步骤S50之后所述步骤S60之前还包括步骤：

S510、根据所述期望臂形参数获得末端期望位置信息和末端期望指向信息；

S511、设定误差范围要求，当所述末端期望位置信息与所述目标期望位置信息的误差满足所述误差范围要求，且当所述末端期望指向信息与所述目标期望指向信息的误差满足所述误差范围要求时，执行所述步骤S60，否则返回执行所述步骤S20。

2.根据权利要求1所述的超冗余机械臂的路径规划方法，其特征在于，所述步骤S40具体包括子步骤：

S401、根据所述目标期望位置信息、所述目标期望指向信息、所述末端当前位置信息和所述末端当前指向信息获得位置信息差和指向信息差；

S402、根据所述位置信息差、所述指向信息差及所述雅可比矩阵计算得出所述末端期望线速度和所述末端期望角速度。

3.根据权利要求1或2任一项所述的超冗余机械臂的路径规划方法，其特征在于，分别对多个所述臂段内的多个所述关节按顺序进行标号，且将所述标号为奇数的所述关节设置为Pitch-Yaw型，所述标号为偶数的所述关节设置为Yaw-Pitch型。

4.一种超冗余机械臂的路径规划装置，所述超冗余机械臂包括多个臂段，多个所述臂段均包括多个小节，多个所述小节两两之间通过关节连接，多个所述臂段内的所述关节按顺序进行标号，且将所述标号为奇数的所述关节设置为Yaw-Pitch型，所述标号为偶数的所述关节设置为Pitch-Yaw型，其特征在于，包括：

臂形描述参数获取及期望路径规划模块，用于获取所述臂段的臂形描述参数并规划所述超冗余机械臂的期望路径，在所述期望路径上取多个期望节点，且分别设有一个所述期望节点处于所述期望路径的首端和末端，并以第一个所述期望节点为目标期望节点；

当前等效臂形参数获取模块，用于根据所述臂形描述参数获得所述机械臂的当前等效臂形参数；

参数信息计算获取模块，用于获取所述目标期望节点的目标期望位置信息和目标期望指向信息，并根据所述前等效臂形参数推获得所述超冗余机械臂的等效臂形的末端当前位置信息、末端当前指向信息和雅可比矩阵；

末端期望速度获取模块，用于根据所述末端当前位置信息、所述末端当前指向信息、所述雅可比矩阵、所述目标期望位置信息和所述目标期望指向信息获得末端期望线速度和末端期望角速度；

期望臂形参数获取模块，用于基于所述雅可比矩阵不断迭代所述末端期望线速度速度和所述末端期望角速度获得各个所述臂段的期望臂形参数；

驱动控制模块，根据所述期望臂形参数获得关节驱动参数，并根据所述关节驱动参数将所述超冗余机械臂向所述目标期望节点驱动；

期望节点遍历模块，用于以下一个所述期望节点为所述目标期望节点，重复所述当前等效臂形参数获取模块、所述参数信息计算获取模块、末端期望速度获取模块、所述期望臂形参数获取模块和所述驱动控制模块的工作直至遍历完所有所述期望节点。

5.根据权利要求4所述的超冗余机械臂的路径规划装置，其特征在于，所述末端期望速度获取模块具体包括：

信息误差计算单元，用于根据所述目标期望位置信息、所述目标期望指向信息、所述末端当前位置信息和所述末端当前指向信息获得位置信息差和指向信息差；

末端期望速度计算单元，用于根据所述位置信息差、所述指向信息差及所述雅可比矩阵计算得出所述末端期望速度。

6.根据权利要求5所述的超冗余机械臂的路径规划装置，其特征在于，所述超冗余机械臂的路径规划装置还包括条件判断模块；所述条件判断模块具体包括：

末端参数信息获取单元，根据所述期望臂形参数获得末端期望位置信息和末端期望指向信息；

判断选择执行单元，用于设定误差范围要求，当所述末端期望位置信息与所述期望位置信息的误差满足所述误差范围要求，且当所述末端期望指向信息与所述期望指向信息的误差满足所述误差范围要求时，执行所述驱动控制模块的工作，否则返回执行所述当前等效臂形参数获取模块的工作。

7.一种超冗余机械臂的路径控制系统，其特征在于，包括超冗余机械臂本体和用于控制所述机械臂运动的驱动器，所述驱动器用于执行如权利要求1至3任一项所述的超冗余机械臂的路径规划方法控制所述机械臂本体的工作。

Images