CN114952838A - 一种基于末端测量反馈的机械臂关节轨迹规划方法 - Google Patents
一种基于末端测量反馈的机械臂关节轨迹规划方法 Download PDFInfo
- Publication number
- CN114952838A CN114952838A CN202210582484.XA CN202210582484A CN114952838A CN 114952838 A CN114952838 A CN 114952838A CN 202210582484 A CN202210582484 A CN 202210582484A CN 114952838 A CN114952838 A CN 114952838A
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- coordinate system
- joint
- vector
- planning
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
一种基于末端测量反馈的机械臂关节轨迹规划方法,涉及机械臂轨迹规划技术领域,针对现有规划方法仅适用于结构化环境,并且拖动示教的方式需要人为辅助的问题,本申请不需要已知的离散点,直接根据期望位置规划机械臂的关节轨迹,因此不局限于几何信息的已知的结构化环境;本申请在线自主规划机械臂的关节轨迹,不需要人为拖动示教,减小了人工工作量,并提高了效率及自主性;本申请实现了轨迹的加加速度连续,提高了轨迹平滑性,减小了执行器运行负担。
Description
技术领域
本发明涉及机械臂轨迹规划技术领域,具体为一种基于末端测量反馈的机械臂关节轨迹规划方法。
背景技术
机械臂的工作环境、操作对象和操作任务要求均日益复杂,对机械臂的轨迹规划技术提出了挑战。当前基于离线点插补和拖动示教的规划方式均存在明显不足:离线点插补规划方法仅适用于结构化环境,如:经过严格标定,所有操作对象几何信息,精度信息均已知的汽车生产线、食品包装生产线等;而基于拖动示教的方式需要人为辅助,增加了人工工作量。
发明内容
本发明的目的是:针对现有规划方法仅适用于结构化环境,并且拖动示教的方式需要人为辅助的问题,提出一种基于末端测量反馈的机械臂关节轨迹规划方法。
本发明为了解决上述技术问题采取的技术方案是:
一种基于末端测量反馈的机械臂关节轨迹规划方法,包括以下步骤:
步骤二:获取机械臂关节角度矢量ql,并根据机械臂关节角度矢量ql得到机械臂末端坐标系相对于机械臂基座坐标系的位姿矩阵bTe;
步骤三:根据目标坐标系相对于机械臂末端坐标系的位姿矩阵eTt以及机械臂末端坐标系相对于机械臂基座坐标系的位姿矩阵bTe得到目标坐标系相对于机械臂基座坐标系的位姿矩阵bTt,基于目标坐标系相对于机械臂基座坐标系的位姿矩阵bTt并采用逆运动学得到关节期望位置矢量qd;
步骤四:基于关节期望位置矢量qd,规划得到机械臂关节参考轨迹;
步骤五:机械臂关节控制器跟踪机械臂关节参考轨迹,驱动机械臂运动。
进一步的,所述目标坐标系相对于机械臂末端坐标系的位姿矩阵eTt表示为:
进一步的,所述机械臂关节角度矢量ql通过关节位置传感器测量得到。
进一步的,所述机械臂末端坐标系相对于机械臂基座坐标系的位姿矩阵bTe表示为:
bTe=forwardkinematics(ql)
其中,ql表示机械臂关节角度矢量,forwardkinematics()表示机械臂的正运动学函数。进一步的,所述关节期望位置矢量qd表示为:
qd=inversekinematics(bTt)
bTt=bTe eTt
其中,inversekinematics()表示机械臂的逆运动学函数,bTt表示目标坐标系相对于机械臂基座坐标系的位姿矩阵,eTt表示目标坐标系相对于机械臂末端坐标系的位姿矩阵,
bTe表示机械臂末端坐标系相对于机械臂基座坐标系的位姿矩阵。
进一步的,所述步骤四的具体步骤为:
步骤四一:设定规划参数k,ξ和ωn,所述规划参数k,ξ和ωn均为正定的对角矩阵;
步骤四二:根据设定的规划参数k,ξ和ωn,得到位置环反馈增益Kp、速度环反馈增益Kv以及加速度环增益Ka:
进一步的,所述位置环反馈增益Kp、速度环反馈增益Kv以及加速度环增益Ka表示为:
Kp=kωn 2
Kv=ωn 2+2kξωn
Ka=k+2ξωn。
其中,∫dt表示积分。
本发明的有益效果是:
1、本申请不需要已知的离散点,直接根据期望位置规划机械臂的关节轨迹,因此不局限于几何信息的已知的结构化环境;
2、本申请在线自主规划机械臂的关节轨迹,不需要人为拖动示教,减小了人工工作量,并提高了效率及自主性;
3、本申请实现了轨迹的加加速度连续,提高了轨迹平滑性,减小了执行器运行负担。
附图说明
图1为机械臂关节坐标系示意图;
图2为本申请原理图;
图3为本申请流程图;
图4为机械臂伺服静止目标的过程示意图1;
图5为机械臂伺服静止目标的过程示意图2;
图6为机械臂伺服静止目标的过程示意图3;
图7为关节参考位置矢量曲线图;
图8为关节参考速度矢量曲线图;
图9为关节参考加速度矢量曲线图;
图10为关节参考加加速度矢量曲线图;
图11为机械臂末端坐标系与目标坐标系的相对位置示意图;
图12为机械臂末端坐标系与目标坐标系的相对姿态示意图;
图13为机械臂伺服运动目标的过程示意图1;
图14为机械臂伺服运动目标的过程示意图2;
图15为机械臂伺服运动目标的过程示意图3;
图16为关节参考位置矢量曲线图;
图17为关节参考速度矢量曲线图;
图18为关节参考加速度矢量曲线图;
图19为关节参考加加速度矢量曲线图;
图20为机械臂末端坐标系与目标坐标系的相对位置示意图;
图21为机械臂末端坐标系与目标坐标系的相对姿态示意图。
具体实施方式
需要特别说明的是,在不冲突的情况下,本申请公开的各个实施方式之间可以相互组合。
具体实施方式一:参照图1具体说明本实施方式,本实施方式所述的一种基于末端测量反馈的机械臂关节轨迹规划方法,具体步骤为:
轨迹规划开始,每个周期内做如下计算:
步骤一、计算目标坐标系相对于机械臂末端坐标系的位姿矩阵eTt;
其中,wz2mtrx()是位姿矢量转换至位姿矩阵的函数;
步骤二、根据机械臂关节角度矢量ql得到机械臂末端坐标系相对于机械臂基座坐标系的位姿矩阵bTe;
bTe=forwardkinematics(ql)
其中,ql由关节位置传感器测量得到,forwardkinematics()是机械臂的正运动学函数;
步骤三、根据步骤一和步骤二得到的eTt和bTe,进而计算目标坐标系相对于机械臂基座坐标系的姿态矩阵bTt,并采用机械臂逆运动学函数求解关节期望位置矢量qd;
步骤三一、由eTt和bTe,得到bTt:
bTt=bTe eTt
步骤三二、采用机械臂逆运动学函数由bTt得到qd:
qd=inversekinematics(bTt)
其中,inversekinematics()是机械臂的逆运动学函数。
步骤四一:人为设定规划参数k,ξ和ωn,三个参数均为正定的对角矩阵;
步骤四二:根据步骤四一中设定的参数,得到位置环反馈增益Kp,速度环反馈增益Kv和加速度环增益Ka:
Kp=kωn 2
Kv=ωn 2+2kξωn
Ka=k+2ξωn
步骤四三:根据步骤四二中得到的Kp、Kv和Ka,步骤三二得到关节期望位置矢量以及机械臂关节角度矢量,规划关节参考加加速度矢量:
步骤五:机械臂关节控制器跟踪关节参考轨迹,驱动机械臂运动。
本申请解决了现有技术中拖动示教的方式需要人为辅助,效率低的问题,另外,随着传感测量技术的发展,机械臂配备有丰富的传感器,大幅提高了感知外部环境信息的能力,如:末端安装有深度相机、红外相机、激光雷达等,可以获得操作目标与机械臂末端之间的相对位置和姿态。这为机器人自主规划提供了硬件基础,但如何基于这些传感信息,快速规划机械臂的运动,并保证轨迹加速度,甚至加加速度连续仍是一个关键难点。而本申请针对该难点,通过直接根据期望位置规划机械臂的关节轨迹,使本申请不局限于几何信息的已知的结构化环境;并且本申请实现了轨迹的加加速度连续,提高了轨迹平滑性,减小了执行器运行负担。
实施例1:
为验证本申请的有效性,以下使用计算机仿真,结合图1至图12对本申请的技术方案做进一步描述。
搭建计算机仿真平台,仿真中所用机械臂有6个旋转自由度,目标静止不动。关节参考位置矢量qr的初始值为[-7.61,76.2,-133.95,-30.27,-97.35,0.0]deg,关节参考速度矢量,关节参考加速度矢量的初始值均为0,启动伺服规划,具体实施步骤如图2所示,在每一个规划周期内做如下运算:
[步骤S2]计算目标坐标系相对于机械臂末端坐标系的位姿矩阵eTt;
[步骤S3]计算机械臂末端坐标系相对于基座坐标系的位姿矩阵bTe
[步骤S4]根据eTt和bTe计算bTt:
[步骤S5]采用机械臂逆运动学函数由bTt得到qd
[步骤S6]设定规划参数k,ξ和ωn,如下:
k=2.4I6×6,ξ=1.2I6×6,ωn=0.8U6×6
其中,I6×6为6×6的单位矩阵。
[步骤S7]计算位置环反馈增益Kp,速度环反馈增益Kv和加速度环增益Ka:
Kp=kωn 2=1.536I6×6,Kv=ωn 2+2kξωn=5.248I6×6,
Ka=k+2ξωn=4.32I6×6
[步骤S8]规划关节参考加加速度矢量:机械臂关节加加速度规划方法流程图如图3。
[步骤S9]得到关节参考加速度矢量,关节参考速度矢量和关节参考位置矢量:
[步骤S10]机械臂关节控制器跟踪关节参考位置矢量,驱动机械臂运动。
[步骤S11]根据机械臂末端坐标系与目标坐标系的相对位姿数据d和判断机械臂是否伺服到位,若伺服到位,则完成伺服,退出;若未完成,重回[步骤S1]。机械臂伺服静止目标的过程如图4所示。机械臂伺服静止目标的关节参考轨迹如图5所示。机械臂伺服运动目标的过程图如图12所示。机械臂伺服运动目标的关节参考轨迹如图13-图16所示。
关节参考位置矢量曲线图、关节参考速度矢量曲线图、关节参考加速度矢量曲线图以及关节参考加加速度矢量曲线图如图6-9所示。
图10和图11是机械臂伺服静止目标时末端坐标系与目标坐标系的相对位姿;
机械臂伺服运动目标时末端坐标系与目标坐标系的相对位姿如图20和图21所示。
实施例2:
案例2中,目标运动。关节参考位置矢量qr的初始值为[-7.61,76.2,-133.95,-30.27,-97.35,0.0]deg,关节参考速度矢量,关节参考加速度矢量的初始值均为0。启动伺服规划,每一个规划周期内做如下运算:
[步骤S2]计算目标坐标系相对于机械臂末端坐标系的位姿矩阵eTt;
[步骤S3]计算机械臂末端坐标系相对于基座坐标系的位姿矩阵bTe
[步骤S4]根据eTt和bTe计算bTt:
[步骤S5]采用机械臂逆运动学函数由bTt得到qd
[步骤S6]设定规划参数k,ξ和ωn,如下:
k=18I6×6,ξ=1.2I6×6,ωn=6U6×6
其中,I6×6为6×6的单位矩阵。
[步骤S7]计算位置环反馈增益Kp,速度环反馈增益Kv和加速度环增益Ka:
Kp=kωn 2=648I6×6,Kv=ωn 2+2kξωn=295.2I6×6,
Ka=k+2ξωn=32.4I6×6
[步骤S8]规划关节参考加加速度矢量:
[步骤S9]得到关节参考加速度矢量,关节参考速度矢量和关节参考位置矢量:
[步骤S10]机械臂关节控制器跟踪关节参考位置矢量,驱动机械臂运动。
需要注意的是,具体实施方式仅仅是对本发明技术方案的解释和说明,不能以此限定权利保护范围。凡根据本发明权利要求书和说明书所做的仅仅是局部改变的,仍应落入本发明的保护范围内。
Claims (10)
1.一种基于末端测量反馈的机械臂关节轨迹规划方法,其特征在于包括以下步骤:
步骤二:获取机械臂关节角度矢量ql,并根据机械臂关节角度矢量ql得到机械臂末端坐标系相对于机械臂基座坐标系的位姿矩阵bTe;
步骤三:根据目标坐标系相对于机械臂末端坐标系的位姿矩阵eTt以及机械臂末端坐标系相对于机械臂基座坐标系的位姿矩阵bTe得到目标坐标系相对于机械臂基座坐标系的位姿矩阵bTt,基于目标坐标系相对于机械臂基座坐标系的位姿矩阵bTt并采用逆运动学得到关节期望位置矢量qd;
步骤四:基于关节期望位置矢量qd,规划得到机械臂关节参考轨迹;
步骤五:机械臂关节控制器跟踪机械臂关节参考轨迹,驱动机械臂运动。
4.根据权利要求1所述的一种基于末端测量反馈的机械臂关节轨迹规划方法,其特征在于所述机械臂关节角度矢量ql通过关节位置传感器测量得到。
5.根据权利要求1所述的一种基于末端测量反馈的机械臂关节轨迹规划方法,其特征在于所述机械臂末端坐标系相对于机械臂基座坐标系的位姿矩阵bTe表示为:
bTe=forwardkinematics(ql)
其中,ql表示机械臂关节角度矢量,forwardkinematics()表示机械臂的正运动学函数。
6.根据权利要求1所述的一种基于末端测量反馈的机械臂关节轨迹规划方法,其特征在于所述关节期望位置矢量qd表示为:
qd=inversekinematics(bTt)
bTt=bTe eTt
其中,inversekinematics()表示机械臂的逆运动学函数,bTt表示目标坐标系相对于机械臂基座坐标系的位姿矩阵,eTt表示目标坐标系相对于机械臂末端坐标系的位姿矩阵,bTe表示机械臂末端坐标系相对于机械臂基座坐标系的位姿矩阵。
9.根据权利要求8所述的一种基于末端测量反馈的机械臂关节轨迹规划方法,其特征在于所述位置环反馈增益Kp、速度环反馈增益Kv以及加速度环增益Ka表示为:
Kp=kωn 2
Kv=ωn 2+2kξωn
Ka=k+2ξωn。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210582484.XA CN114952838B (zh) | 2022-05-26 | 2022-05-26 | 一种基于末端测量反馈的机械臂关节轨迹规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210582484.XA CN114952838B (zh) | 2022-05-26 | 2022-05-26 | 一种基于末端测量反馈的机械臂关节轨迹规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114952838A true CN114952838A (zh) | 2022-08-30 |
CN114952838B CN114952838B (zh) | 2023-05-30 |
Family
ID=82956331
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210582484.XA Active CN114952838B (zh) | 2022-05-26 | 2022-05-26 | 一种基于末端测量反馈的机械臂关节轨迹规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114952838B (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115781683A (zh) * | 2022-12-20 | 2023-03-14 | 实时侠智能控制技术有限公司 | 机械臂的在线轨迹规划方法、装置及计算机可读介质 |
CN117262257A (zh) * | 2023-09-15 | 2023-12-22 | 哈尔滨工业大学 | 一种空间机械臂抓捕非合作喷管的伺服控制方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20050160815A1 (en) * | 2002-03-18 | 2005-07-28 | Lee James B. | Enhancement of sensors for airborne operation |
CN105659726B (zh) * | 2009-12-01 | 2013-06-19 | 北京空间飞行器总体设计部 | 基于多路视觉融合的大型空间机械臂自主运动规划方法 |
CN103295835A (zh) * | 2013-05-24 | 2013-09-11 | 南京航空航天大学 | 一种加速度开关 |
CN105245144A (zh) * | 2015-10-30 | 2016-01-13 | 中国兵器工业集团第二O二研究所 | 一种基于位移无静差的位置伺服***比例控制方法 |
CN107368078A (zh) * | 2017-08-29 | 2017-11-21 | 华南理工大学 | 一种基于加速度传感器反馈的跟踪控制装置及方法 |
-
2022
- 2022-05-26 CN CN202210582484.XA patent/CN114952838B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20050160815A1 (en) * | 2002-03-18 | 2005-07-28 | Lee James B. | Enhancement of sensors for airborne operation |
CN105659726B (zh) * | 2009-12-01 | 2013-06-19 | 北京空间飞行器总体设计部 | 基于多路视觉融合的大型空间机械臂自主运动规划方法 |
CN105659727B (zh) * | 2009-12-01 | 2013-06-19 | 北京空间飞行器总体设计部 | 一种大型空间机械臂在轨操控方法 |
CN103295835A (zh) * | 2013-05-24 | 2013-09-11 | 南京航空航天大学 | 一种加速度开关 |
CN105245144A (zh) * | 2015-10-30 | 2016-01-13 | 中国兵器工业集团第二O二研究所 | 一种基于位移无静差的位置伺服***比例控制方法 |
CN107368078A (zh) * | 2017-08-29 | 2017-11-21 | 华南理工大学 | 一种基于加速度传感器反馈的跟踪控制装置及方法 |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115781683A (zh) * | 2022-12-20 | 2023-03-14 | 实时侠智能控制技术有限公司 | 机械臂的在线轨迹规划方法、装置及计算机可读介质 |
CN115781683B (zh) * | 2022-12-20 | 2024-05-24 | 实时侠智能控制技术有限公司 | 机械臂的在线轨迹规划方法、装置及计算机可读介质 |
CN117262257A (zh) * | 2023-09-15 | 2023-12-22 | 哈尔滨工业大学 | 一种空间机械臂抓捕非合作喷管的伺服控制方法 |
Also Published As
Publication number | Publication date |
---|---|
CN114952838B (zh) | 2023-05-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111399514B (zh) | 一种机器人时间最优轨迹规划方法 | |
CN114952838A (zh) | 一种基于末端测量反馈的机械臂关节轨迹规划方法 | |
CN110355751B (zh) | 控制装置和机器学习装置 | |
CN111230867B (zh) | 机器人运动控制方法、运动控制设备及机器人 | |
JP6351293B2 (ja) | ロボットシステム、および物品の製造方法 | |
CN101362511B (zh) | 基于四个***的飞机部件位姿调整协同控制方法 | |
JP3207728B2 (ja) | 冗長マニピュレータの制御方法 | |
CN113334393B (zh) | 一种机械臂控制方法、***、机器人及存储介质 | |
KR20180069031A (ko) | 로봇의 다이렉트 교시방법 | |
CN108908347B (zh) | 一种面向冗余移动机械臂容错型重复运动规划方法 | |
WO2000045229A1 (en) | Uncalibrated dynamic mechanical system controller | |
CN107457783B (zh) | 基于pd控制器的六自由度机械臂自适应智能检测方法 | |
JP3349652B2 (ja) | オフラインティーチング方法 | |
CN111515928B (zh) | 机械臂运动控制*** | |
CN115480583B (zh) | 飞行作业机器人的视觉伺服跟踪与阻抗控制方法 | |
Navarro-Alarcon et al. | Visually servoed deformation control by robot manipulators | |
CN107145640B (zh) | 中性浮力实验中漂浮基座与机械臂的动态标度规划方法 | |
Bratchikov et al. | Development of digital twin for robotic arm | |
Fahimi et al. | An alternative closed-loop vision-based control approach for Unmanned Aircraft Systems with application to a quadrotor | |
JP2020171989A (ja) | ロボット教示システム | |
CN115366115B (zh) | 一种机械臂控制方法及其控制***和计算机可读存储介质 | |
CN116652939A (zh) | 一种并联机器人无标定视觉伺服柔顺控制方法 | |
CN114840947A (zh) | 一种带约束的三自由度机械臂动力学模型 | |
JP2737325B2 (ja) | ロボットの軌道生成方式 | |
Yang | Research on cruise control of hydrostatic tractor based on CAN bus |
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 |