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

Abstract

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

Description

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

技术领域

本发明属于水下机器人控制技术领域，具体涉及一种考虑多体耦合的水下机器人控制方法。

背景技术

水下机器人是一个多体机构，各刚体之间有强耦合作用，严重影响机器人在水下环境的控制精度。必须应用多体系统动力学改进传统控制方式，提高控制系统的控制精度。

目前，针对机械臂的执行机构是伺服电机的情况，机械臂上的伺服电机只能处于速度控制模式，自身形成一个速度环。这种情况下，多体系统动力学无法直接应用于伺服电机，以对电机施加力控制。对当今的水下机器人而言，这样的控制能力无法满足要求。

发明内容

本发明的目的在于提供一种考虑多体耦合的水下机器人控制方法，该方法充分考虑机械臂的执行机构伺服电机和机械臂对基座的耦合作用，使水下机器人更好适应水下环境，并精准完成复杂的操作任务，比如对设备的打捞，捕获等。

为达到上述目的，本发明采用如下技术方案来实现：

考虑多体耦合的水下机器人控制方法，包括以下步骤：

1)利用多体系统运动学控制规划出水下机器人基座和机械臂的参考轨迹，该参考轨迹以速度信号的形式分别传递给机械臂和基座；

2)机械臂的速度环接收速度信号，与运动学控制模块形成机械臂的位置环；

3)组合体迭代动力学模块接收机械臂速度信号和基座参数，输出机械臂对基座的耦合作用力τ_arm；

4)基座控制器接收关于基座的参考轨迹、控制误差以及机械臂对基座的耦合作用力，计算出应该对基座施加的控制力τ_v。

本发明进一步的在于，步骤4)中，基座的控制力τ_v算法如下：

其中，上式等号左边代表不受控单基座的动力学项，等号右边是控制力与机械臂耦合力的和；M_v＝M_RB+M_A，M_RB是基座的广义质量矩阵，M_A是基座附加质量矩阵；是基座加速度；C_v＝C_RB+C_A，C_RB是基座本体引起的柯氏力和向心力项，为斜对称矩阵，C_A是基座附加质量引起的柯氏力和向心力项；v代表基座速度；D_RB(v)代表基座的粘性水动力，由摩擦力引起；代表惯性坐标系下的恢复力，等于重力和浮力的合力；τ_v＝B_vu_v代表基座的控制推力，B_v是推力分配矩阵，u_v是推力器的推力阵列；τ_arm代表机械臂对基座的作用力。

本发明具有如下的有益效果：

1、运动学控制模块IK接收反馈的机械臂电机速度信号，规划出机械臂期望参考信号，并传递给机械臂伺服电机，对机械臂进行速度控制；

2、利用多体动力学算法，控制计算机接收机械臂运动信号，计算出机械臂每一个关节点的受力大小，层层推进，算出机械臂对基座的耦合作用力。基座控制器再综合基座的参考轨迹、控制误差以及机械臂对基座的耦合作用，计算出应该对基座施加的控制力，使基座更为平稳，提供更稳定的控制平台，提高整个系统的控制效果。

综上，控制系统通过以上两个执行机构(机械臂伺服电机和基座控制器)，可以实现对水下机器人的精准控制。

附图说明

图1.控制系统结构示意框图；

图2.考虑耦合作用的位置误差曲线；

图3.考虑耦合作用的姿态角误差曲线；

图4.考虑耦合作用的控制力曲线；

图5.不考虑耦合作用的位置误差；

图6.不考虑耦合作用的姿态角误差；

图7.不考虑耦合作用的控制律曲线。

具体实施方式

以下结合附图和实施例对本发明做出进一步的说明。

以安装一个六自由度机械臂的水下机器人对机械臂末端位姿的跟踪任务为例。

首先，运动学控制模块IK根据下述公式计算出基座和机械臂的参考轨迹：

式(1.1)中，是计算出的参考轨迹，其中，前六个元素是基座的参考轨迹，后六个元素是机械臂的参考轨迹，将它们分别传给机械臂速度环和基座控制器。与基座雅克比矩阵相关，代表机械臂的线速度，ω_ee代表机械臂的角速度；K_a是误差反馈系数；代表机械臂位置和姿态误差列向量。

机械臂的速度环根据伺服电机的不同，一般采用PI控制或者PID控制。基座控制器按照下式进行设计。

其中，上式等号左边代表不受控单基座的动力学项，等号右边是控制力与机械臂耦合力的和；M_v＝M_RB+M_A，M_RB是基座的广义质量矩阵，M_A是基座附加质量矩阵；是基座加速度；C_v＝C_RB+C_A，C_RB是基座本体引起的柯氏力和向心力项，为斜对称矩阵，C_A是基座附加质量引起的柯氏力和向心力项；v代表基座速度；D_RB(v)代表基座的粘性水动力，由摩擦力引起；代表惯性坐标系下的恢复力，等于重力和浮力的合力；τ_v＝B_vu_v代表基座的控制推力，B_v是推力分配矩阵，u_v是推力器的推力阵列；τ_arm代表机械臂对基座的作用力。

这里，我们应用反馈线性化进行基座控制律设计，所设计的控制律如下：

上三式中，代表推力分配矩阵B_v的逆矩阵；是控制方程中的基座加速度；是一个变化矩阵，是惯性系到体坐标系的旋转矩阵，J_k,o是基座雅克比矩阵；η是基座位置和姿态组成的列向量；下标d代表期望值；下标e代表存在误差的量；K_pv、K_vv和K_iv分别是PID控制中的比例系数矩阵、微分系数矩阵和误差积分系数矩阵。

式(1.3)中，机械臂对基座的耦合力和力矩由牛顿欧拉迭代动力学计算，计算公式为：

式(1.5)～(1.6)中，M_i是包含附加质量的质量矩阵，是包含附加惯量的、相对于质心的惯性张量，是坐标系i的原点到连杆i的质心的矢径，d_i表示作用在连杆上的升力和阻力，表示坐标系i-1的原点到坐标系i的原点的矢径，是坐标系i-1的原点到连杆i的质心的矢径，表示坐标系i-1的原点到连杆i的浮心的矢径。在坐标系中，表示连杆i受到的浮力，gⁱ表示连杆i受到的重力，表示连杆i-1作用在连杆i上的力，表示连杆i-1作用在连杆i上的力矩。默认升力、阻力和重力作用在质心。实际计算时，先用式(1.5)，从基座开始外推，计算机械臂连杆所受的惯性力和惯性力矩，然后利用式(1.6)进行内推，依次计算各个关节之间的作用力和力矩，直到计算出机械臂与基座之间的耦合力和耦合力矩。

声明示例基本参数：

基座参数如表1所示：

表1基座参数

水动力系数根据球形基座进行计算，如式(1.7)所示：

两个机械臂左右对称，这里只给出一个机械臂的参数如表2所示。机械臂的质心和浮心均假设在圆柱体机械臂的几何中心。两个机械臂对称安装，初始时刻两个机械臂的DH参数相同，如表3所示，在仿真过程中，机械臂保持初始构型不变。

表2机械臂参数

表3 DH参数

取K_d＝10I_6×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 (2)

1.考虑多体耦合的水下机器人控制方法，其特征在于，包括以下步骤：

1)利用多体系统运动学控制规划出水下机器人基座和机械臂的参考轨迹，该参考轨迹以速度信号的形式分别传递给机械臂和基座；

2)机械臂的速度环接收速度信号，与运动学控制模块形成机械臂的位置环；

3)组合体迭代动力学模块接收机械臂速度信号和基座参数，输出机械臂对基座的耦合作用力τ_arm；

4)基座控制器接收关于基座的参考轨迹、控制误差以及机械臂对基座的耦合作用力，计算出应该对基座施加的控制力τ_v。

2.根据权利要求1所述的考虑多体耦合的水下机器人控制方法，其特征在于，步骤4)中，基座的控制力τ_v算法如下：

<mrow> <msub> <mi>M</mi> <mi>v</mi> </msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>+</mo> <msub> <mi>C</mi> <mi>v</mi> </msub> <mi>v</mi> <mo>+</mo> <msub> <mi>D</mi> <mrow> <mi>R</mi> <mi>B</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>v</mi> <mo>)</mo> </mrow> <mi>v</mi> <mo>+</mo> <msub> <mi>g</mi> <mrow> <mi>R</mi> <mi>B</mi> </mrow> </msub> <mrow> <mo>(</mo> <msubsup> <mi>R</mi> <mi>B</mi> <mi>I</mi> </msubsup> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>&amp;tau;</mi> <mi>v</mi> </msub> <mo>+</mo> <msub> <mi>&amp;tau;</mi> <mrow> <mi>a</mi> <mi>r</mi> <mi>m</mi> </mrow> </msub> </mrow>

其中，上式等号左边代表不受控单基座的动力学项，等号右边是控制力与机械臂耦合力的和；M_v＝M_RB+M_A，M_RB是基座的广义质量矩阵，M_A是基座附加质量矩阵；是基座加速度；C_v＝C_RB+C_A，C_RB是基座本体引起的柯氏力和向心力项，为斜对称矩阵，C_A是基座附加质量引起的柯氏力和向心力项；v代表基座速度；D_RB(v)代表基座的粘性水动力，由摩擦力引起；代表惯性坐标系下的恢复力，等于重力和浮力的合力；τ_v＝B_vu_v代表基座的控制推力，B_v是推力分配矩阵，u_v是推力器的推力阵列；τ_arm代表机械臂对基座的作用力。