CN110757453A

CN110757453A - 一种超冗余联动机械臂的运动轨迹控制方法、装置及系统

Info

Publication number: CN110757453A
Application number: CN201910954129.9A
Authority: CN
Inventors: 徐文福; 胡忠华; 刘天亮; 梁斌
Original assignee: Shenzhen Graduate School Harbin Institute of Technology
Current assignee: Shenzhen Graduate School Harbin Institute of Technology
Priority date: 2019-10-09
Filing date: 2019-10-09
Publication date: 2020-02-07
Anticipated expiration: 2039-10-09
Also published as: CN110757453B

Abstract

本发明公开了一种超冗余联动机械臂的运动轨迹控制方法、装置及系统，其中，方法通过获取超冗余联动机械臂的结构信息，并根据结构信息建立空间双圆弧约束方程，根据该空间双圆弧约束方程分别建立机械臂第一约束方程和机械臂第二约束方程，再根据第一约束方程和第二约束方程建立用于表示超冗余联动机械臂的圆弧曲线的机械臂全局约束方程，对机械臂全局约束方程进行求解获得各个臂段的驱动关节角度后，根据该驱动关节角度驱动超冗余联动机械臂向规划轨迹运动；解决了现有技术中超冗余机械臂逆解的计算量大，从而降低了控制超冗余机械臂在规划路径上运动的效率，提供了一种高效的、可靠的超冗余联动机械臂的运动轨迹控制方法。

Description

一种超冗余联动机械臂的运动轨迹控制方法、装置及系统

技术领域

本发明涉及机械臂控制技术领域，尤其是涉及一种超冗余联动机械臂的运动轨迹控制方法、装置及系统。

背景技术

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

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

发明内容

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。为此，本发明的一个目的是提供一种高效的、可靠的超冗余联动机械臂的运动轨迹控制方法。

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

第一方面，本发明提供一种超冗余联动机械臂的运动轨迹控制方法，所述超冗余联动机械臂的结构信息包括：具有第一臂段、第二臂段、第三臂段、第四臂段和第五臂段，每个所述臂段依次连接，每个所述臂段均包括数量相同的多个小节，同一个所述臂段中的所述多个小节具有相等的长度，所述多个小节依次通过关节连接，分别对每个所述臂段内的所述关节按顺序进行标号，且将所述标号为奇数的所述关节设置为Pitch-Yaw型，所述标号为偶数的所述关节设置为Yaw-Pitch型，其包括：

根据所述结构信息建立空间双圆弧约束方程；

根据所述空间双圆弧约束方程建立机械臂第一约束方程，所述机械臂第一约束方程用于表示所述第四臂段和所述第五臂段构成的双圆弧曲线；

根据所述空间双圆弧约束方程建立机械臂第二约束方程，所述机械臂第二约束方程用于表示所述第一臂段和所述第二臂段构成的双圆弧曲线；

根据所述机械臂第一约束方程、所述机械臂第二约束方程建立机械臂全局约束方程，所述机械臂全局约束方程用于表示所述超冗余联动机械臂的圆弧曲线；

根据所述机械臂全局约束方程分别求解5个所述臂段的驱动关节角度，并根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动。

进一步地，根据所述空间双圆弧约束方程建立机械臂第一约束方程具体包括：

规划第五臂段末端的第五臂段期望位置信息和第五臂段期望指向信息，并根据所述结构信息、所述第五臂段期望位置信息和所述第五臂段期望指向信息规划所述超冗余联动机械臂的第三臂段末端的第三臂段期望位置信息；

将所述第三臂段末端的期望指向信息进行参数化得到第三臂段期望指向信息表达式；

根据所述超冗余联动机械臂的第四臂段参数信息、所述超冗余联动机械臂的第五臂段参数信息、所述第五臂段期望位置信息、所述第五臂段期望指向信息、所述空间双圆弧约束方程和所述第三臂段期望指向信息表达式建立所述机械臂第一约束方程。

进一步地，所述第四臂段参数信息包括所述超冗余联动机械臂的第四臂段中单个所述小节对应的第四臂段单小节长度，以及所述第四臂段所对应的第四臂段圆心角；

