CN110757453A - 一种超冗余联动机械臂的运动轨迹控制方法、装置及系统 - Google Patents
一种超冗余联动机械臂的运动轨迹控制方法、装置及系统 Download PDFInfo
- Publication number
- CN110757453A CN110757453A CN201910954129.9A CN201910954129A CN110757453A CN 110757453 A CN110757453 A CN 110757453A CN 201910954129 A CN201910954129 A CN 201910954129A CN 110757453 A CN110757453 A CN 110757453A
- Authority
- CN
- China
- Prior art keywords
- arm
- mechanical arm
- super
- constraint equation
- section
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种超冗余联动机械臂的运动轨迹控制方法、装置及系统,其中,方法通过获取超冗余联动机械臂的结构信息,并根据结构信息建立空间双圆弧约束方程,根据该空间双圆弧约束方程分别建立机械臂第一约束方程和机械臂第二约束方程,再根据第一约束方程和第二约束方程建立用于表示超冗余联动机械臂的圆弧曲线的机械臂全局约束方程,对机械臂全局约束方程进行求解获得各个臂段的驱动关节角度后,根据该驱动关节角度驱动超冗余联动机械臂向规划轨迹运动;解决了现有技术中超冗余机械臂逆解的计算量大,从而降低了控制超冗余机械臂在规划路径上运动的效率,提供了一种高效的、可靠的超冗余联动机械臂的运动轨迹控制方法。
Description
技术领域
本发明涉及机械臂控制技术领域,尤其是涉及一种超冗余联动机械臂的运动轨迹控制方法、装置及系统。
背景技术
随着科学技术的不断发展,许多的大型设备经研发生产后广泛应用于各个领域中。而由于大型设备往往都具有非常复杂的内部结构,因此造成了在对这些大型设备进行维修或检测时的操作空间都非常狭小。为了能够在狭小的操作空间内实现对这些大型设备的维修或检测工作,往往通过控制超冗余机械臂进入大型设备的内部中进行相应操作。超冗余机械臂具有穿越狭窄空间、回避障碍物、关节奇异和关节超限的能力,且超冗余机械臂具有较大的灵巧操作空间。
但是,由于超冗余机械臂的自由度较多,导致其运动学方程非常复杂,进而导致超冗余机械臂逆解的计算量急剧增加,从而降低了控制超冗余机械臂在规划路径上运动的效率。因此,如何解决由于超冗余机械臂的应用中在规划路径上运动控制效率低下的问题,成为本领域技术人员需克服的技术难题。
发明内容
本发明旨在至少在一定程度上解决相关技术中的技术问题之一。为此,本发明的一个目的是提供一种高效的、可靠的超冗余联动机械臂的运动轨迹控制方法。
本发明所采用的技术方案是:
第一方面,本发明提供一种超冗余联动机械臂的运动轨迹控制方法,所述超冗余联动机械臂的结构信息包括:具有第一臂段、第二臂段、第三臂段、第四臂段和第五臂段,每个所述臂段依次连接,每个所述臂段均包括数量相同的多个小节,同一个所述臂段中的所述多个小节具有相等的长度,所述多个小节依次通过关节连接,分别对每个所述臂段内的所述关节按顺序进行标号,且将所述标号为奇数的所述关节设置为Pitch-Yaw型,所述标号为偶数的所述关节设置为Yaw-Pitch型,其包括:
根据所述结构信息建立空间双圆弧约束方程;
根据所述空间双圆弧约束方程建立机械臂第一约束方程,所述机械臂第一约束方程用于表示所述第四臂段和所述第五臂段构成的双圆弧曲线;
根据所述空间双圆弧约束方程建立机械臂第二约束方程,所述机械臂第二约束方程用于表示所述第一臂段和所述第二臂段构成的双圆弧曲线;
根据所述机械臂第一约束方程、所述机械臂第二约束方程建立机械臂全局约束方程,所述机械臂全局约束方程用于表示所述超冗余联动机械臂的圆弧曲线;
根据所述机械臂全局约束方程分别求解5个所述臂段的驱动关节角度,并根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动。
进一步地,根据所述空间双圆弧约束方程建立机械臂第一约束方程具体包括:
规划第五臂段末端的第五臂段期望位置信息和第五臂段期望指向信息,并根据所述结构信息、所述第五臂段期望位置信息和所述第五臂段期望指向信息规划所述超冗余联动机械臂的第三臂段末端的第三臂段期望位置信息;
将所述第三臂段末端的期望指向信息进行参数化得到第三臂段期望指向信息表达式;
根据所述超冗余联动机械臂的第四臂段参数信息、所述超冗余联动机械臂的第五臂段参数信息、所述第五臂段期望位置信息、所述第五臂段期望指向信息、所述空间双圆弧约束方程和所述第三臂段期望指向信息表达式建立所述机械臂第一约束方程。
进一步地,所述第四臂段参数信息包括所述超冗余联动机械臂的第四臂段中单个所述小节对应的第四臂段单小节长度,以及所述第四臂段所对应的第四臂段圆心角;
所述第五臂段参数信息包括所述超冗余联动机械臂的第五臂段中单个所述小节对应的第五臂段单小节长度,以及所述第五臂段所对应的第五臂段圆心角。
进一步地,,根据所述空间双圆弧约束方程建立机械臂第二约束方程具体包括:
根据所述第三臂段期望位置信息和所述第三臂段期望指向信息表达式获取第二臂段末端的第二臂段期望位置信息和第二臂段期望指向信息;
根据所述超冗余联动机械臂的第一臂段参数信息、所述超冗余联动机械臂的第二臂段参数信息、所述第二臂段期望位置信息、所述第二臂段期望指向信息、所述空间双圆弧约束方程和所述第三臂段期望指向信息表达式建立所述机械臂第二约束方程。
进一步地,所述第一臂段参数信息包括所述超冗余联动机械臂的第一臂段中单个所述小节对应的第一臂段单小节长度、所述第一臂段所对应的第一臂段圆心角、第一臂段起始位置信息、第一臂段起始指向信息;
所述第二臂段参数信息包括所述超冗余联动机械臂的第二臂段中单个所述小节对应的第二臂段单小节长度,以及所述第二臂段所对应的第二臂段圆心角。
进一步地,根据所述机械臂全局约束方程分别求解5个所述臂段的驱动关节角度,并根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动具体包括:
求解所述机械臂全局约束方程获得所述超冗余联动机械臂的各个臂段末端的各个期望位置信息和各个期望指向信息;
基于各个所述期望位置信息和各个所述期望指向信息通过数学求解方法计算出各个所述臂段的各个所述驱动关节角度;
根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动。
第二方面,本发明提供一种超冗余联动机械臂的运动轨迹控制装置,所述超冗余联动机械臂的结构信息包括:具有第一臂段、第二臂段、第三臂段、第四臂段和第五臂段,每个所述臂段依次连接,每个所述臂段均包括数量相同的多个小节,同一个所述臂段中的所述多个小节具有相等的长度,所述多个小节依次通过关节连接,分别对每个所述臂段内的所述关节按顺序进行标号,且将所述标号为奇数的所述关节设置为Pitch-Yaw型,所述标号为偶数的所述关节设置为Yaw-Pitch型,其包括:
空间双圆弧约束方程建立模块,用于根据所述结构信息建立空间双圆弧约束方程;
机械臂第一约束方程建立模块,用于根据所述空间双圆弧约束方程建立机械臂第一约束方程,所述机械臂第一约束方程用于表示所述第四臂段和所述第五臂段构成的双圆弧曲线;
机械臂第二约束方程建立模块,用于根据所述空间双圆弧约束方程建立机械臂第二约束方程,所述机械臂第二约束方程用于表示所述第一臂段和所述第二臂段构成的双圆弧曲线;
全局约束方程建立模块,用于根据所述机械臂第一约束方程、所述机械臂第二约束方程建立机械臂全局约束方程,所述机械臂全局约束方程用于表示所述超冗余联动机械臂的圆弧曲线;
驱动关节角度求解模块,用于根据所述机械臂全局约束方程分别求解所述超冗余联动机械臂的5个所述臂段的驱动关节角度;
驱动控制模块,用于根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动。
进一步地,所述机械臂第一约束方程建立模块包括:
第五臂段期望位置信息规划单元,用于规划第五臂段末端的第五臂段期望位置信息;
第五臂段期望指向信息规划单元,用于规划所述第五臂段末端的第五臂段期望指向信息;
第三臂段期望位置信息规划单元,用于根据所述结构信息、所述第五臂段期望位置信息和所述第五臂段期望指向信息规划所述超冗余联动机械臂的第三臂段末端的第三臂段期望位置信息;
第三臂段期望指向信息表达式获取单元,用于将所述第三臂段末端的期望指向信息进行参数化得到第三臂段期望指向信息表达式;
第四臂段参数信息获取单元,用于获取所述超冗余联动机械臂的第四臂段参数信息;
第五臂段参数信息获取单元,用于获取所述超冗余联动机械臂的第五臂段参数信息;
机械臂第一约束方程生成单元,用于根据所述超冗余联动机械臂的第四臂段参数信息、所述超冗余联动机械臂的第五臂段参数信息、所述第五臂段期望位置信息、所述第五臂段期望指向信息、所述空间双圆弧约束方程和所述第三臂段期望指向信息表达式建立所述机械臂第一约束方程。
进一步地,所述机械臂第二约束方程建立模块包括:
第二臂段期望位置信息获取单元,用于根据所述第三臂段期望位置信息和所述第三臂段期望指向信息表达式获取第二臂段末端的第二臂段期望位置信息;
第二臂段期望指向信息获取单元,用于根据所述第三臂段期望位置信息和所述第三臂段期望指向信息表达式获取第二臂段末端的第二臂段期望指向信息;
第一臂段参数信息获取单元,用于获取所述超冗余联动机械臂的第一臂段参数信息;
第二臂段参数信息获取单元,用于获取所述超冗余联动机械臂的第二臂段参数信息;
机械臂第二约束方程生成单元,用于根据所述第一臂段参数信息、所述第二臂段参数信息、所述第二臂段期望位置信息、所述第二臂段期望指向信息、所述空间双圆弧约束方程和所述第三臂段期望指向信息表达式建立所述机械臂第二约束方程。
第三方面,本发明提供一种超冗余联动机械臂的运动轨迹控制系统,其包括:超冗余联动机械臂本体和用于控制所述超冗余机械臂本体运动的驱动控制器,所述驱动控制器用于执行所述的超冗余联动机械臂的运动轨迹控制方法控制所述超冗余联动机械臂本体的工作。
本发明的有益效果是:
本发明实施例中一种超冗余联动机械臂的运动轨迹控制方法,通过获取超冗余联动机械臂的结构信息,并根据结构信息建立空间双圆弧约束方程,根据该空间双圆弧约束方程分别建立用于表示第四臂段和第五臂段的机械臂第一约束方程,以及用于表示第一臂段和第二臂段的机械臂第二约束方程,根据第一约束方程和第二约束方程建立用于表示超冗余联动机械臂的圆弧曲线的机械臂全局约束方程,对机械臂全局约束方程进行求解获得各个臂段的驱动关节角度后根据该驱动关节角度驱动超冗余联动机械臂向规划轨迹运动;解决了现有技术中由于超冗余机械臂的自由度较多,导致其运动学方程非常复杂,进而导致超冗余机械臂逆解的计算量急剧增加,从而降低了控制超冗余机械臂在规划路径上运动的效率,提供了一种高效的、可靠的超冗余联动机械臂的运动轨迹控制方法。
附图说明
图1是本发明实施例中超冗余联动机械臂的一具体实施例结构示意图;
图2是本发明实施例中超冗余联动机械臂的第一臂段的一具体实施例关节配置示意图;
图3是本发明实施例中一种超冗余联动机械臂的运动轨迹控制方法的一具体实施例流程示意图;
图4是本发明实施例中建立空间双圆弧约束方程的一具体实施例结构见图;
图5是本发明实施例中一种超冗余联动机械臂的运动轨迹控制装置的一具体实施例模块框图。
具体实施方式
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对照附图说明本发明的具体实施方式。显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图,并获得其他的实施方式。
除非另有定义,本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的,不是旨在于限制本发明。
实施例一:
如图1所示,图1为本发明实施例一种超冗余联动机械臂的运动轨迹控制方法中超冗余联动机械臂的一具体结构示意图,在本实施例中,超冗余联动机械臂包括第一臂段、第二臂段、第三臂段、第四臂段和第五臂段共5个臂段,每个臂段均包括6个小节,且同一个臂段中的小节具有相等的长度(不同臂段中的小节的长度可能不相等),每个小节一一对应一个关节,两个小节之间依次通过关节连接(即臂段间通过同样的关节连接)。其中B0为超冗余联动机械臂的安装基座,5个臂段按顺序进行标号,离安装基座最近的为第一臂段,离安装基座最远的为第五臂段,Pb、tb为超冗余联动机械臂起始安装点(即第一臂段起始端)的位置信息和指向信息,P1、t1,P2、t2,P3、t3,P4、t4,P5、t5分别为超冗余联动机械臂各个臂段末端的位置信息和指向信息。参照图2,图2为本发明实施例中超冗余联动机械臂的第一臂段的一具体实施例关节示意图,其中,臂段内的6个关节按照顺序进行标号,即从1起按照顺序分别标号,形成1、2、3、4、5、6号关节,并将标号为奇数的关节设置为Pitch-Yaw型,即在惯性系OI下先绕ZI轴旋转,再绕YI轴旋转;将标号为偶数的关节设置为Yaw-Pitch型,即先绕YI轴旋转,再绕ZI轴旋转。基于上述的结构,在机械臂联动作用下,使得超冗余联动机械臂的每个臂段都在各自的平面内产生等角度弯曲(不同臂段不一定处于相同的平面内),因此,同一个臂段内的各个小节都在同一空间平面内,且同一臂段内的各个小节都为同一圆弧的切线,即该圆弧为该臂段内各个小节的内切圆。
参照图3,基于上述的超冗余联动机械臂,本发明实施例提供了一种超冗余联动机械臂的运动轨迹控制方法,其包括:
步骤S100、根据结构信息建立空间双圆弧约束方程;具体的,基于上述超冗余联动机械臂的结构信息,可知超冗余联动机械臂的第四臂端、第五臂段构成的双圆弧曲线,以及第一臂段、第二臂段构成的双圆弧曲线满足空间双圆弧约束方程。
则参照图4,图4表示三维空间中的一条双圆弧曲线,其中,表示第一圆弧,表示第二圆弧,和分别表示第一圆弧和第二圆弧的圆心,O1和O2分别表示第一圆弧和第二圆弧的圆心角,R1和R2分别表示第一圆弧和第二圆弧的半径,则基于上述可知该空间双圆弧曲线的已知量包括:
1、空间双圆弧的其实位置及指向(指向即为切线方向):P01和t01;
2、空间双圆弧的终止位置及指向(指向即为切线方向):P41和t41;
3、每个臂段内的小节长度L11和L21;
4、第一圆弧和第二圆弧等分成12份后,每一段小圆弧对应的圆形角满足:
5、第一圆弧和第二圆弧的交点为P21(改点待求),且两个圆弧在该交点处的切线方向相同。
因此,可通过求解空间双圆弧约束方程,从而确定一个双圆弧曲线同时满足上述5个已知量,具体过程包括:
过P01点并沿t01方向做一条直线l1,然后P41过点并沿t41做一条直线l3,假设过P21点并沿其切线方向做一条直线l2,此处,将直线和直线的交点记作P11,把直线l3和直线l2的交点记作P31。则根据圆弧切线的几何性质可得:
其中,k11表示线段P11P01和线段P11P21的长度,k21表示线段P31P21和线段P31P41的长度,因此,点P11和点P31的位置矢量为:
线段P11P31的长度为:
||P11P31||=||P11-P31||=||P01+k11t01-(P41-k21t41)||=k11+k21 (4)
以空间双圆弧的起始点P01为坐标系原点,则P01=[0 0 0]Τ,联立(2)式和(3)式可得:
根据(3)式和(5)式可得:
在第一圆弧和第二圆弧中,可知:
则联立(1)式和(7)式可得:
参照图4,在三角形和中,根据余弦定理可知:
将(2)式代入(9)式中可得:
因此,根据(4)式、(8)式和(10)式可得空间双圆弧约束方程为:
其中,P01、t01、P41和t41为前提已知条件,k11、k21、和是待求量。同时,根据(6)式可知,P21P01和P21P41是k11、k21、和的函数,因此,通过(11)式,可将空间双圆弧求解问题转化为非线性约束方程组的求解。
步骤S200、根据空间双圆弧约束方程建立机械臂第一约束方程,机械臂第一约束方程用于表示第四臂段和第五臂段构成的双圆弧曲线;具体的,根据步骤S100中建立的空间双圆弧约束方程可知,要建立机械臂第一约束方程需要:第四臂段参数信息、第五臂段参数信息、规划第五臂段末端的第五臂段期望位置信息Pe5和第五臂段期望指向信息te5,其中,第四臂段参数信息包括第四臂段的小节长度和第四臂段对应的圆心角,第五臂段参数信息包括第五臂段的小节长度和第五臂段对应的圆形角,根据上述超冗余联动机械臂的结构信息、第五臂段期望位置信息Pe5和第五臂段期望指向信息te5规划超冗余联动机械臂的第三臂段末端的第三臂段期望位置信息Pe3,使得基于上述超冗余联动机械臂的结构信息第五臂段期望位置信息Pe5、第五臂段期望指向信息te5和第三臂段期望位置信息Pe3能够满足超冗余联动机械臂的一个规划轨迹,再通过将第三臂段末端的期望指向信息te3进行参数化得到第三臂段期望指向信息表达式,由于第三臂段末端的指向可以是三维空间的任意方向,因此,其可表示为:
其中,
则根据(11)式可知,机械臂第一约束方程为:
此时,在(14)式中,
则,结合(12)式、(13)式和(14)式可知,te3与参数te3_Ang1和参数te3_Ang2相关,因此,(14)式中的||P21P01||和||P21P41||也与参数te3_Ang1和参数te3_Ang2相关,可知非线性约束方程组(机械臂第一约束方程)可记为:
步骤S300、根据空间双圆弧约束方程建立机械臂第二约束方程,机械臂第二约束方程用于表示第一臂段和第二臂段构成的双圆弧曲线。
其包括:
根据第三臂段期望位置信息和第三臂段期望指向信息表达式获取第二臂段末端的第二臂段期望位置信息和第二臂段期望指向信息;具体的,根据第三臂段期望位置信息和第三臂段期望指向信息(即第三臂段期望指向信息表达式)可得到过点Pe3沿te3方向的直线方程为:
其中,Pe3(i),i=1,2,3表示向量Pe3的第i个元素,te3(i),i=1,2,3表示向量te3的第i个元素。
因此,过如(17)式所表示的直线方程的平面束Sp可以表示为:
(1)当te3(1)≠0时,
(17)式表示的直线方程可以表示:
根据(18)式可得平面束方程为:
(te3(2)+λte3(3))·X-te3(1)·Y-λte3(1)·Z+D1=0 (19)
其中,D1=-te3(2)Pe3(1)+te3(1)Pe3(2)-λ(te3(3)Pe3(1)-te3(1)Pe3(3))。
则,此时平面束中任意平面的法向量可以表示为:
n=[te3(2)+λte3(3) -te3(1) -λte3(1)],λ∈[-∞ +∞] (20)
(2)当te3(2)≠0时,可得平面束方程为:
te3(2)·X-(te3(1)-λte3(3))·Y+λte3(2)·Z+D2=0 (21)
其中,D2=te3(1)Pe3(2)-te3(2)Pe3(1)+λ(te3(3)Pe3(2)-te3(2)Pe3(3))。
此时,平面束中任意平面的法向量可以表示为:
n=[te3(2) -(te3(1)-λte3(3)) λte3(2)],λ∈[-∞ +∞] (22)
(3)当te3(3)≠0时,可得平面束方程为:
te3(3)·X+λte3(3)·Y-(te3(1)-λte3(2))·Z+D3=0 (23)
其中,D3=te3(1)Pe3(3)-te3(1)Pe3(3)+λ(te3(2)Pe3(3)-te3(3)Pe3(2))。
此时,平面束中任意平面的法向量可以表示为:
n=[te3(3) λte3(3) -(te3(1)-λte3(2))],λ∈[-∞ +∞] (24)
由于(17)式表示的直线过点沿方向的直线也在第三臂段所在的平面上,因此根据(20)式或(22)式或(24)式得到第三臂段所在平面的参数化法向量n3。
因此,第二臂段期望位置信息Pe2和第二臂段期望指向信息te2为:
其中,I3×3表示阶数为3的单位矩阵,若n3=[n3x n3y n3z],则
将第一臂段和第二臂段构成的双圆弧曲线进行求解,则通过(25)式可得第二臂段末端的第二臂段期望位置参数信息Pe2和第二臂段期望指向信息te2,并结合第一臂段参数信息,其包括第一臂段中单个第一臂段单小节长度L1、第一臂段所对应的第一臂段圆心角第一臂段起始位置信息、第一臂段起始指向信息,则此时双圆弧曲线的起始点为第一臂段起始位置,即也是超冗余联动机械臂的安装位置点,则该点的位置信息为Pb(即第一臂段起始位置信息),指向信息为tb(即第一臂段起始指向信息)这两个量为已知量,第二臂段参数信息,其包括第二臂段中单个第二臂段单小节长度L2和第一臂段所对应的第一臂段圆心角结合(11)式可得机械臂第二约束方程为:
在(28)式中,
结合(20)式、(22)式、(24)式和(25)式可知,Pe2和te2与参数、参数和参数te3相关,因此(28)式中的||P21P01||和||P21P41||也与参数λ、参数和参数te3相关,因此,非线性约束方程组(28),即机械臂第二约束方程可记为:
步骤S400、根据所述机械臂第一约束方程、所述机械臂第二约束方程建立机械臂全局约束方程,所述机械臂全局约束方程用于表示所述超冗余联动机械臂的圆弧曲线。
具体的,根据(16)式和(30)式可以得到用于表示超冗余联动机械臂的圆弧曲线的机械臂全局约束方程:
其中,010×1表示10行1列的向量且各个元素均为0。
步骤S500、根据机械臂全局约束方程分别求解5个臂段的驱动关节角度,并根据各个驱动关节角度驱动超冗余联动机械臂向规划轨迹运动。
其包括:
求解(31)式表示的机械臂全局约束方程获得超冗余联动机械臂的各个臂段末端的各个期望位置信息和各个期望指向信息。具体的,可以通过牛顿-拉夫逊(拉弗森)方法进行求解,其具体过程如下:
(1)令设定初值点x0,计算得Fsum(x0)。
(2)设定阈值ε,例如ε=0.00001。判断||Fsum(x0)||<ε是否成立,若成立则输出x0,否则计算方程组Fsum(x)的偏导数:
其中,
Fsum(x)=[f1 f2 f3 f4 f5 f6 f7 f8 f9 f10]Τ (33)
(3)根据(32)式迭代得到下一步的参数值:
其中,pinv(·)表示求矩阵的伪逆运算。
(4)将x1赋值于x0,返回上述步骤(1),直至||Fsum(x0)||<ε成立,输出x0即为方程组Fsum(x0)=0的解。
(31)式中各个参数通过数值方法求解得到后,可通过计算得到各个臂段末端的期望位置信息Pei(i=1,2,3,4)和期望指向信息tei(i=1,2,3,4)。
基于上述的各个期望位置信息和期望指向信息,可通过数学求解方法计算出各个臂段的各个关节角度,其包括:
(1)当i=1时,
可得第一臂段所在平面的法向量相对于本体坐标系(即以该臂段起始端为原点建立的坐标系)为:
将第一臂段驱动关节角度设为θ11和θ12,可得第一臂段在其所在平面内的法向量相对于本体坐标系表示为:
则第一臂段所在平面内的弯曲角度余弦值为:
则根据第一臂段驱动关节角度θ11和θ12可得第一臂段在其所在平面内弯曲角度余弦值为:
其中,
根据(36)式、(37)式、(38)式和(39)式可得:
则可求得第一臂段驱动关节角度θ11和θ12为:
(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 (10)
1.一种超冗余联动机械臂的运动轨迹控制方法,所述超冗余联动机械臂的结构信息包括:具有第一臂段、第二臂段、第三臂段、第四臂段和第五臂段,每个所述臂段依次连接,每个所述臂段均包括数量相同的多个小节,同一个所述臂段中的所述多个小节具有相等的长度,所述多个小节依次通过关节连接,分别对每个所述臂段内的所述关节按顺序进行标号,且将所述标号为奇数的所述关节设置为Pitch-Yaw型,所述标号为偶数的所述关节设置为Yaw-Pitch型,其特征在于,包括:
根据所述结构信息建立空间双圆弧约束方程;
根据所述空间双圆弧约束方程建立机械臂第一约束方程,所述机械臂第一约束方程用于表示所述第四臂段和所述第五臂段构成的双圆弧曲线;
根据所述空间双圆弧约束方程建立机械臂第二约束方程,所述机械臂第二约束方程用于表示所述第一臂段和所述第二臂段构成的双圆弧曲线;
根据所述机械臂第一约束方程、所述机械臂第二约束方程建立机械臂全局约束方程,所述机械臂全局约束方程用于表示所述超冗余联动机械臂的圆弧曲线;
根据所述机械臂全局约束方程分别求解5个所述臂段的驱动关节角度,并根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动。
2.根据权利要求1所述的超冗余联动机械臂的运动轨迹控制方法,其特征在于,根据所述空间双圆弧约束方程建立机械臂第一约束方程具体包括:
规划第五臂段末端的第五臂段期望位置信息和第五臂段期望指向信息,并根据所述结构信息、所述第五臂段期望位置信息和所述第五臂段期望指向信息规划所述超冗余联动机械臂的第三臂段末端的第三臂段期望位置信息;
将所述第三臂段末端的期望指向信息进行参数化得到第三臂段期望指向信息表达式;
根据所述超冗余联动机械臂的第四臂段参数信息、所述超冗余联动机械臂的第五臂段参数信息、所述第五臂段期望位置信息、所述第五臂段期望指向信息、所述空间双圆弧约束方程和所述第三臂段期望指向信息表达式建立所述机械臂第一约束方程。
3.根据权利要求2所述的超冗余联动机械臂的运动轨迹控制方法,其特征在于,
所述第四臂段参数信息包括所述超冗余联动机械臂的第四臂段中单个所述小节对应的第四臂段单小节长度,以及所述第四臂段所对应的第四臂段圆心角;
所述第五臂段参数信息包括所述超冗余联动机械臂的第五臂段中单个所述小节对应的第五臂段单小节长度,以及所述第五臂段所对应的第五臂段圆心角。
4.根据权利要求2或3所述的超冗余联动机械臂的运动轨迹控制方法,其特征在于,根据所述空间双圆弧约束方程建立机械臂第二约束方程具体包括:
根据所述第三臂段期望位置信息和所述第三臂段期望指向信息表达式获取第二臂段末端的第二臂段期望位置信息和第二臂段期望指向信息;
根据所述超冗余联动机械臂的第一臂段参数信息、所述超冗余联动机械臂的第二臂段参数信息、所述第二臂段期望位置信息、所述第二臂段期望指向信息、所述空间双圆弧约束方程和所述第三臂段期望指向信息表达式建立所述机械臂第二约束方程。
5.根据权利要求4所述的超冗余联动机械臂的运动轨迹控制方法,其特征在于,所述第一臂段参数信息包括所述超冗余联动机械臂的第一臂段中单个所述小节对应的第一臂段单小节长度、所述第一臂段所对应的第一臂段圆心角、第一臂段起始位置信息、第一臂段起始指向信息;
所述第二臂段参数信息包括所述超冗余联动机械臂的第二臂段中单个所述小节对应的第二臂段单小节长度,以及所述第二臂段所对应的第二臂段圆心角。
6.根据权利要求1、2、3或5所述的超冗余联动机械臂的运动轨迹控制方法,其特征在于,根据所述机械臂全局约束方程分别求解5个所述臂段的驱动关节角度,并根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动具体包括:
求解所述机械臂全局约束方程获得所述超冗余联动机械臂的各个臂段末端的各个期望位置信息和各个期望指向信息;
基于各个所述期望位置信息和各个所述期望指向信息通过数学求解方法计算出各个所述臂段的各个所述驱动关节角度;
根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动。
7.一种超冗余联动机械臂的运动轨迹控制装置,所述超冗余联动机械臂的结构信息包括:具有第一臂段、第二臂段、第三臂段、第四臂段和第五臂段,每个所述臂段依次连接,每个所述臂段均包括数量相同的多个小节,同一个所述臂段中的所述多个小节具有相等的长度,所述多个小节依次通过关节连接,分别对每个所述臂段内的所述关节按顺序进行标号,且将所述标号为奇数的所述关节设置为Pitch-Yaw型,所述标号为偶数的所述关节设置为Yaw-Pitch型,其特征在于,包括:
空间双圆弧约束方程建立模块,用于根据所述结构信息建立空间双圆弧约束方程;
机械臂第一约束方程建立模块,用于根据所述空间双圆弧约束方程建立机械臂第一约束方程,所述机械臂第一约束方程用于表示所述第四臂段和所述第五臂段构成的双圆弧曲线;
机械臂第二约束方程建立模块,用于根据所述空间双圆弧约束方程建立机械臂第二约束方程,所述机械臂第二约束方程用于表示所述第一臂段和所述第二臂段构成的双圆弧曲线;
全局约束方程建立模块,用于根据所述机械臂第一约束方程、所述机械臂第二约束方程建立机械臂全局约束方程,所述机械臂全局约束方程用于表示所述超冗余联动机械臂的圆弧曲线;
驱动关节角度求解模块,用于根据所述机械臂全局约束方程分别求解所述超冗余联动机械臂的5个所述臂段的驱动关节角度;
驱动控制模块,用于根据各个所述驱动关节角度驱动所述超冗余联动机械臂向规划轨迹运动。
8.根据权利要求7所述的超冗余联动机械臂的运动轨迹控制装置,其特征在于,所述机械臂第一约束方程建立模块包括:
第五臂段期望位置信息规划单元,用于规划第五臂段末端的第五臂段期望位置信息;
第五臂段期望指向信息规划单元,用于规划所述第五臂段末端的第五臂段期望指向信息;
第三臂段期望位置信息规划单元,用于根据所述结构信息、所述第五臂段期望位置信息和所述第五臂段期望指向信息规划所述超冗余联动机械臂的第三臂段末端的第三臂段期望位置信息;
第三臂段期望指向信息表达式获取单元,用于将所述第三臂段末端的期望指向信息进行参数化得到第三臂段期望指向信息表达式;
第四臂段参数信息获取单元,用于获取所述超冗余联动机械臂的第四臂段参数信息;
第五臂段参数信息获取单元,用于获取所述超冗余联动机械臂的第五臂段参数信息;
机械臂第一约束方程生成单元,用于根据所述超冗余联动机械臂的第四臂段参数信息、所述超冗余联动机械臂的第五臂段参数信息、所述第五臂段期望位置信息、所述第五臂段期望指向信息、所述空间双圆弧约束方程和所述第三臂段期望指向信息表达式建立所述机械臂第一约束方程。
9.根据权利要求8所述的超冗余联动机械臂的运动轨迹控制装置,其特征在于,所述机械臂第二约束方程建立模块包括:
第二臂段期望位置信息获取单元,用于根据所述第三臂段期望位置信息和所述第三臂段期望指向信息表达式获取第二臂段末端的第二臂段期望位置信息;
第二臂段期望指向信息获取单元,用于根据所述第三臂段期望位置信息和所述第三臂段期望指向信息表达式获取第二臂段末端的第二臂段期望指向信息;
第一臂段参数信息获取单元,用于获取所述超冗余联动机械臂的第一臂段参数信息;
第二臂段参数信息获取单元,用于获取所述超冗余联动机械臂的第二臂段参数信息;
机械臂第二约束方程生成单元,用于根据所述第一臂段参数信息、所述第二臂段参数信息、所述第二臂段期望位置信息、所述第二臂段期望指向信息、所述空间双圆弧约束方程和所述第三臂段期望指向信息表达式建立所述机械臂第二约束方程。
10.一种超冗余联动机械臂的运动轨迹控制系统,其特征在于,包括:超冗余联动机械臂本体和用于控制所述超冗余机械臂本体运动的驱动控制器,所述驱动控制器用于执行如权利要求1至6任一项所述的超冗余联动机械臂的运动轨迹控制方法控制所述超冗余联动机械臂本体的工作。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910954129.9A CN110757453B (zh) | 2019-10-09 | 2019-10-09 | 一种超冗余联动机械臂的运动轨迹控制方法、装置及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910954129.9A CN110757453B (zh) | 2019-10-09 | 2019-10-09 | 一种超冗余联动机械臂的运动轨迹控制方法、装置及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110757453A true CN110757453A (zh) | 2020-02-07 |
CN110757453B CN110757453B (zh) | 2021-03-30 |
Family
ID=69330920
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910954129.9A Active CN110757453B (zh) | 2019-10-09 | 2019-10-09 | 一种超冗余联动机械臂的运动轨迹控制方法、装置及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110757453B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112276953A (zh) * | 2020-10-27 | 2021-01-29 | 清华大学深圳国际研究生院 | 连续型超冗余机械臂的臂型协同规划方法、终端设备和存储介质 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP3257515B2 (ja) * | 1998-06-30 | 2002-02-18 | 日本電気株式会社 | 通信ネットワーク設計回路及びその方法並びにプログラムを記録した機械読み取り可能な記録媒体 |
WO2008133230A1 (ja) * | 2007-04-20 | 2008-11-06 | Nec Corporation | マルチキャストツリー設計装置および方法ならびにプログラム |
CN102554938A (zh) * | 2010-12-31 | 2012-07-11 | 中国科学院计算技术研究所 | 机器人的机械手末端轨迹跟踪方法 |
CN103235513A (zh) * | 2013-04-24 | 2013-08-07 | 武汉科技大学 | 一种基于遗传算法的移动机械臂轨迹规划优化方法 |
CN105183009A (zh) * | 2015-10-15 | 2015-12-23 | 哈尔滨工程大学 | 一种冗余机械臂轨迹控制方法 |
CN105751196A (zh) * | 2016-04-12 | 2016-07-13 | 华南理工大学 | 一种基于主从式的工业机器人协作作业方法 |
-
2019
- 2019-10-09 CN CN201910954129.9A patent/CN110757453B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP3257515B2 (ja) * | 1998-06-30 | 2002-02-18 | 日本電気株式会社 | 通信ネットワーク設計回路及びその方法並びにプログラムを記録した機械読み取り可能な記録媒体 |
WO2008133230A1 (ja) * | 2007-04-20 | 2008-11-06 | Nec Corporation | マルチキャストツリー設計装置および方法ならびにプログラム |
CN102554938A (zh) * | 2010-12-31 | 2012-07-11 | 中国科学院计算技术研究所 | 机器人的机械手末端轨迹跟踪方法 |
CN103235513A (zh) * | 2013-04-24 | 2013-08-07 | 武汉科技大学 | 一种基于遗传算法的移动机械臂轨迹规划优化方法 |
CN105183009A (zh) * | 2015-10-15 | 2015-12-23 | 哈尔滨工程大学 | 一种冗余机械臂轨迹控制方法 |
CN105751196A (zh) * | 2016-04-12 | 2016-07-13 | 华南理工大学 | 一种基于主从式的工业机器人协作作业方法 |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112276953A (zh) * | 2020-10-27 | 2021-01-29 | 清华大学深圳国际研究生院 | 连续型超冗余机械臂的臂型协同规划方法、终端设备和存储介质 |
CN112276953B (zh) * | 2020-10-27 | 2021-12-28 | 清华大学深圳国际研究生院 | 连续型超冗余机械臂的臂型协同规划方法、终端设备和存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN110757453B (zh) | 2021-03-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107263477B (zh) | 一种绳驱动串联关节型蛇形机械臂控制方法 | |
JP5743495B2 (ja) | ロボット制御装置 | |
Palmer et al. | Real-time method for tip following navigation of continuum snake arm robots | |
CN103934528B (zh) | 一种用于电火花加工的六轴联动插补方法 | |
CN110682286B (zh) | 一种协作机器人实时避障方法 | |
CN107891424A (zh) | 一种求解冗余机械臂逆运动学的有限时间神经网络优化方法 | |
CN109176488B (zh) | 一种柔性机器人运动学标定方法及系统 | |
CN112936273B (zh) | 一种绳驱柔性机械臂的速度级运动学建模方法 | |
CN102785248A (zh) | 一种解耦型六自由度工业机器人的运动控制方法 | |
WO2022258054A1 (zh) | 重建器械术野中心的控制方法、系统和存储介质 | |
CN110561419B (zh) | 臂型线约束柔性机器人轨迹规划方法及装置 | |
CN113146600A (zh) | 基于运动学迭代学习控制的柔性机器人轨迹规划方法及装置 | |
Yamada et al. | Steering of pedal wave of a snake-like robot by superposition of curvatures | |
CN110757453B (zh) | 一种超冗余联动机械臂的运动轨迹控制方法、装置及系统 | |
JP2016055404A (ja) | 軌道生成方法、軌道生成装置、ロボット装置、プログラム及び記録媒体 | |
CN108326854B (zh) | 一种多关节机械臂空间函数轨迹运动的逆运动学求解方法 | |
CN109434838B (zh) | 线驱动连续机器人内窥操作的协同运动规划方法及系统 | |
CN113334381A (zh) | 一种可运动解耦的连续体机器人控制方法 | |
CN110561426B (zh) | 一种超冗余机械臂的路径规划方法、装置及控制系统 | |
JP2013039643A (ja) | 6軸ロボットの軸間オフセット検出方法 | |
KR20160003774A (ko) | 다관절 링크 기구의 역운동학 해법, 및 이 역운동학 해법을 이용한 교시 데이터 작성 장치 | |
JPS6095605A (ja) | ロボツトのテイ−チデ−タ補正方法 | |
Ji et al. | A Spatial Path Following Method for Hyper-Redundant Manipulators by Step-by-Step Search and Calculating | |
Wang et al. | Interactive path-following method of snake-like robot | |
Tyapin et al. | Long arm manipulator path interpolation using 4th order b-splines |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |