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

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

串口操作線程讀取串口數(shù)據(jù)，并提取有效數(shù)據(jù)，接著利用函數(shù)PostMessage將有效數(shù)據(jù)分別傳送到主線程和導(dǎo)航線程。主線程將有效數(shù)據(jù)根據(jù)協(xié)議進(jìn)行解包并把數(shù)據(jù)包中包含的AHRS、數(shù)字羅盤、GPS等傳感器和推進(jìn)器、前艙環(huán)境參數(shù)等數(shù)據(jù)顯示到界面上。當(dāng)使用搖桿控制AUV的運(yùn)行時(shí)主線程每隔0．5 s從USB接口接收數(shù)據(jù)，并轉(zhuǎn)換成推進(jì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)提供的位置估計(jì)精度會隨時(shí)間而漂移，所以導(dǎo)航線程采用基于GPS／INS的組合導(dǎo)航算法，用GPS輔助導(dǎo)航，即用GPS信息輔助修正慣導(dǎo)系統(tǒng)的輸出，包括航向角和速度。對AUV的航向角信息修正是通過經(jīng)典的PID控制算法來實(shí)現(xiàn)的，如圖4所示。

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