20kg級自主水下機器人控制軟件設(shè)計與實現(xiàn)

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

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

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