CN113419545B - 一种具有滑移管制的水下机器人优化控制方法 - Google Patents

Abstract

本发明涉及一种具有滑移管制的水下机器人优化控制方法。所述水下机器人优化控制方法包括：首先，分析作用在水下机器人本体的外力，具体包括水动力、推进器推力和重/浮力，并建立水下机器人前向通道和偏航通道的动力学模型；然后，利用滑模控制策略，设计稳定跟踪上期望速度和期望航向角所需要的前向驱动力和偏航驱动力矩，同时，将水下机器人本体所受的力和期望力之和转化至支撑腿髋关节处，并解算出地面对支撑腿的反作用力；在此基础上，面向行走稳定、转向灵活的运动需求，利用最优化方法，设计速度和航向最优控制策略。根据本发明实施例的具有滑移管制的水下机器人优化控制方法能够兼顾行走稳定性、转向灵活性以及节能性。

Description

一种具有滑移管制的水下机器人优化控制方法

技术领域

本发明属于水下机器人控制技术领域，涉及一种具有滑移管制的水下机器人优化控制方法，具体为一种由六个推进器和六条半圆形刀锋腿混合驱动的水下机器人在攀爬运动模态下的速度和航向优化控制方法。

背景技术

水下机器人是海洋工程的重要装备之一，通过搭载不同类型的传感器和执行器，水下机器人可用于海洋资源开发和利用、海洋牧场建设、水下文化遗产保护、搜救和打捞、水库大坝的安全检查、船舶清洗和日常保养、水下安全等领域。

水下机器人要完成水下作业任务，不仅需要有大范围的水下巡游能力，而且需要在不平整工作面上执行攀爬或行走的能力。为此，由六个推进器和六条半圆形刀锋腿混合驱动的新概念水下机器人应运而生，它具有水中巡游、壁面攀爬、直立行走和倒立行走的能力，具有重要的经济和社会效益。

腿式机器人的控制策略大致可以分为两类：基于模型的控制方法和基于学习的控制方法。前者利用水下机器人的动力学模型，采用李雅普诺夫稳定定理或庞加莱映射定理设计合适的控制律。后者将步态参数化，设计合适的性能指标，在大量先验数据支撑下，学习面向性能指标最优的步态或控制力矩。

目前尚无文献开展这类由六个推进器和六条半圆形刀锋腿混合驱动水下机器人控制的研究。针对陆地上的半圆形刀锋腿机器人的控制研究主要有：1)Uluc Saranli等针对半圆形刀锋腿机器人控制问题，在生成六条腿髋关节期望旋转角度的基础上，设计了一种不基于动力学模型的比例-微分控制器，使髋关节旋转角度收敛至期望值。2)Ya-ChengChou等在线性倒立摆的基础上，提出了滚动倒立摆模型，并利用比例-微分控制器，使六足机器人实现变速跳跃运动。3)Yasemin Ozkan Aydin等设计了半圆形刀锋腿机器人的优化控制方法，通过优化比例-微分控制参数和触地角，实现半圆形刀锋腿机器人的定速和越障运动。

不同于陆地上的半圆形刀锋腿机器人的控制研究，由六个推进器和六条半圆形刀锋腿混合驱动的水下机器人速度和航向优化控制方法设计上存在以下难点：1、如何将这类复杂的多体机器人系统的受力因素进行合理的数学表征；2、如何衡量支撑腿纵向滑移和侧向滑移对这类水下机器人运动的影响；3、如何设计一种兼顾行走稳定性和转向灵活性的优化控制方法。

发明内容

(1)技术问题

针对一类由六个推进器和六条半圆形刀锋腿混合驱动水下机器人的速度和航向控制需求，在分析水下机器人所受的水动力、推进器和腿的驱动力、支撑腿与地面作用力等基础上，分析这类机器人的运动机理，提出一种兼顾行走稳定性和转向灵活性的混合驱动具有滑移管制的水下机器人优化控制方法。

(2)技术方案

针对由六个推进器和六条半圆形刀锋腿混合驱动水下机器人行走易打滑问题，本发明提供了一种具有滑移管制的水下机器人优化控制方法。

所述水下机器人包括：六个推进器、六条刀锋腿和机器人本体，其中，四个推进器垂直布置，两个推进器水平布置，六条半圆形刀锋腿的髋关节与机器人本体左右两侧的六个驱动电机连接。

所述具有滑移管制的水下机器人优化控制方法，其利用所述的与地球固连的地面坐标系和与机器人本体固连的体坐标系来描述水下机器人的运动。包括以下步骤：

步骤1：建立地面坐标系和与水下机器人本体固连的体坐标系，确定作用在水下机器人本体的外力；