所述第五臂段参数信息包括所述超冗余联动机械臂的第五臂段中单个所述小节对应的第五臂段单小节长度，以及所述第五臂段所对应的第五臂段圆心角。

进一步地，，根据所述空间双圆弧约束方程建立机械臂第二约束方程具体包括：

根据所述第三臂段期望位置信息和所述第三臂段期望指向信息表达式获取第二臂段末端的第二臂段期望位置信息和第二臂段期望指向信息；

根据所述超冗余联动机械臂的第一臂段参数信息、所述超冗余联动机械臂的第二臂段参数信息、所述第二臂段期望位置信息、所述第二臂段期望指向信息、所述空间双圆弧约束方程和所述第三臂段期望指向信息表达式建立所述机械臂第二约束方程。

进一步地，所述第一臂段参数信息包括所述超冗余联动机械臂的第一臂段中单个所述小节对应的第一臂段单小节长度、所述第一臂段所对应的第一臂段圆心角、第一臂段起始位置信息、第一臂段起始指向信息；

所述第二臂段参数信息包括所述超冗余联动机械臂的第二臂段中单个所述小节对应的第二臂段单小节长度，以及所述第二臂段所对应的第二臂段圆心角。

进一步地，根据所述机械臂全局约束方程分别求解5个所述臂段的驱动关节角度，并根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动具体包括：

求解所述机械臂全局约束方程获得所述超冗余联动机械臂的各个臂段末端的各个期望位置信息和各个期望指向信息；

基于各个所述期望位置信息和各个所述期望指向信息通过数学求解方法计算出各个所述臂段的各个所述驱动关节角度；

根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动。

第二方面，本发明提供一种超冗余联动机械臂的运动轨迹控制装置，所述超冗余联动机械臂的结构信息包括：具有第一臂段、第二臂段、第三臂段、第四臂段和第五臂段，每个所述臂段依次连接，每个所述臂段均包括数量相同的多个小节，同一个所述臂段中的所述多个小节具有相等的长度，所述多个小节依次通过关节连接，分别对每个所述臂段内的所述关节按顺序进行标号，且将所述标号为奇数的所述关节设置为Pitch-Yaw型，所述标号为偶数的所述关节设置为Yaw-Pitch型，其包括：

空间双圆弧约束方程建立模块，用于根据所述结构信息建立空间双圆弧约束方程；

机械臂第一约束方程建立模块，用于根据所述空间双圆弧约束方程建立机械臂第一约束方程，所述机械臂第一约束方程用于表示所述第四臂段和所述第五臂段构成的双圆弧曲线；

机械臂第二约束方程建立模块，用于根据所述空间双圆弧约束方程建立机械臂第二约束方程，所述机械臂第二约束方程用于表示所述第一臂段和所述第二臂段构成的双圆弧曲线；

全局约束方程建立模块，用于根据所述机械臂第一约束方程、所述机械臂第二约束方程建立机械臂全局约束方程，所述机械臂全局约束方程用于表示所述超冗余联动机械臂的圆弧曲线；

驱动关节角度求解模块，用于根据所述机械臂全局约束方程分别求解所述超冗余联动机械臂的5个所述臂段的驱动关节角度；

驱动控制模块，用于根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动。

进一步地，所述机械臂第一约束方程建立模块包括：

第五臂段期望位置信息规划单元，用于规划第五臂段末端的第五臂段期望位置信息；

第五臂段期望指向信息规划单元，用于规划所述第五臂段末端的第五臂段期望指向信息；

第三臂段期望位置信息规划单元，用于根据所述结构信息、所述第五臂段期望位置信息和所述第五臂段期望指向信息规划所述超冗余联动机械臂的第三臂段末端的第三臂段期望位置信息；

第三臂段期望指向信息表达式获取单元，用于将所述第三臂段末端的期望指向信息进行参数化得到第三臂段期望指向信息表达式；

第四臂段参数信息获取单元，用于获取所述超冗余联动机械臂的第四臂段参数信息；

第五臂段参数信息获取单元，用于获取所述超冗余联动机械臂的第五臂段参数信息；

