CN114217521B - 基于推进器矢量布局的水下机器人的全姿态运动控制方法 - Google Patents

Abstract

本发明涉及到自主水下机器人运动控制领域，具体说是基于推进器矢量布局的水下机器人的全姿态运动控制方法。包括以下步骤：控制计算：根据水下机器人的结构与姿态构造姿态球形模型，根据该模型，设计基于航向角偏差弧线的PID航向控制方法，计算满足水下机器人各个姿态下的纵倾与偏航所需力矩；推力分配：通过推进器矢量布局结构以及水下机器人的机构，对推进器产生的纵倾力矩与偏航力矩进行计算，并设计横滚自适应的矢量布局推力与力矩分配方法，从而完成对水下机器人的运动控制。本发明基于矢量布局推进器的自主水下机器人的全姿态运动控制方法，保证推进器矢量布局的全姿态运动自主水下机器人能够全姿态平稳航行。

Description

基于推进器矢量布局的水下机器人的全姿态运动控制方法

技术领域

本发明涉及到自主水下机器人运动控制领域，具体说是基于推进器矢量布局的水下机器人的全姿态运动控制方法。

背景技术

自主水下机器人是进行海洋科学研究的有效工具。在深海资源调查、海洋科学研究以及海洋工程等领域得到了广泛的应用。现在自主水下机器人通常设计为近水平航行或水下滑翔运动，使其可以应用在较平坦的海底，或者在一定深度海水中航行应用，并取得了丰硕的实验结果。然而随着自主水下机器人不断的研究创新与应用，自主水下机器人承担任务复杂性越来越高，随自主水下机器人要求也越来越高。现有的自主水下机器人已经不能应对新的应用场景，近水平航行与水下滑翔运动，无法满足复杂地形的应用。因此，研究全姿态运动的自主水下机器人是必要的。这种机器人与现有的机器人比有以下优点：

(1)更广的适用性。全姿态运动自主水下机器人能够应对更多现在自主水下机器人无法实现的任务。如近海底航行，海底考古等。比如：在进行近海底光学拍照的任务时，当遇到障碍时，自主水下机器人只能通过抛载上浮放弃任务的方式解决，而全姿态运动自主水下机器人能够更好地越过障碍继续执行任务。

(2)更好的操控性。全姿态运动自主水下机器人可以实现任意姿态的运动，使其能够方便地通过任意狭小的空间。

(3)更高的安全性。而全姿态运动自主水下机器人可以通过任意调整潜水器自身的姿态的方式，使其能沿着障碍平稳航行

全姿态运动自主水下机器人一般具有以下特点：稳心高为0，无净浮力，具备矢量推力。

推进器的矢量布局也是实现自主水下机器人的重要方式。但同时推进器的矢量布局与全姿态运动控制也带了控制上的难题。

发明内容

本发明目的是提供一种基于推进器矢量布局自主水下机器人的全姿态运动控制方法，充分考虑了大纵倾角的影响，以及水流造成无规律横滚影响，保证自主水下机器人在水下能够进行全姿态稳定航行。

本发明为实现上述目的所采用的技术方案是：

基于推进器矢量布局的水下机器人的全姿态运动控制方法，包括以下步骤：

控制计算：包括航向控制以及纵倾控制，所述航向控制根据水下机器人的结构与姿态构造姿态球形模型，根据该模型，基于航向角偏差弧线的PID航向控制方法，得到满足水下机器人全姿态下的航向所需力矩，所述纵倾控制根据PID控制方法得到满足水下机器人全姿态下的纵倾所需力矩；

推力分配：通过推进器矢量布局结构以及水下机器人的机构，对每个推进器产生的纵倾力矩与航向力矩进行计算，进而利用横滚自适应的矢量布局推力与力矩分配方法，从而完成对水下机器人的运动控制。

还包括，对水下机器人进行控制之后，通过水动力仿真，得到当前时刻自主水下机器人的速度与姿态，通过与期望值进行比较，完成对所述运动控制方法的正确性验证。

其特征在于，所述航向控制具体为：

将姿态球形模型中航向角偏差所对的弧线长度为PID的输入，得到水下机器人需要产生的航向所需力矩C_N作为四个推进器所需的总航向力矩N，所述航向所需力矩C_N为：

