1.本实用新型属于水下机器人技术领域，具体涉及一种基于视觉的水下机器人航向控制系统。


背景技术：

2.水下机器人是一种可在水下移动、具有视觉和感知系统、通过遥控或自主操作方式，并可使用机械手或其他工具代替或辅助人去完成水下作业任务的装置。目前，水下机器人主要应用于海洋领域，如海洋科考、石油开采、救捞作业、海底管道和电缆检查、海上养殖等，在江河湖泊、城市水道治理等领域也有一定的应用。在海洋领域，由于作业环境开阔、下潜深度大、海浪汹涌等特点，水下机器人一般设计的体积和重量都很大，价格也比较昂贵，遇到设备故障，维修成本很大。核电站的水下作业环境与海洋作业有很大的不同，例如核电站水下环境一般空间都比较狭小、各种构件复杂、磁场受到屏蔽，因此常规水下机器人不能直接适用于在核电站水环境下开展作业。
3.在核电环境中，机器人运行的平稳性是机器人能否实现精确高效水下作业的关键，航向控制和深度控制是机器人在水平和垂直方向平稳运动运动的基础。通常在海洋或者江河湖泊中作业时，水下机器人采用的定航向方式是基于gps 信号或者基于惯导推算航迹方式。基于gps信号方式定位误差较大，在海洋大范围运动的环境中尚可使用，同时核电环境通常都有较厚的金属或者构筑物阻挡，gps信号被屏蔽或者衰减，通过gps信号在核电水下环境中实现定位基本不可能。基于惯导推算航迹利用加速度和角加速度积分实现，随着时间推移误差会越来越大，并且在中间过程无法通过外部信息加以修正，因此也难以实现在核电水下环境的应用。因此，需要提供一种水下浮游机器人航向控制系统。


技术实现要素：