机械臂第一约束方程生成单元，用于根据所述超冗余联动机械臂的第四臂段参数信息、所述超冗余联动机械臂的第五臂段参数信息、所述第五臂段期望位置信息、所述第五臂段期望指向信息、所述空间双圆弧约束方程和所述第三臂段期望指向信息表达式建立所述机械臂第一约束方程。

进一步地，所述机械臂第二约束方程建立模块包括：

第二臂段期望位置信息获取单元，用于根据所述第三臂段期望位置信息和所述第三臂段期望指向信息表达式获取第二臂段末端的第二臂段期望位置信息；

第二臂段期望指向信息获取单元，用于根据所述第三臂段期望位置信息和所述第三臂段期望指向信息表达式获取第二臂段末端的第二臂段期望指向信息；

第一臂段参数信息获取单元，用于获取所述超冗余联动机械臂的第一臂段参数信息；

第二臂段参数信息获取单元，用于获取所述超冗余联动机械臂的第二臂段参数信息；

机械臂第二约束方程生成单元，用于根据所述第一臂段参数信息、所述第二臂段参数信息、所述第二臂段期望位置信息、所述第二臂段期望指向信息、所述空间双圆弧约束方程和所述第三臂段期望指向信息表达式建立所述机械臂第二约束方程。

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

本发明的有益效果是：

本发明实施例中一种超冗余联动机械臂的运动轨迹控制方法，通过获取超冗余联动机械臂的结构信息，并根据结构信息建立空间双圆弧约束方程，根据该空间双圆弧约束方程分别建立用于表示第四臂段和第五臂段的机械臂第一约束方程，以及用于表示第一臂段和第二臂段的机械臂第二约束方程，根据第一约束方程和第二约束方程建立用于表示超冗余联动机械臂的圆弧曲线的机械臂全局约束方程，对机械臂全局约束方程进行求解获得各个臂段的驱动关节角度后根据该驱动关节角度驱动超冗余联动机械臂向规划轨迹运动；解决了现有技术中由于超冗余机械臂的自由度较多，导致其运动学方程非常复杂，进而导致超冗余机械臂逆解的计算量急剧增加，从而降低了控制超冗余机械臂在规划路径上运动的效率，提供了一种高效的、可靠的超冗余联动机械臂的运动轨迹控制方法。

附图说明

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

图2是本发明实施例中超冗余联动机械臂的第一臂段的一具体实施例关节配置示意图；

图3是本发明实施例中一种超冗余联动机械臂的运动轨迹控制方法的一具体实施例流程示意图；

图4是本发明实施例中建立空间双圆弧约束方程的一具体实施例结构见图；

图5是本发明实施例中一种超冗余联动机械臂的运动轨迹控制装置的一具体实施例模块框图。

具体实施方式

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

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

实施例一：

如图1所示，图1为本发明实施例一种超冗余联动机械臂的运动轨迹控制方法中超冗余联动机械臂的一具体结构示意图，在本实施例中，超冗余联动机械臂包括第一臂段、第二臂段、第三臂段、第四臂段和第五臂段共5个臂段，每个臂段均包括6个小节，且同一个臂段中的小节具有相等的长度(不同臂段中的小节的长度可能不相等)，每个小节一一对应一个关节，两个小节之间依次通过关节连接(即臂段间通过同样的关节连接)。其中B₀为超冗余联动机械臂的安装基座，5个臂段按顺序进行标号，离安装基座最近的为第一臂段，离安装基座最远的为第五臂段，P_b、t_b为超冗余联动机械臂起始安装点(即第一臂段起始端)的位置信息和指向信息，P₁、t₁，P₂、t₂，P₃、t₃，P₄、t₄，P₅、t₅分别为超冗余联动机械臂各个臂段末端的位置信息和指向信息。参照图2，图2为本发明实施例中超冗余联动机械臂的第一臂段的一具体实施例关节示意图，其中，臂段内的6个关节按照顺序进行标号，即从1起按照顺序分别标号，形成1、2、3、4、5、6号关节，并将标号为奇数的关节设置为Pitch-Yaw型，即在惯性系O_I下先绕Z_I轴旋转，再绕Y_I轴旋转；将标号为偶数的关节设置为Yaw-Pitch型，即先绕Y_I轴旋转，再绕Z_I轴旋转。基于上述的结构，在机械臂联动作用下，使得超冗余联动机械臂的每个臂段都在各自的平面内产生等角度弯曲(不同臂段不一定处于相同的平面内)，因此，同一个臂段内的各个小节都在同一空间平面内，且同一臂段内的各个小节都为同一圆弧的切线，即该圆弧为该臂段内各个小节的内切圆。