其中，P为自主水下机器人的纵倾角，l为推进器到质心的距离，P为水下机器人实时的纵倾角度，e_P为纵倾角输出偏差。

所述姿态球形模型为：以水下机器人的质心为球的球心、质心与水下机器人推进器的距离为半径，每条半径的方向为水下机器人的姿态。

所述纵倾控制具体为：

将纵倾角作为PID的输入，得到水下机器人的纵倾所需力矩C_M作为四个推进器所需的总纵倾力矩M，所述纵倾所需力矩C_M为：

其中，e_P为纵倾角输出偏差,K_i表示纵倾角控制输出偏差积分的系数，K_p表示纵倾角控制输出偏差比例的系数，K_d表示纵倾角控制输入偏差查分的系数。

所述推力分配，包括以下步骤：

根据水下机器人的结构，获取每个推进器的前向推力和所需力矩的关系；

根据水下机器人的结构，获取横滚自适应条件下，水下机器人每个推进器的前向推力和所需力矩的关系。

所述水下机器人具有4个推进器，在垂直于水下机器人的航行方向上，每两个相邻的推进器之间的夹角为90度，因此，四个推进器的推力T_i与水下机器人总前向推力、纵倾力矩、偏航力矩的关系为：

其中，X为四个推进器的总前向推力，T_i为每个推进器的推力，i＝1～4，θ_T为自主水下机器人推进器与水下机器人轴线方向存在夹角，l为推进器到质心的距离，r为推进器到水下机器人中轴线的距离。

由于[X,C_M,C_N]^T＝A[T₁,T₂,T₃,T₄]^T，因此，水下机器人四个推进器的推力与水下机器人总前向推力、纵倾力矩、偏航力矩的关系A表示为：

横滚自适应条件下，水下机器人四个推进器的前向推力与水下机器人总前向推力、纵倾力矩、偏航力矩的关系A表示为：

其中，R为横滚角。

所述水动力仿真计算分别计算水下机器人当前时刻的轴向力、侧向力、垂向力、横摇力矩、纵倾力矩以及航向力矩。

本发明具有以下有益效果及优点：

1.本发明保证自主水下机器人能够全姿态航行。该方法突破原有自主水下机器人的只能近水平航行或者水下滑翔的限制，将自主水下机器人的航行方向拓展到全姿态。极大提高自主水下机器人的运动能力，使自主水下机器人能执行任务有更高的复杂性，更广的适用性，更好的操控性，更高的安全性。并通过设计基于航向角偏差弧线的PID航向控制方法保证了自主水下机器人在极端大倾斜角的姿态也能航行。

2.本发明保证推进器矢量布局的自主水下机器人航行稳定。该方法保证了自主水下机器人能够平稳航行，充分考虑了具有矢量布局推进器的自主水下机器人的特点，设计横滚自适应的矢量布局推力与力矩分配方法，保证了该结构的自主水下机器人能够适应水流造成的横滚影响，保证其能够平稳航行。

附图说明

图1为推进器矢量布局自主水下机器人的全姿态运动控制方法流程图；

图2为自主水下机器人推进器矢量布局自主水下机器人的全姿态运动控制方法数据流程图；

图3为全姿态运动自主水下机器人结构图；

图4为自主水下机器人航向角控制图；

图5为自主水下机器人矢量布局推进器力产生力矩的力臂示意图；

图6为自主水下机器人的四个矢量推进器布局图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

整个系统的工作流程如下：首先船上的科学家划定探测区域，并利用本发明计算每个自主水下机器人工作的起始位置、探测测线的长度、角度等信息。然后，母船航行到指定位置对自主水下机器人进行布放，自主水下机器人下水后立即开展预置的探测任务。

整个基于推进器矢量布局自主水下机器人的全姿态运动控制方法流程如图1所示。包括以下步骤：

a)第一步，全姿态自主水下机器人控制。设计纵倾角PID控制方法，根据潜水器的质心、长度与姿态构造姿态球形模型，设计基于偏差航向角所对弧线的PID航向控制方法。获得纵倾角与航向角调整量。

