[发明专利]考虑多体耦合的水下机器人控制方法

说明书

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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接收反馈的机械臂电机速度信号，规划出机械臂期望参考信号，并传递给机械臂伺服电机，对机械臂进行速度控制；