CN113325859B - 一种游走混合型水下机器人沿斜面直立攀爬行走控制方法 - Google Patents

Abstract

本发明提出了一种游走混合型水下机器人沿斜面直立攀爬行走控制方法。首先，将水下机器人沿斜面直立攀爬行走的运动模型分解为沿纵轴的前向运动模型以及沿横平面的平面运动模型；然后，获取水下机器人的前向力，同时获取水下机器人的偏航力矩；其次，根据解算出的前向力和偏航力矩，分别计算水下机器人两个水平推进器的推力，各支撑腿所需的前向力以及各支撑腿用于转向所需的切向力，然后得到水下机器人用于前向运动和转向运动所需要的各支撑腿前向力，再根据支撑腿的支撑角，反解求出各支撑腿髋关节的驱动力矩及各支撑腿作用于斜面的垂向力；最后，根据支撑腿作用于斜面的垂向力，计算出垂向推进器的推力值，以保证机器人本体的垂向运动稳定性。

Description

一种游走混合型水下机器人沿斜面直立攀爬行走控制方法

技术领域

本发明涉及水下足式机器人控制领域，具体为一种游走混合型水下机器人沿斜面直立攀爬行走控制方法。

背景技术

随着科学技术的进步和海洋工程及其产业的蓬勃发展，水下机器人成为完成各类水下作业任务的重要工具，其代替人类在提高生产力方面发挥着极为关键的作用。本申请描述的游走混合型水下机器人，依靠六个推进器和六条半圆形刀锋腿间的协同配合，可以实现水下机器人在结构或非结构的环境中行走。为了提高机器人对水下环境的适应能力，游走混合型水下机器人除了需要具备平面直立行走能力外，还需具备在一定斜面上直立攀爬行走能力。直立攀爬行走控制策略是游走混合型水下机器人运动控制策略的重要组成部分，是确保游走混合型水下机器人高效作业的重要环节。具体地，当斜面上存在作业任务时，依靠直立攀爬行走控制方法可以保证游走混合型水下机器人顺利攀爬至目标任务点，并进行任务作业。因此，游走混合型水下机器人直立攀爬行走控制是一个很有实际意义的研究。

发明内容

(1)技术问题

要实现游走混合型水下机器人的直立攀爬行走，需要解决两个方面的控制问题：1)如何保证游走混合型水下机器人以期望的速度和航向沿水下斜面的航路点运动；2)如何协调控制游走混合型水下机器人的六个推进器与六条半圆形刀锋腿，以保证机器人行走的稳定性。

(2)技术方案

首先，将游走混合型水下机器人直立攀爬行走控制分解为水下机器人跟踪期望速度的前向运动控制及在横平面内的参考直线路径跟随控制。在此基础上，将水下机器人前向运动控制及参考直线路径跟随控制所需要的力和力矩来对水下机器人各支撑腿的前向力和各推进器的推力进行分配。

根据本发明的一方面，提供了一种游走混合型水下机器人沿斜面直立攀爬行走控制方法，包括：

将水下机器人沿斜面直立攀爬行走的运动模型分解为沿纵轴的前向运动模型以及沿横平面的平面运动模型；

针对前向运动模型，根据水下机器人期望行走速度，采用比例控制，获取水下机器人的前向力，同时将水下机器人跟随参考直线路径的平面运动模型转化为包含横向误差和航向角误差的级联系统，基于反步思想设计水下机器人控制律，获取水下机器人的偏航力矩；

根据解算出的前向力和偏航力矩，分别计算水下机器人两个水平推进器的推力，各支撑腿所需的前向力以及各支撑腿用于转向所需的切向力，然后根据运动叠加原理得到水下机器人用于前向运动和转向运动所需要的各支撑腿前向力，再根据支撑腿的支撑角，反解求出各支撑腿髋关节的驱动力矩及各支撑腿作用于斜面的垂向力；

根据支撑腿作用于斜面的垂向力，计算出垂向推进器的推力值。

进一步地，针对前向运动模型，根据水下机器人期望行走速度，采用比例控制，获取水下机器人的前向力为：

τ_X＝-X_uu_c+k_p(u-u_c)

式中，τ_X为水下机器人的前向力，X_u为水下机器人轴向流体阻尼系数，k_p为比例系数，u_c为水下机器人期望的前向速度，u为水下机器人实际的前向速度。

