CN114620207A - 一种水下机器人的推力分配器及推力分配方法 - Google Patents

Abstract

本发明公开了一种水下机器人的推力分配器及推力分配方法，涉及水下技术领域，该方法分别对水平纵向期望力、水平横向期望力和转艏力矩在各个水平推进器上进行单自由度力或力矩分配得到三个初始推力，然后利用三个待求系数对三个初始推力进行加权的水平推力合成，然后基于水下机器人当前作业任务对各个自由度的运动需求对应的指示各个待求系数取得更大取值的期望优先级的目标函数，在推进器的推力范围所确定的约束条件下求解得到各个推进器的指令推力值；该方法从水下机器人作业任务需求考虑来进行推力分配，通过适当牺牲某些自由度上的控制性能以优先保证关键自由度上的控制性能，能够保证作业任务能够顺利完成。

Description

一种水下机器人的推力分配器及推力分配方法

技术领域

本发明涉及水下技术领域，尤其是一种水下机器人的推力分配器及推力分配方法。

背景技术

水下机器人被广泛应用于海洋资源开发、深海打捞作业、核电检测等领域。水下机器人搭载有推进器以实现运动控制，目前常见的配置方式是水下机器人搭载有4台水平推进器及2台垂直推进器，控制系统通过调节6台推进器转速实现水下机器人的深度、水平面位置及航向的运动控制。

控制系统在调节6台推进器转速时，需要根据控制算法计算出需求力/力矩，然后对水下机器人各推进器的推力大小进行合理的推力分配，从而使推进器产生的合成力/力矩满足水下机器人控制所期望的力/力矩。

但是水下机器人的水下作业环境较为复杂，会受到各种影响，当水下机器人在强扰动环境或极限运动的任务工况下，通过目前的推力分配方法驱动的推进器产生的推力可能达不到控制系统输出的期望推力，此时控制系统与推力分配构成了一个非常复杂的非线性系统，很难估计水下机器人的运动趋势，容易导致作业任务失败。

发明内容

本发明人针对上述问题及技术需求，提出了一种水下机器人的推力分配器及推力分配方法，本发明的技术方案如下：

一种水下机器人的推力分配方法，该方法包括：

确定水下机器人的垂向期望力τ_dz、水平纵向期望力τ_ldx和水平横向期望力τ_ldy和转艏力矩τ_ldn，其中水平纵向是沿着水下机器人艏部向前的方向，水平横向是水平面上垂直于水平纵向朝向水下机器人右侧的方向，垂向是垂直于水平面的方向，水下机器人搭载有若干个水平推进器和垂直推进器；

对水平纵向期望力τ_ldx在各个水平推进器上进行单自由度推力分配得到各个水平推进器的第一初始推力T_ldx，对水平横向期望力τ_ldy在各个水平推进器上进行单自由度推力分配得到各个水平推进器的第二初始推力T_ldy，对转艏力矩τ_ldn在各个水平推进器上进行单自由度力矩分配得到各个水平推进器的第三初始推力T_ldn；

构建每个水平推进器的水平推力合成公式为T_ld＝k₁×T_ldn+k₂×T_ldx+k₃×T_ldy，其中，k₁、k₂、k₃均为[0,1]范围内的待求系数，并根据水下机器人当前作业任务对各个自由度的运动需求确定目标函数，目标函数指示各个待求系数取得更大取值的期望优先级，水平纵向移动的运动需求越大、k₂的期望优先级越大，水平横向移动的运动需求越大、k₃的期望优先级越大，艏部转动的运动需求越大、k₁的期望优先级越大；

基于目标函数在推进器的推力范围所确定的约束条件下求解各个水平推进器的水平推力合成公式，得到待求系数k₁、k₂、k₃的取值，并代入每个水平推进器的水平推力合成公式得到每个水平推进器的指令推力值；

对垂向期望力τ_dz在各个垂直推进器上进行推力分配得到各个垂直推进器的指令推力值，按照各个垂直推进器的指令推力值和各个水平推进器的指令推力值控制各个推进器。

一种水下机器人的推力分配器，该推力分配器包括处理器及其相连的接口电路，推力分配器通过接口电路分别连接总控台和水下机器人搭载的推进器，处理器用于执行本申请提供的推力分配方法，并通过接口电路进行外部交互。

