一种用于水下机器人的切换推进器位置部署和动力分配的方法

1.本发明属于机器人控制领域，涉及到机器人推进器的位置部署和动力分配。


背景技术：

2.目前水下机器人的控制技术日趋成熟，其中很重要的一方面是推进器的位置部署和动力分配方案。在大多数场景中，水下机器人通过前进后退、上升下降、偏航这三个自由度的运动即可完成指定任务，每个自由度的运动一般由多个推进器协同作用完成。实现上述三个自由度的运动至少需要三个推进器，其中一个推进器提供使机器人上升下降的动力，另外两个推进器共同提供使机器人前进后退和做偏航运动的动力。随着推进器数量增加，通过合理的位置部署和动力分配方案能够实现水下机器人在更多自由度上的运动。现有的各种推进器位置部署和动力分配方案在部分推进器突然发生故障会使水下机器人立刻失去对应自由度上的运动功能，在难以及时打捞上岸的情况下不能继续完成既定任务，甚至会引发一系列安全问题。本发明针对上述问题，提出一种能够实时切换推进器位置部署和动力分配的设计方案，适用于不同场景下的水下机器人。


技术实现要素：

3.本发明的目的是针对现有技术背景中的不足，提出一种用于水下机器人的切换推进器位置部署和动力分配的方法，该方法设计了能够使推进器改变在水下机器人上的位置和提供的动力方向的结构，分析推进器提供的动力和转速确定发生故障的推进器，合理改变其他推进器在机器人上的位置部署和提供的动力方向保障机器人在各个自由度上的运动功能，并能够据此调整机体坐标系，重新进行每个推进器的动力分配。
4.本发明的目的可以通过如下技术方案实现，所述方法包括：
5.1)基于水下机器人的结构初始化动力分配方案，具体内容包括：
6.(1)在水下机器人布置推进器的位置安装单轴滑台，将推进器安装在二维云台上改变推进器提供的动力方向，再将二维云台安装在单轴滑台上，能够改变推进器在机器人上的位置。
7.(2)水下机器人上的推进器，其一般初始位置关于机器人中心对称，则单轴滑台的位置也关于水下机器人中心对称，推进器能够从机器人的边缘移动到中心。
8.(3)当推进器为水下机器人在某一自由度运动提供的动力的合力方向改变时，机体坐标系改变。
9.(4)根据水下机器人动力分配方案，控制器求解得到的输出控制量与水下机器人推进器提供的动力存在如下关系：
10.u＝w
·f11.其中，u＝[u
1 u
2 u
3 u4]
t
为水下机器人的输出控制量，f＝[f
1 f2...fn]
t
为水下机器人每个推进器提供的动力组成的向量，w为水下机器人的动力分配矩阵，其具体值由水下
机器人的结构和推进器的数量、位置以及提供的动力方向决定。
[0012]
通过矩阵求逆或伪逆得到推进器的动力：
[0013]
f＝w-1
·u[0014]
若w为方阵且可逆，则直接求逆运算得到w-1
，否则通过伪逆运算得到w-1
，最终得到所需的推进器动力f，然后根据推进器电压与动力的关系控制输出电压。
[0015]
2)比较每个推进器对应的测速传感器数据，具体步骤为：
[0016]
推进器提供的动力f和转速v满足如下关系：
[0017]
f＝kv2[0018]
对于相同环境下同一种型号规格的推进器，k为定值，在使用前通过多次实验测得该k值。水下机器人运动时，计算每个提供动力的推进器的动力并测量其转速，求动力和转速平方的比值并做比较。
[0019]
3)根据步骤2)得到的每个推进器的动力和转速平方的比值，找到与步骤2)中的k值有明显差异的值，该值对应的推进器即为发生故障的推进器。
[0020]
4)根据步骤1)水下机器人的结构与步骤3)得到的发生故障的推进器，改变推进器的位置部署，具体包括：
[0021]
(1)若与某发生故障的推进器关于水下机器人中心对称的推进器未发生故障，此时为机器人提供某一自由度运动所需动力的推进器一定没有全部发生故障，则将该位置未发生故障的推进器移动到机器人中心位置，使其提供使机器人上下方向运动的动力。
[0022]
(2)若为机器人提供某一自由度运动所需动力的推进器全部发生故障，则必须改变为其他自由度运动提供动力的推进器的位置和方向弥补失去的自由度上的运动功能。
[0023]
5)根据步骤4)得到的推进器位置和方向，调整机体坐标系，具体包括：
[0024]
(1)根据步骤4)的结果，判断水下机器人前进时推进器提供的动力的合力方向相比初始状态是否发生变化。
[0025]
(2)若步骤(1)的结果为未发生变化，则不需要调整机体坐标系。
[0026]
(3)若步骤(1)的结果为发生变化，且机体坐标系使用右-前-上坐标系，则调整机体坐标系的y轴正方向与水下机器人前进时推进器提供的动力的合力方向一致，z轴正方向与水下机器人上升时推进器提供的动力的合力方向一致。
[0027]
(4)若步骤(1)的结果为发生变化，且机体坐标系使用前-右-下坐标系，则调整机体坐标系的x轴正方向与水下机器人前进时推进器提供的动力的合力方向一致，z轴正方向与水下机器人下降时推进器提供的动力的合力方向一致。
[0028]
6)根据步骤4)和步骤5)的结果，按照步骤1)和步骤4)所述方法确定每个推进器的动力分配。
[0029]
本发明具有以下优点和积极效果：
[0030]
1)适用不同大小、形状和功能的水下机器人。
[0031]
2)提供改变每个推进器位置和方向的方法。
[0032]
3)提供判断每个推进器是否发生故障的方法。
[0033]
4)能够在至多半数推进器同时发生故障时提供根据不同的推进器位置部署进行动力分配的方法，保障机器人在各个自由度上的运动功能。
[0034]
5)能够改变水下机器人的机体坐标系，增加灵活性和普适性。
附图说明
[0035]
图1为本发明实施的水下机器人的切换推进器位置部署和动力分配的方法流程图；
[0036]
图2为本发明实施例中水下机器人总体结构和推进器初始位置示意图；
[0037]
图3为本发明实施例中底盘上工件位置俯视及坐标系示意图；
[0038]
图4为本发明实施例中二维云台的两个舵机的位置以及和推进器连接方式示意图；
[0039]
图5为本发明实施例中顶层板上工件位置俯视及坐标系示意图；
[0040]
图6为本发明实施例中水下机器人初始坐标系示意图；
[0041]
图7为本发明实施例中只有5号推进器发生故障时的顶层板俯视图；
[0042]
图8为本发明实施例中只有3号推进器发生故障时的底盘俯视图；
[0043]
图9为本发明实施例中只有3号推进器发生故障时的底盘侧视图；
[0044]
图10为本发明实施例中5号6号推进器同时发生故障机器人上下运动时底盘俯视图；
[0045]
图11为本发明实施例中1号2号推进器同时发生故障机器人左右运动时底盘俯视图；
[0046]
图12为本发明实施例中1号、3号和4号推进器同时发生故障时顶层板俯视图；
[0047]
图13为本发明实施例中1号、3号和4号推进器同时发生故障机器人左右运动时底盘俯视图；
[0048]
图14为本发明实施例中1号、3号和4号推进器同时发生故障机器人上下运动时底盘俯视图。
具体实施方式
[0049]
下面结合实施方式举例和附图对本发明做进一步详细的描述，但本发明的实施方式不限于此。
[0050]
图1提供了一种用于水下机器人的切换推进器位置部署和动力分配的方法，该方法包括如下步骤：
[0051]
1)基于水下机器人的结构初始化动力分配方案，具体内容包括：
[0052]
(1)在水下机器人布置推进器的位置安装单轴滑台，将推进器安装在二维云台上改变推进器提供的动力方向，再将二维云台安装在单轴滑台上，能够改变推进器在机器人上的位置。
[0053]
(2)水下机器人上的推进器，其一般初始位置关于机器人中心对称，则单轴滑台的位置也关于水下机器人中心对称，能够使推进器从机器人的边缘移动到中心。
[0054]
(3)当推进器为水下机器人在某一自由度运动提供的动力的合力方向改变时，机体坐标系改变。
[0055]
(4)根据水下机器人动力分配方案，控制器求解得到的输出控制量与水下机器人推进器提供的动力存在如下关系：
[0056]
u＝w
·f[0057]
其中，u＝[u
1 u
2 u
3 u4]
t
为水下机器人的输出控制量，f＝[f
1 f2...fn]
t
为水下机
器人每个推进器的动力组成的向量，w为水下机器人的动力分配矩阵，其具体值由水下机器人的结构和推进器的数量以及位置决定。
[0058]
通过矩阵求逆或伪逆得到推进器的动力：
[0059]
f＝w-1
·u[0060]
若w为方阵且可逆，则直接求逆运算得到w-1
，否则通过伪逆运算得到w-1
，最终得到所需的推进器动力f，然后根据推进器电压与动力的关系控制输出电压。
[0061]
图2为一种水下机器人总体结构和推进器的初始位置分布示意图，主要由底盘、中间层板、顶层板和四块侧面板搭建组成基本结构，在底盘上的四个推进器分别安装在四个二维云台上，在顶层板上的两个推进器分别安装在两个二维云台上，每个二维云台安装在单轴滑台上。
[0062]
图3为底盘上单轴滑台、二维云台和推进器位置部署俯视及坐标系图，底盘的形状为正八边形，单轴滑台的位置关于底盘的中心对称。在每个单轴滑台上安装二维云台，将推进器按照图3所示的位置安装在二维云台上，箭头所指方向为推进器正转时提供的动力方向。
[0063]
图4为二维云台的两个舵机的位置以及和推进器连接方式示意图。二维云台主要由两个舵机连接组成。舵机1与单轴滑台连接，其转轴与单轴滑台所在机器人盘面平行，单轴滑台能够改变二维云台在机器人盘面所在二维空间内的位置，舵机1旋转能够协同单轴滑台改变推进器在三维空间内的位置和提供的动力方向；舵机2与推进器连接，其转轴与舵机1的转轴垂直，其旋转能够改变推进器提供的动力方向。
[0064]
图5为顶层板上单轴滑台、二维云台和推进器位置部署俯视及坐标系图，顶层板的形状为正八边形，单轴滑台的位置关于底盘的中心对称。在每个单轴滑台上安装二维云台，将推进器按照图5所示的位置安装在二维云台上。5号推进器正转时提供的动力竖直向上，6号推进器正转时提供的动力竖直向下。
[0065]
图6为水下机器人的坐标系图，采用右-前-上坐标系，并结合图2与图5的坐标系和对推进器的编号进行如下定义：
[0066]
(1)机器人中心为坐标系原点。
[0067]
(2)x轴沿5号推进器所在单轴滑台方向，通过机器人重心指向机器人向右运动的方向。
[0068]
(3)y轴平行于底盘并沿垂直于5号推进器所在单轴滑台方向，通过机器人重心指向机器人前进方向。
[0069]
(4)z轴通过机器人重心，竖直向上。
[0070]
(5)俯仰角θ为水下机器人绕机体坐标系x轴旋转的角度，逆时针旋转为正。
[0071]
(6)横滚角φ为水下机器人绕机体坐标系y轴旋转的角度，逆时针旋转为正。
[0072]
(7)偏航角ψ为水下机器人绕机体坐标系z轴旋转的角度，逆时针旋转为正。
[0073]
选用右-前-上坐标系为水下机器人的机体坐标系。定义输出控制量u＝[x y zψ]
t
，其中，x,y,z表示水下机器人沿三轴的直线运动，ψ表示水下机器人的偏航运动。由于上述四个自由度的运动能够满足大部分场景下的需求，所以此处不考虑俯仰和横滚运动。定义编号为i的推进器提供的动力大小为fi，i＝1，2，3，4，5，6。
[0074]
以图2所示的水下机器人为例，其动力分配存在如下关系：
[0075][0076]
上述关系中w不是方阵，通过伪逆运算方式得到w-1
：
[0077][0078]
由此得到水下机器人动力分配情况然后根据推进器电压与动力的关系控制输出电压。
[0079]
2)比较每个推进器对应的测速传感器数据，具体步骤为：
[0080]
推进器提供的动力f和转速v满足如下关系：
[0081]
f＝kv2[0082]
对于相同环境下同一种型号规格的推进器，k为定值，在使用前通过多次实验测得该k值。水下机器人运动时，计算每个提供动力的推进器的动力并测量其转速，求动力和转速平方的比值并做比较。
[0083]
3)根据步骤2)的得到的每个推进器的动力和转速平方的比值，找到与步骤2)中k值有明显差异的值，该值对应的推进器即为发生故障的推进器。
[0084]
4)根据步骤1)水下机器人的结构与步骤3)得到的发生故障的推进器，改变推进器的位置部署。
[0085]
5)根据步骤4)得到的推进器位置和方向，调整机体坐标系。
[0086]
6)根据步骤4)和步骤5)的结果，按照步骤1)和步骤4)所述方法确定每个推进器的动力分配。
[0087]
图2所示的水下机器人有推进器发生故障存在以下三种情况：
[0088]
1)只有一个推进器发生故障：
[0089]
(1)5号或6号推进器发生故障。
[0090]
以5号推进器发生故障为例进行分析，此时为机器人上下运动提供动力的推进器并未全部发生故障，将6号推进器沿单轴滑台移动至顶层板中央即可，为保障水下机器人的自平衡，5号推进器也应沿单轴滑台移动至滑台中点，但不参与后续动力分配。为机器人在其他自由度运动提供动力的推进器未发生故障，不需要调整机体坐标系。图7为调整推进器位置后的顶层板俯视图，6号推进器正转时提供的动力方向竖直向下。
[0091]
其动力分配存在如下关系：
[0092][0093]
上述关系中w不是方阵，通过伪逆运算方式得到w-1
：
[0094][0095]
(2)1号、2号、3号或4号推进器发生故障。
[0096]
以3号推进器发生故障为例进行分析，此时水下机器人的上下运动不受影响，但是其他三个自由度的运动均受到影响。此时将与3号推进器关于水下机器人底盘中心对称的2号推进器移动至底盘中心位置使其为机器人的左右运动提供动力，当3号推进器不参与动力分配时机器人在其他自由度上的运动不受影响。图8为调整推进器位置后的底盘俯视图，使机器人沿x和y轴移动的动力方向发生变化，需要将机体坐标系绕z轴顺时针旋转135度。顶层板上的推进器不需要调整。图9为调整推进器位置后的底盘侧视图，可见2号推进器提供动力时水流不被阻挡，不影响机器人沿x轴方向运动。
[0097]
其动力分配存在如下关系：
[0098][0099]
上述关系中w不是方阵，通过伪逆运算方式得到w-1
：
[0100][0101]
2)有两个推进器同时发生故障：
[0102]
(1)5号和6号推进器同时发生故障。
[0103]
此时水下机器人不能上下运动，其他自由度的运动不受影响。将底盘上的任意一个推进器移动到底盘中心为机器人上下运动和横向运动提供动力。
[0104]
下面以移动2号推进器为例分析，底盘俯视图和坐标系同图8，侧视图同图9，3号推进器为机器人偏航运动提供动力。旋转2号推进器至图10所示位置可以为机器人上下运动
提供动力，2号推进器正转时提供的动力竖直向上。此外需要将机体坐标系绕z轴顺时针旋转135度。顶层板上的推进器不需要调整。
[0105]
其动力分配存在如下关系：
[0106][0107]
上述关系中w是奇异矩阵，通过伪逆运算方式得到w-1
：
[0108][0109]
(2)底盘的两个推进器同时发生故障。
[0110]
此时水下机器人沿x轴的移动受影响，其他自由度不受影响。
[0111]
以1号和2号推进器同时发生故障为例分析。底盘的推进器位置不需要调整，顶层板的推进器位置调整至图7所示，6号推进器正转时提供的动力方向竖直向下，5号推进器不参与动力分配。当6号推进器旋转至图11所示位置时，提供使机器人沿x轴方向运动的动力，此时不需要调整机体坐标系。
[0112]
其动力分配存在如下关系：
[0113][0114]
上述关系中w不是方阵，通过伪逆运算方式得到w-1
：
[0115][0116]
(3)底盘和顶层板上各有一个推进器同时发生故障。
[0117]
以3号推进器和5号推进器同时发生故障为例，此时推进器和机体坐标系的调整以及动力分配方案与5号6号推进器同时发生故障时的情况相同，如图5、图8、图9与图10所示。
[0118]
3)有三个推进器发生故障：
[0119]
(1)底盘的两个推进器和顶层板的一个推进器同时发生故障。
[0120]
以1号、2号和5号推进器同时发生故障为例分析，此时推进器和机体坐标系的调整以及动力分配方案与1号2号推进器同时发生故障时的情况相同，如图3、图7和图11所示。
[0121]
(2)底盘的一个推进器和5号6号推进器同时发生故障。
[0122]
以3号、5号和6号推进器同时发生故障为例分析，此时推进器和机体坐标系的调整与5号6号推进器同时发生故障时的情况相同，如图5、图8、图9与图10所示。动力分配方案改
变，相比之下3号推进器不参与动力分配。
[0123]
其动力分配存在如下关系：
[0124][0125]
上述关系中w不是方阵，通过伪逆运算方式得到w-1
：
[0126][0127]
(3)底盘的三个推进器同时发生故障。
[0128]
以1号、3号、4号推进器同时发生故障为例分析，此时水下机器人只保留上下运动功能，其他自由度的运动均受到影响。根据上述的只有3号推进器发生故障的情况，2号推进器能够同时为机器人的上下左右运动提供动力，这里将顶层板推进器位置和方向以及机体坐标系调整至图12所示位置，使5号与6号推进器为机器人沿y轴运动和偏航运动提供动力。将底盘推进器位置和方向以及机体坐标系调整至图13所示位置，2号推进器为机器人沿x轴方向运动提供动力。将底盘推进器位置和方向以及机体坐标系调整至图14所示位置，2号推进器为机器人上下运动提供动力，2号推进器正转时提供的动力竖直向上。相比初始状态，将机体坐标系绕z轴逆时针旋转45度与机体坐标系保持一致。
[0129]
其动力分配存在如下关系：
[0130][0131]
上述关系中w不是方阵，通过伪逆运算方式得到w-1
：
[0132][0133]
以上所述仅为本发明优选的实施例，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明所公开的范围内，根据本发明的发明构思或者技术方案加以等同替换或改变，都属于本发明的保护范围。