进一步地，将水下机器人跟随参考直线路径的平面运动模型转化为包含横向误差和航向角误差的级联系统，基于反步思想设计水下机器人控制律，获取水下机器人的偏航力矩为：

式中，k₂、k₃为控制参数；m₁₁，m₂₂，m₃₃，d₃₃均为水下机器人的水动力参数；水下机器人在运载体坐标系下x_b轴方向(前向)的速度为u，y_b轴方向的速度为v，航向角速度为r；ψ_d为航路点坐标系下机器人的期望航向角；ψ_e为航路点坐标系下航向角跟踪误差；r_e为航路点坐标系下航向角速度跟踪误差。

进一步地，根据解算出的前向力和偏航力矩，计算水下机器人两个水平推进器的推力为：

其中，T₅为左水平推进器产生的推力，T₆为右水平推进器产生的推力，R_S为两个水平推进器的力臂长度，F_TX为水平推进器产生的前向力，F_TN为水平推进器产生偏航力矩。

进一步地，根据解算出的前向力和偏航力矩，计算机器人第i条腿的髋关节转矩τ_i为：

l_r为支撑腿等效连杆长度，θ为髋关节旋转角度，F_li为第i条支撑腿的合力。

进一步地，根据支撑腿作用于斜面的垂向力，计算出垂向推进器的推力值为：

其中，F_lZ＝F_l1Z+F_l3Z+F_l5Z或者F_lZ＝F_l2Z+F_l4Z+F_l6Z为机器人各支撑腿的支撑合力，F_liZ为第i条腿的支撑力，

F_liZ＝-F_litanθ_i (i＝1、3、5；2、4、6)

θ_i为第i条支撑腿的髋关节旋转角度，并且θ₁＝θ₃＝θ₅＝θ或者θ₂＝θ₄＝θ₆＝θ。

进一步地，水平推进器与支撑腿各提供一半的前向力和偏航力矩。

(3)有益效果

本发明的有益效果主要体现在以下方面：

1)将水下机器人攀爬运动控制问题聚焦为一类任务驱动型的水下机器人跟踪期望速度的前向运动控制问题及在横平面内的参考直线路径跟随控制问题，使得复杂的问题得以简化；

2)将水下机器人攀爬运动控制所需要的力和力矩进行运动分解，在水下机器人运载体坐标系x_by_b平面分解为两个水平推进器的推力以及各支撑腿的合力。在此基础上，基于各支撑腿的合力得到水下机器人四个垂向推进器的垂向推力。通过各推进器与支撑腿间的协同配合，实现了水下机器人直立攀爬行走控制。

附图说明

附图1为游走混合型水下机器人、坐标系、六个推进器和六个半圆形刀锋腿布局和纵向推进器力臂R_s的示意图。

附图2为游走混合型水下机器人的航路点、航路点坐标系、实际跟随路径和参考跟随路径示意图。

附图3为航路点和直线参考路径示意图。

附图4为游走混合型水下机器人直线路径跟随示意图。

附图5为游走混合型水下机器人受力分析示意图。

附图6为游走混合型水下机器人六条半圆形刀锋腿编号示意图。

附图7(a)和(b)分别为游走混合型水下机器人前向运动时支撑腿作用力示意图和转向运动时支撑腿作用力示意图。

附图8为游走混合型水下机器人髋关节旋转角度θ的示意图

附图9为游走混合型水下机器人直立攀爬行走控制系统框图。

附图10为游走混合型水下机器人基于前向力和偏航力矩的足力分配流程框图。

附图11为游走混合型水下机器人直立攀爬行走时的三维运动轨迹图。

附图12(a)和(b)分别为游走混合型水下机器人直立攀爬行走时的xy平面运动轨迹图和深度变化响应曲线。

附图13为游走混合型水下机器人直线路径跟随控制过程中的横向跟踪误差响应曲线。

附图14为游走混合型水下机器人直线路径跟随控制过程中的实际航向角和航向角跟踪误差的响应曲线。

附图15(a)和(b)分别为游走混合型水下机器人直线路径跟随控制过程中的横向速度和航向角速度变化的响应曲线。

附图16为游走混合型水下机器人轴向速度跟踪和轴向速度误差响应曲线。

附图17为游走混合型水下机器人直线路径跟随控制过程中6个推进器的推力响应曲线。

附图18(a)和(b)分别为游走混合型水下机器人直立攀爬行走过程中腿1、腿3、腿5髋关节转矩响应曲线和腿2、腿4、腿6髋关节转矩响应曲线。