本发明的有益技术效果是：

本申请公开了一种水下机器人的推力分配方法，该方法从水下机器人作业任务需求考虑来进行推力分配，能够有效避免推力分配过饱和问题，同时能够使合成的实际推力能够按照预先设定的期望优先级进行推力输出，通过适当牺牲某些自由度上的控制性能以优先保证关键自由度上的控制性能，更符合水下机器人在设定作业任务下的推力需求，从而保证作业任务能够顺利完成，该方法计算过程简单，实时性好。

本申请还公开了一种水下机器人的推力分配器，该推力分配器具有丰富的外设接口，可以适配多种类型的推进器，具有良好的适应性。同时，独立的推力分配器能够使水下机器人的推力分配与其他系统解耦，可以独立开发、升级及换装。

附图说明

图1是一个实施例中的推力分配方法的信息流示意图。

图2是另一个实施例中的推力分配方法的信息流示意图。

图3是一个实施例中的推力分配器的结构示意图。

具体实施方式

下面结合附图对本发明的具体实施方式做进一步说明。

本申请公开了一种水下机器人的推力分配方法，请参考图1所示的流程图，该方法包括如下步骤：

步骤110，确定水下机器人的垂向期望力τ_dz、水平纵向期望力τ_ldx和水平横向期望力τ_ldy和转艏力矩τ_ldn。本申请所指的水平纵向是沿着水下机器人艏部向前的方向，水平横向是水平面上垂直于水平纵向朝向水下机器人右侧的方向，垂向是垂直于水平面的方向。

这里的τ_dz、τ_ldx、τ_ldy、τ_ldn能够反映水下机器人的期望推力和期望力矩，且不会导致推进器的推力超出推力范围而产生推力过饱和现象。

水下机器人搭载有若干个水平推进器和垂直推进器，且推进器的装配位置都是固定已知的，在一个实施例中，水下机器人搭载有四台水平推进器，其中两台水平推进器安装在水下机器人的前部且与水下机器人的轴线正方向的夹角均为θ，另外两台水平推进器安装在水下机器人的后部且与水下机器人的轴线反方向的夹角均为θ，水下机器人的重心到推进器的轴线的距离均为l_d，另外，水下机器人搭载有两台垂直推进器，且两台垂直推进器分别安装在水下机器人的上下两侧。

步骤120，对水平纵向期望力τ_ldx在各个水平推进器上进行单自由度推力分配得到各个水平推进器的第一初始推力T_ldx，对水平横向期望力τ_ldy在各个水平推进器上进行单自由度推力分配得到各个水平推进器的第二初始推力T_ldy，对转艏力矩τ_ldn在各个水平推进器上进行单自由度力矩分配得到各个水平推进器的第三初始推力T_ldn。

得到各个水平推进器的第一初始推力T_ldx、第二初始推力T_ldy和第三初始推力T_ldn的方法包括：

其中，

基于水平面推力合成矩阵B的伪逆矩阵B⁺得到，水平面推力合成矩阵B基于水下机器人搭载的水平推进器的装配位置所确定。基于上述提到的四个水平推进器的装配位置，所确定的水平面推力合成矩阵B为：

则基于B⁺＝B^T(BB^T)^-1的公式即可以得到水平面推力合成矩阵B的伪逆矩阵B⁺。以伪逆矩阵

来表示伪逆矩阵中的各个元素，则上述计算初始推力过程中需要使用到的列向量

具体写为：

由此可以得到四个水平推进器的第一初始推力T_ldx、第二初始推力T_ldy和第三初始推力T_ldn表示为：

步骤130，构建每个水平推进器的水平推力合成公式为T_ld＝k₁×T_ldn+k₂×T_ldx+k₃×T_ldy，其中，k₁、k₂、k₃均为[0,1]范围内的待求系数。在得到每个水平推进器三个初始推力后，若直接进行推力叠加，则还是会出现推进器推力过饱和的情况，因此为了避免出现推力过饱和问题，本申请采用利用待求系数进行加权的推力合成。

基于水下机器人搭载四个水平推进器的实施例，构建的四个水平推进器的水平推力合成公式分别为：

