1.本发明涉及水下智能装备技术领域，具体涉及一种水下仿生柔性多功能常驻机器人、系统和控制方法。
背景技术：
2.目前水下机器人技术作为人类探索海洋的最重要的手段，在探索的过程中对各种智能海洋装备有着极大的需求，作为人类未来可持续发展起着至关重要的作用。由于水下空间及环境又极为复杂，各种水下结构和障碍物对机器人的工作又有极大的制约，尤其是现有水下机器人大都为开架式结构，受环境影响因素较大，尤其是在面对水下急流、越障、洞穿、沉船救援、打捞、海洋平台巡航等空间相对狭窄和复杂的任务载荷几乎无法完成其任务。
3.现有文献也公开了一些水下机器人，但仍与不足，具体分析如下：
4.姿态控制：在文献中阐述是采用抽绳的结构设计控制机器人的行动方向，水下机器人通过旋转驱动模块带动波动鳍面形成类似正弦波动，与周围流体形成反作用力的方式为该机器人提供前进动力，该种设计结构比较简单，受周围环境因素影响较大，无法实现精准的控制。
5.结构设计：水下机器人采用多组鳍的结构设计，通过鳍的摆动为装置提供动力，该种方式会增加机器人侧面截面面积，在急流中增加其阻力，对机器人本身会产生极大的扰动，为了抵抗扰动会增加能量消耗，水下生物缠绕其结构尤其是缠绕仿生鳍将使其失去动力使其无法前进。
6.另外，文献阐述通过四组周向布置的张拉驱动件的协调控制，可是实现对张拉绳的拉拽作用，在柔性板的弯曲变形的作用下可实现任意方向弯曲运动，该种设计中多种变量的引用，造成了多力矩耦合，尤其是柔性板的采用增加了其非线性的因素，该种设计难以实现有效的精准方向控制，而水下对柔性板会产生极大压力，而通过张拉绳的拉拽需要克服极大的水下压力，使该控制方式不可靠。
技术实现要素：
7.技术目的：针对上述技术问题，本发明提出了一种水下仿生柔性多功能常驻机器人及其控制算法，结构紧凑，具有仿生弯曲功能，整体呈现流线型，密度与流体接近，在机器人行进过程中，可实现路径的自主规划和推力控制自动分配，适应水下复杂的环境状况，有效满足多种洞穿等相关任务载荷要求。
8.技术方案：为实现上述技术目的，本发明采用了如下技术方案：
9.一种水下仿生柔性多功能常驻机器人，其特征在于：所述机器人整体呈流线型结构，由多个独立密封舱室连结构成，包括头部密封舱、颈部转向舱、躯干部分、尾部转向舱和尾舱；其中，
10.所述躯干部分包括躯干主舱、推进器固定舱和躯干转向舱，躯干主舱的前端和后
端分别设有多个推进器固定舱，推进器固定舱和躯干主舱之间设置躯干转向舱，相邻两个推进器固定舱之间设置躯干转向舱；躯干主舱内设有控制中心，控制中心的处理器包括pid控制器和lqr控制器；
11.所述颈部转向舱连接头部密封舱和躯干部分最前端的推进器固定舱，尾部转向舱连接躯干部分最后端的推进器固定舱和尾舱，颈部转向舱、尾部转向舱和各个躯干转向舱的舱内均设有转向机构，躯干主舱和各个推进器固定舱的舱内均设有推进机构，转向机构和推进机构的驱动器与控制中心电信号连接。
12.作为优选，设置于躯干主舱前的推进器固定舱，其内部的推进机构包括设置于前端的正转推力向上的垂直推进器，和设置于后端的正转推力向左的水平推进器；
13.设置于躯干主舱后的推进器固定舱，其内部的推进机构包括设置于前端的正转推力向下的垂直推进器，和设置于后端的正转推力向右的水平推进器；
14.所述躯干主舱，其内部的推进机构包括设置于前端的一个正转推力向上的垂直推进器，和对称设置于后端两侧的两个与轴线平行的水平推进器。
15.作为优选，所述头部密封舱、颈部转向舱、躯干主舱、推进器固定舱、躯干转向舱、尾部转向舱和尾舱，其与相邻舱段连接的一端均设有连接结构，所述连接结构包括密封法兰、底座、密封圈和若干螺栓，密封法兰通过螺栓固定于所在舱段的端部，密封法兰的圆周上开设密封槽，密封槽内装有密封圈，底座通过螺栓固定于密封法兰；
16.所述转向机构包括水平转向器和垂直转向器，水平转向器安装在所在舱段前端的底座上，垂直转向器安装在所在舱段后端的底座上，水平转向器和垂直转向器的摆动连杆呈90度啮合。
17.作为优选，所述垂直转向器还包括安装在底座上的控制主板、电机、固定器和减速器，所述底座通过螺栓连接控制主板，控制主板上方设置有电机，电机侧壁连接固定器一端，固定器另一端连接减速机侧壁，通过固定器连接电机和减速器起到固定作用，电机主轴输出端连接减速器，减速器输出端与垂直转向器的摆动连杆连接，躯干主舱内的控制中心电信号连接控制主板。
18.作为优选，设置于躯干主舱前的各个躯干转向舱内的水平转向器设置为与x方向呈5度夹角，垂直转向器设置成与y方向呈5度夹角，设置于躯干主舱前的各个躯干转向舱内的水平转向器设置为与x方向呈-5度夹角，垂直转向器设置成与y方向呈-5度夹角；
19.所述x方向和y方向，是指在局部舱体坐标系中的方向，即以各个舱段的形心位置为坐标原点，沿着舱段的中心轴线方向向前为x方向，z方向垂直于x方向竖直向上，y方向垂直于xoz平面。
20.作为优选，设置于躯干主舱内的控制中心包括电池、侧扫声呐、处理器、集成主板、数据处理中心和信号发生器；
21.所述电池为机器人的各个电器元件提供能源；
22.所述侧扫声呐通过发射声波和收集回声，探测包括周围环境和地形地貌在内的信息；
23.所述信号发生器用于对信号进行放大和输出；
24.所述集成主板用于将各种硬件设备进行连接，实现数据通信；
25.所述处理器是运算和控制核心，信息处理和程序运算；
26.所述数据处理中心是将所有的环境数据和内部运行数据进行整理、归纳，并将处理后的数据传输到处理器，用于提升处理器的运行和决策效率。
27.作为优选，所述机器人的密度与所在水下工作环境的流体密度的差值小于预设值；
28.所述颈部转向舱、尾部转向舱、躯干主舱、各个推进器固定舱和躯干转向舱的外部均设有柔性密封波纹管。
29.一种水下仿生柔性多功能常驻机器人系统，其特征在于，包括：
30.如权利要求1所述的水下仿生柔性多功能常驻机器人，用于工作在水下环境中，尾舱的末端设置用于与驻泊机构连接的停泊锚点；
31.驻泊机构，固定于水底平台，用于为所述水下仿生柔性多功能常驻机器人提供停泊位置和能量补给；
32.控制器，用于与所述水下仿生柔性多功能常驻机器人的控制中心通过无线网络进行数据和信号交互。
33.一种水下仿生多功能常驻机器人的控制方法，其特征在于，包括步骤：
34.a1、水下仿生多功能常驻机器人运行之前，预设运行参数，生成预定参考轨迹；
35.a2、水下仿生多功能常驻机器人运行过程中，实时采集机器人的位姿信息，包括各舱段的角度，角度包括俯仰角和偏航角；
36.a3、根据步骤a2中采集到的位姿信息、步骤a1中预设的运行参数和生成的预定参考轨迹，计算得到各舱段的角度误差；
37.a4、将步骤a3得到的角度误差输入pid控制器，pid控制器输出各个舱段的第一参考角度，所述pid控制器的pid参数预先通过ziegler-nichols法则或试验方法确定；
38.a5、将步骤a2中采集到的位姿信息输入lqr控制器，lqr控制器输出各个舱段的第二参考角度，同时根据观测器估计的系统总扰动实时补偿控制量，所述lqr控制器的lqr控制律预先设计并根据机器人运行的实际情况进行调整；
39.a6、根据pid控制器输出的第一参考角度，和所述lqr控制器输出的第二参考角度，计算得到用于控制机器人运行的各舱段的调整角度和调整角速度，根据调整角度和调整角速度生成控制指令；
40.s7、机器人对应舱段的驱动器根据所述控制指令，驱动对应的推进器或转向器运动，实现对应舱体的运动状态调整。
41.作为优选，所述步骤a5中所述lqr控制率，以如下步骤b1至b7方法设计：
42.b1、建立水下仿生多功能常驻机器人的俯仰和偏航姿态的解耦模型：
[0043][0044]
其中，b
ij
(i,j＝1,2,3,4)为俯仰和偏航运动方程中的控制系数，f
θ
和f
ψ
是总扰动函数；
[0045]
b2、定义则建立俯仰控制系统的状态空间方程：
[0046][0047]
其中，
[0048][0049][0050]
b3、引入lqr控制指标函数，
[0051][0052]
其中，为各项状态误差的代价函数，为各项输入状态的代价函数；
[0053]
对应的离散系统黎卡提方程为：
[0054][0055]
此时lqr线性反馈增益矩阵为：
[0056][0057]
b4、设计机器人位姿的状态扩张观测器
[0058][0059]
其中，
[0060][0061]
β1＝3ω
01
,β2＝3ω
012
,β3＝ω
013
,β4＝3ω
02
,β5＝3ω
022
,β6＝ω
023
,
[0062]
表示对状态量x的观测值,表示对输出y的观测值，ω
01
、ω
02
定义为观测器的带宽；
[0063]
在状态观测准确的情况下,机器人运动方程为：
[0064][0065]
和分别为观测器对偏航和俯仰总扰动的估计值；
[0066]
用李雅普诺夫法证明观测器的稳定性：
[0067]
定义为状态估计误差,将式(2)和(6)代入得:
[0068]
[0069]
其中
[0070]
根据李雅普诺夫稳定性判别方法,因为为hurwitz矩阵,h是有界的,满足李雅普诺夫稳定条件,因此状态变量是稳定的；
[0071]
b5、定义控制律：
[0072][0073]
b6、引入误差e1＝x
1 d-x1,e4＝x
4 d-x4，
[0074]
其中x
1 d
和x
4 d
分别为俯仰和偏航的参考信号；
[0075]
定义z1＝e1,z2＝x2,z3＝e4,z4＝x5，确定基于误差的机器人俯仰和偏航状态空间方程：
[0076][0077]
其中，根据lti系统可控性判据，矩阵的秩等于矩阵的维数，所以系统可控；
[0078]
b7、确定引入误差后的控制律为其中扰动补偿控制律为：
[0079][0080]
lqr控制律为：
[0081][0082]
其中，
[0083][0084]
至此lqr控制律设计完成。
[0085]
有益效果：由于采用了上述技术方案，本发明具有如下有益效果：
[0086]
(1)本发明提出的水下仿生柔性多功能常驻机器人外表面设置有柔性仿生材料外壳不仅可以起到防水作用，还可以减小水下滑行阻力，该机器人具备材料的柔软性、优良的环境适应性、超强的安全性，能够改变传统水下仿生柔性多功能常驻机器人材料的强度，使该水下仿生柔性多功能常驻机器人与环境、水下生物等具有更优异的互动性。
[0087]
(2)水下仿生柔性多功能常驻机器人在行进的过程中，通过距离传感器、双目摄像头、侧扫声呐的同时启用，可以对周围环境有着精确的判断，控制中心将数据汇总和分析，结合任务载荷的目的，可实现路径的自主规划，实现推力控制自动分配，实现转向装置和推进器自主推力分配。
[0088]
(3)本发明提出的水下仿生柔性多功能常驻机器人的结构紧凑，具有仿生弯曲功能，可以有效的适应洞穿等相关任务载荷，如洞穴救援、水下勘测、沉船探测、管道检查等相对狭窄的水下空间的任务载荷，整体呈现流线型，横截面积较小，水下阻力微弱，整体密度与流体接近，对能源需求较少，更加容易控制，有效解决因水下漂移而产生的位置惯性变化。
[0089]
(4)本发明提出的水下仿生柔性多功能常驻机器人的控制方法，针对现有技术中水下柔性机器人、系统参数时变、大惯性、纯滞后等特点，采用带有扰动补偿的lqr pid联合控制策略，通过运动策略将跟踪控制和防撞控制相结合，确保执行子任务的最高优先级。
[0090]
(5)本发明采用速度和航向角速率的控制器，能将误差收敛到零，观测器稳定性是从李雅普诺夫稳定性中获得的，以保证跟踪参考路径。
[0091]
(6)本发明提出的水下仿生柔性多功能常驻机器人的控制方法，将精准的控制算法的长期应用，可以使水下机器人获得自主学习能力，全面实现应对各种作业环境和作业载荷，依靠控制算法模型可以使水下机器人突破海量环境变量和硬件发展瓶颈，进而辅助人脑进行决策，以实现人工智能深度应用。
附图说明
[0092]
图1为水下仿生柔性多功能常驻机器人的结构示意图；
[0093]
图2为图1中各转向舱的内部连接图；
[0094]
图3为头部密封舱的结构示意图；
[0095]
图4为密封法兰的结构示意图；
[0096]
图5为柔性密封波纹管的结构示意图；
[0097]
图6为垂直转向器的结构示意图；
[0098]
图7为推进器固定舱的结构示意图；
[0099]
图8为躯干主舱的结构示意图；
[0100]
图9为躯干主舱的内部布置图；
[0101]
图10为控制流程图；
[0102]
图11为机器人和驻泊机构连接的示意图；
[0103]
图12为机器人的坐标系；
[0104]
图13为关节模块受力分析图；
[0105]
图14为在世界坐标系xoz平面中关节模块的受力分析示意图；
[0106]
图中：1-控制器，2-距离传感器，3-双目摄像头，4-螺栓，5-颈部转向舱，6-垂直推进器，7-第一躯干转向舱，8-垂直推进器，9-右水平推进器，10-螺栓，11-螺栓，12-推进器固定舱，13-螺栓，14-螺栓，15-尾舱，16-尾部转向舱，17-水平推进器，18-垂直推进器，19-第二躯干转向舱，20-左水平推进器，21-躯干主舱，22-螺栓，23-螺栓，24-水平推进器，25-推进器固定舱，26-螺栓，27-头部密封舱，28-灯光，29-密封法兰，30-底座，31-密封法兰，32-底座，33-转向器，34-转向器，35-密封法兰，36-底座，37-转向器，38-转向器，39-密封法兰，40-密封法兰，41-底座，42-转向器，43-转向器，44-底座，45-深度传感器，46-锚点，47-密封法兰，48-转向器，49-转向器，50-水下驻泊机构，51-柔性仿生外壳，52-密封槽，53-密封槽，54-密封槽，55-密封槽，56-密封槽，57-密封槽，58-密封槽，59-密封法兰，60-底座，61-螺
栓，62-减速器，63-摆动连杆，64-固定器，65-控制主板，66-处理器，67-集成主板，68-数据处理中心，69-侧扫声呐，70-信号发生器，71-电池，72-底座，73-密封槽，74-密封槽，75-电机，76-底座，77-密封槽，78-密封法兰，79-柔性密封波纹管。
具体实施方式
[0107]
下面结合附图对本发明的实施例作详细的说明。
[0108]
实施例一
[0109]
如图1至图11所示，本实施例提供了一种水下仿生柔性多功能常驻机器人装置，包括水上部分和水下部分两部分，两者之间通过无线网络进行数据和信号的交互。
[0110]
其中，水上部分是由控制器1对水下仿生柔性多功能常驻机器人的运行数据、运行环境等实时监测，水下部分由驻泊机构50和仿生柔性多功能常驻机器人两部分构成，水上部分与水下仿生柔性多功能常驻机器人通过网络可进行实时信息交互，控制器1可以在任意地理位置对水下仿生柔性多功能常驻机器人进行任务载荷发布，实现远程终端控制和任务载荷的随时执行，具有高机动性，更容易操作。
[0111]
如图1所示，水下仿生柔性多功能常驻机器人的主体结构由头部密封舱27、躯干、尾舱15三部分多段密封舱依次连接组成。
[0112]
如图1和图2所示，头部密封舱27前部在双目摄像头3下方设置有灯光28，灯光28上部设置有距离传感器2。头部密封舱27右侧与颈部转向舱5左侧连接，头部密封舱27右侧内部设置有密封法兰29，通过螺栓4连接密封法兰29和头部密封舱27右侧，颈部转向舱5左侧边压缩设置的密封圈，使头部密封舱27右侧和颈部转向舱5左侧实现独立密封，密封法兰29右侧中部设置有颈部转向机构安装底座30。如图3所示，头部密封舱27右侧外部设置有密封槽52，如图4所示，密封法兰29的左、右侧设置有密封槽73、密封槽74，在各个密封槽内设置有密封圈。
[0113]
如图1所示，躯干部分主要由颈部转向舱5、推进器固定舱25、第一躯干转向舱7、躯干主舱21、第二躯干转向舱19、推进器固定舱12、尾部转向舱16依次连接构成，且每个舱段均独立密封。
[0114]
颈部转向舱5外部设置有如图5所示的柔性密封波纹管79，柔性密封波纹管79右侧与推进器固定舱25左侧连接。如图2和图6所示，柔性密封波纹管79内部设置有水平转向器49和垂直转向器48组成，两者摆动连杆呈90度啮合，通过转向器的相互扭动可以带动相邻的舱段实现水平或者竖直方向的摆动，其中水平转向器49与密封法兰29上设置的底座30连接，垂直转向器48与推进器固定舱25左侧设置的密封法兰47上设置的底座72连接。底座72上通过螺栓61安装控制主板65，控制主板65上设置有电机75，电机75与减速器62连接，减速器62的输出端连接摆动连杆63，在减速器62外部设置有固定装置64，用于固定减速器62和电机75。控制主板65、电机75、减速器62连接和固定装置64位于颈部转向舱7内。在控制主板的作用下，通过驱动电机和减速器实现摆动连杆绕着自身主轴实现
±
10度角度摆动，通过两个摆动连杆相互啮合带动相邻的舱段，实现预定角度的转动。其他转动机构和推进机构的驱动器均电信号连接控制主板。
[0115]
如图1、图2和图7所示，推进器固定舱25右侧与第一躯干转向舱7左侧外部连接，推进器固定舱25左、右两侧外部设置有密封槽53、54，密封槽内设置有密封圈，推进器固定舱
25内部左、右两侧分别设置有密封法兰47、31，密封法兰47、31的左、右侧设置有密封槽73、74，密封槽内设置有密封圈，通过螺栓26连接密封法兰47。推进器固定舱25左侧、颈部转向舱5右侧压缩设置密封圈，使推进器固定舱25左侧和颈部转向舱5右侧实现独立密封。
[0116]
如图1、图2和图8所示，躯干转向舱7外部设置有柔性密封波纹管79，柔性密封波纹管右侧与躯干主舱21左侧连接，柔性密封波纹管内部设置有水平转向器33和垂直转向器34组成，水平转向器34与水平方向呈5度夹角，垂直转向器34与竖直方向呈5度夹角，两者啮合角度呈90度，通过水平转向器33和垂直转向器34的相互扭动，可以带动相邻的舱段即推进器固定舱25和躯干主舱21实现水平或者竖直方向的摆动，其中水平转向器33与密封法兰31上设置的安装底座32连接，垂直转向器34与躯干主舱21左侧上设置的安装底座60连接。螺栓23连接密封法兰31、推进器固定舱25右侧、躯干转向舱7左侧并压缩设置密封圈，使推进器固定舱25右侧和躯干转向舱7左侧实现独立密封，推进器固定舱25前部设置有垂直推进器6，推进器固定舱25后部设置有水平推进器24，两者之间呈相互垂直状态，密封法兰31右侧中部设置有躯干转向机构安装底座32。
[0117]
如图1、图2和图9所示，躯干主舱21右侧与躯干转向舱19左侧外部连接，躯干主舱21左、右两侧外部设置有密封槽57、58，密封槽57、58内设置有密封圈，躯干主舱21内部左、右侧两侧分别设置有密封法兰59、35，密封法兰59、35左、右侧设置有密封槽73、74，密封槽内设置有密封圈，通过螺栓22连接密封法兰57，躯干主舱21左侧、躯干转向舱7右侧并压缩设置密封圈，使躯干转向舱7右侧和躯干主舱21左侧实现独立密封，螺栓10连接密封法兰35、躯干主舱21右侧、躯干转向舱19左侧并压缩设置密封圈，使躯干主舱21右侧和躯干转向舱19左侧实现独立密封，躯干主舱21前部设置有垂直推进器8，躯干主舱21的左、右侧分别设置有与轴线平行的水平推进器9、20，通过水平推进器9、20控制，可以实现整个装置的前进、后退、旋转等运动姿态。如图10所示，密封法兰35右侧中部设置有躯干转向机构安装底座36，在躯干主舱21内设置有控制中心，包括电池71、侧扫声呐69、处理器66、集成主板67、数据处理中心68、信号发生器70，侧扫声呐69通过发射声波和收集回声，探测包括周围环境和地形地貌在内的信息，信号发生器70用于对信号进行放大和输出，集成主板67用于将各种硬件设备进行连接，实现数据通信，处理器66是运算和控制核心，进行信息处理和程序运算，数据处理中心68是将所有的环境数据和内部运行数据进行整理、归纳，并将处理后的数据传输到处理器，用于提升处理器的运行和决策效率。
[0118]
如图1和图2所示，躯干转向舱19外侧设置有柔性密封波纹管79，柔性密封波纹管右侧与推进器固定舱12左侧连接，柔性密封波纹管内部设置有水平转向器37和垂直转向器38组成，水平转向器37与水平方向呈-5度夹角，垂直转向器38与垂直方向呈-5度夹角，两者啮合角度呈90度，通过水平转向器37和垂直转向器38的相互扭动，可以带动相邻的舱段即躯干主舱21和推进器固定舱12实现水平或者竖直方向的摆动，其中水平转向器37与密封法兰35上设置的安装底座36连接，垂直转向器38与推进器固定舱12左侧上设置的安装底座76连接。
[0119]
如图1、图2和图7所示，推进器固定舱12右侧与尾部转向舱16左侧连接，推进器固定舱12左、右两侧外部设置有密封槽55、56，密封槽内设置有密封圈，推进器固定舱12内部左、右两侧设置有密封法兰39、40，密封法兰左、右侧设置有密封槽73、74，密封槽内设置有密封圈，通过螺栓11连接法兰39、推进器固定舱12左侧、躯干转向舱19右侧并压缩设置的密
封圈，使推进器固定舱12左侧和躯干转向舱右侧实现独立密封，螺栓13连接法兰40、推进器固定舱12右侧、尾部转向舱16左侧并压缩设置的密封圈，使推进器固定舱12右侧和尾部转向舱左侧实现独立密封，推进器固定舱12前部设置有垂直推进器18，推进器固定舱后部设置有水平推进器17，两者之间呈相互垂直状态，密封法兰40右侧中部设置有躯干转向机构安装底座41。
[0120]
如图1和图2所示，尾部转向舱16外侧设置有柔性密封波纹管79，柔性密封波纹管右侧与尾舱15连接，柔性密封波纹管内部设置有水平转向器42和垂直转向器43组成，两者啮合角度呈90度。通过转向器的相互扭动，可以带动相邻的舱段实现水平或者竖直方向的摆动，其中水平转向器42与密封法兰40上设置的安装底座41连接，垂直转向器43与尾舱15左侧设置的安装底座44连接。
[0121]
如图1和图2所示，尾舱15呈扁平状，尾舱左侧设置有密封槽77，密封槽内设置有密封圈，尾舱左侧内部设置有密封法兰78，密封法兰78左、右侧设置有密封槽73、74，密封槽内设置有密封圈，通过螺栓14连接密封法兰78、尾舱左侧、尾部转向舱右侧并压缩设置密封圈，使尾部转向舱右侧和尾舱左侧实现独密封，尾舱右侧设置有深度传感器45和停泊锚点46，与水下驻停机构连接进行交互。
[0122]
本发明的水下仿生柔性多功能常驻机器人的头部密封舱27前部设置有双目摄像头3，主要原理是利双目摄像头3的距离来捕捉物体的不同视角，从而获得不同的拍摄效果，利用双目视觉原理来计算物体的深度信息，从而获得更准确的拍摄结果，从而使两个摄像头捕捉到的画面来计算物体的距离，从而获得更精确的拍摄效果。将周围环境数据传输到控制中心，控制中心对数据进行分析后，控制中心在任务载荷基础上通过控制各推进器工作状况和游动姿态，实现自主路径规划。
[0123]
水下仿生柔性多功能常驻机器人设置有四段转向舱，即颈部转向舱5、第一躯干转向舱7、第二躯干转向舱19和尾部转向舱16，各个转向舱内设置有转向机构，优选地将颈部转向舱5和尾部转向舱17内的转向机构设置成水平转向器和垂直转向器组成的结构，水平转向器和垂直转向器两者呈90度啮合，通过转向器的相互扭动，可以带动相邻的舱段实现水平或者竖直方向的摆动。第一躯干转向舱7内水平转向器与水平方向呈5度夹角，垂直转向器与竖直方向呈5度夹角，两者啮合角度呈90度，通过转向器的相互扭动，可以带动相邻的舱段实现水平或者竖直方向的摆动。第二躯干转向舱19内水平转向器与水平方向呈-5度夹角，垂直转向器与垂直方向呈-5度夹角，两者啮合角度呈90度，通过转向器的相互扭动，可以带动相邻的舱段实现水平或者竖直方向的摆动。该角度的设置是通过大量的仿生结构学研究模拟爬行类生物运动特性而建立的几何关系，目的是为了在机器人进行仿生游动时，使其运动姿态更流畅，从结构上降低因多力矩耦合而对行进姿态产生的扰动。通过实验和仿生软件的虚拟研究，采用四组转向机构的相互扭动，不仅可以使水下仿生柔性多功能常驻机器人实现蛇类生物的身体扭动特性穿越复杂障碍物，更可以依靠在扭动的过程中通过与周围流体的相互作用，为机器人自身前进提供一定动力。水下仿生柔性多功能常驻机器人的转向机构不仅可以实现任意频率的相对摆动，也可以实现任意角度的控制和停留。
[0124]
本发明的水下仿生柔性多功能常驻机器人区别于传统水下机器人，其各段密封舱独立密封，在提升设备安全可靠性的同时，更将部件关节化，仿生化，使机器人具有更加广阔的作业空间和作业方式，水下仿生柔性多功能常驻机器不仅可以实现水下直航，更可以
将身体弯曲成多种几何形状用于不同的任务载荷。在密封舱外部设置有仿生柔性外壳即柔性密封波纹管，不仅可以起到防水作用，还可以减小水下滑行阻力，该机器人具备材料的柔软性、优良的环境适应性、超强的安全性，能够改变传统水下仿生柔性多功能常驻机器人材料的强度，使该水下仿生柔性多功能常驻机器人与环境、水下生物等具有更优异的互动性。
[0125]
在本实施例中，可在头部密封舱27下方可以设置有接口，用于连接水下机械手臂、水下电锯、水下清洁刷等不同的末端执行器，可以增加和扩展水下仿生柔性常驻机器人的作业范围。可在各个密封舱下设置灯光，用于水下光线不足情况下的光照补充，以便双目摄像头获取清晰图像。可在个舱段设置水密插件，用于连接各舱段用于数据传输控制和能量传递。
[0126]
如图11所示，本实施例提供了一种为上述机器人提供停泊点的驻泊机构50，其固定于海底平台，为水下仿生柔性多功能常驻机器人常驻提供庇护位置和能量补充。
[0127]
驻泊机构的整体呈框架式结构，通过与水下仿生柔性多功能常驻机器人的咬合使水下仿生柔性多功能常驻机器人可以稳定的固定于驻泊机构内，驻泊机构可以停放单只水下仿生柔性多功能常驻机器人，也可以停放多只水下仿生柔性多功能常驻机器人，为多只水下仿生柔性多功能常驻机器人协同作业提供硬件保障。驻泊机构的设立，不仅可以使水下仿生柔性多功能常驻机器人在水下有安全的停泊场所，实现机器人长期的水下驻航的目的，实现无间断长期的水下工作，不需要人员在任务载荷结束后对设备进行回收，更可以随时根据岸上控制器的控制实现快速及时的使水下仿生柔性多功能常驻机器人进入工作场所进行工作，使装置具有更加高效的机动性，尤其是海洋平台监测等技术领域具有极高的应用市场。
[0128]
本发明公布的一种设计方案完整水下仿生柔性多功能常驻机器人，其机械结构、推进方式、控制算法等均有公布，区别于其他水下仿生机器人具有极高实用性和先进性，该设计方案完整可实现水下长期驻泊、自主路径规划、仿生精准姿态控制、远程控制交互、模块切换、双目识别等。
[0129]
水下仿生柔性多功能常驻机器人不仅具有常规水下机器人的基本特性，而且该水下仿生柔性多功能常驻机器人整体呈现流线型，横截面积较小，水下阻力微弱，整体密度与流体接近，对能源需求较少，更加容易控制，有效解决因水下漂移而产生的位置惯性变化。行进的过程中距离传感器、双目摄像头、侧扫声呐的同时启用可以使水下机器人对周围环境有着精确的判断，在控制中心将数据汇总和分析，结合任务载荷的目的，可实现路径的自主规划，实现推力控制自动分配，实现转向装置和推进器自主推力分配。
[0130]
机器人外表面设置有柔性仿生材料外壳不仅可以起到防水作用，还可以减小水下滑行阻力，该机器人具备材料的柔软性、优良的环境适应性、超强的安全性，能够改变传统水下仿生柔性多功能常驻机器人材料的强度，使该水下仿生柔性多功能常驻机器人与环境、水下生物等具有更优异的互动性。该发明的结构紧凑，又具有仿生弯曲功能，可以有效的适应洞穿等相关任务载荷，如洞穴救援、水下勘测、沉船探测、管道检查等相对狭窄的水下空间的任务载荷。
[0131]
实施例二
[0132]
本实施例提供一种控制方法，用于实施例一所述水下仿生柔性多功能常驻机器人的姿态控制，具体可实现的方式如下：
[0133]
前进：躯干主舱侧壁设置的水平推进器同速正转可以使装置实现前进姿态。
[0134]
后退：躯干主舱侧壁设置的水平推进器同速反转可以使装置实现后退姿态。
[0135]
水平方向转向：躯干主舱侧壁设置的水平推进器一只正向转动，一只反向转动，或者两只推进器差速转动、或者单只推进器工作，都可以实现装置在水平面内顺时针、逆时针的转动。
[0136]
上升：垂直推进器6、垂直推进器8、垂直推进器18正向转动可以实现装置的上升。
[0137]
下潜：垂直推进器6、垂直推进器8、垂直推进器18反向转动可以实现装置的下潜。
[0138]
头部下潜：垂直推进器6正向转动，垂直推进器18反向转动，水平推进器9和水平推进器20正向转动可以实现装置头部下潜。
[0139]
尾部下潜：垂直推进器6反向转动，垂直推进器18正向转动，水平推进器9和水平推进器20反向转动可以实现装置尾部下潜。
[0140]
水平方向波浪游动：四只水平转向器(49、33、37、42)同时启动后，在水平推进器9和水平推进器20的共同作用下会以一定角度带动相邻舱段实现水平方向上的仿生波浪游动。
[0141]
垂直方向波浪游动：四只垂直转向器(48、34、38、43)同时启动后，在推进器9和推进器20的共同作用下会以一定角度带动相邻舱段实现垂直方向上的仿生波浪游动。
[0142]
通过以上一些基本运动姿态的组合可以使水下仿生柔性多功能常驻机器人实现多种运动姿态的实现。
[0143]
具体地，水下仿生多功能常驻机器人的控制方法，包括步骤：
[0144]
a1、水下仿生多功能常驻机器人运行之前，预设运行参数，生成预定参考轨迹；
[0145]
a2、水下仿生多功能常驻机器人运行过程中，实时采集机器人的位姿信息，包括各舱段的角度，角度包括俯仰角和偏航角；
[0146]
a3、根据步骤a2中采集到的位姿信息、步骤a1中预设的运行参数和生成的预定参考轨迹，计算得到各舱段的角度误差；
[0147]
a4、将步骤a3得到的角度误差输入pid控制器，pid控制器输出各个舱段的第一参考角度，所述pid控制器的pid参数预先通过ziegler-nichols法则或试验方法确定；
[0148]
a5、将步骤a2中采集到的位姿信息输入lqr控制器，lqr控制器输出各个舱段的第二参考角度，同时根据观测器估计的系统总扰动实时补偿控制量，所述lqr控制器的lqr控制律预先设计并根据机器人运行的实际情况进行调整；
[0149]
a6、根据pid控制器输出的第一参考角度，和所述lqr控制器输出的第二参考角度，计算得到用于控制机器人运行的各舱段的调整角度和调整角速度，根据调整角度和调整角速度生成控制指令；
[0150]
s7、机器人对应舱段的驱动器根据所述控制指令，驱动对应的推进器或转向器运动，实现对应舱体的运动状态调整。
[0151]
本发明为提高水下机器人柔性动作跟踪及动力定位精度，针对水下多关节机器人仿生柔性动作耦合控制问题，如系统参数时变、大惯性、纯滞后等特点，水下非线性控制如果采用通常的控制方法很难使多个自由度都得到很好的控制效果，使用了扰动补偿的lqr pid联合控制策略，即pid控制器用于低频控制，保证机器人运动的稳定性，低频体现在系统低频动态响应上，减少控制器计算压力；lqr控制器用于高频控制，提供机器人的动态相应
与跟踪性能，高频体现在系统高频动态响应上，同时使用观测器估计系统总扰动作补偿给lqr输出信号，可以在一定程度上抵消系统扰动。两种控制器的输出信号按一定的权重进行叠加，作为机器人执行机构的控制输入。两种控制器的参数协调一致，可一定程度上相互制约和补偿，以得到满意的控制效果。在模拟仿真实验和实物实验中结果中均表明，该方法可以验证该模型的合理性和进行动力定位可行性，所有性能均满足设计要求。
[0152]
下面对本发明的控制原理、坐标系、机器人运行过程中的位姿、受力分析等进行详细说明。
[0153]
柔性机器人在水中按规律地摆动身体和水产生相对运动，每个关节模块会受到水产生的反作用力，从而产生相应的不稳定运动。建立一个精确的水下运动受力的力学模型是准确模拟机器人水下运动的关键。
[0154]
假设水下环境是不可压缩、无旋的黏性流体；将柔性机器人的各个舱段视为关节模块，各个关节模块近似看作是规则的圆柱体，整个身体是由多个模块化的关节模块组成的链式结构。
[0155]
1、坐标系及运动参数定义：
[0156]
如图12所示，定义了世界(惯性)坐标系、机体(舱1-5)坐标系。
[0157]
定义世界坐标系为右手坐标系，世界坐标系原点oe建立在海平面上，xe轴平行于海平面向东，ze轴垂直于海平面竖直指向天空，ye平行于海平面，垂直于xeoze平面，简写为xoz平面。
[0158]
定义局部舱体坐标系为右手坐标系。各关节舱体坐标系的原点oi(i＝1～5)固定在各个舱体的形心，xi轴为舱体的轴线向前，zi轴垂直于xi轴向上，yi轴垂直于xioizi平面。
[0159]
纵向位移x：方向与xe方向相同。
[0160]
横向位移y：方向与ye方向相同。
[0161]
垂向位移z：方向与ze方向相同。
[0162]
俯仰角θ：定义舱头下偏方向为正，舱体坐标系xi轴与xoy平面之间的夹角。
[0163]
偏航角ψ：定义舱体左偏航时方向为正，舱体坐标系xi轴在xoy平面上的投影与xe轴之间所形成的夹角。
[0164]
机器人各个关节旋转角是由相邻舱体坐标系oi与o
i 1
之间的位姿关系确定的：
[0165]
关节俯仰角θi：定义关节顺时针偏转时方向为正。坐标轴xi与舱体坐标系x
i 1oi 1yi 1
平面之间的夹角，其中i表示第几个关节。如以4个关节为例，i＝1,2,3,4，
[0166]
关节偏航角ψi：定义关节逆时针偏转时方向为正，关节轴xi在舱体坐标系x
i 1oi 1yi 1
平面上的投影与x
i 1
轴之间的夹角。
[0167]
舱体速度分量(u,v,w)表示在舱体坐标系中的速度分量。其中：
[0168]
u与oi的xi轴方向一致，表示沿xi轴的速度分量。
[0169]
v与oi的yi轴方向一致，表示沿yi轴的速度分量。
[0170]
w与oi的zi轴方向一致，表示沿zi轴的速度分量。
[0171]
舱体角速度(p,q,r)表示舱体坐标系相对于世界坐标系的转动角速度。其中：
[0172]
p表示绕oi的xi轴的滚转角速度分量。
[0173]
q表示绕oi的yi轴的俯仰角速度分量。
[0174]
r表示绕oi的zi轴的偏航角速度分量。
[0175]
2、运动学分析
[0176]
2.1、舱体位置
[0177]
舱体坐标系原点位移在世界坐标系下表示为角位移为其中左上标为参考坐标系，当参考坐标系为自身时，省略左上标。各相邻舱体坐标系原点相对于中间舱体坐标系oi的位置ip
i 1
为：
[0178][0179][0180][0181]
其中，li为各舱体长度的二分之一，i＝1,2,3,4
iri 1
和
iri-1
分别为坐标系o
i 1
和o
i-1
到相邻中间舱体坐标系的转换矩阵，表示为：
[0182]
iri 1
＝rot(z,ψ
i 1
)rot(y,θ
i 1
)
[0183][0184][0185][0186]
r-1
表示r的逆运算，因为转换矩阵为正交矩阵，因此，r＝r-1
即：
[0187][0188]
2.2、舱体速度
[0189]
各舱体的线速度ivi和角速度iωi为：
[0190][0191][0192]
其中，表示关节i偏航角速度，表示关节i俯仰角速度。
[0193]
2.3、舱体加速度
[0194]
各舱体的角加速度
iai
和线加速度
iai
分别为：
[0195][0196][0197]
其中为机体系的俯仰、偏航、横滚角加速度。
[0198]
3、动力学分析
[0199]
图13为纵垂面上各舱段受力分析情况，将水下机器人的每个关节模块看作是独立的受力微元，fi为作用在各舱段上的浮力；
[0200]fai
为由附加质量效应引起的惯性类水动力；f
x
、fy、fz为流体力f
di
在局部坐标系下3个方向的分量，τ为受到流体的所有力矩作用。
[0201]fdi
为各舱段所受阻力；gi为重力；f
i-1
→i、f
i 1
→i表示各舱段间的相互作用力；m
ai
表示附加质量效应产生的力矩；mi、m
i-1
表示关节间的作用力矩，i＝1,2,3,4。
[0202]ft
为前进推进器推力，方向沿着推进舱轴向方向向前。
[0203]
力矩分析如下式所示：
[0204][0205]
上式中，
[0206]
i 1
mi→
i 1
为后侧舱对前侧舱产生的力矩，imi→
i 1
\
i-1mi-1
→i的定义与
i 1
mi→
i 1
类似；ji为各舱段的转动惯量矩阵。
[0207]
机器人通过改变各个关节角度来控制机器人整体位姿，将上式中三式相加，得如下运动方程：
[0208]mai 1-m
gi 1
m
ai-m
gi
m
ai-1-m
gi-1
i 1
mi→
i 1-i
mi→
i 1
im
i-1
→
i-i-1
mi→
i-1-j
i 1
α
i 1-jiα
i-j
i-1
α
i-1
＝0
[0209]
其中mg为重力产生的重力矩经整理可转换为如下形式：
[0210][0211]
[0212][0213]
其中，η是关于状态变量和u的非线性函数。
[0214]
m是机器人舱体惯性矩阵，
[0215][0216][0217]
建立状态空间表达方式为：
[0218][0219]
由上式得状态空间方程：
[0220][0221]
上式中，x＝[x zθv
x v
z q]
t
,u＝[θ1θ2θ3θ4]
t
,η(x,u)代表关于x和u的函数。该模型将关节角度作为系统的输人量，可用于基于模型的控制器设计。
[0222]
2.4、控制器设计。
[0223]
针对水下多关节机器人仿生柔性动作耦合控制问题，水下机器人柔性动作跟踪及动力定位精度，本发明提出了带有扰动补偿lqr pid控制策略。采用柔性仿生动作及特定作业动作控制的lqr pid控制策略，制定水下柔性机器人的蜿蜒运动、螺旋运动、直线运动、侧向运动及特定动作运动等各关节推进器动作控制流程，利用机器人动力学模型构建动作跟踪误差状态方程，将动作控制器进行整合，使得水下机器人接收到决策规划系统给出的期望动作指令后可以进行高精度动作跟踪和动力定位。
[0224]
蜿蜒步态运动时各关节模块控制方程如下：
[0225][0226]
以蜿蜒运动为例，式中，a控制关节模块转动的幅值、ω控制关节模块转动频率，β控制关节模块转动初始相位角，θ是偏航角度，ψ是俯仰角度，ti为第i个关节动作的时刻；t为关节动作持续时间；机器人关节俯仰和偏航的最大转角为
±
75
°
，d是下潜的角度。目标是使机器人能够在水平面内执行蜿蜒的运动前进，同时下潜至指定深度。
[0227]
根据lqr理论构建水下机器人运动过程的动作控制器，建立机器人俯仰和偏航姿态解耦模型为：
[0228][0229]
其中，b
ij
(i,j＝1,2,3,4)为俯仰和偏航运动方程中的控制系数。f
θ
和f
ψ
是总扰动函数。定义则俯仰控制系统的状态空间方程为：
[0230][0231]
y＝cx du
[0232]
式中，y是输出方程；
[0233][0234]
是有界的。
[0235]
引入lqr控制指标函数为：
[0236][0237]
式中，为各项状态误差的代价函数,为各项输入状态的代价函数。对应的离散系统黎卡提方程为：
[0238][0239]
此时lqr线性反馈增益矩阵为：
[0240][0241]
设计机器人位姿的状态扩张观测器
[0242][0243][0244]
其中，
[0245][0246]
β1＝3ω
01
,β2＝3ω
012
,β3＝ω
013
,β4＝3ω
02
,β5＝3ω
022
,β6＝ω
023
,
[0247]
表示对状态量x的观测值,表示对输出y的观测值，ω
01
、ω
02
定义为eso的带宽。
[0248]
在状态观测准确的情况下,机器人运动方程为:
[0249][0250]
和分别为观测器对偏航和俯仰总扰动的估计值。
[0251]
用李雅普诺夫法证明观测器的稳定性
[0252]
定义为状态估计误差,将式(2)和(6)代入得:
[0253]
[0254]
其中
[0255]
根据李雅普诺夫稳定性判别方法,因为为hurwitz矩阵,h是有界的,满足李雅普诺夫稳定条件,因此状态变量是稳定的。
[0256]
定义控制律：
[0257][0258]
引入误差e1＝x
1 d-x1,e4＝x
4 d-x4，
[0259]
其中x
1 d
和x
4 d
分别为俯仰和偏航的参考信号；
[0260]
定义z1＝e1,z2＝x2,z3＝e4,z4＝x5，确定基于误差的机器人俯仰和偏航状态空间方程：
[0261][0262]
其中，根据lti系统可控性判据,矩阵的秩等于矩阵的维数,所以系统可控。
[0263]
控制律为其中扰动补偿控制律为:
[0264][0265]
lqr控制律为：
[0266][0267]
其中，
[0268][0269]
至此lqr控制律设计完成，选择pid控制的输出变量：选取舱体角度(θ,ψ)作为pid控制的输出变量。确定pid控制的输入信号：选取舱体角度(俯仰角、偏航角)与期望角度的误差作为pid控制的输入信号。
[0270]
pid参数调整：通过ziegler-nichols法则或试验方法确定pid控制器的比例、积分、微分系数。加入lqr扰动补偿，以提高机器人跟踪精度。
[0271]
本发明可通过运动策略将跟踪控制和防撞控制相结合，确保执行子任务的最高优先级。速度和航向角速率的控制器，能将误差收敛到零，观测器稳定性是从李雅普诺夫稳定性中获得的，以保证跟踪参考控制信号。精准的控制算法可以使水下机器人获得良好的抗干扰能力，全面实现应对各种作业环境和作业载荷，依靠控制算法模型可以使水下机器人突破海量环境变量和硬件发展瓶颈，进而辅助人脑进行决策，以实现人工智能深度应用。
[0272]
以上显示和描述了本发明的基本原理、主要特征和优点。本行业的技术人员应该
了解，上述实施例不以任何形式限制本发明，凡采用等同替换或等效变换的方式所获得的技术方案，均落在本发明的保护范围内。