参照图3，基于上述的超冗余联动机械臂，本发明实施例提供了一种超冗余联动机械臂的运动轨迹控制方法，其包括：

步骤S100、根据结构信息建立空间双圆弧约束方程；具体的，基于上述超冗余联动机械臂的结构信息，可知超冗余联动机械臂的第四臂端、第五臂段构成的双圆弧曲线，以及第一臂段、第二臂段构成的双圆弧曲线满足空间双圆弧约束方程。

则参照图4，图4表示三维空间中的一条双圆弧曲线，其中，

表示第一圆弧，

表示第二圆弧，

和

分别表示第一圆弧和第二圆弧的圆心，O₁和O₂分别表示第一圆弧和第二圆弧的圆心角，R₁和R₂分别表示第一圆弧和第二圆弧的半径，则基于上述可知该空间双圆弧曲线的已知量包括：

1、空间双圆弧的其实位置及指向(指向即为切线方向)：P₀₁和t₀₁；

2、空间双圆弧的终止位置及指向(指向即为切线方向)：P₄₁和t₄₁；

3、每个臂段内的小节长度L₁₁和L₂₁；

4、第一圆弧和第二圆弧等分成12份后，每一段小圆弧对应的圆形角满足：

5、第一圆弧和第二圆弧的交点为P₂₁(改点待求)，且两个圆弧在该交点处的切线方向相同。

因此，可通过求解空间双圆弧约束方程，从而确定一个双圆弧曲线同时满足上述5个已知量，具体过程包括：

过P₀₁点并沿t₀₁方向做一条直线l₁，然后P₄₁过点并沿t₄₁做一条直线l₃，假设过P₂₁点并沿其切线方向做一条直线l₂，此处，将直线和直线的交点记作P₁₁，把直线l₃和直线l₂的交点记作P₃₁。则根据圆弧切线的几何性质可得：

其中，k₁₁表示线段P₁₁P₀₁和线段P₁₁P₂₁的长度，k₂₁表示线段P₃₁P₂₁和线段P₃₁P₄₁的长度，因此，点P₁₁和点P₃₁的位置矢量为：

线段P₁₁P₃₁的长度为：

||P₁₁P₃₁||＝||P₁₁-P₃₁||＝||P₀₁+k₁₁t₀₁-(P₄₁-k₂₁t₄₁)||＝k₁₁+k₂₁ (4)

以空间双圆弧的起始点P₀₁为坐标系原点，则P₀₁＝[0 0 0]^Τ，联立(2)式和(3)式可得：

根据(3)式和(5)式可得：

在第一圆弧和第二圆弧中，可知：

则联立(1)式和(7)式可得：

参照图4，在三角形和中，根据余弦定理可知：

将(2)式代入(9)式中可得：

因此，根据(4)式、(8)式和(10)式可得空间双圆弧约束方程为：

其中，P₀₁、t₀₁、P₄₁和t₄₁为前提已知条件，k₁₁、k₂₁、和

是待求量。同时，根据(6)式可知，P₂₁P₀₁和P₂₁P₄₁是k₁₁、k₂₁、

和

的函数，因此，通过(11)式，可将空间双圆弧求解问题转化为非线性约束方程组的求解。