具体实施方式

下面结合具体实施例来描述本发明：

整体而言，根据本发明的一种游走混合型水下机器人沿斜面直立攀爬行走控制方法，将游走混合型水下机器人直立攀爬行走控制分解为水下机器人跟踪期望速度的前向运动控制及在横平面内的参考直线路径跟随控制。在此基础上，将水下机器人前向运动控制及参考直线路径跟随控制所需要的力和力矩来对水下机器人各支撑腿的前向力和各推进器的推力进行分配。

附图1为游走混合型水下机器人、坐标系、六个推进器和六个半圆形刀锋腿布局和纵向推进器力臂R_s的示意图。其中，坐标系包括地面坐标系{G}和运载体坐标系{B}，以及六个推进器推力和水平推力力臂R_s的示意图。

如图1所示，根据本发明的水下机器人包括六条半圆形仿生刀锋腿、四个垂直推进器以及两个水平推进器。其中，六条半圆形仿生刀锋腿对称设置在水下机器人本体两侧。四个垂直推进器对称设置在水下机器人本体顶部。两个水平推进器对称设置在水下机器人本体与六条半圆形仿生刀锋腿不同的一侧。

如图6所示，编号为1、2、3的腿足在机器人的左侧，编号为4、5、6的腿足在机器人的右侧。当机器人采用三足步态行走时，则编号为1、3、5腿足为一组，编号为2、4、6的腿足为另一组。需要说明的是，编号为1、2、3的腿足与编号为4、5、6的腿足对称设置在六足机器人两侧。腿足2、5分别位于两则中间位置，腿足1、3与腿足2的距离相等，腿足4、6与腿足5的距离相等。

根据本发明的示例性实施例，水下机器人沿其纵轴的前向运动控制为：

τ_X＝-X_uu_c+k_p(u-u_c)

式中，τ_X为水下机器人的前向力，X_u为水下机器人轴向流体阻尼系数，k_p为比例系数，u_c为水下机器人期望的前向速度，u为水下机器人实际的前向速度。

水下机器人沿其横平面的参考直线路径跟随控制包括以下步骤：

步骤1.1：定义三个坐标系与两类航向角。

地面坐标系{G}：地面坐标系Oxyz是固定的坐标系，以地面某基准点为坐标原点O，x轴指向地理正北，y轴指向地理正东，z轴垂直于x轴和y轴且构成右手坐标系，该坐标系又称为全局坐标系。其中，Oxy平面称为地面坐标系的水平面，Oxz平面称为地面坐标系的垂直面，如附图1所示。

运载体坐标系{B}:运载体坐标系O_bx_by_bz_b是随水下机器人运动的动坐标系。假定该水下机器人的重浮心重合且位于机体质心处，因此将运载体坐标系的原点O_b选取在水下机器人的质心处，坐标轴沿惯性主轴方向，其中x_b轴沿水下机器人纵轴并指向前方；y_b轴位于横平面内并垂直于x_b轴向右；z_b轴垂直于x_b轴和y_b轴且构成右手坐标系。其中，O_bx_by_b平面称为水下机器人的横平面，O_bx_bz_b称为水下机器人的纵平面，如附图1所示。

附图2为航路点、航路点坐标系{W_i}、实际跟随路径和参考跟随路径示意图。参考跟随直线路径为P_i′P_i′₊₁，实际跟随直线路径为P_iP_i+1。当游走混合型水下机器人跟踪斜面P_iP_i+1直线路径时，实际上是以一定俯仰角跟踪直线路径P_i′P_i′₊₁。

航路点坐标系{W_i}：航路点坐标系O_wx_wy_wz_w是随水下机器人航路点变化的动坐标系。根据作业任务需求，通常在水下机器人的作业斜面上设置若干个航路点P_i(x_i,y_i,z_i)(i＝1、2K n)。假定水下机器人的前一个航路点为P_i，后一个航路点为P_i+1，且航路点P_i+1(x_i+1,y_i+1,z_i+1)在通过航路点P_i且与地面坐标系水平面Oxy平行的平面上投影为P′_i+1(x′_i+1,y′_i+1,z′_i+1)，则航路点坐标系{W_i}以P′_i作为坐标原点，x_w轴指向P′_i+1，y_w轴位于与地面坐标系水平面Oxy平行的平面内并垂直于x_w轴向右，z_w轴垂直于x_w轴和y_w轴构成右手坐标系，如附图2所示。