根据水下机器人当前作业任务对各个自由度的运动需求确定目标函数，该目标函数指示各个待求系数的期望优先级，一个待求系数的期望优先级越大，该待求系数越优先取得更大取值，比如k₁的期望优先级越大，则三个待求系数中，k₁优先取得最大值为1。水平纵向移动的运动需求越大、k₂的期望优先级越大；水平横向移动的运动需求越大、k₃的期望优先级越大，艏部转动的运动需求越大、k₁的期望优先级越大。比如以水下机器人定向横移观测任务为例，此时需要优先保证水下机器人航向性能(对应艏部转动的运动需求)，其次需要保证水下机器人水平纵向位置的调节能力(对应水平纵向移动的运动需求)，最后多余推进能力用于产生横移(对应水平横向移动的运动需求)，因此此时k₁的期望优先级最大、k₂的期望优先级次大、k₃的期望优先级最小。水下机器人的诸如横移观测、对接回收等各种典型的作业任务下各个待求系数的期望优先级预先配置设定。

步骤140，基于目标函数在推进器的推力范围所确定的约束条件下求解各个水平推进器的水平推力合成公式，得到待求系数k₁、k₂、k₃的取值。

每个水平推进器的水平推力合成公式所要满足的约束条件为-T_max≤k₁×T_ldn+k₂×T_ldx+k₃×T_ldy≤T_max，其中推进器的推力范围为[-T_max,T_max]，符号表示推力方向。本申请以各个推进器的推力范围相同为例，当推力范围不同时，同样可以基于本申请的方法实现。

则以包含四个水平推进器为例，所有水平推力器的水平推力合成公式结合三个待求系数所要满足的取值范围所确定的所有约束条件可以写为：

在目标函数下求解约束条件时，首先将期望优先级最大的待求系数取为1，然后在各个水平推进器的水平推力合成公式满足约束条件的基础上，取期望优先级次大的待求系数的最大值。在期望优先级最大和次大的两个待求系数取值确定，且在各个水平推进器的水平推力合成公式满足约束条件的基础上，取期望优先级最小的待求系数的最大值。

比如在一个实例中，假定k₁的期望优先级最大，k₂次之，k₃最小。则首先取k₁＝1，然后在满足上述约束条件的情况下取k₂可以取到的最大值，假设取到k₂＝0.8，在k₁和k₂取值确定的基础上，在满足上述约束条件的情况下取k₃可以取到的最大值。

本申请一个实施例基于线性规划的方法进行上述求解，在期望优先级最大的待求系数已经取为1的基础上，p个水平推进器中每个水平推进器的水平推力合成公式所要满足的-T_max≤k₁×T_ldn+k₂×T_ldx+k₃×T_ldy≤T_max的约束条件共计2p个线性不等式，加上另外两个待求系数要在[0,1]范围的要求构成的2个线性不等式，则此时一共构成了2p+4个线性不等式。比如在上述举例中，包括4个推进器时，在期望优先级最大的待求系数已经取为1的基础上共构成12个线性不等式，然后基于线性规划的方法求解这2p+4个线性不等式。具体的：

(1)以另外两个待求系数为横纵坐标，由2p+4个线性不等式的边界条件所确定的2p+4个直线中所有两两相交的直线的交点，确定所有交点坐标，每个交点坐标的横纵坐标值即为两个待求系数的取值。

比如在首先取k₁＝1的基础上，以k₂为横坐标、k₃为纵坐标，求取2p+4个直线的交点，每个交点坐标表示为(k₂，k₃)即包含两个待求系数的取值。

(2)将所有交点坐标代入上述约束条件中计算，筛选出所有交点坐标中满足所有2p+4个线性不等式的交点坐标作为候选交点坐标。

(3)将所有候选交点坐标中包含的期望优先级次大的待求系数的取值最大的一组候选交点坐标包含的两个待求系数作为剩余另外两个待求系数的取值；当存在至少两组候选交点坐标包含的期望优先级次大的待求系数的取值相同时，将其中包含的期望优先级最小的待求系数的取值也最大的一组候选交点坐标包含的两个待求系数作为剩余另外两个待求系数的取值。