步骤S200、根据空间双圆弧约束方程建立机械臂第一约束方程，机械臂第一约束方程用于表示第四臂段和第五臂段构成的双圆弧曲线；具体的，根据步骤S100中建立的空间双圆弧约束方程可知，要建立机械臂第一约束方程需要：第四臂段参数信息、第五臂段参数信息、规划第五臂段末端的第五臂段期望位置信息P_e5和第五臂段期望指向信息t_e5，其中，第四臂段参数信息包括第四臂段的小节长度和第四臂段对应的圆心角，第五臂段参数信息包括第五臂段的小节长度和第五臂段对应的圆形角，根据上述超冗余联动机械臂的结构信息、第五臂段期望位置信息P_e5和第五臂段期望指向信息t_e5规划超冗余联动机械臂的第三臂段末端的第三臂段期望位置信息P_e3，使得基于上述超冗余联动机械臂的结构信息第五臂段期望位置信息P_e5、第五臂段期望指向信息t_e5和第三臂段期望位置信息P_e3能够满足超冗余联动机械臂的一个规划轨迹，再通过将第三臂段末端的期望指向信息t_e3进行参数化得到第三臂段期望指向信息表达式，由于第三臂段末端的指向可以是三维空间的任意方向，因此，其可表示为：

其中，

则根据(11)式可知，机械臂第一约束方程为：

此时，在(14)式中，

其中，L₄和L₅分别表示第四臂段的小节长度和第五臂段的小节长度，

和

分别表示第四臂段对应的圆心角和第五臂段对应的圆形角。

则，结合(12)式、(13)式和(14)式可知，t_e3与参数t_{e3_Ang1}和参数t_{e3_Ang2}相关，因此，(14)式中的||P₂₁P₀₁||和||P₂₁P₄₁||也与参数t_{e3_Ang1}和参数t_{e3_Ang2}相关，可知非线性约束方程组(机械臂第一约束方程)可记为：

步骤S300、根据空间双圆弧约束方程建立机械臂第二约束方程，机械臂第二约束方程用于表示第一臂段和第二臂段构成的双圆弧曲线。

其包括：

根据第三臂段期望位置信息和第三臂段期望指向信息表达式获取第二臂段末端的第二臂段期望位置信息和第二臂段期望指向信息；具体的，根据第三臂段期望位置信息和第三臂段期望指向信息(即第三臂段期望指向信息表达式)可得到过点P_e3沿t_e3方向的直线方程为：

其中，P_e3(i)，i＝1，2，3表示向量P_e3的第i个元素，t_e3(i)，i＝1，2，3表示向量t_e3的第i个元素。

因此，过如(17)式所表示的直线方程的平面束S_p可以表示为：

(1)当t_e3(1)≠0时，

(17)式表示的直线方程可以表示：

根据(18)式可得平面束方程为：

(t_e3(2)+λt_e3(3))·X-t_e3(1)·Y-λt_e3(1)·Z+D₁＝0 (19)

其中，D₁＝-t_e3(2)P_e3(1)+t_e3(1)P_e3(2)-λ(t_e3(3)P_e3(1)-t_e3(1)P_e3(3))。

则，此时平面束中任意平面的法向量可以表示为：

n＝[t_e3(2)+λt_e3(3) -t_e3(1) -λt_e3(1)],λ∈[-∞ +∞] (20)

(2)当t_e3(2)≠0时，可得平面束方程为：

t_e3(2)·X-(t_e3(1)-λt_e3(3))·Y+λt_e3(2)·Z+D₂＝0 (21)

其中，D₂＝t_e3(1)P_e3(2)-t_e3(2)P_e3(1)+λ(t_e3(3)P_e3(2)-t_e3(2)P_e3(3))。

此时，平面束中任意平面的法向量可以表示为：

n＝[t_e3(2) -(t_e3(1)-λt_e3(3)) λt_e3(2)],λ∈[-∞ +∞] (22)

(3)当t_e3(3)≠0时，可得平面束方程为：

t_e3(3)·X+λt_e3(3)·Y-(t_e3(1)-λt_e3(2))·Z+D₃＝0 (23)

其中，D₃＝t_e3(1)P_e3(3)-t_e3(1)P_e3(3)+λ(t_e3(2)P_e3(3)-t_e3(3)P_e3(2))。

此时，平面束中任意平面的法向量可以表示为：

n＝[t_e3(3) λt_e3(3) -(t_e3(1)-λt_e3(2))],λ∈[-∞ +∞] (24)

由于(17)式表示的直线过点沿方向的直线也在第三臂段所在的平面上，因此根据(20)式或(22)式或(24)式得到第三臂段所在平面的参数化法向量n₃。

因此，第二臂段期望位置信息P_e2和第二臂段期望指向信息t_e2为：

其中，