定义矢量

的方位角ψ_pi为

定义地面坐标系下水下机器人的航向角为ψ_G。因此，航路点坐标系下水下机器人的航向角ψ_i可表示为

ψ_i＝ψ_G-ψ_pi

具体地，角度ψ_pi、ψ_G和ψ_i的示意图如附图3所示。

步骤1.2：在航路点坐标系{W_i}下，将水下机器人本体跟随参考直线路径P_i′P_i′₊₁的动力学和运动学模型转化为包含有横向误差和航向角误差的级联系统。

假设水下机器人在运载体坐标系下x_b轴方向的速度为u，y_b轴方向的速度为v，航向角速度为r。原点O_b距航路点坐标系{W_i}x_w轴的横向位移为y_e。因此，令航路点坐标系下机器人的期望航向角为

ψ_d＝-arctan(k₁y_e)

其中，k₁为大于零的常数。

机器人期望的航向角速度为

定义航路点坐标系{W_i}下航向角跟踪误差为

ψ_e＝ψ_i-ψ_d

定义航路点坐标系{W_i}下航向角速度跟踪误差

r_e＝r-r_d

则水下机器人本体跟随参考路径P′_iP′_i+1的动力学和运动学模型转化为包含有横向误差和航向角误差的级联系统

式中，x_l＝[y_e v]^T，x_p＝[ψ_e r_e]^T，N为输入的偏航力矩；f₁(x_l)、f₂(x_p,N)和g(x_l,x_p)的定义的详细叙述详见下文。

步骤1.3：偏航控制律的设计。偏航控制的目标是通过合理设计偏航力矩N使机器人的横向误差y_e渐近收敛到零。偏航控制律可设计为

式中，k₂、k₃为控制参数；

d₃₃＝-Z_q均为水下机器人的水动力参数。

需要说明的是，在偏航力矩N的控制下，当参数k₁＜0.307时，级联系统全局一致渐近稳定。此时，水下机器人的横向误差和航向角误差能收敛于零。

水下机器人各支撑腿的前向力和各推进器的推力分配包括以下步骤：

步骤2.1：分析各支撑腿产生的合前向力和合偏航力矩

机器人在行走过程中，腿的运动状态可以分为支撑相和摆动相。在每个行走周期机器人在步态L(腿1、腿3、腿5同时处于支撑相，腿2、腿4、腿6同时处于摆动相)或者在步态R(腿2、腿4、腿6同时处于支撑相，腿1、腿3、腿5同时处于摆动相)之间交替运动时，其本体姿态的稳定性通常是可以保证的，称该步态为三足步态。6条腿的布局如附图6所示，当机器人以三足步态行走时：

对于前向运动，令F_lix为第i条支撑腿作用于本体的前向力。以机器人步态L(腿1、腿3和腿5为支撑相)为例，步态R(腿2、腿4和腿6同时处于支撑相)的推导以及表达与步态L相同，在此不再赘述。为了避免额外偏航力矩产生，腿1、腿3、腿5对应产生的前向力F_l1x、F_l3x、F_l5x应满足

F_l1x+F_l3x＝F_l5x

如图7(a)所示，机器人各支撑腿作用于本体的合前向力为

F_lX＝F_l1x+F_l3x+F_l5x＝2F_l5x

对于转向运动，当机器人沿运载体坐标系z_b轴顺时针转向时，令F_liN为第i条支撑腿作用于本体的前向力，F′_liN为第i条支撑腿沿转向圆周切线方向的切向力，如图7(b)所示，前向力F_liN与切向力F′_liN之间的关系为

F_liN＝F′_liNsecα_i

式中，α_i为机器人质心与支撑腿髋关节之间的连线l_bi与y_b轴之间的夹角。由于支撑腿作用于本体的合偏航力矩F_lN是由机器人3条支撑腿的切向力共同产生的，因此合偏航力矩的表达式为

F_lN＝F′_l1Nl_b1+F′_l3Nl_b3-F′_l5Nl_b5＝F_l1Nl₁+F_l3Nl₃-F_l5Nl₅

其中，力F_l1N、F_l3N、F_l5N、F′_l1N、F′_l3N、F′_l5N，长度l_b1、l_b3、l_b5和角度α₁、α₃、α₅的示意图如附图7(b)所示,同时，l_i＝l_bi cosα_i，i＝1，3，5。

步骤2.2：计算各支撑腿产生的前向力和偏航力矩