步骤2：建立水下机器人前向通道和偏航通道的动力学模型，利用滑模控制策略，确定水下机器人稳定跟踪上期望速度和期望航向角所需要的前向驱动力和偏航驱动力矩；同时，建立水下机器人垂向通道、侧向通道、横滚通道、俯仰通道的动力学模型，并利用比例控制策略，确定水下机器人在四个通道上运动状态保持稳定的期望驱动力；

步骤3：将水下机器人本体前向驱动力和偏航驱动力矩以及在四个通道上运动状态保持稳定的期望驱动力转化至支撑腿髋关节处，计算地面对支撑腿的反作用力；

步骤4：针对步态约束需求，且兼顾行走稳定性和转向灵活性的运动需求，基于最优控制律，对水下机器人的速度和航向进行控制。

根据本发明的示例性实施例，作用在水下机器人本体的外力包括：水动力、推进器推力、重力和浮力，其中，水动力包括：理想流体惯性力、流体黏性位置力和流体黏性阻尼力。

根据本发明的示例性实施例，步骤2中利用滑模控制策略，确定水下机器人稳定跟踪上期望速度和期望航向角所需要的前向驱动力和偏航驱动力矩时，所需要的前向驱动力u_fx和偏航驱动力矩u_my分别为：

其中，m₀为水下机器人本体的质量，λ₁₁为水下机器人本体的前向附加质量，λ₅₅为水下机器人本体的偏航附加转动惯量，J_y为水下机器人本体的偏航转动惯量；k₁、k₂、k₃、k₄和λ均为大于零的常数；ψ、ψ_d分别为水下机器人的当前航向角和参考航向角；滑模面s₁和s₂分别定义为s₁＝v_x-v_xd，

v_x和v_xd分别为水下机器人沿体坐标系Ox方向的当前速度和期望速度，w_y为水下机器人绕体坐标系Oy方向旋转的角速度；X_B和X_G分别为水下机器人浮力、重力在体坐标系Ox方向的分量，M_By为水下机器人浮力矩在体坐标系Oy方向的分量；X_αμ和X_wμ为水下机器人绕体坐标系Ox方向旋转的流体黏性位置力和流体黏性阻尼力，M_αμy和M_wμy分别为水下机器人绕体坐标系Oy方向旋转的流体黏性位置力矩和流体黏性阻尼力矩。

根据本发明的示例性实施例，步骤2中建立水下机器人垂向通道、侧向通道、横滚通道、俯仰通道的动力学模型，并利用比例控制策略，确定水下机器人在这四个通道上运动状态保持稳定的期望驱动力时，期望垂向驱动力u_fy、期望侧向驱动力u_fz、绕体坐标系Ox旋转的期望横滚驱动力矩u_mx和绕体坐标系Oz旋转的期望俯仰驱动力矩u_mz可设计为

其中，k₅、k₆、k₇和k₈均为大于零的常数；w_x和w_z为水下机器人绕体坐标系Ox和Oz方向旋转的角速度；v_y和v_z为水下机器人沿体坐标系Oy和Oz方向的速度；Y_B、Z_B和Y_G、Z_G分别为水下机器人浮力、重力在机器人本体垂向和侧向的分量；M_Bx、M_Bz分别为水下机器人浮力矩在体坐标系Oy和Oz方向的分量；Y_αμ和Z_αμ分别为水下机器人沿体坐标系Oy和Oz方向的流体黏性位置力，Y_wμ和Z_wμ分别为水下机器人沿体坐标系Oy和Oz方向的流体黏性阻尼力，M_αμx和M_αμz分别为水下机器人体坐标系Ox和Oz方向旋转的流体黏性位置力矩，M_wμx和M_wμz分别为水下机器人体坐标系Ox和Oz方向旋转的流体黏性阻尼力矩。

根据本发明的示例性实施例，步骤3中将水下机器人本体前向驱动力和偏航驱动力矩以及在四个通道上运动状态保持稳定的期望驱动力转化至支撑腿髋关节处，计算地面对支撑腿的反作用力时，

水下机器人本体作用于第i个腿髋关节处的前向力、垂向力和侧向力分别为F_ix、F_iy和F_iz可表示为：

其中，i＝1,2,…,6，X_T、Y_T、Z_T分别为六个推进器推力在体坐标系Ox、Oy和Oz方向上的分量，M_Tx、M_Ty、M_Tz分别为六个推进器推力在体坐标系Ox、Oy和Oz方向上形成的力矩；d₁、d₂分别为水下机器人腿髋关节之间的纵向和侧向距离；a_i为第i条腿的触地状态，当a_i＝1时，表明第i条腿已触地，当a_i＝0时，表明第i条腿未触地；

地面对支撑腿的反作用力在地面坐标系O₀x₀、O₀y₀和O₀z₀方向上的分量N_ix、N_iy和N_iz可表示为