为第三臂段对应的圆心角，L₃为第三臂段中各小节的长度，表示轴角公式，具体计算方式为：

其中，I_3×3表示阶数为3的单位矩阵，若n₃＝[n_3x n_3y n_3z]，则

将第一臂段和第二臂段构成的双圆弧曲线进行求解，则通过(25)式可得第二臂段末端的第二臂段期望位置参数信息P_e2和第二臂段期望指向信息t_e2，并结合第一臂段参数信息，其包括第一臂段中单个第一臂段单小节长度L₁、第一臂段所对应的第一臂段圆心角第一臂段起始位置信息、第一臂段起始指向信息，则此时双圆弧曲线的起始点为第一臂段起始位置，即也是超冗余联动机械臂的安装位置点，则该点的位置信息为P_b(即第一臂段起始位置信息)，指向信息为t_b(即第一臂段起始指向信息)这两个量为已知量，第二臂段参数信息，其包括第二臂段中单个第二臂段单小节长度L₂和第一臂段所对应的第一臂段圆心角

结合(11)式可得机械臂第二约束方程为：

在(28)式中，

结合(20)式、(22)式、(24)式和(25)式可知，P_e2和t_e2与参数、参数

和参数t_e3相关，因此(28)式中的||P₂₁P₀₁||和||P₂₁P₄₁||也与参数λ、参数

和参数t_e3相关，因此，非线性约束方程组(28)，即机械臂第二约束方程可记为：

步骤S400、根据所述机械臂第一约束方程、所述机械臂第二约束方程建立机械臂全局约束方程，所述机械臂全局约束方程用于表示所述超冗余联动机械臂的圆弧曲线。

具体的，根据(16)式和(30)式可以得到用于表示超冗余联动机械臂的圆弧曲线的机械臂全局约束方程：

其中，0^10×1表示10行1列的向量且各个元素均为0。

步骤S500、根据机械臂全局约束方程分别求解5个臂段的驱动关节角度，并根据各个驱动关节角度驱动超冗余联动机械臂向规划轨迹运动。

其包括：

求解(31)式表示的机械臂全局约束方程获得超冗余联动机械臂的各个臂段末端的各个期望位置信息和各个期望指向信息。具体的，可以通过牛顿-拉夫逊(拉弗森)方法进行求解，其具体过程如下：

(1)令设定初值点x₀，计算得F_sum(x₀)。

(2)设定阈值ε，例如ε＝0.00001。判断||F_sum(x₀)||<ε是否成立，若成立则输出x₀，否则计算方程组F_sum(x)的偏导数：

其中，

F_sum(x)＝[f₁ f₂ f₃ f₄ f₅ f₆ f₇ f₈ f₉ f₁₀]^Τ (33)

(3)根据(32)式迭代得到下一步的参数值：

其中，pinv(·)表示求矩阵的伪逆运算。

(4)将x₁赋值于x₀，返回上述步骤(1)，直至||F_sum(x₀)||<ε成立，输出x₀即为方程组F_sum(x₀)＝0的解。

(31)式中各个参数通过数值方法求解得到后，可通过计算得到各个臂段末端的期望位置信息P_ei(i＝1,2,3,4)和期望指向信息t_ei(i＝1,2,3,4)。

基于上述的各个期望位置信息和期望指向信息，可通过数学求解方法计算出各个臂段的各个关节角度，其包括：

(1)当i＝1时，

可得第一臂段所在平面的法向量相对于本体坐标系(即以该臂段起始端为原点建立的坐标系)为：

将第一臂段驱动关节角度设为θ₁₁和θ₁₂，可得第一臂段在其所在平面内的法向量相对于本体坐标系表示为：

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

则根据第一臂段驱动关节角度θ₁₁和θ₁₂可得第一臂段在其所在平面内弯曲角度余弦值为：

其中，

根据(36)式、(37)式、(38)式和(39)式可得：

则可求得第一臂段驱动关节角度θ₁₁和θ₁₂为：

(2)当i＝2,3,4,5时

根据上述(1)中的内容可知，均可求得第i-1的驱动关节角度，因此第i臂段起始端坐标系(以第i臂段起始端为原点建立的坐标系)相对于超冗余联动机械臂安装坐标系(即以机械臂安装位置为原点建立的坐标系，其余上述惯性坐标系一致)的旋转变换矩阵为：