机器人本体上的合前向力F_X和合偏航力矩F_N是由各推进器和支撑腿共同产生的。解算出的水下机器人的轴向力τ_X和步骤3中解算出的水下机器人的偏航力矩N，可推导出机器人本体的合前向力F_X＝τ_X和合偏航力矩F_N＝N。

优选地，本发明设定水平推进器与支撑腿各提供一半的前向力和偏航力矩，即水平推进器和支撑腿各自作用于机器人本体的前向力和偏航力矩相同，有

其中，F_TX为水平推进器产生的前向力，F_TN为水平推进器产生偏航力矩；F_lX为支撑腿产生的前向力，F_lN为支撑腿产生的偏航力矩。

需要说明的是，水平推进器与支撑腿可以按照其它的比例(例如，1：2，2：1，4；3，3：4等)提供前向力和偏航力矩。比例的改变并不会影响其它等式的推导。

步骤2.3：计算水下机器人直立攀爬行走过程中两个水平推进器所需要的推力。

两个水平器的推力共同产生机器人前向力和偏航力矩为

其中，T₅为左水平推进器产生的推力，T₆为右水平推进器产生的推力，R_S为两个纵向推进器的力臂长度，如附图1所示。

因此，左右水平推进器所需的推力值为

步骤2.4：计算水下机器人直立攀爬行走过程中各支撑腿的髋关节转矩。

机器人髋关节带动支撑腿转动，从而使机器人沿斜面行走。以步态L为例，机器人各支撑腿的髋关节转矩的计算方法为：

首先，求取机器人各支撑腿的前向力

有

其中，

为步态L的足力分配矩阵BL₁₃₅的伪逆第1列的所有元素，足力分配矩阵BL₁₃₅可定义为

其次，求取机器人各支撑腿的偏航力矩

有

式中，

为步态L的足力分配矩阵BL₁₃₅伪逆的第2列所有元素。

最后，利用叠加原理可得，机器人腿的合力F_l＝[F_l1,F_l3,F_l5]^T为

当已知第i条支撑腿的合力F_li时，可得机器人第i条腿的髋关节转矩τ_i为

其中，l_r为支撑腿等效连杆长度，θ为髋关节旋转角度，角度θ的示意图如附图8所示。

步骤2.5：计算水下机器人直立攀爬行走过程中各垂向推进器推力。

为了使机器人在垂直方向的稳定运动，就需要利用四个垂向推进器产生的垂推力抵抗支撑腿作用于机器人本体的力，即机器人在运载体坐标系z_b轴方向的垂向推力T_Z与机器人腿的支撑合力F_lZ之间满足如下关系式：

F_lZ＝-T_Z

其中，T_Z为4个垂向推进器的垂向推力之和。根据第i条支撑腿的合力F_li和髋关节旋转角度θ_i可得，第i条腿的支撑力F_liZ为

F_liZ＝-F_litanθ_i (i＝1、3、5)

其中，由于支撑腿1、3、5髋关节旋转角度一致，因此有θ₁＝θ₃＝θ₅＝θ。

可得机器人各支撑腿的支撑合力F_lZ＝F_l1Z+F_l3Z+F_l5Z。同时，四个垂向推进器推力分别为

附图3为游走混合型水下机器人跟随直线路径的示意图。ψ_G为游走混合型水下机器人在地面坐标系下的航向角，ψ_pi为直线参考路径在地面坐标系下的方位角，ψ_i为{W_i}坐标系下游走混合型水下机器人的航向角，y_e为游走混合型水下机器人相对参考直线路径P′_iP′_i+1的横向偏移量。

为了避免游走混合型水下机器人的大角度转向问题，本发明通过在平面内增加两个航路点P′_i和P′_i+1的方式，使实际航向角ψ_G与参考直线路径P′_iP′_i+1的方位角ψ_pi之间的夹角ψ_i始终保持为45°。这种方法将有效地避免混合型水下机器人跟踪大角度的问题，进而提升了跟踪效果，如附图4所示。

接下来对水下机器人在直线路径跟随控制过程中的全局稳定性做出证明。

定理：对于级联系统

如果级联系统满足以下3个假设，则说明级联系统全局一致渐进稳定。

假设1：当存在连续可微函数

满足

其中，W(x₁)为正定函数，c＞0、η＞0为常数；则称系统

是全局一致渐近稳定的。

假设2：函数g(t,x₁,x₂)满足

其中，

