热词 | 航行 水下 反光 障碍物 障碍 摄像头 摄像 上位 信息 姿态 | ||
专利类型 | 发明公开 | 法律事件 | 公开; 实质审查; |
专利有效性 | 实质审查 | 当前状态 | 实质审查 |
申请号 | CN202311417003.0 | 申请日 | 2023-10-27 |
公开(公告)号 | CN117687428A | 公开(公告)日 | 2024-03-12 |
申请人 | 西北工业大学宁波研究院; | 申请人类型 | 科研院所 |
发明人 | 曹勇; 谢钰; 张文泽; 郝艺伟; 王鹏昊; 马淑敏; 曹永辉; 潘光; | 第一发明人 | 曹勇 |
权利人 | 西北工业大学宁波研究院 | 权利人类型 | 科研院所 |
当前权利人 | 西北工业大学宁波研究院 | 当前权利人类型 | 科研院所 |
省份 | 当前专利权人所在省份:浙江省 | 城市 | 当前专利权人所在城市:浙江省宁波市 |
具体地址 | 当前专利权人所在详细地址:浙江省宁波市高新区清逸路218弄 | 邮编 | 当前专利权人邮编:315000 |
主IPC国际分类 | G05D1/485 | 所有IPC国际分类 | G05D1/485 ; G05D101/10 |
专利引用数量 | 0 | 专利被引用数量 | 0 |
专利权利要求数量 | 9 | 专利文献类型 | A |
专利代理机构 | 宁波甬致专利代理有限公司 | 专利代理人 | 费玲玲; |
摘要 | 本 发明 提供一种 水 下航行器自主游动时的路径规划方法,在水中的障碍物上以及水下航行器上均设置反光球,水下航行器中设置有下位机;在水下设置多个高速摄像头;对高速摄像头进行环境光去噪处理;对高速摄像头依次进行空间 位置 标定和动态标定;通过上位机将位置信息和 姿态 信息依次转换为三维点数据和三维模型;再通过上位机根据三维模型计算水下航行器的航向信息,并将航向信息传输至下位机,使得水下航行器根据航向信息躲避障碍物;本发明能够完全应用在小范围的水域,对于水下航行器的算 力 要求更小。 | ||
权利要求 | 1.一种水下航行器自主游动时的路径规划方法,其特征在于,包括以下步骤: |
||
说明书全文 | 一种基于动作捕捉的水下航行器的自主游动方法及系统技术领域背景技术[0002] 在海洋牧场的监测和渔业管理等领域,往往利用电子围栏划分不同的水域,在电子围栏围起来的水域范围内,通过一些专门的水下航行器,诸如仿蝠鲼水下航行器,可以检测电子围栏内不同深度及区域的水质情况,帮助监测海洋生物的健康情况;同时,水下航行器也可以通过携带摄像设备和传感器来检查水下养殖笼、种植设施等结构物的完整性、稳定性和功能性。 [0003] 而在电子围栏围起来的水域范围内,一般会陈设有各种障碍物,诸如浮标、养殖笼等;在该领域中,有一种水下航行器通过手动遥控的方式进行控制,也即需要依靠人工操控,当遇到水域较深、障碍物较多的地方,人工的控制会很困难,很容易撞到障碍物上,对水下航行器造成损坏;还有一种传统的水下航行器,根据障碍物的所在位置,通过一系列水下自主游动的算法,实时计算游动路径,虽然效果显著,但需要在水下航行器上安置各种传感器,进而对水下航行器的算力要求较高,也即对水下航行器内置处理器的算力要求较高。 发明内容[0004] 本发明解决的问题是:提供一种基于动作捕捉的水下航行器的自主游动方法及系统,能够完全应用在海洋牧场的监测和渔业管理等小范围的水域,对于水下航行器的算力要求更小。 [0005] 为解决上述问题,本发明提供一种水下航行器自主游动时的路径规划方法,包括以下步骤: [0006] S1、在水中的每个障碍物上以及水下航行器上均设置有用于反馈障碍物和水下航行器的位置信息以及姿态信息的反光球,水下航行器中设置有用于控制水下航行器航向的下位机;在水下设置多个高速摄像头,高速摄像头用于根据反光球的反射光捕捉水下航行器以及障碍物的位置信息以及姿态信息,并将位置信息以及姿态信息传输至上位机; [0007] S2、对高速摄像头进行环境光去噪处理; [0008] S3、对高速摄像头依次进行空间位置标定和动态标定,以供校正大地坐标系; [0009] S4、通过高速摄像头获得位置信息和姿态信息,并通过上位机将位置信息和姿态信息转换为水下航行器以及障碍物的三维点数据; [0010] S5、通过上位机将三维点数据转换为水下航行器以及障碍物的三维模型; [0011] S6、通过上位机根据三维模型计算水下航行器的航向信息,并将航向信息传输至下位机,使得水下航行器根据航向信息躲避障碍物。 [0012] 本方法的有益效果为,该方法可以通过高速摄像头根据反光球的反射光捕捉到障碍物和水下航行器的位置信息以及姿态信息,然后通过上位机的计算处理,完成对水下航行器的航向信息的计算,再把航向信息传输至下位机,使得下位机控制水下航行器躲避障碍物,本方法对于水下航行器的算力要求更小。 [0013] 进一步地,水下航行器上的反光球为多个,至少一个反光球设置在水下航行器的质心位置的外表面;每个障碍物上的反光球为多个,且每个障碍物的质心位置的外表面至少设置有一个反光球。 [0014] 该设置可以更准确地反应障碍物和水下航行器的位置信息以及姿态信息,当质心在障碍物或者水下航行器内部时,可以把反光球设置在障碍物或者水下航行器的质心位置的外表面,不影响高速摄像头的捕捉,且只需要在上位机中设置相应的补偿值即可。 [0016] [0017] 其中,第一个方程为振幅方程,ai表示振幅,表示振幅的二阶导数,表示振幅的一阶导数,γi表示控制振幅收敛速度的正常数,Ai表示期望振幅;第二个方程为相位方程,φi表示第i个单元的相位,φj表示第j个单元的相位,表示第i个单元的相位的一阶导数,fi表示固有频率,ωij表示第j个单元对第i个单元的耦合权重, 表示期望相位差;第三个方程为输出方程,θi表示输出值;其中ai、γi、φi、ωij为状态参数,Ai、fi、 为输入的控制参数。 [0019] 进一步地,步骤S2中,对高速摄像头进行消除环境光噪点处理具体包括以下步骤: [0020] S21、遮挡水中的强环境光,将一L字形的杆体记为L杆,在L杆上至少设置四个反光球,其中两个反光球设置在L杆的长轴上,一个反光球设置在L杆的垂直连接处,另一个反光球设置在L杆的短轴上; [0021] S22、预设高速摄像头能够捕捉到的反光球的反射光光强范围; [0022] S23、将L杆平放在水底; [0023] S23、通过所有高速摄像头捕捉L杆上的反光球,并通过调节反射光光强范围,直至所有反光球均能被所有高速摄像头捕捉。 [0024] 该方法使得高速摄像头先进行预处理,从而不会在后续过程中发生高速摄像头无法捕捉到反光球的情况。 [0025] 进一步地,步骤S3中空间位置标定的具体包括以下步骤: [0026] S31、通过所有高速摄像头捕捉平放在水底的L杆上的反光球,获得L杆的位置信息和姿态信息; [0027] S32、通过上位机将L杆的位置信息和姿态信息转换为L杆的三维点数据; [0028] S33、通过上位机将三维点数据生成L杆的三维模型; [0029] S34、根据三维模型,通过上位机判断L杆是否成型;如果是,则将L杆的长轴标记为X轴,将L杆的短轴标记为Y轴,水的深度方向标记为Z轴;如果否,调整高速摄像头的曝光时间,并返回步骤S2。 [0030] 该方法可以校正大地坐标系,在上位机中形成供计算且与实际水域情况符合的三维坐标轴。 [0031] 进一步地,步骤S3中动态标定的具体包括以下步骤: [0032] S301、将一长度固定的杆体记为T杆; [0033] S302、在T杆的两端分别设置一反光球; [0034] S303、将T杆没入水中并移动,并在固定时间内,通过所有高速摄像头捕捉T杆上的反光球,获得T杆的位置信息和姿态信息; [0035] S304、通过上位机将T杆的位置信息和姿态信息转换为T杆的三维点数据; [0036] S305、通过上位机将T杆的三维点数据生成T杆的三维模型; [0037] S306、根据T杆的三维模型,通过上位机判断所有高速摄像头是否能够在不同时间段均捕捉到T杆上的反光球,如果是,结束;如果否,调整高速摄像头的曝光时间,并返回步骤S2。 [0038] 该方法可以确保高速摄像头能够捕捉到在移动的反光球,从而确保能够捕捉到在移动的水下航行器以及障碍物。 [0039] 进一步地,步骤S6中,计算水下航行器的航向信息具体包括以下步骤:S61、计算合力函数F(q);其中,引力势场的势场函数Uatt(q)如下; [0040] [0041] 上式中,η为正比例增益系数;ρ(q,qg)为矢量,大小为水下航行器位置q和目标点qg之间的欧式距离|q‑qg|,方向为水下航行器指向目标点的方向; [0042] 引力势场的负梯度作用力函数Fatt(q)如下: [0043] [0044] 斥力势场的势场函数Urep(q)如下: [0045] [0046] 上述二式中, 表示势场函数Uatt(q)的负梯度;k为正比例系数;ρ0为常数,表示障碍物对水下航行器产生作用的最大影响范围; [0047] 斥力势场的负梯度作用力函数Frep(q)如下: [0048] [0049] 合力势场函数U(q)如下: [0050] U(q)=Uatt(q)+Urep(q); [0051] 则,得到合力函数F(q)如下: [0052] [0053] S62、在X轴以及Y轴形成的二维平面上,将合力函数F(q)的两个矢量方向作为水下航行器的航向。 [0054] 该方法可以有效计算出水下航行器的航向信息,从而使得水下航行器根据该航向躲避障碍物。 [0055] 进一步地,反光球为被动式红外反光球,高速摄像头为红外高速摄像头。 [0056] 红外线的波长相对于可见光更长,该设置可以使得反光球更能有效被捕捉。 [0057] 本发明还提供一种基于动作捕捉的水下航行器的自主游动系统,包括: [0058] 多个障碍物模块,每个障碍物模块均包括用于反馈障碍物模块的位置信息以及姿态信息的反光球单元; [0059] 水下航行器模块,水下航行器模块包括用于反馈水下航行器模块的位置信息以及姿态信息的反光球单元以及用于控制水下航行器模块的下位机单元; [0060] 多个高速摄像头模块,每个高速摄像头模块均用于根据反光球单元捕捉障碍物模块以及水下航行器模块的位置信息以及姿态信息; [0061] 水域模块,所有障碍物模块、高速摄像头模块以及水下航行器模块均设置在水域模块中; [0062] 上位机模块,上位机模块与下位机单元以及高速摄像头模块通信连接,上位机模块用于显示并处理高速摄像头模块捕捉到的位置信息以及姿态信息,并将处理后的位置信息以及姿态信息传输至下位机单元,使得下位机单元根据接收到的位置信息以及姿态信息控制水下航行器模块。附图说明 [0063] 图1为本发明中方法步骤示意图; [0064] 图2为本发明中系统在实际应用时的示意图; [0065] 图3为本发明实施例中仿蝠鲼水下航行器设置反光球的示意图; [0066] 图4为本发明实施例中仿蝠鲼水下航行器三维点数据形成三维模型示意图; 具体实施方式[0067] 为使本发明的上述目的、特征和优点能够更为明显易懂,下面结合附图对本发明的具体实施例做详细的说明。 [0068] 本实施例通过一种水下航行器自主游动时的路径规划方法,包括以下步骤: [0069] S1、在水中的每个障碍物上以及水下航行器上均设置有用于反馈障碍物和水下航行器的位置信息以及姿态信息的反光球,水下航行器中设置有用于控制水下航行器航向的下位机;在水下设置多个高速摄像头,高速摄像头用于根据反光球的反射光捕捉水下航行器以及障碍物的位置信息以及姿态信息,并将位置信息以及姿态信息传输至上位机; [0070] S2、对高速摄像头进行环境光去噪处理; [0071] S3、对高速摄像头依次进行空间位置标定和动态标定,以供校正大地坐标系; [0072] S4、通过高速摄像头获得位置信息和姿态信息,并通过上位机将位置信息和姿态信息转换为水下航行器以及障碍物的三维点数据; [0073] S5、通过上位机将三维点数据转换为水下航行器以及障碍物的三维模型; [0074] S6、通过上位机根据三维模型计算水下航行器的航向信息,并将航向信息传输至下位机,使得水下航行器根据航向信息躲避障碍物。 [0075] 进一步来说,水下航行器上的反光球为多个,至少一个反光球设置在水下航行器的质心位置的外表面;每个障碍物上的反光球为多个,且每个障碍物的质心位置的外表面至少设置有一个反光球。 [0076] 进一步来说,下位机包括MCU,MCU中的CPG相位振荡器模型如下: [0077] [0078] 其中,第一个方程为振幅方程,ai表示振幅, 表示振幅的二阶导数, 表示振幅的一阶导数,γi表示控制振幅收敛速度的正常数,Ai表示期望振幅;第二个方程为相位方程,φi表示第i个单元的相位,φj表示第j个单元的相位, 表示第i个单元的相位的一阶导数,fi表示固有频率,ωij表示第j个单元对第i个单元的耦合权重, 表示期望相位差;第三个方程为输出方程,θi表示输出值;其中ai、γi、φi、ωij为状态参数,Ai、fi、 为输入的控制参数。 [0079] 进一步来说,对高速摄像头进行消除环境光噪点处理具体包括以下步骤: [0080] S21、遮挡水中的强环境光,将一L字形的杆体记为L杆,在L杆上至少设置四个反光球,其中两个反光球设置在L杆的长轴上,一个反光球设置在L杆的垂直连接处,另一个反光球设置在L杆的短轴上; [0081] S22、预设高速摄像头能够捕捉到的反光球的反射光光强范围; [0082] S23、将L杆平放在水底; [0083] S23、通过所有高速摄像头捕捉L杆上的反光球,并通过调节反射光光强范围,直至所有反光球均能被所有高速摄像头捕捉。 [0084] 进一步来说,步骤S3中空间位置标定的具体包括以下步骤: [0085] S31、通过所有高速摄像头捕捉平放在水底的L杆上的反光球,获得L杆的位置信息和姿态信息; [0086] S32、通过上位机将L杆的位置信息和姿态信息转换为L杆的三维点数据;S33、通过上位机将三维点数据生成L杆的三维模型; [0087] S34、根据三维模型,通过上位机判断L杆是否成型;如果是,则将L杆的长轴标记为X轴,将L杆的短轴标记为Y轴,水的深度方向标记为Z轴;如果否,调整高速摄像头的曝光时间,并返回步骤S2。 [0088] 进一步来说,步骤S3中动态标定的具体包括以下步骤: [0089] S301、将一长度固定的杆体记为T杆; [0090] S302、在T杆的两端分别设置一反光球; [0091] S303、将T杆没入水中并移动,并在固定时间内,通过所有高速摄像头捕捉T杆上的反光球,获得T杆的位置信息和姿态信息; [0092] S304、通过上位机将T杆的位置信息和姿态信息转换为T杆的三维点数据; [0093] S305、通过上位机将T杆的三维点数据生成T杆的三维模型; [0094] S306、根据T杆的三维模型,通过上位机判断所有高速摄像头是否能够在不同时间段均捕捉到T杆上的反光球,如果是,结束;如果否,调整高速摄像头的曝光时间,并返回步骤S2。 [0095] 进一步来说,步骤S6中,计算水下航行器的航向具体包括以下步骤:S61、计算合力函数F(q);其中,引力势场的势场函数Uatt(q)如下; [0096] [0097] 上式中,η为正比例增益系数;ρ(q,qg)为矢量,大小为水下航行器位置q和目标点qg之间的欧式距离|q‑qg|,方向为水下航行器指向目标点的方向; [0098] 引力势场的负梯度作用力函数Fatt(q)如下: [0099] [0100] 斥力势场的势场函数Urep(q)如下: [0101] [0102] 上述二式中, 表示势场函数Uatt(q)的负梯度;k为正比例系数;ρ0为常数,表示障碍物对水下航行器产生作用的最大影响范围; [0103] 斥力势场的负梯度作用力函数Frep(q)如下: [0104] [0105] 合力势场函数U(q)如下: [0106] U(q)=Uatt(q)+Urep(q); [0107] 则,得到合力函数F(q)如下: [0108] [0109] S62、在X轴以及Y轴形成的二维平面上,将合力函数F(q)的两个矢量方向作为水下航行器的航向。 [0110] 具体的,障碍物主要为固定范围水域中的浮标、养殖笼等,主要是养殖笼,水中的鱼群等生物并非障碍物,因为水下航行器本身的行进速度较慢,鱼群等生物本身也有绕开障碍物的自主意识,所以鱼群等生物并非障碍物,不用设置反光球;障碍物上的反光球数量可以根据障碍物的实际尺寸灵活调整,且每个障碍物上在质心位置的外表面至少固定有一个反光球,质心位置外表面的反光球可以充分反应障碍物的位置信息,每个障碍物上的其余反光球均固定设置在边缘处,用于呈现出障碍物的边缘轮廓,从而显示姿态信息;水下航行器同理,本实施例中,如附图3所示,水下航行器为仿蝠鲼水下航行器,且至少一个反光球固定在仿蝠鲼水下航行器的质心位置处的外表面,其余反光球固定设置在仿蝠鲼水下航行器的两翼处和头部处,这使得仿蝠鲼水下航行器上的所有反光球在经过高速摄像头捕捉后,会在上位机中形成如附图4所示的三维模型,质心位置外表面的反光球可以充分反应障碍物的位置信息,两翼处和头部处的反光球可以呈现出仿蝠鲼水下航行器的边缘轮廓,从而显示姿态信息;MCU为stm32。 [0111] 在本实施例中,上位机为计算机设备,在该上位机中通过动作捕捉软件时间对水下航行器以及障碍物的位置信息以及姿态信息转换为三维点数据和三维模型的操作过程,动作捕捉软件为Qualisys Track Manager;高速摄像头需要均匀地分布在水域周围;在步骤S21中,通过上位机软件中的马赛克处理遮挡水中的强环境光,强环境光是指通过调节各高速摄像头参数均无法消除的光,所以需要通过马赛克进行遮挡;步骤S3中的空间位置标定如附图4所示,L杆平放时,如果观察到上位机中的L杆形状成型,也即长轴和短轴均已明显显示,则将L杆的长轴标记为X轴,将L杆的短轴标记为Y轴,水的深度方向标记为Z轴,这样就可以在上位机中形成供计算且与实际水域情况符合的三维坐标轴,从而完成空间位置标定;动态标定是为了确保高速摄像头能够捕捉到在移动的反光球,进而能够捕捉到在移动的水下航行器以及障碍物。水下航行器的航向只在XY平面上,还有一个Z轴,可以通过尾鳍的上下偏置来改变,实际上就是把三维空间的运动转换为二维平面加上Z轴,而尾鳍和胸鳍实际上是互不影响的,本实施例中,通过水下航行器胸鳍的作用,在二维平面上改变航向。 [0112] 另外,本实施例中,通过MATLAB在上位机中显示出仿蝠鲼水下航行器上各反光球的三维坐标与障碍物上各反光球的三维坐标,然后通过步骤S6完成对航向信息的计算。 [0113] 在本发明的优选实施例中,反光球为被动式红外反光球,高速摄像头为红外高速摄像头。由于红外线相对于可将光波长较长,更有利于捕捉。 [0114] 如附图2所示,本发明还提供一种基于动作捕捉的水下航行器的自主游动系统,包括: [0115] 多个障碍物模块,每个障碍物模块均包括用于反馈障碍物模块的位置信息以及姿态信息的反光球单元; [0116] 水下航行器模块,水下航行器模块包括用于反馈水下航行器模块的位置信息以及姿态信息的反光球单元以及用于控制水下航行器模块的下位机单元; [0117] 多个高速摄像头模块,每个高速摄像头模块均用于根据反光球单元捕捉障碍物模块以及水下航行器模块的位置信息以及姿态信息; [0118] 水域模块,所有障碍物模块、高速摄像头模块以及水下航行器模块均设置在水域模块中; [0119] 上位机模块,上位机模块与下位机单元以及高速摄像头模块通信连接,上位机模块用于显示并处理高速摄像头模块捕捉到的位置信息以及姿态信息,并将处理后的位置信息以及姿态信息传输至下位机单元,使得下位机单元根据接收到的位置信息以及姿态信息控制水下航行器模块。 |