技术特征：
1.一种用于水下机器人的切换推进器位置部署和动力分配的方法，其特征在于，所述方法包括以下步骤：1)基于水下机器人的结构初始化动力分配方案，具体包括：(1-1)利用单轴滑台、二维云台搭建结构，使得该结构能够调整推进器在水下机器人上的位置和提供的动力方向以及机体坐标系；(1-2)确定初始状态下的动力分配方案；2)根据步骤1)获取的动力分配方案，实时采集每个推进器提供的动力及其转速并进行处理，通过分析确定发生故障的推进器；3)根据步骤2)的分析结果，调整部分推进器在机器人上的位置部署和提供的动力方向，保障机器人在各个自由度上的运动功能；4)根据步骤3)得到的各个推进器位置和提供的动力方向，调整机体坐标系；5)根据步骤4)的最终调整结果，切换动力分配方案。2.根据权利要求1所述的一种用于水下机器人的切换推进器位置部署和动力分配的方法，其特征在于，步骤(1-1)具体实现方法包括：在水下机器人布置推进器的位置安装单轴滑台，将推进器安装在二维云台上用于改变推进器提供的动力方向，再将二维云台安装在单轴滑台上，用于改变推进器在水下机器人上的位置；水下机器人上的推进器的初始位置关于水下机器人中心对称，单轴滑台的位置也关于水下机器人中心对称，推进器能够从水下机器人的边缘移动到中心；当推进器为水下机器人在某一自由度运动提供的动力的方向改变时，机体坐标系改变。3.根据权利要求2所述的一种用于水下机器人的切换推进器位置部署和动力分配的方法，其特征在于，确定初始状态下的动力分配方案，具体包括：输出控制量与水下机器人推进器提供的动力存在如下关系：u＝w
·
f其中，u＝[u
1 u
2 u
3 u4]
t
为水下机器人的输出控制量，f＝[f
1 f
2 ... f
n
]
t
为水下机器人每个推进器提供的动力组成的向量，w为水下机器人的动力分配矩阵，其具体值由水下机器人的结构和推进器的数量、位置以及提供的动力方向决定；通过矩阵求逆或伪逆得到推进器提供的动力：f＝w-1
·
u若w为方阵且可逆，则直接求逆运算得到w-1
，否则通过伪逆运算得到w-1
，最终得到所需的推进器动力f，然后根据推进器电压与动力的关系控制输出电压。4.根据权利要求1所述的一种用于水下机器人的切换推进器位置部署和动力分配的方法，其特征在于，实时采集每个推进器提供的动力f及其转速v并进行处理，具体包括：每个推进器提供的动力f和转速v满足如下关系：f＝kv2水下机器人运动时，计算每个提供动力的推进器的动力并测量其转速，得到动力和转速平方的比值即k值。5.根据权利要求4所述的一种用于水下机器人的切换推进器位置部署和动力分配的方
法，其特征在于，通过比较确定发生故障的推进器，具体包括：对于相同环境下同一种型号规格的推进器，k为定值，在使用前通过多次实验测得该k值，通过比较获取与该k值有明显差异的值，其对应的推进器为发生故障的推进器。6.根据权利要求1所述的一种用于水下机器人的切换推进器位置部署和动力分配的方法，其特征在于，调整部分推进器在水下机器人上的位置部署和提供的动力方向，保障机器人在各个自由度上的运动功能，具体包括：若与某发生故障的推进器关于水下机器人中心对称的推进器未发生故障，此时为水下机器人提供某一自由度运动所需动力的推进器一定没有全部发生故障，则将该位置未发生故障的推进器移动到机器人中心位置，使其提供使机器人上下方向运动的动力；若为机器人提供某一自由度运动所需动力的推进器全部发生故障，则必须改变为其他自由度运动提供动力的推进器的位置和方向弥补失去的运动自由度。7.根据权利要求1所述的一种用于水下机器人的切换推进器位置部署和动力分配的方法，其特征在于，调整机体坐标系，具体包括：判断水下机器人前进时推进器提供的动力方向相比初始状态是否发生变化；若未发生变化，则不需要调整机体坐标系；若发生变化，且机体坐标系使用右-前-上坐标系，则调整后的机体坐标系的y轴正方向与水下机器人前进时推进器提供的动力的方向一致，z轴正方向与水下机器人上升时推进器提供的动力的方向一致；若发生变化，且机体坐标系使用前-右-下坐标系，则调整后的机体坐标系的x轴正方向与水下机器人前进时推进器提供的动力方向一致，z轴正方向与水下机器人下降时推进器提供的动力的方向一致。8.根据权利要求1所述的一种用于水下机器人的切换推进器位置部署和动力分配的方法，其特征在于，切换动力分配方案，具体包括：输出控制量与水下机器人推进器提供的动力存在如下关系：u＝w
·
f其中，u＝[u
1 u
2 u
3 u4]
t
为水下机器人的输出控制量，f＝[f
1 f
2 ... f
n
]
t
为水下机器人每个推进器提供的动力组成的向量，w为水下机器人的动力分配矩阵，其具体值由水下机器人的结构和推进器的数量以及位置决定；通过矩阵求逆或伪逆得到推进器的动力：f＝w-1
·
u若w为方阵且可逆，则直接求逆运算得到w-1
，否则通过伪逆运算得到w-1
，最终得到所需的推进器动力f，然后根据推进器电压与动力的关系控制输出电压。

技术总结
本发明提出了一种用于水下机器人的切换推进器位置部署和动力分配的方法，包括如下步骤：1.初始化动力分配方案；2.根据动力分配方案，实时采集每个推进器提供的动力及其转速并进行处理，通过比较获得明显异常值确定发生故障的推进器；3.根据分析结果，调整部分推进器在机器人上的位置和提供的动力方向，保障机器人在各个自由度上的运动功能；4.根据各个推进器位置和提供的动力方向，调整机体坐标系；5.根据最终调整结果，切换动力分配方案。本发明可以在水下机器人至多半数推进器同时发生故障时切换推进器位置部署和动力分配方案，调整机体坐标系，在多种情况下保障机器人各自由度上的运动功能，具有较强的灵活性和普适性。具有较强的灵活性和普适性。具有较强的灵活性和普适性。


技术研发人员：翟超 李佳运 祝孟杰 王龙旺 熊宇 刘文齐 赵鸿宇
受保护的技术使用者：中国科学技术大学
技术研发日：2023.06.14
技术公布日：2023/9/20