比如，k₂的期望优先级次大，k₃的期望优先级最小。则将所有候选交点坐标按照k₂降序进行排序，取其中k₂最大的候选交点坐标的坐标值作为k₂和k₃的取值。若有多个候选交点坐标中的k₂的取值相等，则选取k₂的取值相等且最大的多个候选交点坐标包含的k₃最大的候选交点坐标的坐标值作为k₂和k₃的取值。在一个实例中，假设表示为(k₂，k₃)的候选交点坐标分别为(0.8,0.5)、(0.9,0.2)、(0.9,0.4)、(0.7,0.7)，则确定(0.9,0.2)、(0.9,0.4)中的k₂取值相等且最大，并从中选取k₃最大的(0.9,0.4)，由此可以确定k₂＝0.9、k₃＝0.4。

通过上述方法求得k₁、k₂、k₃后，代入每个水平推进器的水平推力合成公式得到每个水平推进器的指令推力值。比如代入

中即可以求得四个水平推进器的指令推力值T_{ld_1}、T_{ld_2}、T_{ld_3}、T_{ld_4}。

步骤150，对垂向期望力τ_dz在各个垂直推进器上进行推力分配得到各个垂直推进器的指令推力值。在包括两个垂直推进器的基础上，两个垂直推进器的指令推力值均为

然后可以按照各个垂直推进器的指令推力值和各个水平推进器的指令推力值控制各个推进器。具体的，将每个推进器的指令推力值根据预先拟合得到的推进器的转速-推力曲线换算得到推进器的指令转速，然后控制推进器按照对应的指令转速进行转动。

在本申请的推力分配方法的执行过程中，上述步骤110中所确定的τ_dz、τ_ldx、τ_ldy、τ_ldn基于针对水下机器人的期望推力和期望力矩确定，期望推力和期望力矩具体包括垂向推力理论值、水平纵向推力理论值τ_dx、水平横向推力理论值τ_dy和转艏力矩理论值τ_dn，由于垂直方向上仅有驱动方向相反的两个垂直推进器，因此直接将垂向推力理论值直接作为垂向期望力τ_dz。但本申请并不直接将水平面上的τ_dx、τ_dy、τ_dn作为τ_ldx、τ_ldy、τ_ldn，而是会先检测这些理论值是否会带来推力过饱和的问题，也即包括如下步骤，请参考图2：

步骤210，接收垂向推力理论值、水平纵向推力理论值τ_dx、水平横向推力理论值τ_dy和转艏力矩理论值τ_dn。这些理论值一般是用户在总控台处设置的。

步骤220，利用水平面推力合成矩阵B对水平纵向推力理论值τ_dx、水平横向推力理论值τ_dy和转艏力矩理论值τ_dn进行推力分配，得到各个水平推进器的理论推力。水平面推力合成矩阵B的定义和在实例中的具体形式如上述步骤120所示，基于水平面推力合成矩阵B的伪逆矩阵B⁺＝B^T(BB^T)^-1，得到四个水平推进器的理论推力分别为：

步骤230，当存在至少一个水平推进器的理论推力超出推进器的推力范围时，根据推进器的推力范围对水平纵向推力理论值τ_dx、水平横向推力理论值τ_dy和转艏力矩理论值τ_dn进行限幅处理，分别得到水平纵向期望力τ_ldx、水平横向期望力τ_ldy和转艏力矩τ_ldn。

以所有水平推进器的推力范围均为上述提到的[-T_max,T_max]为例，也即检测是否对于任意的参数j都满足-T_max≤T_dj≤T_max，在本申请的实例中j＝1、2、3、4。若存在一个或多个不满足上述不等式，则表示存在推进器的理论推力超出该推力范围，若直接按照τ_dx、τ_dy、τ_dn进行推力分配会带来推力过饱和的问题，导致有一个或多个水平推进器所要实现的理论推力超出其所能实现的范围而难以达成所需要的推力，容易导致作业任务失败。所以对τ_dx、τ_dy、τ_dn分别进行限幅，得到不会产生推力过饱和现象的τ_ldx、τ_ldy、τ_ldn，限幅处理得到的τ_ldx、τ_ldy、τ_ldn为：

然后从上述步骤110开始执行，得到各个水平推进器的令推力值。

步骤240，若所有水平推进器的理论推力都在推进器的推力范围内，则表示直接按照τ_dx、τ_dy、τ_dn进行推力分配不会带来推力过饱和的问题，则此时可以直接将水平推进器的理论推力作为对应水平推进器的指令推力值。然后直接上述步骤150。

