CN107436605B - 考虑多体耦合的水下机器人控制方法 - Google Patents

考虑多体耦合的水下机器人控制方法 Download PDF

Info

Publication number
CN107436605B
CN107436605B CN201710620365.8A CN201710620365A CN107436605B CN 107436605 B CN107436605 B CN 107436605B CN 201710620365 A CN201710620365 A CN 201710620365A CN 107436605 B CN107436605 B CN 107436605B
Authority
CN
China
Prior art keywords
base
force
mechanical arm
representing
matrix
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.)
Expired - Fee Related
Application number
CN201710620365.8A
Other languages
English (en)
Other versions
CN107436605A (zh
Inventor
刘继朝
方群
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN201710620365.8A priority Critical patent/CN107436605B/zh
Publication of CN107436605A publication Critical patent/CN107436605A/zh
Application granted granted Critical
Publication of CN107436605B publication Critical patent/CN107436605B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/04—Control of altitude or depth
    • G05D1/06—Rate of change of altitude or depth
    • G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种考虑多体耦合的水下机器人控制方法，包括以下步骤：1)利用多体系统运动学控制规划出水下机器人基座和机械臂的参考轨迹，该参考轨迹以速度信号的形式分别传递给机械臂和基座；2)机械臂的速度环接收速度信号，与运动学控制模块形成机械臂的位置环；3)组合体迭代动力学模块接收机械臂速度信号和基座参数，输出机械臂对基座的耦合作用力；4)基座控制器接收关于基座的参考轨迹、控制误差以及机械臂对基座的耦合作用力，计算出基座应该施加的控制力。

Description