为连续函数；

假设3：系统

为全局一致渐近稳定的前提为

其中，φ(·)为K类函数。

直线路径跟随的控制目标是令水下机器人沿任意初始状态出发，设计偏航力矩N使横向跟踪误差y_e渐近收敛到零。令航向角ψ作为横向跟踪误差y_e的虚拟控制量，选择期望航向角ψ_d为

ψ_d＝-arctan(k₁y_e)

期望航向角速度为

那么，航向角跟踪误差和航向角速度跟踪误差为

ψ_e＝ψ-ψ_d

令x_l＝[y_e v]^T，x_p＝[ψ_e r_e]^T，控制输入为N,水下机器人两自由度(横向和偏航)运动方程可写作级联形式

式中，

对于名义系统

选取Lyapunov函数

式中λ＞0，对上式求导并将

代入可得

为使

有

同时选择λ满足

因此有

为使

负定，应满足

由b²-4ac＜0可得

为保证λ存在应满足

解得

对于上式可以理解为，水下机器人的质心与沿{W_i}坐标x轴正前方的导引点之间的距离应该满足

时，才能保证水下机器人直线路径跟随的稳定性。由上式可知，导引点选取距离的远近与水下机器人质量和前向速度成正比，与侧向阻尼成反比，根据水下机器人水动力参数，当常值航速为0.4m/s时，

对系统式

采用反馈线性化方法，选择如下偏航力矩控制律使航向角误差全局指数稳定。

取η≥0，使得满足

取c＝2max{λ,1}，使得

假设1成立。

考虑关联项有：

假设2成立。

在偏航力矩控制律的作用下，系统

指数稳定，假设3成立。

综上所述可知，在偏航控制律N的作用下，当参数k₁＜0.307时，级联系统全局一致渐近稳定。

附图5为以前向运动为例游走混合型水下机器人受力分析示意图。A点为足端与地面接触点；F_X为作用于水下机器人本体的合前向力；F_TX为推进器产生的合前向力；F_lX为各支撑腿产生的合前向力。

附图6为游走混合型水下机器人六条半圆形刀锋腿编号示意图。编号为1、2、3的腿在机器人的左侧，编号为4、5、6的腿在机器人的右侧。当机器人采用三足步态行走时，则编号为1、3、5腿为一组，编号为2、4、6的腿为另一组。这两组腿通过交替支撑方式执行行走任务。

附图7(a)和(b)分别为三足步态下游走混合型水下机器人前向运动时支撑腿作用力示意图和转向运动时支撑腿作用力示意图。图7(b)中，令第i条腿支撑髋关节距离机器人质心的位置为l_bi,可得质心与第i个髋关节之间的连线l_bi与O_by_b轴之间的夹角α_i为

其中，l_i为质心与第i个髋关节之间的连线l_bi在O_by_b轴上的投影。

附图8为游走混合型水下机器人的髋关节旋转角度示意图。髋关节旋转角度θ定义为髋关节中垂线与等效连杆l_r之间的夹角，旋转方向沿顺时针方向。

附图9为游走混合型水下机器人直立攀爬行走控制系统框图。将游走混合型水下机器人沿斜面直立攀爬运动控制分解为水下机器人跟踪期望速度的前向运动控制和在横平面的二维直线路径跟随控制。在此基础上，根据机器人上的合力和合力矩控制需求，进行推进器的推力分配与各支撑腿的前向力分配。首先，针对水下机器人的前向运动模型，根据期望的行走速度，采用比例控制，获取水下机器人所需要的前向力；同时将水下机器人跟随期望直线路径的运动模型转化为包含横向跟踪误差和航向角误差的级联系统，基于反步思想设计控制律，获取水下机器人所需要的偏航力矩；其次，根据水下机器人所需的偏航力矩和前向力，以及运动过程中腿的垂向支撑力与推进器垂向推力之间的约束关系，设计支撑腿前向力分配算法，通过协调控制各推进器和各支撑腿使游走混合型水下机器人实现稳定的直立攀爬行走任务。

附图10为游走混合型水下机器人基于前向力和偏航力矩的足力分配流程框图。以步态L为例，水下机器人足力分配矩阵BL₁₃₅为

其中，l_i＝l_bi cosα_i，l_bi为第i条支撑腿髋关节轴心与机器人本体质心之间的连线在平面O_bx_by_b的投影。

同理，在步态R下(腿2、腿4和腿6为支撑腿)的足力分配矩阵BL₂₄₆为