本申请还公开了一种水下机器人的推力分配器以实现上述推力分配方法，请参考图3，该推力分配器包括处理器及其相连的接口电路，处理器可以采用Arm处理器，推力分配器通过接口电路分别连接总控台和水下机器人搭载的推进器。处理器用于执行上述图1和2所示实施例的推力分配方法，并通过接口电路进行外部交互。具体的，接口电路包括以太网通讯接口电路和推进器接口电路，推力分配器通过以太网通讯接口电路连接总控台并接收垂向推力理论值、水平纵向推力理论值τ_dx、水平横向推力理论值τ_dy和转艏力矩理论值τ_dn。推力分配器通过推进器接口电路连接推进器并发送与推进器的指令推力值对应的控制指令，推进器接口电路包括CAN总线接口电路、RS485总线接口电路和模拟量输出电路中的至少一种。该推力分配器可以与水下机器人的其他控制系统解耦，实现独立控制、开发、升级和换装。另外配备丰富的推进器接口电路，可以适配不同类型的推进器。

以上所述的仅是本申请的优选实施方式，本发明不限于以上实施例。可以理解，本领域技术人员在不脱离本发明的精神和构思的前提下直接导出或联想到的其他改进和变化，均应认为包含在本发明的保护范围之内。

Claims (10)

1.一种水下机器人的推力分配方法，其特征在于，所述方法包括：

确定所述水下机器人的垂向期望力τ_dz、水平纵向期望力τ_ldx和水平横向期望力τ_ldy和转艏力矩τ_ldn，其中水平纵向是沿着水下机器人艏部向前的方向，水平横向是水平面上垂直于水平纵向朝向所述水下机器人右侧的方向，垂向是垂直于水平面的方向，所述水下机器人搭载有若干个水平推进器和垂直推进器；

对所述水平纵向期望力τ_ldx在各个水平推进器上进行单自由度推力分配得到各个水平推进器的第一初始推力T_ldx，对所述水平横向期望力τ_ldy在各个水平推进器上进行单自由度推力分配得到各个水平推进器的第二初始推力T_ldy，对所述转艏力矩τ_ldn在各个水平推进器上进行单自由度力矩分配得到各个水平推进器的第三初始推力T_ldn；

构建每个水平推进器的水平推力合成公式为T_ld＝k₁×T_ldn+k₂×T_ldx+k₃×T_ldy，其中，k₁、k₂、k₃均为[0,1]范围内的待求系数，并根据所述水下机器人当前作业任务对各个自由度的运动需求确定目标函数，所述目标函数指示各个待求系数取得更大取值的期望优先级，水平纵向移动的运动需求越大、k₂的期望优先级越大，水平横向移动的运动需求越大、k₃的期望优先级越大，艏部转动的运动需求越大、k₁的期望优先级越大；

基于所述目标函数在推进器的推力范围所确定的约束条件下求解各个水平推进器的水平推力合成公式，得到待求系数k₁、k₂、k₃的取值，并代入每个水平推进器的水平推力合成公式得到每个水平推进器的指令推力值；

对所述垂向期望力τ_dz在各个垂直推进器上进行推力分配得到各个垂直推进器的指令推力值，按照各个垂直推进器的指令推力值和各个水平推进器的指令推力值控制各个推进器。

2.根据权利要求1所述的方法，其特征在于，所述方法还包括：

接收垂向推力理论值、水平纵向推力理论值τ_dx、水平横向推力理论值τ_dy和转艏力矩理论值τ_dn，并将所述垂向推力理论值直接作为所述垂向期望力τ_dz；

利用水平面推力合成矩阵B对水平纵向推力理论值τ_dx、水平横向推力理论值τ_dy和转艏力矩理论值τ_dn进行推力分配，得到各个水平推进器的理论推力，所述水平面推力合成矩阵B由所述水下机器人搭载的各个水平推进器的装配位置所确定；

当存在至少一个水平推进器的理论推力超出推进器的推力范围时，根据推进器的推力范围对水平纵向推力理论值τ_dx、水平横向推力理论值τ_dy和转艏力矩理论值τ_dn进行限幅处理，分别得到所述水平纵向期望力τ_ldx、水平横向期望力τ_ldy和转艏力矩τ_ldn。