考虑多体耦合的水下机器人控制方法
技术领域
本发明属于水下机器人控制技术领域，具体涉及一种考虑多体耦合的水下机器人控制方法。
背景技术
水下机器人是一个多体机构，各刚体之间有强耦合作用，严重影响机器人在水下环境的控制精度。必须应用多体系统动力学改进传统控制方式，提高控制系统的控制精度。
目前，针对机械臂的执行机构是伺服电机的情况，机械臂上的伺服电机只能处于速度控制模式，自身形成一个速度环。这种情况下，多体系统动力学无法直接应用于伺服电机，以对电机施加力控制。对当今的水下机器人而言，这样的控制能力无法满足要求。
发明内容
本发明的目的在于提供一种考虑多体耦合的水下机器人控制方法，该方法充分考虑机械臂的执行机构伺服电机和机械臂对基座的耦合作用，使水下机器人更好适应水下环境，并精准完成复杂的操作任务，比如对设备的打捞，捕获等。
为达到上述目的，本发明采用如下技术方案来实现：
考虑多体耦合的水下机器人控制方法，包括以下步骤：
1)利用多体系统运动学控制规划出水下机器人基座和机械臂的参考轨迹，该参考轨迹以速度信号的形式分别传递给机械臂和基座；
2)机械臂的速度环接收速度信号，与运动学控制模块形成机械臂的位置环；
3)组合体迭代动力学模块接收机械臂速度信号和基座参数，输出机械臂对基座的耦合作用力τarm；
4)基座控制器接收关于基座的参考轨迹、控制误差以及机械臂对基座的耦合作用力，计算出应该对基座施加的控制力τv。
本发明进一步的在于，步骤4)中，基座的控制力τv算法如下：
Figure BDA0001361546420000022
其中，上式等号左边代表不受控单基座的动力学项，等号右边是控制力与机械臂耦合力的和；Mv＝MRB+MA，MRB是基座的广义质量矩阵，MA是基座附加质量矩阵；
Figure BDA0001361546420000023
是基座加速度；Cv＝CRB+CA，CRB是基座本体引起的柯氏力和向心力项，为斜对称矩阵，CA是基座附加质量引起的柯氏力和向心力项；v代表基座速度；DRB(v)代表基座的粘性水动力，由摩擦力引起；
Figure BDA0001361546420000021
代表惯性坐标系下的恢复力，等于重力和浮力的合力；τv＝Bvuv代表基座的控制推力，Bv是推力分配矩阵，uv是推力器的推力阵列；τarm代表机械臂对基座的作用力。
本发明具有如下的有益效果：
1、运动学控制模块IK接收反馈的机械臂电机速度信号，规划出机械臂期望参考信号，并传递给机械臂伺服电机，对机械臂进行速度控制；
2、利用多体动力学算法，控制计算机接收机械臂运动信号，计算出机械臂每一个关节点的受力大小，层层推进，算出机械臂对基座的耦合作用力。基座控制器再综合基座的参考轨迹、控制误差以及机械臂对基座的耦合作用，计算出应该对基座施加的控制力，使基座更为平稳，提供更稳定的控制平台，提高整个系统的控制效果。
综上，控制系统通过以上两个执行机构(机械臂伺服电机和基座控制器)，可以实现对水下机器人的精准控制。
附图说明
图1.控制系统结构示意框图；
图2.考虑耦合作用的位置误差曲线；
图3.考虑耦合作用的姿态角误差曲线；
图4.考虑耦合作用的控制力曲线；
图5.不考虑耦合作用的位置误差；
图6.不考虑耦合作用的姿态角误差；
图7.不考虑耦合作用的控制律曲线。
具体实施方式
以下结合附图和实施例对本发明做出进一步的说明。
以安装一个六自由度机械臂的水下机器人对机械臂末端位姿的跟踪任务为例。
首先，运动学控制模块IK根据下述公式计算出基座和机械臂的参考轨迹：
Figure BDA0001361546420000031
式(1.1)中，
Figure BDA0001361546420000032
是计算出的参考轨迹，其中，前六个元素是基座的参考轨迹，后六个元素是机械臂的参考轨迹，将它们分别传给机械臂速度环和基座控制器。
Figure BDA0001361546420000033
与基座雅克比矩阵相关，
Figure BDA0001361546420000034
代表机械臂的线速度，ωee代表机械臂的角速度；Ka是误差反馈系数；
Figure BDA0001361546420000035
代表机械臂位置和姿态误差列向量。
机械臂的速度环根据伺服电机的不同，一般采用PI控制或者PID控制。基座控制器按照下式进行设计。
Figure BDA0001361546420000041
其中，上式等号左边代表不受控单基座的动力学项，等号右边是控制力与机械臂耦合力的和；Mv＝MRB+MA，MRB是基座的广义质量矩阵，MA是基座附加质量矩阵；
Figure BDA0001361546420000042
是基座加速度；Cv＝CRB+CA，CRB是基座本体引起的柯氏力和向心力项，为斜对称矩阵，CA是基座附加质量引起的柯氏力和向心力项；v代表基座速度；DRB(v)代表基座的粘性水动力，由摩擦力引起；
Figure BDA0001361546420000043
代表惯性坐标系下的恢复力，等于重力和浮力的合力；τv＝Bvuv代表基座的控制推力，Bv是推力分配矩阵，uv是推力器的推力阵列；τarm代表机械臂对基座的作用力。
这里，我们应用反馈线性化进行基座控制律设计，所设计的控制律如下：
Figure BDA0001361546420000044
Figure BDA0001361546420000045
Figure BDA0001361546420000046
上三式中，
Figure BDA00013615464200000410
代表推力分配矩阵Bv的逆矩阵；
Figure BDA0001361546420000047
是控制方程中的基座加速度；
Figure BDA0001361546420000048
是一个变化矩阵，
Figure BDA0001361546420000049
是惯性系到体坐标系的旋转矩阵，Jk,o是基座雅克比矩阵；η是基座位置和姿态组成的列向量；下标d代表期望值；下标e代表存在误差的量；Kpv、Kvv和Kiv分别是PID控制中的比例系数矩阵、微分系数矩阵和误差积分系数矩阵。
式(1.3)中，机械臂对基座的耦合力和力矩由牛顿欧拉迭代动力学计算，计算公式为：
Figure BDA0001361546420000051
Figure BDA0001361546420000052
式(1.5)～(1.6)中，Mi是包含附加质量的质量矩阵，
Figure BDA0001361546420000053
是包含附加惯量的、相对于质心的惯性张量，
Figure BDA0001361546420000054
是坐标系i的原点到连杆i的质心的矢径，di表示作用在连杆上的升力和阻力，
Figure BDA0001361546420000055
表示坐标系i-1的原点到坐标系i的原点的矢径，
Figure BDA0001361546420000056
是坐标系i-1的原点到连杆i的质心的矢径，
Figure BDA0001361546420000057
表示坐标系i-1的原点到连杆i的浮心的矢径。在坐标系中，
Figure BDA0001361546420000058
表示连杆i受到的浮力，gi表示连杆i受到的重力，
Figure BDA0001361546420000059
表示连杆i-1作用在连杆i上的力，
Figure BDA00013615464200000510
表示连杆i-1作用在连杆i上的力矩。默认升力、阻力和重力作用在质心。实际计算时，先用式(1.5)，从基座开始外推，计算机械臂连杆所受的惯性力和惯性力矩，然后利用式(1.6)进行内推，依次计算各个关节之间的作用力和力矩，直到计算出机械臂与基座之间的耦合力和耦合力矩。
声明示例基本参数：
基座参数如表1所示：
表1基座参数
Figure BDA00013615464200000511
Figure BDA0001361546420000061
水动力系数根据球形基座进行计算，如式(1.7)所示：
Figure BDA0001361546420000062
两个机械臂左右对称，这里只给出一个机械臂的参数如表2所示。机械臂的质心和浮心均假设在圆柱体机械臂的几何中心。两个机械臂对称安装，初始时刻两个机械臂的DH参数相同，如表3所示，在仿真过程中，机械臂保持初始构型不变。
表2机械臂参数
Figure BDA0001361546420000063
表3 DH参数
Figure BDA0001361546420000071
取Kd＝10I6×6，λ＝10，c＝10，控制力在每个自由度上的大小限制为[-100,100]N，仿真步长为0.01s，仿真时间为300s。
仿真结果如下：
考虑机械臂对基座的耦合作用，仿真结果如图2～图4所示：
不考虑机械臂对基座的耦合作用，仿真结果如图5～图7所示：
分别将图2与图5、图3与图6、图4与图7做对比分析可见，加入动力学控制，考虑机械臂对基座耦合作用后：
1)误差收敛更平稳，更迅速。
2)误差收敛后，振幅大幅度减小，尤其是姿态角误差(图6、图3)，振幅减半。
3)控制力更平稳，图7控制力振幅几乎是图4控制力振幅的5倍。
通过以上示例，可充分证明本发明的控制效果。对比传统运动学控制，展现了更快速、平稳和精确的控制优势。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (1)