b)第二步，推力与力矩计算。首先设计计算矢量布局推进器的自主水下机器人水平姿态下的推力与力矩方法，并基于此，设计横滚自适应的矢量布局推力与力矩分配方法，保证矢量布局全姿态自主水下机器人在水下平稳航行，并获得各个推进器转速。

c)第三步，设计推进器矢量布局的全姿态运动自主水下机器人的水动力仿真方程。根据自主水下机器人的转速，计算自主水下机器人姿态。

为了方便理解该控制方法，图2展示了该运动控制方法数据流。其中A₀表示T₀时刻AUV预设的自主水下机器人姿态，A₁表示T₁时刻AUV反馈的实际自主水下机器人的姿态角，E表示T₁时刻AUV的自主水下机器人姿态角误差值，A表示经过PID控制算法调整后的下一个时段的AUV姿态角。C表示基于PID控制算法输出量，T表示各个推进器推力分配值。

如图3所示，为实现全姿态运动设计的自主水下机器人构造图。其中l为推进器到质心的距离，r为推进器到中轴线的距离，θ_T为推进器安装角度，p为力矩。

第一步控制计算

矢量布局运载器航行运动对纵倾与航向控制采用PID控制算法。其中PID控制离散化后变为：

e表示控制输出偏差，K_i表示控制输出偏差积分的系数，K_p表示控制输出偏差比例的系数，K_d表示控制输入偏差差分的系数。

1.纵倾控制

e_P为纵倾角输出偏差,K_i表示纵倾角控制输出偏差积分的系数，K_p表示纵倾角控制输出偏差比例的系数，K_d表示纵倾角控制输入偏差查分的系数。此时设由PID计算的自主水下机器人需要产生的纵倾力矩C_M。

即

2.航向控制

用于全姿态的自主水下机器人与常规自主水下机器人航行运动控制有很大的区别，特别是当自主水下机器人进行大纵倾角航行运动时。即如果按照自主水下机器人纵倾为0°时的控制量去调整自主水下机器人处于大纵倾角姿态的航向角时，会发生严重的超调现象。如图4所示，我们根据潜水器的质心、长度与姿态构造水下机器人姿态球形模型。其中O为水下机器人的质心，A,B,C,D,为推进器位置，半径为质心到推进器的轴向距离。OA,OB,OC,OD代表水下机器人在水下的四种姿态。

当自主水下机器人纵倾为0时，即自主水下机器人出在水平状态时，也是常规自主水下机器人航行控制方式应用场景，由PID控制方法，自主水下机器人航向角调节量只与自主水下机器人的航向偏差成正相关的关系。如图4所示，自主水下机器人从OC姿态需要航向角偏差120度，需要调整到OD姿态。常规自主水下机器人应以120度为PID输入。但是当自主水下机器人航行时纵倾角接近90°时，无论航向角偏差有多大，自主水下机器人在纵倾调节上只需要很小的调整量，就能满足要求，甚至于趋近于0。如图4所示，以自主水下机器人纵倾角60°为例子，如果按照自主水下机器人纵倾为0°时的控制量去调整自主水下机器人航向角时，会发生严重的超调现象。因此我们根据潜水器的质心、长度与姿态构造球形水下机器人姿态球形模型，并提出“基于偏差航向角所对弧线的PID航向控制方法”。

从原理上，从图4的水下机器人姿态球形模型可以看出，自主水下机器人姿态的调整，即为尾端从A到B的调整，从C到D的调整。从A到B可以描述为从C到D可以描述为因此，航向角控制也可以演变为于偏差航向角所对弧线的PID航向控制方法。从实践上，也验证了该方法的正确性。

在基于航向角偏差弧线的全姿态自主水下机器人航向控制方法中，e_P为纵倾角输出偏差,K_i表示纵倾角控制输出误差积分的系数，K_p表示纵倾角控制输出误差比例的系数，K_d表示纵倾角控制输入误差查分的系数。其中航向角偏差弧线输出误差，就是摒弃传统pid姿态控制采用纵倾角角偏差作为输入，即∠AO'B而是采用∠AO'B圆心角对的弧线长度作为偏差输入，即的长度。

此时设由PID计算的自主水下机器人需要产生的偏航力矩为：