其中，i＝1,2,…,6；q_i为第i条腿的旋转角度；τ_bi和M_ifz分别为将力F_ix、F_iy和F_iz由C_i平移至D_i产生额外的俯仰力矩和横滚力矩；τ_wi为第i条刀锋腿绕C_i点旋转时产生的旋转阻尼力矩，τ_i为第i条刀锋腿的驱动电机所产生的驱动力矩；l_r为刀锋腿的半径，C_i为第i条刀锋腿的髋关节位置，D_i为第i条刀锋腿的触地点位置。

根据本发明的示例性实施例，步骤4中最优控制律为：

其中，i＝1,…,4，Γ_i为大于零的常数，μ_x为刀锋腿纵向摩擦系数，μ_z为刀锋腿侧向摩擦系数。

(3)有益效果

根据本发明实施例的具有滑移管制的水下机器人优化控制方法能够兼顾行走稳定性、转向灵活性以及节能性。

附图说明

图1为混合驱动水下机器人速度和航向控制系统设计思路原理框图。

图2为混合驱动水下机器人定义的地面坐标系和体坐标系。

图3为混合驱动水下机器人六条腿髋关节位置示意图。

图4为混合驱动水下机器人六个推进器安装位置示意图。

图5为混合驱动水下机器人本体受到的合力、合力矩和半圆形支撑腿的分力示意图。

图6为混合驱动水下机器人本体施加在支撑腿髋关节的力平移至触地点的示意图。

图7为驱动电机施加在支撑腿髋关节的力矩和旋转阻尼力矩的示意图。

图8为混合驱动水下机器人以期望速度和恒定期望航向角运动时机器人本体前向速度变化曲线及前向速度误差曲线。

图9为混合驱动水下机器人以期望速度和恒定期望航向角运动时机器人航向角变化曲线及航向角误差曲线。

图10为混合驱动水下机器人以期望速度和恒定期望航向角运动时六个推进器推力变化曲线。

图11为混合驱动水下机器人以期望速度和恒定期望航向角运动时驱动电机施加在六条腿髋关节处的力矩变化曲线。

图12为混合驱动水下机器人以期望速度跟踪时变航向角时机器人本体前向速度变化曲线及前向速度误差曲线。

图13为混合驱动水下机器人以期望速度跟踪时变航向角时机器人航向角变化曲线及航向角误差曲线。

图14为混合驱动水下机器人以期望速度跟踪时变航向角时机器人质心在xOz平面的运动曲线及机器人质心在Oy方向上的变化曲线。

图15为混合驱动水下机器人以期望速度跟踪时变航向角时六个推进器推力变化曲线。

图16为混合驱动水下机器人以期望速度跟踪时变航向角时驱动电机施加在六条腿髋关节处的力矩变化曲线。

具体实施方式

在介绍具体实施方式之前，首先对混合驱动水下机器人进行以下几点假设：

假设一：半圆形刀锋腿和机器人本体均为刚体，且机器人本体为长方体，关于纵平面xOy和横平面xOz均对称；

假设二：水下机器人完全浸没于流体中，并处于全沾湿状态；

假设三：流体动力位置力和阻尼力满足线性假设；

假设四：不考虑半圆形刀锋腿的质量；

假设五：六条刀锋腿的驱动电机均匀对称地固连于机器人本体两侧，且中间一组刀锋腿固连于机器人本体侧向的中心点处；同时，机器人本体的重心在六台驱动电机构成的平面内。

下面结合附图对实施例进行阐述和说明：

其次，对附图进行阐述和说明：

图1为混合驱动水下机器人速度和航向控制系统设计思路原理框图。

具体地，分析作用在水下机器人本体的外力，包括水动力、推进器推力和重/浮力。推导水下机器人前向通道和偏航通道的动力学模型；利用滑模控制策略，设计稳定跟踪上期望前向速度和期望航向角所需要的前向驱动力和偏航驱动力矩；将水下机器人本体所受的力或期望力转化至支撑腿髋关节处；在此基础上，解算出地面对支撑腿的反作用力；针对步态约束需求，且兼顾行走稳定性和转向灵活性的运动需求，利用最优化方法，设计具有滑移管制的混合型水下机器人航速和航向优化控制方法。

图2为混合驱动水下机器人定义的地面坐标系和体坐标系。地面坐标系O₀x₀y₀z₀的各轴与地球固连，即相对地面静止不动。地面系的原点选在行走初始时刻机器人本体的重心；O₀x₀轴在水平面内并指向前进方向；O₀y₀轴铅直向上；O₀z₀轴垂直于O₀x₀和O₀y₀轴，其方向使该坐标系成为右手坐标系。体坐标系Oxyz也称固连系。该坐标系与机器人本体固连，也就是相对机器人本体静止不动的坐标系。体坐标系的原点选在机器人本体的重心；Ox轴沿机器人本体纵轴并指向前方；Oy轴垂直于Ox轴并指向上方；Oz轴垂直于轴Ox和Oy，其方向满足右手坐标系旋转法则。