1.考虑多体耦合的水下机器人控制方法，其特征在于，包括以下步骤：
1)利用多体系统运动学控制规划出水下机器人基座和机械臂的参考轨迹，该参考轨迹以速度信号的形式分别传递给机械臂和基座；
2)机械臂的速度环接收速度信号，与运动学控制模块形成机械臂的位置环；
3)组合体迭代动力学模块接收机械臂速度信号和基座参数，输出机械臂对基座的耦合作用力τarm；
步骤1)-3)的具体实现方法如下：
首先，运动学控制模块IK根据下述公式计算出基座和机械臂的参考轨迹：
Figure FDA0002709864430000011
式(1.1)中，
Figure FDA0002709864430000012
是计算出的参考轨迹，其中，前六个元素是基座的参考轨迹，后六个元素是机械臂的参考轨迹，将它们分别传给机械臂速度环和基座控制器；
Figure FDA0002709864430000013
与基座雅克比矩阵相关，
Figure FDA0002709864430000014
代表机械臂的线速度，ωee代表机械臂的角速度；Ka是误差反馈系数；
Figure FDA0002709864430000015
代表机械臂位置和姿态误差列向量；
机械臂的速度环根据伺服电机的不同，采用PI控制或者PID控制；
基座控制器按照下式进行设计：
Figure FDA0002709864430000016
其中，上式等号左边代表不受控单基座的动力学项，等号右边是控制力与机械臂耦合力的和；Mv＝MRB+MA，MRB是基座的广义质量矩阵，MA是基座附加质量矩阵；
Figure FDA0002709864430000018
是基座加速度；Cv＝CRB+CA，CRB是基座本体引起的柯氏力和向心力项，为斜对称矩阵，CA是基座附加质量引起的柯氏力和向心力项；v代表基座速度；DRB(v)代表基座的粘性水动力，由摩擦力引起；
Figure FDA0002709864430000017
代表惯性坐标系下的恢复力，等于重力和浮力的合力；τv＝Bvuv代表基座的控制推力，Bv是推力分配矩阵，uv是推力器的推力阵列；τarm代表机械臂对基座的作用力；
这里，应用反馈线性化进行基座控制律设计，所设计的控制律如下：
Figure FDA0002709864430000021
Figure FDA0002709864430000022
Figure FDA0002709864430000023
上三式中，
Figure FDA0002709864430000024
代表推力分配矩阵Bv的逆矩阵；
Figure FDA0002709864430000025
是控制方程中的基座加速度；
Figure FDA0002709864430000026
是一个变化矩阵，
Figure FDA0002709864430000027
是惯性系到体坐标系的旋转矩阵，Jk,o是基座雅克比矩阵；η是基座位置和姿态组成的列向量；下标d代表期望值；下标e代表存在误差的量；Kpv、Kvv和Kiv分别是PID控制中的比例系数矩阵、微分系数矩阵和误差积分系数矩阵；
式(1.3)中，机械臂对基座的耦合力和力矩由牛顿欧拉迭代动力学计算，计算公式为：
Figure FDA0002709864430000028
Figure FDA0002709864430000029
式(1.5)～(1.6)中，Mi是包含附加质量的质量矩阵，
Figure FDA00027098644300000210
是包含附加惯量的、相对于质心的惯性张量，
Figure FDA00027098644300000211
是坐标系i的原点到连杆i的质心的矢径，di表示作用在连杆上的升力和阻力，
Figure FDA00027098644300000212
表示坐标系i-1的原点到坐标系i的原点的矢径，
Figure FDA00027098644300000213
是坐标系i-1的原点到连杆i的质心的矢径，
Figure FDA00027098644300000214
表示坐标系i-1的原点到连杆i的浮心的矢径；在坐标系中，
Figure FDA00027098644300000215
表示连杆i受到的浮力，gi表示连杆i受到的重力，fi i表示连杆i-1作用在连杆i上的力，
Figure FDA00027098644300000216
表示连杆i-1作用在连杆i上的力矩；默认升力、阻力和重力作用在质心；实际计算时，先用式(1.5)，从基座开始外推，计算机械臂连杆所受的惯性力和惯性力矩，然后利用式(1.6)进行内推，依次计算各个关节之间的作用力和力矩，直到计算出机械臂与基座之间的耦合力和耦合力矩；
4)基座控制器接收关于基座的参考轨迹、控制误差以及机械臂对基座的耦合作用力，计算出应该对基座施加的控制力τv；具体计算如下：
Figure FDA0002709864430000031
其中，上式等号左边代表不受控单基座的动力学项，等号右边是控制力与机械臂耦合力的和；Mv＝MRB+MA，MRB是基座的广义质量矩阵，MA是基座附加质量矩阵；
Figure FDA0002709864430000033
是基座加速度；Cv＝CRB+CA，CRB是基座本体引起的柯氏力和向心力项，为斜对称矩阵，CA是基座附加质量引起的柯氏力和向心力项；v代表基座速度；DRB(v)代表基座的粘性水动力，由摩擦力引起；
Figure FDA0002709864430000032
代表惯性坐标系下的恢复力，等于重力和浮力的合力；τv＝Bvuv代表基座的控制推力，Bv是推力分配矩阵，uv是推力器的推力阵列；τarm代表机械臂对基座的作用力。
CN201710620365.8A 2017-07-26 2017-07-26 考虑多体耦合的水下机器人控制方法 Expired - Fee Related CN107436605B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710620365.8A CN107436605B (zh) 2017-07-26 2017-07-26 考虑多体耦合的水下机器人控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710620365.8A CN107436605B (zh) 2017-07-26 2017-07-26 考虑多体耦合的水下机器人控制方法