4.本实用新型的目的在于针对浮游式水下机器人在核电站反应堆压力容器、乏燃料水池等水下环境作业时，由于钢结构对地磁场的屏蔽和削弱作用以及传感器自身特性缺陷，导致机器人的姿态传感器无法提供准确的航向信息的技术问题，提供了一种基于视觉的水下浮游机器人航向控制系统。
5.本实用新型的技术方案如下：
6.一种基于视觉的水下浮游机器人航向控制系统，包括浮游机器人、控制计算机和视频采集装置；所述浮游机器人的顶部安装有光源，所述视频采集装置安装在水池边沿；所述控制计算机通过控制网线与机器人驱动控制模块连接，机器人驱动控制模块通过机器人脐带缆与浮游机器人连接；所述控制计算机通过视频网线与摄像机驱动解码模块连接，摄像机驱动解码模块过视频传输电缆与视频采集装置连接。
7.进一步地，所述浮游机器人水平方向和垂直方向分别布置有一个或多个推进器，用于实现浮游机器人的水平平移、浮沉、转艏、定深和定航向功能。
8.进一步地，所述浮游机器人整体通过配重块调整至悬浮或正浮力状态。
9.进一步地，所述控制计算机发送的浮游机器人控制指令通过机器人驱动控制模块转化为各个推进器的动力输出；视频采集装置采集的视频图像经过摄像机驱动解码模块解码输出至控制计算机。
10.进一步地，所述光源包含第一led灯和第二led灯，分别采用独立的水密结构封装，内部均具有驱动电路能够控制led灯的点亮和熄灭频率。
11.进一步地，所述视频采集装置为摄像机。
12.进一步地，所述第一led灯的点亮和熄灭时间间隔小于摄像机的曝光时间周期，所述第二led灯的点亮和熄灭时间间隔大于摄像机的曝光时间周期。
13.进一步地，所述第一led灯和第二led灯的安装间距l＞2*l
max
，l
max
为单个led灯在相邻图像帧中最大的运动位移，其计算公式为l
max
＝v
rmax
*t
c
，式中v
rmax
为浮游机器人在水下的最大运动速度、t
c
为摄像机拍摄每帧图像的时间间隔。
14.本实用新型还提供了一种采用上述基于视觉的水下浮游机器人航向控制系统的航向控制方法，依次包括如下步骤：
15.s1.控制计算机的航向控制器接收到浮游机器人的目标航向信息；
16.s2.摄像机拍照发送给摄像机驱动解码模块，然后提取照片中的led灯对应的光斑位置信息；
17.s3.控制计算机通过摄像机的安装位置信息、s2获得的光斑位置信息和水池的结构尺寸信息，计算出浮游机器人的当前航向；
18.s4.航向控制器将s1中接收到的目标航向信息和s3计算得到的当前航向信息进行比较，判断当前航向是否等于目标航向；
19.s5.浮游机器人根据s4的判断结果选择执行过程：当目标航向等于当前航向，浮游机器人完成本次航向定位控制；当目标航向不等于当前航向，航向控制器计算浮游机器人推进器的水平推力分配，并控制推进器运动实现浮游机器人航向调整；
20.s6.重复执行步骤2～步骤5，直至浮游机器人完成航向定位控制。
21.进一步地，所述s2.摄像机拍照，提取照片中的光斑位置信息通过以下步骤实现：
22.s2.1位于水池上方的摄像机拍摄浮游机器人及其搭载的第一led灯和第二 led灯的视频图像；
23.s2.2摄像机拍摄的视频图像通过视频传输缆传输至摄像机驱动解码模块，摄像机驱动解码模块通过内置的光斑识别和提取算法，提取视频图像中光斑的位置信息；
24.假设在当前图像帧摄像机拍摄到的第一led灯和第二led灯的光斑经过转换在二维坐标中的位置为p1(x1、y1)和p2(x2、y2)，在下一帧图像中第一led 灯和第二led灯的位置变为p3(x3、y3)和p4(x4、y4)，分别计算p3、p4到 p1、p2的距离。
[0025][0026][0027][0028][0029]
因此l
p1p3
和l
p1p4
中，距离小于l
max
对应的点即为p1点在相邻图像帧中的位置，同理
l
p2p3
和l
p2p4
中，距离小于l
max
对应的点即为p2点在相邻图像帧中的位置；
[0030]
s2.3摄像机驱动解码模块将s2.2.获得的光斑位置信息通过视频网线发送至控制计算机。
[0031]
本实用新型的有益效果在于：
[0032]
本实用新型通过摄像机拍摄搭载于机器人本体的led灯，可实现机器人航向信息的快速提取，以实现机器人的定航向控制，从而实现浮游机器人的高效水下作业。本实用新型可实现在核电特殊环境中对水下浮游机器人的精确航向定位控制，提高水下机器人的运动控制性能，大幅提升水下作业效率。
附图说明
[0033]
图1为本实用新型的基于视觉的水下浮游机器人航向控制系统的结构示意图；
[0034]
图2为本实用新型的基于视觉的水下浮游机器人航向控制系统中led灯光斑检测原理图；
[0035]
图3为本实用新型的基于视觉的水下浮游机器人航向控制系统的航向控制原理图；
[0036]
图4为本实用新型的基于视觉的水下浮游机器人航向控制方法流程示意图。
[0037]
图中：1
‑
第一led灯、2
‑
第二led灯、3
‑
浮游机器人、4
‑
摄像机、5
‑
机器人驱动控制模块、6
‑
摄像机驱动解码模块、7
‑
控制计算机、8
‑
水池、9
‑
机器人脐带缆、10
‑
视频传输电缆、11
‑
控制网线、12
‑
视频网线。
具体实施方式
[0038]
下面结合附图及具体实施例对本实用新型作进一步详细说明。
[0039]
本实施例提供了一种基于视觉的水下浮游机器人航向控制系统，其总体结构如图1所示，包含第一led灯1和第二led灯2、浮游机器人3、摄像机4、机器人驱动控制模块5、摄像机驱动解码模块6、控制计算机7、水池8、机器人脐带缆9、视频传输电缆10、控制网线11和视频网线12。所述浮游机器人3 的顶部安装有两个作为光源的led灯，第一led灯1和第二led灯2以一定间距l固定安装于浮游机器人3顶部，所述摄像机4安装在水池8边沿的上边沿；所述控制计算机7通过控制网线11与机器人驱动控制模块5连接，机器人驱动控制模块5通过机器人脐带缆9与浮游机器人3连接；所述控制计算机7 通过视频网线12与摄像机驱动解码模块6连接，摄像机驱动解码模块6过视频传输电缆10与摄像机4连接。
[0040]
所述第一led灯1和第二led灯2分别采用独立的水密结构封装，其内部驱动电路通过控制led灯的点亮和熄灭频率以区分两个led灯。
[0041]
浮游机器人3水平方向布置4个推进器、垂直方向布置2个推进器，机器人整体通过配重块调整至微正浮力状态，在6个推进器组合作用下，机器人可实现水平平移、浮沉、转艏、定深和定航向功能。根据浮游机器人3实际应用水环境不同，比如自来水、海水、硼酸水等，密度会有所差异，一般浮游机器人3 每次下水前会通过增加配重块调整至基本处于悬浮状态，微正浮力即在悬浮状态下去掉部分配重块即可。
[0042]
控制计算机7发送的浮游机器人控制指令通过机器人驱动模块5转化为各个推进器的动力输出，以实现浮游机器人的各种运动功能。摄像机4采集的视频图像经过相机驱动
解码模块6解码输出至控制计算机7，采用光斑识别和提取算法，可得出两个led灯在每帧图像中的位置，由每帧图像中led灯光斑位置的不同即可推算出浮游机器人航向的变化。
[0043]
航向定位的难点在于对led灯位置信息的提取和识别。设置第一led灯1 点亮和熄灭时间间隔小于摄像机4的曝光时间周期，第二led灯2点亮和熄灭时间间隔大于摄像机4的曝光时间周期。因此在摄像机4的成像视场中，第一 led灯1一直呈现为一个亮的光斑，通过设置合适的第二led灯2的频闪周期，可以使得第二led灯2的成像光斑周期性间隔出现。
[0044]
如图2所示，假设在当前图像帧摄像机4拍摄到的led灯光斑经过转换在二维坐标中的位置为p1(x1、y1)和p2(x2、y2)，在下一帧图像中led灯的位置变为p3(x3、y3)和p4(x4、y4)，分别计算p3、p4到p1、p2的距离。
[0045][0046][0047][0048][0049]
根据浮游机器人在水下的最大运动速度v
rmax
和摄像机拍摄每帧图像的时间间隔t
c
可以计算出单个led灯在相邻图像帧中最大的运动位移为 l
max
＝v
rmax
*t
c
。为保证第一led灯1的光斑在相邻图像帧中不会出现在第二 led灯2的成像区间内，在第一led灯1和第二led灯2的初始安装时要求两者之间距离l＞2*l
max
。因此l
p1p3
和l
p1p4
中，距离小于l
max
对应的点即为p1点在相邻图像帧中的位置，同理l
p2p3
和l
p2p4
中，距离小于l
max
对应的点即为 p2点在相邻图像帧中的位置。
[0050]
例如，经过对两帧图像中led光斑位置的计算得出对应关系为p3对应p1， p4对应p2，则可计算出当前航向为：
[0051][0052]
根据图3所示的航向控制原理图，经图像识别获得的航向角θ
heading
作为反馈输入至控制计算机7中的航向pid控制器，控制器通过比较目标输入航向角θ
in
和当前航向角θ
heading
来输出信号控制水平推进器的推力分配，以实现浮游机器人的航向控制，直到当前航向角θ
heading
达到目标航向角θ
in
为止。
[0053]
本实施例还提供了一种采用上述基于视觉的水下浮游机器人航向控制系统的水下浮游机器人航向控制方法，其流程如图4所示，依次包括如下步骤：
[0054]
s1.控制计算机7的航向控制器接收到浮游机器人3的目标航向信息，即目标输入航向角θ
in
；
[0055]
s2.摄像机4拍照，提取照片中的光斑位置信息；
[0056]
s2.1位于水池8上方的摄像机4拍摄浮游机器人3及其搭载的第一led灯 1和第二led灯2的视频图像；
[0057]
s2.2摄像机4拍摄的视频图像通过视频传输缆10传输至摄像机驱动解码模块6，摄像机驱动解码模块6通过内置的光斑识别和提取算法，提取视频图像中光斑的位置信息；
[0058]
假设在当前图像帧摄像机4拍摄到的第一led灯1和第二led灯2的光斑经过转换在二维坐标中的位置为p1(x1、y1)和p2(x2、y2)，在下一帧图像中第一led灯1和第二led灯2的位置变为p3(x3、y3)和p4(x4、y4)，分别计算p3、p4到p1、p2的距离。
[0059][0060][0061][0062][0063]
因此l
p1p3
和l
p1p4
中，距离小于l
max
对应的点即为p1点在相邻图像帧中的位置，同理l
p2p3
和l
p2p4
中，距离小于l
max
对应的点即为p2点在相邻图像帧中的位置。
[0064]
s2.3摄像机驱动解码模块6将s2.2.获得的光斑位置信息通过视频网线发送 10至控制计算机7；
[0065]
s3.控制计算机7通过摄像机4的安装位置信息、s2.3获得的光斑位置信息和水池8的结构尺寸信息，计算出浮游机器人3的当前航向信息；
[0066]
例如，经过对两帧图像中第一led灯1和第二led灯2de光斑位置的计算得出对应关系为p3对应p1，p4对应p2，则可计算出当前航向角为：
[0067][0068]
s4.航向控制器将s1中接收到的目标航向和s3计算得到的当前航向进行比较，判断当前航向是否等于目标航向；
[0069]
根据图3所示的航向控制原理图，将s3经图像识别获得的航向角θ
heading
作为反馈输入至控制计算机7中的航向pid控制器，航向pid控制器通过比较s1 接收到的目标输入航向角θ
in
和当前航向角θ
heading
来输出信号控制浮游机器人3 的水平推进器的推力分配，以实现浮游机器人3的航向控制，直到当前航向角θ
heading
达到目标航向角θ
in
为止。
[0070]
s5.浮游机器人3根据判定结果选择执行过程；
[0071]
s5.1.目标航向等于当前航向，浮游机器人3完成本次航向定位控制；
[0072]
s5.2.目标航向不等于当前航向，航向控制器计算推进器的水平推力分配，并控制推进器运动实现浮游机器人3航向调整；
[0073]
s6.重复执行步骤2～步骤5，直至浮游机器人3完成航向定位控制。
[0074]
显然，本领域的技术人员可以对本实用新型进行各种改动和变型而不脱离本实用新型的精神和范围。倘若这些修改和变型属于本实用新型权利要求及其等同技术的范围之内，则本实用新型也意图包含这些改动和变型在内。