图3为混合驱动水下机器人六条腿髋关节位置示意图。六条腿均匀布置于机器人本体的两侧，且腿3与腿4的连线经过机器人本体的中点并与体坐标系中的Ox轴垂直。同时，d₁为腿1与腿2(腿3与腿4、腿5与腿6)之间的距离，d₂为腿1与腿3(腿3与腿5、腿2与腿4、腿4与腿6)之间的距离。

图4为混合驱动水下机器人六个推进器安装位置示意图。4个推进器垂直布置，2个推进器水平布置，这6个推进器皆内嵌于机器人本体中。同时，l₁为垂直推进器1与垂直推进器2(垂直推进器3与垂直推进器4)之间的距离，l₂为垂直推进器1与垂直推进器4(垂直推进器2与垂直推进器3)之间的距离，l₃为水平推进器5与水平推进器6之间的距离。

图5为混合驱动水下机器人本体受到的合力、合力矩和半圆形支撑腿的分力示意图。具体地，水下机器人本体受到的三个合力为X_b、Y_b和Z_b，三个合力矩为M_bx、M_by和M_bz；水下机器人本体作用于支撑腿髋关节的三个分力为F_ix、F_iy和F_iz，其中下标i为六个腿的编号。

图6为混合驱动水下机器人本体施加在支撑腿髋关节的力平移至触地点的示意图。具体为水下机器人本体施加在支撑腿髋关节的力F_ix、F_iy和F_iz由髋关节C_i点平移至触地点D_i的示意图，平移后将产生额外的俯仰力矩τ_bi和横滚力矩M_ifz。此外，图6(b)中的q_i为第i条腿的旋转角度，l_r为腿的半径。

图7为驱动电机施加在支撑腿髋关节的力矩和旋转阻尼力矩的示意图。具体地，左图中τ_i为安装于机器人本体上的驱动电机所产生的驱动力矩；τ_bi为F_ix、F_iy和F_iz由C_i平移至D_i时产生的额外俯仰力矩；τ_wi为全浸没于水中的刀锋腿围绕C_i点旋转时产生旋转阻尼力矩。图7中右图描述了在电机驱动力矩τ_i和机器人本体作用于髋关节力F_ix、F_iy和F_iz共同作用下地面对支撑腿的反作用力N_ix、N_iy和N_iz。

最后，结合实施例和附图对本发明进行描述：

具有滑移管制的混合驱动水下机器人速度和航向最优控制器的设计步骤如下：

步骤1：建立地面坐标系和与水下机器人本体固连的体坐标系；分析作用在水下机器人本体的外力，具体包括水动力、推进器推力和重/浮力。

实施例所建立的地面坐标系和与水下机器人本体固连的体坐标系如图2所示。

为了研究水下机器人本体的动力学特性，选取以下运动参数：x，z，ψ，v_x，v_y，v_z，w_x，w_y，w_z，q_i(i＝1,...,6)。

具体地，x，z分别为水下机器人本体浮心在地面坐标中前向和侧向方向的投影；ψ为水下机器人本体的偏航角；v_x，v_y，v_z分别为机器人本体在体坐标系下的前向、垂向和侧向速度；w_x，w_y，w_z分别为机器人本体绕体坐标系Ox轴、Oy轴和Oz轴的角速度。如图7(b)所示，q_i(i＝1,...,6)为水下机器人六条腿的旋转角度，规定顺时针为正，定义髋关节C_i处刀锋腿的切线为向量C_iS，且规定向量C_iS与Ox轴一致时角度q_i为零。

水下机器人六条腿通常是以离散的方式接触地面。为此，定义矩阵

来描述水下机器人六条腿触地状态：

其中，六条腿的序号定义如图3所示，各腿是否触地可以利用贴于刀锋腿的电阻式薄膜压力传感器(FSR402)进行判断。

例如，当水下机器人的腿1-4-5与腿2-3-6交替支撑与摆动时，矩阵A定义为

接下来分析作用在水下机器人本体的外力，具体包括水动力、推进器推力和重/浮力。

①机器人本体所受的水动力

水下机器人本体所受的水动力可以分为三类：理想流体惯性力、流体黏性位置力和流体黏性阻尼力。

考虑到机器人本体关于纵平面xOy和横平面xOz均对称，理想流体惯性力在体坐标系中可表示为

其中，λ₁₁、λ₂₂和λ₃₃分别为水下机器人的前向、纵向和侧向的附加质量，λ₄₄、λ₅₅和λ₆₆为水下机器人的附加横滚、偏航和俯仰转动惯量。

流体黏性位置力在体坐标系中的分量为

