本文主要介紹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網路的狀態圖：