Publications (2)

Publication Number Publication Date
CN107436605A CN107436605A (zh) 2017-12-05
CN107436605B true CN107436605B (zh) 2020-11-27

Family

ID=60459971

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710620365.8A Expired - Fee Related CN107436605B (zh) 2017-07-26 2017-07-26 考虑多体耦合的水下机器人控制方法

Country Status (1)

Country Link
CN (1) CN107436605B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110096063B (zh) * 2019-05-06 2021-12-28 西北工业大学深圳研究院 一种基于测距仪的水下清洗机器人自主倒立着落船底控制方法
CN112527008B (zh) * 2020-12-21 2023-05-02 福州大学 基于低复杂度规定性能的作业型飞行机器人控制方法
CN113021360B (zh) * 2021-05-28 2021-08-03 苏州艾利特机器人有限公司 一种降低机器人对安装平台作用力的方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101733749A (zh) * 2009-12-22 2010-06-16 哈尔滨工业大学 空间机器人多领域统一建模与仿真系统
CN104483835A (zh) * 2014-11-06 2015-04-01 中国运载火箭技术研究院 一种基于t-s模糊模型的柔性航天器多目标综合控制方法
CN104793497A (zh) * 2015-04-20 2015-07-22 天津理工大学 基于多体系统离散时间传递矩阵法的机器人动力学建模方法
CN104808512A (zh) * 2015-03-03 2015-07-29 北京空间飞行器总体设计部 一种航天器多级驱动刚柔耦合响应的获取方法
CN106891335A (zh) * 2017-03-23 2017-06-27 北京空间飞行器总体设计部 一种空间机器人在轨抓捕过程的柔顺与协调控制方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101733749A (zh) * 2009-12-22 2010-06-16 哈尔滨工业大学 空间机器人多领域统一建模与仿真系统
CN104483835A (zh) * 2014-11-06 2015-04-01 中国运载火箭技术研究院 一种基于t-s模糊模型的柔性航天器多目标综合控制方法
CN104808512A (zh) * 2015-03-03 2015-07-29 北京空间飞行器总体设计部 一种航天器多级驱动刚柔耦合响应的获取方法
CN104793497A (zh) * 2015-04-20 2015-07-22 天津理工大学 基于多体系统离散时间传递矩阵法的机器人动力学建模方法
CN106891335A (zh) * 2017-03-23 2017-06-27 北京空间飞行器总体设计部 一种空间机器人在轨抓捕过程的柔顺与协调控制方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基座弹性影响下空间站柔性关节机械臂的鲁棒自适应滑模控制及双重弹性振动主动抑制;梁捷等;《载人航天》;20161231;第22卷(第6期);第790-795页 *