其中，ρ为水的密度，

v为水下机器人的速度，v_x、v_y和v_z分别为水下机器人速度在Ox轴、Oy轴和Oz轴的分量；S_x、S_y和S_z分别为机器人本体在Ox、Oy和Oz方向的横截面积；C_x、C_y和C_z分别为以横截面积S_x、S_y和S_z为特征面积的阻力因数；

为水下机器人本体的俯仰力矩因数对攻角α的位置导数；攻角α＝-arctan(v_y/v_x)；

为水下机器人本体的横滚力矩因数对侧滑角β的位置导数；

为水下机器人本体的偏航力矩因数对侧滑角β的位置导数；侧滑角角

流体黏性阻尼力在体坐标系中的分量为

其中，L为水下机器人本体的长度；

水下机器人升力因数对角速度w_z的旋转导数，

水下机器人侧力因数对角速度w_y的旋转导数；

水下机器人横滚力矩因数对角速度w_x的旋转导数，

为无量纲角速度；

水下机器人偏航力矩因数对角速度w_y的旋转导数，

为无量纲角速度；

水下机器人俯仰力矩因数对角速度w_z的旋转导数，

为无量纲角速度。

可推导水动力在体坐标的力和力矩为

②推进器作于机器人本体的力

混合型水下机器人本体配有六个推进器，其推力分布如图4所示。六个推进器推力在体坐标系上的投影为

其中，T_i(i＝1,…,6)为第i个推进器的推力值；C_t为推力分配矩阵，其可表示为

其中，l_i(i＝1,2,3)为水下机器人各推进器间的距离，具体定义如图4所示。

③作于机器人本体的重/浮力

水下机器人本体的浮力为B，浮力在体坐标系的各分量为

其中，θ为水下机器人本体的俯仰角，

为水下机器人本体的横滚角。

同理，重力G在体坐标系的各分量为

其中，重力G＝m₀g，m₀为水下机器人本体的质量。

浮力矩在体坐标系的分量为

其中，x_c、y_c和z_c分别为机器人本体浮心距质心的距离(浮心在前为正)，浮心下移量(向下移为负)和浮心侧移量(向右侧为正)。

步骤2：推导水下机器人前向通道和偏航通道的动力学模型；利用滑模控制策略，设计稳定跟踪上期望前向速度和期望航向角所需要的前向驱动力和偏航驱动力矩。

根据步骤1中的动力学分析结果，可推导出水下机器人本体前向和偏航通道的动力学方程为

其中，ΔX和ΔM_y为模型不确定和未知外界扰动之和，且假设模型不确定和未知外界扰动之和的上界已知，即|ΔX|＜D_x和|ΔM_y|＜D_y，D_x和D_y为大于零的常数；u_fx和u_my分别为推进器和刀锋腿共同作用下的前向驱动力和偏航驱动力矩。

定义期望的运动速度和航向角分别为v_xd和ψ_d，设计如下所示的滑模面：

其中，λ为大于零的常数。

设计如下滑模趋近律：

其中，k_i(i＝1,…,4)均为大于零的常数，为了保证系统的稳定性需

设计如下控制律：

同理，根据步骤2中的动力学分析结果，可推导出水下机器人本体垂向、侧向、横滚和俯仰通道的动力学方程为

其中，u_fy、u_fz、u_mx和u_mz分别为推进器和刀锋腿共同作用下的垂向、侧向期望驱动力和横滚、俯仰方向的期望驱动力矩。

设计如下控制律：

其中，k₅、k₆、k₇和k₈均为大于零的常数。

上述控制律可进一步写为

步骤3：将水下机器人本体所受的力或期望力转化至支撑腿髋关节处；在此基础上，解算出地面对支撑腿的反作用力。

混合型水下机器人推进器和刀锋腿的驱动力最终反映在机器人本体的惯性力、水动力和腿对地面的作用力上。通过分析腿对地面的作用力，可根据地面的摩擦系数判断腿是否会在纵向或侧向打滑，进而为混合型水下机器人的控制器设计提供技术参考。

为此，本步骤将分析水下机器人支撑腿作用于地面的力。

为了使水下机器人按期望的航速和航向运动且在其他四个自由度上保持稳定，本步骤利用上一步骤解算出的控制律u_fx、u_fy、u_fz、u_mx、u_my和u_mz代替水下机器人本体在各方向受到的水动力和惯性力。

如图5所示，水下机器人本体施加给支撑腿的三个力(X_b、Y_b、Z_b)和三个力矩(M_bx、M_by、M_bz)表示为

为了分析各支撑腿对地面的作用力，利用电阻式薄膜压力传感器获取触地矩阵A，可将水下机器人本体受到的力分解至各支撑腿的髋关节处，具体分解方法如下