其中P为自主水下机器人的纵倾角。基于偏差航向角所对弧线的PID航向控制方法可以很好地避免由于自主水下机器人在进行全姿态航行时特别是大纵倾角航行时对航向角控制带来的超调现象。

第二步推力分配

1求解力与力矩

自主水下机器人的设计构造如图3所示，自主水下机器人推进器与自主水下机器人轴线方向存在夹角θ_T，l为推进器到质心的距离，r为推进器到中轴线的距离。

如图5所示，我们将推进器T₁产生力臂P₁位置、方向与大小关系抽象出来。线段OD的长度即为力臂大小，那么易得下式：

求得力臂P₁＝r·sinθ_T+l·cosθ_T。

如图6所示控制自主水下机器人姿态与运动的是四个推进器产生的力矩产生的推力T(向前)与力矩(纵倾力矩M与偏航力矩N)。图中以推进器T₁为例，T₁产生力臂P₁，该力矩在纵倾与偏航方向上的力矩分别为M₁与N₁。

因此，得到：

又可以表达为：[X,M,N]^T＝A[T₁,T₂,T₃,T₄]^T

2横滚自适应下力与力矩

常规自主水下机器人通常只是保持水平姿态航行，常规自主水下机器人通过一定的稳心高与一定的正浮力，产生扶正力矩，可以保证自主水下机器人平稳的航行。但是，单方向的扶正力矩显然不适用于需要全姿态航行的自主水下机器人，特别是在大纵倾角的情况下。因此，本自主水下机器人在受到水流等扰动的影响时，自主水下机器人会产生一定的横滚。

在发生横滚情况下，自主水下机器人依旧采用原有的控制量进行姿态控制将会导致自主水下机器人航行失控。因此，矢量布局自主水下机器人在进行全姿态航行控制时，需要去除横滚影响。我们提出了“横滚自适应的推力与力矩分配方法”。如图3所示，R为横滚角。可以得到：

由此我们获得了全姿态自主水下机器人的推力与力矩分配方法，该方法

水动力仿真计算

第三步，水动力仿真计算

水动力仿真也是推进器矢量布局自主水下机器人控制方法研究的重要组成部分，是验算该方法的依据，通过水动力仿真计算可以实时获得当前自主水下机器人的速度与姿态。

水下运载器的主艇体为回转体流线型，水下航行并进行定深/定向或变深/变向航行时，其动力学模型可以参考美国泰勒舰船研究和发展中心发表的修正版潜艇标准运动方程运动模型。我们根据推进器矢量布局自主水下机器人的特点对自主水下机器人动力学模型进行了具体化改造。在水动力方程中，一些前人使用的方法及符号不再重复赘述，只对更改部分进行简单介绍。

1)轴向力方程：

根据推进器矢量布局自主水下机器人的特点，对推力部分进行了具体化改造，向前推力为(T₁+T₂+T₃+T₄)cosθ_T，全姿态运动自主水下机器人需要0净浮力因此并去掉重力与浮力部分(W-B)sinθ。

2)侧向力方程：

根据推进器矢量布局自主水下机器人的特点，对推力部分进行了具体化改造，向右侧产生推力为(T₂-T₄)sinθ_T，全姿态运动自主水下机器人需要0净浮力因此并去掉重力与浮力部分

3)垂向力方程：

根据推进器矢量布局自主水下机器人的特点，对推力部分进行了具体化改造，向下侧产生推力为(T₁-T₃)sinθ_T，全姿态运动自主水下机器人需要0净浮力因此并去掉重力与浮力部分

4)横摇力矩方程：

根据推进器矢量布局自主水下机器人的特点，全姿态运动自主水下机器人需要0净浮力，因此去掉重力与浮力部分

5)纵倾力矩方程：

根据推进器矢量布局自主水下机器人的特点，对力矩部分进行了具体化改造，产生纵倾力矩为(T₁-T₃)rsinθ_T+(T₁-T₃)lcosθ_T，全姿态运动自主水下机器人需要0净浮力，因此去掉

6)偏航力矩方程：

根据推进器矢量布局自主水下机器人的特点，对力矩部分进行了具体化改造，产生偏航力矩为(T₂-T₄)rsinθ_T+(T₂-T₄)lcosθ_T，全姿态运动自主水下机器人需要0净浮力，因此去掉