为了验证游走混合型水下机器人沿斜面直立攀爬行走控制方法，以NED坐标系{G}为地面坐标系，选取水下机器人的初始位置坐标为p₀(8,110,92)，初始航向角为ψ₀＝-0.78rad，预设游走混合型水下机器人的期望前向速度为0.4m/s，横向速度和垂向速度均为0m/s，k₁＝0.1，k₂＝5，k₃＝5；考虑直线参考路径的方位角与水下机器人的航向角夹角应为45°，选取连接航路点p₁(18，110，92)和航路点p₂(48，100，92)的直线p₁p₂作为第一段直线参考路径，选取连接航路点p₂(48，100，92)和航路点p₃(58，110，92)的直线p₂p₃作为第二段直线参考路径，当水下机器人距离第一个目标航路点小于2m时，切换直线航迹坐标系。

图11为游走混合型水下机器人直立攀爬行走时的三维运动轨迹图。首先，游走混合型水下机器人由初始状态开始，以一定的前向速度和航向角速度缩小与第一段直线参考路径的横向误差和航向角误差，并最终趋于零；其次，当水下机器人质心与航路点之间的距离小于2m时，切换参考路径为第二段直线参考路径；最后，水下机器人以一定前向速度和航向角速度缩小与第二段直线参考路径的横向误差和航向角误差，并最终趋近于零，直至到达终点。

图12(a)为游走混合型水下机器人在地面坐标系xy平面上的运动轨迹，图12(b)为游走混合型水下机器人深度的响应曲线。由图可知，在斜面上水下机器人沿z轴负方向攀爬了20m左右的距离。

图13为游走混合型水下机器人直线路径跟随控制过程中的横向跟踪误差响应曲线。由图可知，水下机器人与两段直线参考路径的横向误差都能较好地收敛为零。在112s左右时，水下机器人的直线参考路径发生了切换，瞬时横向误差大约为1.8m左右。

图14为游走混合型水下机器人直线路径跟随控制过程中的实际航向角和航向角跟踪误差的响应曲线。图中，实线代表地面坐标系下的航向角，点画线代表{W_i}坐标系下的航向角。由于第一段直线参考路径坐标系的方向与地面坐标系相同，因此两个航向角曲线重叠；当直线参考路径到达切换点时，水下机器人的瞬时航向角误差为-0.78rad，地面坐标系下的航向角最终趋近于0.78rad，{W_i}坐标系下的航向角趋于零。

图15为游走混合型水下机器人直线路径跟随控制过程中的横向速度和航向角速度变化的响应曲线。图15(a)代表水下机器人的横向速度，图15(b)代表水下机器人的航向角速度。由图可知，航向角速度变化较为明显的两个时间点为初始时刻和切换参考路径时刻：初始时刻，水下机器人由相对静止状态开始运动，推进器和支撑腿会产生较大的瞬时航向角速度，因此航向角速度较大；切换参考路径时，由于产生了较大的航向角误差，因此水下机器人会通过产生较大的航向角速度来跟踪航向角误差，使得此时的航向角速度较大。当航向角速度较大时，相应的横向速度也会随之增大。

附图16为游走混合型水下机器人轴向速度跟踪和轴向速度误差响应曲线。由图可知，水下机器人的轴向速度u能有效收敛至期望轴向速度u_c，且控制轴向速度误差能收敛至±0.05m/s。

附图17为直立攀爬行走过程中水下机器人的推进器推力变化规律。图中，T₁、T₂、T₃、T₄为四个垂向推进器的推力值，根据垂向推力的控制算法，四个垂向推进器的推力值保持一致，且沿本体坐标系z轴正方向；T₅、T₆为两个水平方向推进器，这两个推进器在提供前向作用力的同时，还需要提供偏航力矩。垂向推进器推力T₁、T₂、T₃、T₄用于抵消腿产生的额外垂向力，T₅、T₆为水平方向推进器用于抵抗主矢、主矩的一半，即

和