其中，F_ix、F_iy和F_iz分别为水下机器人本体作用于第i个腿的前向力、垂向力和侧向力，如图5右图所示。

接下来，将水下机器人本体作用于刀锋腿髋关节的力F_ix、F_iy和F_iz平移至腿与地面的接触点，即如图6所示，将力F_ix、F_iy和F_iz由C_i平移至D_i。在平移过程中，将产生额外的俯仰力矩τ_bi和横滚力矩M_ifz，其可表示为

其中，C_i为第i条刀锋腿髋关节位置；D_i为第i条刀锋腿的触地点；q_i为水下机器人第i条刀锋腿的旋转角度；l_r为半圆形刀锋腿的半径。

由于刀锋腿全浸没于水中，当第i个刀锋腿绕C_i点旋转时会产生旋转阻尼力矩τ_wi，其可表示为

其中，

为各刀锋腿旋转角度变化率；k_w1和k_w2为待辨识的参数。

如图7(a)所示，O_i为各刀锋腿圆心；F_τix、F_τiy分别为各刀锋腿作用于触地点的前向力和垂向力；τ_i为安装于机器人本体上的驱动电机所产生的驱动力矩；τ_bi为F_ix、F_iy和F_iz由C_i平移至D_i时产生的额外力矩；τ_wi为全浸没于水中的刀锋腿围绕C_i点旋转时产生旋转阻尼力矩。

图7(a)中

线段C_iD_i的长度为

因此，当力矩τ_i、τ_wi和τ_bi共同施加于第i个腿时，其对地面的作用力分别为

其中，F_τix、F_τiy的方向定义如图7(a)所示。

如图7(b)所示，C_iS为髋关节C_i处刀锋腿的切线；将水下机器人本体作用于C_i点的力F_ix、F_iy、F_iz和力矩τ_i、τ_wi转化至地面D_i点。地面对第i条刀锋腿的前向、垂向、侧向反作用力N_ix、N_iy和N_iz可表示为

其中，mod(·)为求余函数。

步骤4：针对行走稳定性和转向灵活性的运动需求，利用最优化方法，设计最优控制策略。

为了使混合型水下机器人的各刀锋腿在地面的纵向不打滑，需确保刀锋腿所能提供的最大牵引力大于地面对刀锋腿纵向的反作用力，即

|N_ix|≤μ_xN_iy

其中，μ_x为刀锋腿纵向摩擦系数。

同时，为了使混合型水下机器人能转向，需确保地面产生的偏航力矩大于地面所能提供的最大阻尼力矩，即

其中，μ_z为刀锋腿侧向摩擦系数。

为了让混合型水下机器人兼顾行走稳定性、转向灵活性和能耗最优性，设计如下性能指标函数：

其中，Γ_i(i＝1,…,4)为大于零的常数。

最大化上述性能指标，即六个推进器的推力和支撑腿髋关节的力矩，即可求得兼顾行走稳定性、转向灵活性和能耗最优性的控制输入量。

为了使水下机器人按照三足步态行走，支撑腿关节力矩由上述算法求解，摆动腿髋关节力矩依据支撑腿关节角度的变化采用比例微分控制方法求解，具体求解过程如下：

根据支撑腿关节角度的变化，规划摆动腿期望角度q_jd：

q_jd＝M_j(q_i-q_s)+q_f

其中，q_s为步态规划中的触地角，q_f为离地角；定义六条腿编号对应关系为：1和2对应、3和4对应、5和6对应，j取值与i取值对应，例如当i＝1时，j＝2；M_j为摆动相与支撑相之比，即：

因此，摆动腿髋关节力矩τ_j如下：

为了使水下机器人保持三足步态稳定行走，摆动腿和支撑腿步态切换时进行初始化，初始化阶段各条腿髋关节力矩如下：

其中τ_i为支撑腿关节力矩，τ_j为摆动腿关节力矩。

为了验证上述混合水下机器人航速航向控制系统的有效性，利用ROS+Gazebo进行了仿真。水下机器人质心初始位置位于(258.93m,-99.43m,-255.49m)，即初始时刻机器人位置坐标x₀＝258.93m、y₀＝-99.43m、z₀＝-255.49m；初始前向速度v_x0＝0m/s；初始航向角ψ₀＝0rad；六条腿初始关节角分别为q₁＝q₄＝q₅＝3.6rad、q₂＝q₃＝q₆＝3.1rad。本实施例中，设定期望航速v_xd＝0.15arctan(0.05t)m/s，其中t为仿真时间；设定期望航向ψ_d＝0.4rad；滑模控制参数为：λ＝1、k₁＝0.09、k₂＝0.0125、k₃＝6、k₄＝0.02、k₅＝k₆＝k₇＝k₈＝1.2；摆动腿控制参数为：k_p1＝10、k_d1＝0.5；恢复三足步态初始值控制参数为：k_p2＝10、k_d2＝0.5；触地角q_s＝3.06rad、离地角q_f＝3.32rad。