根据以上留个水动力方程，我们可以实时计算推进器矢量布局的全姿态自主水下机器人在水下的姿态，并由此调整控制系数，以及验证该运动控制方法的正确性。

本发明设计纵倾角PID控制方法与基于航向角偏差弧线的PID航向控制方法。获得纵倾角与航向角调整量。

本发明设计矢量布局推进器的推力与力矩计算方法。基于此，设计横滚自适应的矢量布局推力与力矩分配方法，保证矢量布局全姿态自主水下机器人在水下平稳航行，并获得各个推进器转速。

本发明根据全姿态自主水下机器人的独特结构，设计全姿态运动自主水下机器人的水动力仿真方程。根据自主水下机器人的转速，计算自主水下机器人姿态。

Claims (3)

1.基于推进器矢量布局的水下机器人的全姿态运动控制方法，其特征在于，包括以下步骤：

控制计算：包括航向控制以及纵倾控制，所述航向控制根据水下机器人的结构与姿态构造姿态球形模型，根据该模型，基于航向角偏差弧线的PID航向控制方法，得到满足水下机器人全姿态下的航向所需力矩，所述纵倾控制根据PID控制方法得到满足水下机器人全姿态下的纵倾所需力矩；

推力分配：通过推进器矢量布局结构以及水下机器人的机构，对每个推进器产生的纵倾力矩与航向力矩进行计算，进而利用横滚自适应的矢量布局推力与力矩分配方法，从而完成对水下机器人的运动控制；

所述航向控制具体为：

将姿态球形模型中航向角偏差所对的弧线长度为PID的输入，得到水下机器人需要产生的航向所需力矩C_N作为四个推进器所需的总航向力矩N，所述航向所需力矩C_N为：

其中，P为自主水下机器人的纵倾角，l为推进器到质心的距离，e_P为纵倾角输出偏差；

所述姿态球形模型为：以水下机器人的质心为球的球心、质心与水下机器人推进器的距离为半径，每条半径的方向为水下机器人的姿态；

所述纵倾控制具体为：

将纵倾角作为PID的输入，得到水下机器人的纵倾所需力矩C_M作为四个推进器所需的总纵倾力矩M，所述纵倾所需力矩C_M为：

其中，e_P为纵倾角输出偏差,K_i表示纵倾角控制输出偏差积分的系数，K_p表示纵倾角控制输出偏差比例的系数，K_d表示纵倾角控制输入偏差查分的系数；

所述推力分配，包括以下步骤：

根据水下机器人的结构，获取每个推进器的前向推力和所需力矩的关系；

根据水下机器人的结构，获取横滚自适应条件下，水下机器人每个推进器的前向推力和所需力矩的关系；

所述水下机器人具有4个推进器，在垂直于水下机器人的航行方向上，每两个相邻的推进器之间的夹角为90度，因此，四个推进器的推力T_i与水下机器人总前向推力、纵倾力矩、偏航力矩的关系为：

其中，X为四个推进器的总前向推力，T_i为每个推进器的推力，i＝1～4，θ_T为自主水下机器人推进器与水下机器人轴线方向存在夹角，l为推进器到质心的距离，r为推进器到水下机器人中轴线的距离；

由于[X,C_M,C_N]^T＝A[T₁,T₂,T₃,T₄]^T，因此，水下机器人四个推进器的推力与水下机器人总前向推力、纵倾力矩、偏航力矩的关系A表示为：

横滚自适应条件下，水下机器人四个推进器的前向推力与水下机器人总前向推力、纵倾力矩、偏航力矩的关系A表示为：

其中，R为横滚角。

2.根据权利要求1所述的基于推进器矢量布局的水下机器人的全姿态运动控制方法，其特征在于，还包括，对水下机器人进行控制之后，通过水动力仿真，得到当前时刻自主水下机器人的速度与姿态，通过与期望值进行比较，完成对所述运动控制方法的正确性验证。

3.根据权利要求2所述的基于推进器矢量布局的水下机器人的全姿态运动控制方法，其特征在于，水动力仿真计算分别计算水下机器人当前时刻的轴向力、侧向力、垂向力、横摇力矩、纵倾力矩以及航向力矩。