本发明涉及水下机器人，具体是涉及一种锚泊设施负载下水下机器人铅垂面的动力学模型建立方法、模型参数辨识实验方法及装置。
背景技术：
1、锚泊设施是一种由锚、锚链和其他辅助设备构成的系统，常见的锚泊设施包含有水下节点、水下风筝以及水下潜标监测系统等。其中，潜标监测系统作为现代海洋离岸监测的重要基础设施，通过在潜标系统上面悬挂各种功能设备测量不同的海洋环境指标，从而自主实时对海洋水文、气象等要素进行全方面连续的监测。在潜标监测系统工作时，由于受到环境因素等的影响，需要水下机器人完成对潜标系统进行检修维护、布放回收等操作。在作业过程中，为了提高水下机器人的稳定性需要对其设置控制器系统，而设置控制器的前提是需要有准确的水下机器人动力学模型。因此，研究锚泊设施负载与水下机器人耦合下的多体动力学模型对于后续控制器的设计具有重要意义。
2、现有技术中，如公开号为cn112306076a的专利申请公开了一种混合驱动水下机器人动力学模型建立方法。其中首先确定该水下机器人的运动参数和自由度，将所有支撑腿折合成一条有质量的虚拟支撑腿，利用拉格朗日原理建立在水动力和驱动力作用下的混合驱动水下机器人矢状面动力学模型；在此基础上，针对有质量摆动腿难以建模问题，将其对水下机器人动力学模型的影响参数化，建立有质量摆动腿的混合驱动水下机器人的矢状面动力学模型。
3、又如公开号为cn110334411a的专利申请公开了一种基于huber m估计的水下机器人动力学模型参数辨识方法。其中通过建立水下机器人动力学模型；根据建立的模型，确定所需辨识的动力学参数；采用基于huber损失函数的递推最小二乘法，辨识水下机器人动力学模型的参数估计值；根据辨识得到的水下机器人动力学模型的参数估计值，更新水下机器人的当前状态值；得到每个采样时刻的参数估计值，并取平均值作为辨识结果。上述现有技术都围绕着水下机器人本体进行动力学建模与参数辨识，而缺少了水下机器人与作业环境耦合下的建模与参数辨识。
技术实现思路
1、发明目的：针对以上问题，本发明提供一种更加准确的锚泊设施负载下水下机器人铅垂面的动力学模型建立方法。
2、本发明还提供一种由上述动力学模型建立方法所建立得到的模型的模型参数辨识实验方法及装置。
3、技术方案：为解决上述问题，本发明采用一种锚泊设施负载下水下机器人铅垂面的动力学模型建立方法，包括以下步骤：
4、(1)基于惯性坐标系和水下机器人载体坐标系，构建水下机器人载体的六自由度动力学模型；
5、(2)基于水下机器人载体的六自由度动力学模型，消去与铅垂面无关的运动量，得到水下机器人载体的铅垂面动力学模型；
6、(3)考虑锚泊设施负载对水下机器人在铅垂面上力的作用，得到锚泊设施负载下水下机器人铅垂面动力学模型。
7、进一步的，所述锚泊设施负载下水下机器人铅垂面动力学模型如下：
8、
9、其中，mrb水下机器人的刚体质量惯性矩阵；crb(v)为水下机器人的刚体向心力矩阵；v为水下机器人速度和角速度矩阵；水下机器人线加速度和角加速度矩阵；τ为水下机器人推进器推力和力矩矩阵；g(η)为水下机器人重浮力矩阵；ma为附加质量惯性矩阵；ca(v)为附加质量产生的类似科氏力矩阵；d(v)为流体阻力矩阵；f为锚泊设施负载力矩阵。
10、本发明还采用一种用于上述动力学模型建立方法的模型参数辨识实验方法，包括以下步骤：
11、(1)将水下机器人置于水下模拟环境；
12、(2)测量并计算水下机器人的相关参数；
13、(3)对水下机器人进行多组推进器推力试验，获得多组推力试验数据；
14、(4)对水下机器人施加模拟锚泊设施负载的力，并进行多组推进器推力试验，获得多组锚泊设施负载下的推力试验数据；
15、(5)基于最小二乘法原理获得锚泊设施负载下水下机器人铅垂面动力学模型的估计值关系式，根据步骤(2)、(3)、(4)获得的数据，计算得到动力学模型的模型参数。
16、进一步的，所述步骤(3)对水下机器人进行多组推进器推力试验，获得多组推力试验数据具体为：改变水下机器人推进器的控制电压，以t0为电压步长从t1一直增加到t2，记录每组的推力值数据，并拟合得到推进器控制电压-推力曲线图；
17、所述步骤(3)进行多组推进器推力试验，获得多组锚泊设施负载下的推力试验数据具体为：改变水下机器人推进器的控制电压，以t3为电压步长从t4一直增加到t5，记录每组传感器的数据。
18、进一步的，所述对水下机器人施加模拟锚泊设施负载的力所产生的锚泊设施负载力矩阵f为：
19、
20、其中，ta1为模拟锚泊设施负载对水下机器人施加的第一方向力；ta2为模拟锚泊设施负载对水下机器人施加的第二方向力；α为第一方向力在惯性坐标系中的方向夹角；θ0为模拟锚泊设施负载对水下机器人作用位置到水下机器人原点的距离在载体坐标系中的方向夹角；θ为水下机器人的俯仰角；第二方向力的方向在惯性坐标系中与坐标轴重合。
21、进一步的，所述步骤(5)中基于最小二乘原理获得锚泊设施负载下水下机器人铅垂面动力学模型的估计值关系式包括铅垂面纵向方程、铅垂面垂向方程和铅垂面纵倾方程的估计值关系式：
22、铅垂面纵向方程的估计值关系式为：
23、
24、θ1的最小二乘法估计值为：
25、铅垂面垂向方程的估计值关系式为：
26、
27、θ2的最小二乘法估计值为：
28、铅垂面纵倾方程的估计值关系式为：
29、
30、θ3的最小二乘法估计值为：
31、其中，k＝1,…,n；τ合x,k、τ合z,k、τ合m,k分别为第k组的铅垂面纵向耦合力、铅垂面垂向耦合力、铅垂面纵倾耦合力；为水下机器人第k组的线加速度，为水下机器人第k组的角加速度，v1k为水下机器人第k组的线速度，v1k＝(uk,vk,wk)；v2k为水下机器人第k组的角速度，v2k＝(pk,qk,rk)；t为矩阵的转置；xu、xu|u|、zw|w|、mq、mq|q|为动力学模型待辨识的参数。
32、本发明还采用一种用于上述模型参数辨识实验方法的实验装置，包括水下机器人、实验框架、位移监测装置和模拟锚泊设施负载的装置；
33、所述模拟锚泊设施负载的装置包括浮球、绳子和拉力传感器；所述绳子一端与浮球连接，另外一端与实验框架连接，且绳子与浮球和实验框架的连接处均设置有拉力传感器；所述水下机器人包括机械手，机械手与绳子固定连接，所述位移监测装置用于监测水下机器人的位移数据。
34、进一步的，所述位移监测装置包括工业相机、伺服电机、直线导轨模组、安装底座，所述工业相机固定在安装底座上与直线导轨模组连接，伺服电机驱动安装底座沿直线导轨模组上下运动，从而使得工业相机上下运动，所述工业相机用于摄取水下机器人的图像，监测水下机器人的位移数据。
35、进一步的，所述水下机器人包括外壳体、固定框架、电子舱、推进器以及传感器，推进器和传感器固定安装在固定框架上，外壳体对应推进器设置开孔，电子舱安装于固定框架中间内部，固定框架头部固定有多普勒传感器，尾部固定有深度计；电子舱内部安装为整个系统供电的供电模块、数据采集模块、控制模块和通讯模块；所述控制模块用于控制推进器的转速调节，数据采集模块用于采集控制电压模拟量，所述通讯模块用于接收水下机器人的控制指令。
36、进一步的，所述通讯模块包括上位机和下位机，上位机和下位机之间通过rs232信号进行数据交换；所述上位机软件用于向下位机发送控制指令，同时接收、处理、显示和保存下位机发送来的数据；所述下位机用于接收处理上位机、各传感器和数据采集模块发送来的数据，同时向上位机发送各传感器和推进器电压实时数据。
37、有益效果：本发明相对于现有技术，其显著优点是考虑了水下机器人和锚泊设施的耦合作用力，建立锚泊设施负载下水下机器人铅垂面动力学模型，使得动力学模型更加准确，对于后续控制器的设计具有重要意义。此外，本发明提供一套完整的实验装置和实验方法，使得动力学模型参数辨识的过程更加规范化和标准化，可以有效地降低实验误差，保证实验结果的精确度。本发明提供的实验装置亦可用于水下机器人其余领域的研究。
1.一种锚泊设施负载下水下机器人铅垂面的动力学模型建立方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的动力学模型建立方法，其特征在于，所述锚泊设施负载下水下机器人铅垂面动力学模型如下：
3.一种用于权利要求1或2所述的动力学模型建立方法的模型参数辨识实验方法，其特征在于，包括以下步骤：
4.根据权利要求3所述的模型参数辨识实验方法，其特征在于，所述步骤(3)对水下机器人进行多组推进器推力试验，获得多组推力试验数据具体为：改变水下机器人推进器的控制电压，以t0为电压步长从t1一直增加到t2，记录每组的推力值数据，并拟合得到推进器控制电压-推力曲线图；
5.根据权利要求3所述的模型参数辨识实验方法，其特征在于，所述对水下机器人施加模拟锚泊设施负载的力所产生的锚泊设施负载力矩阵f为：
6.根据权利要求5所述的模型参数辨识实验方法，其特征在于，所述步骤(5)中基于最小二乘原理获得锚泊设施负载下水下机器人铅垂面动力学模型的估计值关系式包括铅垂面纵向方程、铅垂面垂向方程和铅垂面纵倾方程的估计值关系式：
7.一种用于权利要求3所述模型参数辨识实验方法的实验装置，其特征在于，包括水下机器人(3)、实验框架、位移监测装置和模拟锚泊设施负载的装置；
8.根据权利要求7所述的实验装置，其特征在于，所述位移监测装置包括工业相机(105)、伺服电机(102)、直线导轨模组(103)、安装底座(104)，所述工业相机(105)固定在安装底座(104)上与直线导轨模组(103)连接，伺服电机(102)驱动安装底座(104)沿直线导轨模组(103)上下运动，从而使得工业相机(105)上下运动，所述工业相机用于摄取水下机器人的图像，监测水下机器人(3)的位移数据。
9.根据权利要求7所述的实验装置，其特征在于，所述水下机器人(3)包括外壳体(302)、固定框架(313)、电子舱(4)、推进器以及传感器，推进器和传感器固定安装在固定框架(313)上，外壳体(302)对应推进器设置开孔，电子舱(4)安装于固定框架(313)中间内部，固定框架(313)头部固定有多普勒传感器(311)，尾部固定有深度计(312)；电子舱(4)内部安装为整个系统供电的供电模块、数据采集模块、控制模块和通讯模块；所述控制模块用于控制推进器的转速调节，数据采集模块用于采集控制电压模拟量，所述通讯模块用于接收水下机器人的控制指令。
10.根据权利要求9所述的实验装置，其特征在于，所述通讯模块包括上位机和下位机，上位机和下位机之间通过rs232信号进行数据交换；所述上位机软件用于向下位机发送控制指令，同时接收、处理、显示和保存下位机发送来的数据；所述下位机用于接收处理上位机、各传感器和数据采集模块发送来的数据，同时向上位机发送各传感器和推进器电压实时数据。