图8为水下机器人前向速度及其误差变化曲线，其中上图中虚线代表期望航速，实线代表真实航速，下图中虚线代表±0.05m/s的误差界限，实线代表真实前向速度误差。由图示结果可知，水下机器人能够较好的跟踪期望航行速度，前向速度误差基本保持在0.05m/s以内。

图9为水下机器人偏航角及其误差变化曲线，其中上图中虚线代表期望航向角，实线代表真实航向角，下图中虚线代表±0.05rad的误差界限，实线代表真实航向角误差。由图示结果可知，水下机器人能够根据航向误差及时调整航向角，并保持期望航向航行，航向误差不超过0.05rad。

图10为六个推进器的推力变化曲线。由图可知，在前30s的航向收敛过程中，水平推进器T₅和T₆的推力值大概互为相反数，且垂推推力值变化趋势大致相同。这间接表明解算出的水平推进器推力有纠正水下机器人航向角的趋势，并且四个垂推在协调控制水下机器人作用于地面压力值大小。

图11为六条刀锋腿髋关节转矩变化曲线，其中虚线代表1、2号腿的髋关节转矩，点画线代表3、4号腿的髋关节转矩，实线代表5、6号腿的髋关节转矩。由于水下机器人采用三足步态行走，因此腿1、4、5(腿2、3、6)的力矩变化趋势大体相同。

为进一步验证航速和航向控制系统的有效性，设定期望航向ψ_d＝0.15sin(0.02t)rad，其中t为仿真时间，其它参数设置不变，再次进行仿真，仿真结果如图12-图16所示。

图12为水下机器人前向速度及其误差变化曲线，其中上图中虚线代表期望航速，实线代表真实航速，下图中虚线代表±0.05m/s的误差界限，实线代表真实前向速度误差。图13为水下机器人偏航角及其误差变化曲线，其中上图中虚线代表期望航向角，实线代表真实航向角，下图中虚线代表±0.1rad的误差界限，实线代表真实航向角误差。

由图12、图13可以看出，水下机器人基本能够按照期望航速和期望航向航行，前向速度误差基本不超过0.05m/s。跟踪时变航向时航向角误差相比跟踪恒定航向角时航向角误差有所增大，图13所示航向角误差基本稳定在0.1rad以内。

图14为水下机器人以期望速度跟踪时变航向角时机器人质心在xOz平面的运动曲线及机器人质心在Oy方向上的变化曲线。图14所示结果表明机器人能够跟踪不断变化的期望航向角，航行轨迹为S形曲线。

图15为六个推进器的推力变化曲线。由图可知，在前50s的航向收敛过程中，水平推进器T₆的推力值出现负数，且垂推推力值变化趋势大致相同。这间接表明解算出的水平推进器推力有纠正水下机器人航向角的趋势，并且四个垂推在协调控制水下机器人作用于地面压力值大小。

图16为六条刀锋腿髋关节转矩变化曲线，其中虚线代表1、2号腿的髋关节转矩，点画线代表3、4号腿的髋关节转矩，实线代表5、6号腿的髋关节转矩。由于水下机器人采用三足步态行走，因此腿1、4、5(腿2、3、6)的力矩变化趋势大体相同。该实施例符合控制要求。

以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明，任何熟悉本专业的技术人员，在不脱离本发明技术方案范围内，当可利用上述揭示的技术内容作出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作任何的简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (2)

1.一种具有滑移管制的水下机器人优化控制方法，所述水下机器人包括：六个推进器、六条刀锋腿和机器人本体，其中，四个推进器垂直布置，两个推进器水平布置，六条半圆形刀锋腿的髋关节与机器人本体左右两侧的六个驱动电机连接；其特征在于，所述具有滑移管制的水下机器人优化控制方法包括以下步骤：

步骤1：建立地面坐标系和与水下机器人本体固连的体坐标系，确定作用在水下机器人本体的外力；

步骤2：建立水下机器人前向通道和偏航通道的动力学模型，利用滑模控制策略，确定水下机器人稳定跟踪上期望速度和期望航向角所需要的前向驱动力和偏航驱动力矩；同时，建立水下机器人垂向通道、侧向通道、横滚通道、俯仰通道的动力学模型，并利用比例控制策略，确定水下机器人在四个通道上运动状态保持稳定的期望驱动力；

步骤3：将水下机器人本体前向驱动力和偏航驱动力矩以及在四个通道上运动状态保持稳定的期望驱动力转化至支撑腿髋关节处，计算地面对支撑腿的反作用力；

步骤4：针对步态约束需求，且兼顾行走稳定性和转向灵活性的运动需求，基于最优控制律，对水下机器人的速度和航向进行控制；

