20kg級自主水下機器人控制軟件設計與實現

3 軟件實現
3．1 多線程實現
一個進程可有多個線程，使用多線程可提高軟件的執行效率。該控制軟件共有3個線程組成，包括一個主線程、一個導航線程和在成功打開串口后利用API函數CreateThread創建的一個串口操作線程(如圖3所示)。

串口操作線程讀取串口數據，并提取有效數據，接著利用函數PostMessage將有效數據分別傳送到主線程和導航線程。主線程將有效數據根據協議進行解包并把數據包中包含的AHRS、數字羅盤、GPS等傳感器和推進器、前艙環境參數等數據顯示到界面上。當使用搖桿控制AUV的運行時主線程每隔0．5 s從USB接口接收數據，并轉換成推進器轉速以及方向舵的方向角信息，且將這些信息發送到串口操作線程寫入串口。
在主線程中創建串口操作線程的代碼如下：
hThread=CreateThread(NULL，0，ThreadProc，(LPVOID)this，0，NULL)；
在串口操作線程中將有效數據發送到主線程的代碼如下：
PostMessage(*pDlg，WM_MYMSGl，
(WPARAM)buf，(LPARAM)Length)；
3．2 關鍵算法
由于慣性導航系統提供的位置估計精度會隨時間而漂移，所以導航線程采用基于GPS／INS的組合導航算法，用GPS輔助導航，即用GPS信息輔助修正慣導系統的輸出，包括航向角和速度。對AUV的航向角信息修正是通過經典的PID控制算法來實現的，如圖4所示。

設Ji-1，Ji為AUV的2個節點，AUV即A點從Ji-1到Ji點運行。設正北方向矢量為，根據圖5按照下式可計算出角度θ。角度θ計算公式為：