3.根据权利要求2所述的方法，其特征在于，

所述水下机器人搭载有四台水平推进器，其中两台水平推进器安装在所述水下机器人的前部且与所述水下机器人的轴线正方向的夹角均为θ，另外两台水平推进器安装在所述水下机器人的后部且与所述水下机器人的轴线反方向的夹角均为θ，所述水下机器人的重心到推进器的轴线的距离均为l_d，确定的水平面推力合成矩阵B为

则所述得到各个水平推进器的理论推力，包括：

其中，B⁺是水平面推力合成矩阵B的伪逆矩阵，且有B⁺＝B^T(BB^T)^-1。

4.根据权利要求3所述的方法，其特征在于，所述根据推进器的推力范围对水平纵向推力理论值τ_dx、水平横向推力理论值τ_dy和转艏力矩理论值τ_dn进行限幅处理，包括分别得到τ_ldx、τ_ldy、τ_ldn为：

其中，推进器的推力范围为[-T_max,T_max]，符号表示推力方向。

5.根据权利要求2所述的方法，其特征在于，所述方法还包括：

若所有水平推进器的理论推力都在推进器的推力范围内，则直接将所述水平推进器的理论推力作为对应水平推进器的指令推力值。

6.根据权利要求3所述的方法，其特征在于，得到第一初始推力T_ldx、第二初始推力T_ldy和第三初始推力T_ldn的方法包括：

其中，

基于水平面推力合成矩阵B的伪逆矩阵B⁺得到，当伪逆矩阵

时，

7.根据权利要求1所述的方法，其特征在于，所述基于所述目标函数在推进器的推力范围所确定的约束条件下求解各个水平推进器的水平推力合成公式，包括：

将期望优先级最大的待求系数取为1，在各个水平推进器的水平推力合成公式满足所述约束条件的基础上，取期望优先级次大的待求系数的最大值；在期望优先级最大和次大的两个待求系数取值确定，且在各个水平推进器的水平推力合成公式满足所述约束条件的基础上，取期望优先级最小的待求系数的最大值。

8.根据权利要求7所述的方法，其特征在于，在期望优先级最大的待求系数取为1的基础上，求解另外两个待求系数的方法包括：

基于线性规划的方法求解2p+4个线性不等式，2p+4个线性不等式包括p个水平推进器中每个水平推进器的水平推力合成公式所要满足的-T_max≤k₁×T_ldn+k₂×T_ldx+k₃×T_ldy≤T_max的约束条件构成的2p个线性不等式，以及另外两个待求系数要在[0,1]范围的要求构成的2个线性不等式；

以另外两个待求系数为横纵坐标，由2p+4个线性不等式的边界条件所确定的2p+4个直线中所有两两相交的直线的交点，确定所有交点坐标；

筛选出所有交点坐标中满足所有2p+4个线性不等式的交点坐标作为候选交点坐标；

将所有候选交点坐标中包含的期望优先级次大的待求系数的取值最大的一组候选交点坐标包含的两个待求系数作为剩余另外两个待求系数的取值；当存在至少两组候选交点坐标包含的期望优先级次大的待求系数的取值相同时，将其中包含的期望优先级最小的待求系数的取值也最大的一组候选交点坐标包含的两个待求系数作为剩余另外两个待求系数的取值。

9.一种水下机器人的推力分配器，其特征在于，所述推力分配器包括处理器及其相连的接口电路，所述推力分配器通过所述接口电路分别连接总控台和所述水下机器人搭载的推进器，所述处理器用于执行如权利要求1-8所述的推力分配方法，并通过所述接口电路进行外部交互。

10.根据权利要求9所述的推力分配器，其特征在于，所述接口电路包括以太网通讯接口电路和推进器接口电路，所述推力分配器通过所述以太网通讯接口电路连接总控台并接收垂向推力理论值、水平纵向推力理论值τ_dx、水平横向推力理论值τ_dy和转艏力矩理论值τ_dn，所述推力分配器通过所述推进器接口电路连接所述推进器并发送与推进器的指令推力值对应的控制指令，所述推进器接口电路包括CAN总线接口电路、RS485总线接口电路和模拟量输出电路中的至少一种。

Images