其中，

则可得第i臂段所在平面的法向量相对于本体坐标系表示为：

将第i臂段的驱动关节角度设为θ_i1和θ_i2，则可得第i臂段所在平面的法向量相对于本体坐标系表示为：

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

结合(44)式、(45)式和(46)式可得：

因此可求得：

根据(41)式和(48)式可求得超冗余联动机械臂各个臂段的驱动关节角度，将求得的驱动关节角度写入控制程序中控制超冗余联动机械臂向规划轨迹运动。

综上所述，本发明实施例中一种超冗余联动机械臂的运动轨迹控制方法，其，通过获取超冗余联动机械臂的结构信息，并根据结构信息建立空间双圆弧约束方程，根据该空间双圆弧约束方程分别建立用于表示第四臂段和第五臂段的机械臂第一约束方程，以及用于表示第一臂段和第二臂段的机械臂第二约束方程，根据第一约束方程和第二约束方程建立用于表示超冗余联动机械臂的圆弧曲线的机械臂全局约束方程，对机械臂全局约束方程进行求解获得各个臂段的驱动关节角度后根据该驱动关节角度驱动超冗余联动机械臂向规划轨迹运动；解决了现有技术中由于超冗余机械臂的自由度较多，导致其运动学方程非常复杂，进而导致超冗余机械臂逆解的计算量急剧增加，从而降低了控制超冗余机械臂在规划路径上运动的效率，提供了一种高效的、可靠的超冗余联动机械臂的运动轨迹控制方法。

实施例二：

参照图5，本发明实施例提供了一种超冗余联动机械臂的运动轨迹控制装置，用于控制如实施例一中所描述的超冗余联动机械臂，其包括：

空间双圆弧约束方程建立模块，用于根据结构信息建立空间双圆弧约束方程；

机械臂第一约束方程建立模块，用于根据空间双圆弧约束方程建立机械臂第一约束方程，机械臂第一约束方程用于表示第四臂段和第五臂段构成的双圆弧曲线；

机械臂第二约束方程建立模块，用于根据空间双圆弧约束方程建立机械臂第二约束方程，机械臂第二约束方程用于表示第一臂段和第二臂段构成的双圆弧曲线；

全局约束方程建立模块，用于根据机械臂第一约束方程、机械臂第二约束方程建立机械臂全局约束方程，机械臂全局约束方程用于表示超冗余联动机械臂的圆弧曲线；

驱动关节角度求解模块，用于根据机械臂全局约束方程分别求解超冗余联动机械臂的5个臂段的驱动关节角度；

驱动控制模块，用于根据各个驱动关节角度驱动超冗余联动机械臂向规划轨迹运动。

其中，机械臂第一约束方程建立模块包括：

第五臂段期望指向信息规划单元，用于规划第五臂段末端的第五臂段期望指向信息；

第三臂段期望位置信息规划单元，用于根据结构信息、第五臂段期望位置信息和第五臂段期望指向信息规划超冗余联动机械臂的第三臂段末端的第三臂段期望位置信息；

第三臂段期望指向信息表达式获取单元，用于将第三臂段末端的期望指向信息进行参数化得到第三臂段期望指向信息表达式；

第四臂段参数信息获取单元，用于获取超冗余联动机械臂的第四臂段参数信息；

第五臂段参数信息获取单元，用于获取超冗余联动机械臂的第五臂段参数信息；

机械臂第一约束方程生成单元，用于根据超冗余联动机械臂的第四臂段参数信息、超冗余联动机械臂的第五臂段参数信息、第五臂段期望位置信息、第五臂段期望指向信息、空间双圆弧约束方程和第三臂段期望指向信息表达式建立机械臂第一约束方程。

机械臂第二约束方程建立模块包括：

第二臂段期望位置信息获取单元，用于根据第三臂段期望位置信息和第三臂段期望指向信息表达式获取第二臂段末端的第二臂段期望位置信息；

第二臂段期望指向信息获取单元，用于根据第三臂段期望位置信息和第三臂段期望指向信息表达式获取第二臂段末端的第二臂段期望指向信息；

