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按照下式可计算出角度θ。角度θ计算公式为：