附图18(a)为水下机器人在直立攀爬行走过程中腿1、腿3、腿5的髋关节转矩变化响应曲线。图中，点画线代表腿1的髋关节转矩，实线代表腿3的髋关节转矩，虚线代表腿5的髋关节转矩。在沿直线运动过程中，腿1和腿3的髋转矩相等且合转矩等于腿5的髋关节转矩。在转向过程中，处于转向外侧的腿1和腿3的髋关节增大转矩，腿5的髋关节减小转矩，实现差动转向。腿2、腿4、腿6的髋关节转矩响应曲线如图18(b)所示。图中，点画线代表腿2的髋关节转矩，实线代表腿4的髋关节转矩，虚线代表腿6的髋关节转矩。在沿直线运动过程中，腿4和腿6的髋转矩相等且合转矩等于腿2的髋关节转矩。在转向过程中，处于转向外侧的腿2的髋关节增大转矩，腿4和腿6的髋关节减小转矩，实现差动转向。

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

Claims (6)

1.一种游走混合型水下机器人沿斜面直立攀爬行走控制方法，其特征在于，所述行走控制方法包括：

将水下机器人沿斜面直立攀爬行走的运动模型分解为沿纵轴的前向运动模型以及沿横平面的平面运动模型；

针对前向运动模型，根据水下机器人期望行走速度，采用比例控制，获取水下机器人的前向力，同时将水下机器人跟随参考直线路径的平面运动模型转化为包含横向误差和航向角误差的级联系统，基于反步思想设计水下机器人控制律，获取水下机器人的偏航力矩；

根据解算出的前向力和偏航力矩，分别计算水下机器人两个水平推进器的推力，各支撑腿所需的前向力以及各支撑腿用于转向所需的切向力，然后根据运动叠加原理得到水下机器人用于前向运动和转向运动所需要的各支撑腿前向力，再根据支撑腿的支撑角，反解求出各支撑腿髋关节的驱动力矩及各支撑腿作用于斜面的垂向力；

根据支撑腿作用于斜面的垂向力，计算出垂向推进器的推力值；

其中，将水下机器人跟随参考直线路径的平面运动模型转化为包含横向误差和航向角误差的级联系统，基于反步思想设计水下机器人控制律，获取水下机器人的偏航力矩为：

式中，k₂、k₃为控制参数；m₁₁，m₂₂，m₃₃，d₃₃均为水下机器人的水动力参数；水下机器人在运载体坐标系下y_b轴方向的速度为v，航向角速度为r；ψ_d为航路点坐标系下机器人的期望航向角；ψ_e为航路点坐标系下航向角跟踪误差；r_e为航路点坐标系下航向角速度跟踪误差。

2.根据权利要求1所述的游走混合型水下机器人沿斜面直立攀爬行走控制方法，其特征在于，针对前向运动模型，根据水下机器人期望行走速度，采用比例控制，获取水下机器人的前向力为：

τ_X＝-X_uu_c+k_p(u-u_c)

式中，τ_X为水下机器人的前向力，X_u为水下机器人轴向流体阻尼系数，k_p为比例系数，u_c为水下机器人期望的前向速度，u为水下机器人实际的前向速度。

3.根据权利要求1所述的游走混合型水下机器人沿斜面直立攀爬行走控制方法，其特征在于，根据解算出的前向力和偏航力矩，计算水下机器人两个水平推进器的推力为：

其中，T₅为左水平推进器产生的推力，T₆为右水平推进器产生的推力，R_S为两个水平推进器的力臂长度，F_TX为水平推进器产生的前向力，F_TN为水平推进器产生偏航力矩。

4.根据权利要求1所述的游走混合型水下机器人沿斜面直立攀爬行走控制方法，其特征在于，根据解算出的前向力和偏航力矩，计算机器人第i条腿的髋关节转矩τ_i为：

l_r为支撑腿等效连杆长度，θ为髋关节旋转角度，F_li为第i条支撑腿的合力。

5.根据权利要求4所述的游走混合型水下机器人沿斜面直立攀爬行走控制方法，其特征在于，根据支撑腿作用于斜面的垂向力，计算出垂向推进器的推力值为：

其中，F_lZ＝F_l1Z+F_l3Z+F_l5Z或者F_lZ＝F_l2Z+F_l4Z+F_l6Z为机器人各支撑腿的支撑合力，F_liZ为第i条腿的支撑力，

F_liZ＝-F_litanθ_i(i＝1、3、5；2、4、6)

θ_i为第i条支撑腿的髋关节旋转角度，并且θ₁＝θ₃＝θ₅＝θ或者θ₂＝θ₄＝θ₆＝θ。

6.根据权利要求3或者4所述的游走混合型水下机器人沿斜面直立攀爬行走控制方法，其特征在于，水平推进器与支撑腿各提供一半的前向力和偏航力矩。

Images