一种基于动作捕捉的水下航行器的自主游动方法及系统

本发明涉及水中航行器姿态控制，具体而言，涉及一种基于动作捕捉的水下航行器的自主游动方法及系统。

背景技术：

1、在海洋牧场的监测和渔业管理等领域，往往利用电子围栏划分不同的水域，在电子围栏围起来的水域范围内，通过一些专门的水下航行器，诸如仿蝠鲼水下航行器，可以检测电子围栏内不同深度及区域的水质情况，帮助监测海洋生物的健康情况；同时，水下航行器也可以通过携带摄像设备和传感器来检查水下养殖笼、种植设施等结构物的完整性、稳定性和功能性。

2、而在电子围栏围起来的水域范围内，一般会陈设有各种障碍物，诸如浮标、养殖笼等；在该领域中，有一种水下航行器通过手动遥控的方式进行控制，也即需要依靠人工操控，当遇到水域较深、障碍物较多的地方，人工的控制会很困难，很容易撞到障碍物上，对水下航行器造成损坏；还有一种传统的水下航行器，根据障碍物的所在位置，通过一系列水下自主游动的算法，实时计算游动路径，虽然效果显著，但需要在水下航行器上安置各种传感器，进而对水下航行器的算力要求较高，也即对水下航行器内置处理器的算力要求较高。

技术实现思路

1、本发明解决的问题是：提供一种基于动作捕捉的水下航行器的自主游动方法及系统，能够完全应用在海洋牧场的监测和渔业管理等小范围的水域，对于水下航行器的算力要求更小。

2、为解决上述问题，本发明提供一种水下航行器自主游动时的路径规划方法，包括以下步骤：

3、s1、在水中的每个障碍物上以及水下航行器上均设置有用于反馈障碍物和水下航行器的位置信息以及姿态信息的反光球，水下航行器中设置有用于控制水下航行器航向的下位机；在水下设置多个高速摄像头，高速摄像头用于根据反光球的反射光捕捉水下航行器以及障碍物的位置信息以及姿态信息，并将位置信息以及姿态信息传输至上位机；

4、s2、对高速摄像头进行环境光去噪处理；

5、s3、对高速摄像头依次进行空间位置标定和动态标定，以供校正大地坐标系；

6、s4、通过高速摄像头获得位置信息和姿态信息，并通过上位机将位置信息和姿态信息转换为水下航行器以及障碍物的三维点数据；

7、s5、通过上位机将三维点数据转换为水下航行器以及障碍物的三维模型；

8、s6、通过上位机根据三维模型计算水下航行器的航向信息，并将航向信息传输至下位机，使得水下航行器根据航向信息躲避障碍物。

9、本方法的有益效果为，该方法可以通过高速摄像头根据反光球的反射光捕捉到障碍物和水下航行器的位置信息以及姿态信息，然后通过上位机的计算处理，完成对水下航行器的航向信息的计算，再把航向信息传输至下位机，使得下位机控制水下航行器躲避障碍物，本方法对于水下航行器的算力要求更小。

10、进一步地，水下航行器上的反光球为多个，至少一个反光球设置在水下航行器的质心位置的外表面；每个障碍物上的反光球为多个，且每个障碍物的质心位置的外表面至少设置有一个反光球。

11、该设置可以更准确地反应障碍物和水下航行器的位置信息以及姿态信息，当质心在障碍物或者水下航行器内部时，可以把反光球设置在障碍物或者水下航行器的质心位置的外表面，不影响高速摄像头的捕捉，且只需要在上位机中设置相应的补偿值即可。

12、进一步地，下位机包括mcu，mcu中的cpg相位振荡器模型如下：

13、

14、其中，第一个方程为振幅方程，ai表示振幅，表示振幅的二阶导数，表示振幅的一阶导数，γi表示控制振幅收敛速度的正常数，ai表示期望振幅；第二个方程为相位方程，φi表示第i个单元的相位，φj表示第j个单元的相位，表示第i个单元的相位的一阶导数，fi表示固有频率，ωij表示第j个单元对第i个单元的耦合权重，表示期望相位差；第三个方程为输出方程，θi表示输出值；其中ai、γi、φi、ωij为状态参数，ai、fi、为输入的控制参数。

15、该模型使得cpg相位振荡器生成的节律信号能够有效控制水下航行器的航向。

16、进一步地，步骤s2中，对高速摄像头进行消除环境光噪点处理具体包括以下步骤：

17、s21、遮挡水中的强环境光，将一l字形的杆体记为l杆，在l杆上至少设置四个反光球，其中两个反光球设置在l杆的长轴上，一个反光球设置在l杆的垂直连接处，另一个反光球设置在l杆的短轴上；