步骤2中利用滑模控制策略，确定水下机器人稳定跟踪上期望速度和期望航向角所需要的前向驱动力和偏航驱动力矩时，所需要的前向驱动力u_fx和偏航驱动力矩u_my分别为：

其中，m₀为水下机器人本体的质量，λ₁₁为水下机器人本体的前向附加质量，λ₅₅为水下机器人本体的偏航附加转动惯量，J_y为水下机器人本体的偏航转动惯量；k₁、k₂、k₃、k₄和λ均为大于零的常数；ψ、ψ_d分别为水下机器人的当前航向角和参考航向角；滑模面s₁和s₂分别定义为

v_x和v_xd分别为水下机器人沿体坐标系Ox方向的当前速度和期望速度，w_y为水下机器人绕体坐标系Oy方向旋转的角速度；X_B和X_G分别为水下机器人浮力、重力在体坐标系Ox方向的分量，M_By为水下机器人浮力矩在体坐标系Oy方向的分量；X_αμ和X_wμ为水下机器人绕体坐标系Ox方向旋转的流体黏性位置力和流体黏性阻尼力，M_αμy和M_wμy分别为水下机器人绕体坐标系Oy方向旋转的流体黏性位置力矩和流体黏性阻尼力矩；

步骤2中建立水下机器人垂向通道、侧向通道、横滚通道、俯仰通道的动力学模型，并利用比例控制策略，确定水下机器人在这四个通道上运动状态保持稳定的期望驱动力时，期望垂向驱动力u_fy、期望侧向驱动力u_fz、绕体坐标系Ox旋转的期望横滚驱动力矩u_mx和绕体坐标系Oz旋转的期望俯仰驱动力矩u_mz可设计为

其中，k₅、k₆、k₇和k₈均为大于零的常数；w_x和w_z为水下机器人绕体坐标系Ox和Oz方向旋转的角速度；v_y和v_z为水下机器人沿体坐标系Oy和Oz方向的速度；Y_B、Z_B和Y_G、Z_G分别为水下机器人浮力、重力在机器人本体垂向和侧向的分量；M_Bx、M_Bz分别为水下机器人浮力矩在体坐标系Oy和Oz方向的分量；Y_αμ和Z_αμ分别为水下机器人沿体坐标系Oy和Oz方向的流体黏性位置力，Y_wμ和Z_wμ分别为水下机器人沿体坐标系Oy和Oz方向的流体黏性阻尼力，M_αμx和M_αμz分别为水下机器人体坐标系Ox和Oz方向旋转的流体黏性位置力矩，M_wμx和M_wμz分别为水下机器人体坐标系Ox和Oz方向旋转的流体黏性阻尼力矩；

步骤3中将水下机器人本体前向驱动力和偏航驱动力矩以及在四个通道上运动状态保持稳定的期望驱动力转化至支撑腿髋关节处，计算地面对支撑腿的反作用力时，

水下机器人本体作用于第i个腿髋关节处的前向力、垂向力和侧向力分别为F_ix、F_iy和F_iz可表示为：

其中，i＝1,2,…,6，X_T、Y_T、Z_T分别为六个推进器推力在体坐标系Ox、Oy和Oz方向上的分量，M_Tx、M_Ty、M_Tz分别为六个推进器推力在体坐标系Ox、Oy和Oz方向上形成的力矩；d₁、d₂分别为水下机器人腿髋关节之间的纵向和侧向距离；a_i为第i条腿的触地状态，当a_i＝1时，表明第i条腿已触地，当a_i＝0时，表明第i条腿未触地；

地面对支撑腿的反作用力在地面坐标系O₀x₀、O₀y₀和O₀z₀方向上的分量N_ix、N_iy和N_iz可表示为

其中，i＝1,2,...,6；q_i为第i条腿的旋转角度；τ_bi和M_ifz分别为将力F_ix、F_iy和F_iz由C_i平移至D_i产生额外的俯仰力矩和横滚力矩；τ_wi为第i条刀锋腿绕C_i点旋转时产生的旋转阻尼力矩，τ_i为第i条刀锋腿的驱动电机所产生的驱动力矩；l_r为刀锋腿的半径，C_i为第i条刀锋腿的髋关节位置，D_i为第i条刀锋腿的触地点位置；

步骤4中最优控制律为：

其中，Γ₁、Γ₂、Γ₃和Γ₄均为大于零的常数，μ_x为刀锋腿纵向摩擦系数，μ_z为刀锋腿侧向摩擦系数。

2.根据权利要求1所述的具有滑移管制的水下机器人优化控制方法，其特征在于：作用在水下机器人本体的外力包括：水动力、推进器推力、重力和浮力，其中，水动力包括：理想流体惯性力、流体黏性位置力和流体黏性阻尼力。

Images