第一臂段参数信息获取单元，用于获取超冗余联动机械臂的第一臂段参数信息；

第二臂段参数信息获取单元，用于获取超冗余联动机械臂的第二臂段参数信息；

机械臂第二约束方程生成单元，用于根据第一臂段参数信息、第二臂段参数信息、第二臂段期望位置信息、第二臂段期望指向信息、空间双圆弧约束方程和第三臂段期望指向信息表达式建立机械臂第二约束方程。

本发明实施例中一种超冗余联动机械臂的运动轨迹控制装置其实现的过程原理可与实施例一中所述的超冗余联动机械臂的运动轨迹控制方法相互参照对应，在此不做赘述。

综上，本发明实施例提供了一种高效的、可靠的超冗余联动机械臂的运动轨迹控制装置。

实施例三：

本发明实施例提供了一种超冗余联动机械臂的运动轨迹控制系统，其包括：超冗余联动机械臂本体和用于控制所述超冗余机械臂本体运动的驱动控制器，所述驱动控制器用于执行如实施例一所述的超冗余联动机械臂的运动轨迹控制方法控制所述超冗余联动机械臂本体的工作。解决了现有技术中超冗余机械臂控制系统效率低的技术问题，提供了一种高效的、可靠的超冗余联动机械臂的运动轨迹控制系统。

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

Claims

1.一种超冗余联动机械臂的运动轨迹控制方法，所述超冗余联动机械臂的结构信息包括：具有第一臂段、第二臂段、第三臂段、第四臂段和第五臂段，每个所述臂段依次连接，每个所述臂段均包括数量相同的多个小节，同一个所述臂段中的所述多个小节具有相等的长度，所述多个小节依次通过关节连接，分别对每个所述臂段内的所述关节按顺序进行标号，且将所述标号为奇数的所述关节设置为Pitch-Yaw型，所述标号为偶数的所述关节设置为Yaw-Pitch型，其特征在于，包括：

根据所述结构信息建立空间双圆弧约束方程；

2.根据权利要求1所述的超冗余联动机械臂的运动轨迹控制方法，其特征在于，根据所述空间双圆弧约束方程建立机械臂第一约束方程具体包括：

3.根据权利要求2所述的超冗余联动机械臂的运动轨迹控制方法，其特征在于，

所述第四臂段参数信息包括所述超冗余联动机械臂的第四臂段中单个所述小节对应的第四臂段单小节长度，以及所述第四臂段所对应的第四臂段圆心角；

4.根据权利要求2或3所述的超冗余联动机械臂的运动轨迹控制方法，其特征在于，根据所述空间双圆弧约束方程建立机械臂第二约束方程具体包括：

5.根据权利要求4所述的超冗余联动机械臂的运动轨迹控制方法，其特征在于，所述第一臂段参数信息包括所述超冗余联动机械臂的第一臂段中单个所述小节对应的第一臂段单小节长度、所述第一臂段所对应的第一臂段圆心角、第一臂段起始位置信息、第一臂段起始指向信息；

6.根据权利要求1、2、3或5所述的超冗余联动机械臂的运动轨迹控制方法，其特征在于，根据所述机械臂全局约束方程分别求解5个所述臂段的驱动关节角度，并根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动具体包括：

7.一种超冗余联动机械臂的运动轨迹控制装置，所述超冗余联动机械臂的结构信息包括：具有第一臂段、第二臂段、第三臂段、第四臂段和第五臂段，每个所述臂段依次连接，每个所述臂段均包括数量相同的多个小节，同一个所述臂段中的所述多个小节具有相等的长度，所述多个小节依次通过关节连接，分别对每个所述臂段内的所述关节按顺序进行标号，且将所述标号为奇数的所述关节设置为Pitch-Yaw型，所述标号为偶数的所述关节设置为Yaw-Pitch型，其特征在于，包括：

8.根据权利要求7所述的超冗余联动机械臂的运动轨迹控制装置，其特征在于，所述机械臂第一约束方程建立模块包括：

9.根据权利要求8所述的超冗余联动机械臂的运动轨迹控制装置，其特征在于，所述机械臂第二约束方程建立模块包括：

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