本文主要介绍ROV水下目标跟踪的简易demo软件实现的思路。

一、视觉模块

视觉模块的任务为：通过单目相机识别目标，并计算目标中心位置与图像中心位置的偏差，通过PID控制器得到控制量。demo中得到的控制量可以理解为竖直方向的推力
，以及偏航（yaw）方向的转矩。分别实现在图像坐标下y方向和x方向的目标跟踪。    

1、单目相机驱动

单目相机接在树莓派的USB接口上，可以选择的ROS工具包很多，只要确保图像以标准的ros图片格式发布即可。我选择的是usb_cam（http://wiki.ros.org/usb_cam）。岸上PC可以直接订阅发布的topic进行目标识别。 在此需要说明的是，单纯采用这种方式，不做压缩处理，图像传输至岸上的PC将会有一定的滞后，对跟踪控制会造成较大影响。由于本demo偏向于验证性质，故我并未采用实时视频传输的算法。 同时，若是AUV的情形，则完全不用考虑视频传输滞后的问题，依靠AUV内部高算力的开发板可以实现图像的实时处理，类似usb_cam的工具包我认为足以满足需求。

2、目标识别模块

目标识别这部分基本上是深度学习那套东西，在此我选择了Yolov3，原因是我发现Yolov3使用还是比较广泛，而且也较容易上手。Yolov3在ros下的开发包为http://wiki.ros.org/darknet_ros。当然对于像我这样的新手来说，还是需要学习一下Yolov3的使用：https://pjreddie.com/darknet/yolo/。安装配置darknet_ros，录制数据集训练模型这部分看情况有时间的话我可能会写点东西，毕竟当初踩了点坑。 usb_cam模块发布标准ros图像的topic，darknet_ros模块订阅该topic，处理后发布目标的bounding_boxes，即目标在图像的位置框图。 当然，在此完全没有必要一定用Yolo的框架，可以自由的选择其他识别算法框架替代。模块化一直是ros开发的优势。

3、目标中心与图像中心偏差计算模块

此模块代码对应cabin_vision/object_deviation.cpp。这部分没什么好介绍的，无非是订阅目标位置框图，计算其中心位置与图像中心位置的偏差，然后发布。  

4、PID控制器

此模块代码对应cabin_behaviors/pid_tracking.cpp。这部分也没什么好介绍的，无非是订阅位置偏差，PID处理后发布控制量。在此值得一提的是借助ros的动态配置（dynamic_reconfigure），可以在测试中动态调节PID参参数，非常方便。    

二、基础运动模块

基础运动模块的任务为：将总控制量（力、转矩）根据机械结构模型、机器人状态等分配到每个推进器，计算出每个推进器所需提供的推力，然后根据推进器电机模型计算该推进器的PWM波，最后将PWM波通过串口下发至MCU，实现对推进器的控制。基础运动模块实际上是俄亥俄州大学水下机器人队伍的开源项目（https://uwrt.engineering.osu.edu/）进行的二次开发。遗憾的是当时我进行开发的版本不是目前其github上的最新版本。后续我计划再对里面的几个模块进行进一步解析。  

1、推力分配矩阵计算模块

此模块代码对应cabin_controllers/thruster_controller.cpp。模块功能是将总的推力、转矩分配到各推进器，得到推进器推力分配矩阵。 在此需要注意的是模块需要非线性优化库ceres-solver（http://www.ceres-solver.org/index.html）。 实际上代码中亦有定深部分代码，需要配合IMU使用。具体大致是根据rov姿态计算当前rov所受的浮力浮心，然后计算抵消该浮力各推进器所需的推力。测试过程中硬件模块并未配置IMU，故我将代码中的定深部分开关关闭了。

2、PWM波矩阵计算模块

此模块代码对应cabin_controllers/pwm_controllers.cpp。这部分的核心只有一个，根据推进器与PWM波的关系曲线，计算推力所需的PWM波。由于俄亥俄州大学水下机器人队伍的项目亦bluerobotics的T200推进器，我可以直接套用相关电机的参数文件。

3、PWM数据波串口下发模块

此模块代码对应cabin_controllers/serial_to_mcu.cpp。将pwm波，水下灯强度按协议（可以自由定义）转成16进制数据流下发至MCU。demo里的协议将来会放到硬件框架介绍的文章中。这部分我借用了ROS里的serial库，sudo apt install ros-<distro>-serial安装即可。但我还是推荐采用源码（http://wiki.ros.org/serial）安装，将serial的源码放到项目里编译调用。因为早前我的电脑已经安装了serial库，所以github的demo里只有库的调用，没有serial库的代码。

4、手动控制模块

此模块代码对应cabin_teleop。这部分代码是手柄控制rov运动的具体实现，为了录制效果更好的数据包进行训练，顺便也写了水下灯亮度的手柄控制。模块借助ROS里的joystick库，我在测试时将其当工具使用，sudo apt-get install ros-<distro>-joystick-drivers即可。但我依旧推荐源码安装（http://wiki.ros.org/joystick_drivers）。实际上这部分也没有什么好介绍的，无非是手柄按键与控制量的映射。在此我需要特别说明一下的是不同型号的手柄按同一按键发出的信息也可能是不同的，这一点要特别注意。比如demo里就有两款手柄，分别是bluerov使用的logicool手柄，以及我本人玩游戏使用的小鸡T4手柄，按键的对应关系需要在头文件中确定。

三、模块分布

软件框架中cabin_controllers/serial_to_mcu以及usb_cam必须运行于机器人本体开发板，因为二者需要开发板的串口或USB口的信息。以本次测试为例，二者均运行于bluerov的树莓派上。实际上，测试中cabin_controllers均运行于此。   软件框架的其余部分既可以运行于岸上PC，也可以运行于机器人本体开发板（算力满足的前提下）。以本次测试为例，树莓派显然无法满足实时处理图像的需求，故将darknet_ros模块运行于岸上PC。实际上测试中cabin_vision、cabin_behaviors、cabin_teleop均运行于此。   最后再放一张测试中整个ros网络的状态图：