Also Published As

Publication number Publication date
CN107436605A (zh) 2017-12-05

Similar Documents

Publication Publication Date Title
CN106945020B (zh) 一种空间双机械臂系统运动协调控制方法
Kim et al. Cooperative aerial manipulation using multirotors with multi-dof robotic arms
Arleo et al. Control of quadrotor aerial vehicles equipped with a robotic arm
Moosavian et al. Free-flying robots in space: an overview of dynamics modeling, planning and control
CN104589349B (zh) 一种混合悬浮微重力环境下带有单关节机械臂的组合体自主控制方法
CN107436605B (zh) 考虑多体耦合的水下机器人控制方法
CN109015658B (zh) 一种用于捕获翻滚目标的空间双臂机器人控制方法
Caccavale et al. Cooperative impedance control for multiple UAVs with a robotic arm
CN112558621A (zh) 一种基于解耦控制的飞行机械臂系统
CN107263466A (zh) 空间机器人基于二次规划问题的基座无扰控制方法
CN111506095B (zh) 一种双刚体特征点间饱和固定时间相对位姿跟踪控制方法
CN107458630B (zh) 悬挂式主动重力补偿系统悬挂点跨尺度跟踪方法
CN107145640B (zh) 中性浮力实验中漂浮基座与机械臂的动态标度规划方法
Hirano et al. Caging-based grasp with flexible manipulation for robust capture of a free-floating target
CN112327892B (zh) 一种auv误差受限的抗扰控制方法
CN110032198A (zh) 一种水下特种机器人艇-手多体动力学建模方法及装置
Han et al. Capture and detumble of a non-cooperative target without a specific gripping point by a dual-arm space robot
CN110641738B (zh) 一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法
CN108829126A (zh) 一种考虑通讯延时的auv集群协调控制方法
CN111975770B (zh) 一种采用空间双臂机器人的自旋目标抓捕方法
CN117301064B (zh) 一种基于固定时间收敛的双臂机器人安全协同控制方法
Sagara et al. Performance comparison of control methods using a dual-arm underwater robot-Computed torque based control and resolved acceleration control for UVMS
Sagara et al. Digital RAC with a disturbance observer for underwater vehicle-manipulator systems
Wu et al. Contact dynamics and control of a space manipulator capturing a rotating object
Sagara et al. Digital resolved acceleration control of underwater robot with multiple manipulators

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
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20201127