18、s22、预设高速摄像头能够捕捉到的反光球的反射光光强范围；

19、s23、将l杆平放在水底；

20、s23、通过所有高速摄像头捕捉l杆上的反光球，并通过调节反射光光强范围，直至所有反光球均能被所有高速摄像头捕捉。

21、该方法使得高速摄像头先进行预处理，从而不会在后续过程中发生高速摄像头无法捕捉到反光球的情况。

22、进一步地，步骤s3中空间位置标定的具体包括以下步骤：

23、s31、通过所有高速摄像头捕捉平放在水底的l杆上的反光球，获得l杆的位置信息和姿态信息；

24、s32、通过上位机将l杆的位置信息和姿态信息转换为l杆的三维点数据；

25、s33、通过上位机将三维点数据生成l杆的三维模型；

26、s34、根据三维模型，通过上位机判断l杆是否成型；如果是，则将l杆的长轴标记为x轴，将l杆的短轴标记为y轴，水的深度方向标记为z轴；如果否，调整高速摄像头的曝光时间，并返回步骤s2。

27、该方法可以校正大地坐标系，在上位机中形成供计算且与实际水域情况符合的三维坐标轴。

28、进一步地，步骤s3中动态标定的具体包括以下步骤：

29、s301、将一长度固定的杆体记为t杆；

30、s302、在t杆的两端分别设置一反光球；

31、s303、将t杆没入水中并移动，并在固定时间内，通过所有高速摄像头捕捉t杆上的反光球，获得t杆的位置信息和姿态信息；

32、s304、通过上位机将t杆的位置信息和姿态信息转换为t杆的三维点数据；

33、s305、通过上位机将t杆的三维点数据生成t杆的三维模型；

34、s306、根据t杆的三维模型，通过上位机判断所有高速摄像头是否能够在不同时间段均捕捉到t杆上的反光球，如果是，结束；如果否，调整高速摄像头的曝光时间，并返回步骤s2。

35、该方法可以确保高速摄像头能够捕捉到在移动的反光球，从而确保能够捕捉到在移动的水下航行器以及障碍物。

36、进一步地，步骤s6中，计算水下航行器的航向信息具体包括以下步骤：s61、计算合力函数f(q)；其中，引力势场的势场函数uatt(q)如下；

37、

38、上式中，η为正比例增益系数；ρ(q,qg)为矢量，大小为水下航行器位置q和目标点qg之间的欧式距离|q-qg|，方向为水下航行器指向目标点的方向；

39、引力势场的负梯度作用力函数fatt(q)如下：

40、

41、斥力势场的势场函数urep(q)如下：

42、

43、上述二式中，表示势场函数uatt(q)的负梯度；k为正比例系数；ρ0为常数，表示障碍物对水下航行器产生作用的最大影响范围；

44、斥力势场的负梯度作用力函数frep(q)如下：

45、

46、合力势场函数u(q)如下：

47、u(q)＝uatt(q)+urep(q)；

48、则，得到合力函数f(q)如下：

49、

50、s62、在x轴以及y轴形成的二维平面上，将合力函数f(q)的两个矢量方向作为水下航行器的航向。

51、该方法可以有效计算出水下航行器的航向信息，从而使得水下航行器根据该航向躲避障碍物。

52、进一步地，反光球为被动式红外反光球，高速摄像头为红外高速摄像头。

53、红外线的波长相对于可见光更长，该设置可以使得反光球更能有效被捕捉。

54、本发明还提供一种基于动作捕捉的水下航行器的自主游动系统，包括：

55、多个障碍物模块，每个障碍物模块均包括用于反馈障碍物模块的位置信息以及姿态信息的反光球单元；

56、水下航行器模块，水下航行器模块包括用于反馈水下航行器模块的位置信息以及姿态信息的反光球单元以及用于控制水下航行器模块的下位机单元；

57、多个高速摄像头模块，每个高速摄像头模块均用于根据反光球单元捕捉障碍物模块以及水下航行器模块的位置信息以及姿态信息；

58、水域模块，所有障碍物模块、高速摄像头模块以及水下航行器模块均设置在水域模块中；

59、上位机模块，上位机模块与下位机单元以及高速摄像头模块通信连接，上位机模块用于显示并处理高速摄像头模块捕捉到的位置信息以及姿态信息，并将处理后的位置信息以及姿态信息传输至下位机单元，使得下位机单元根据接收到的位置信息以及姿态信息控制水下航行器模块。