20kg级自主水下机器人控制软件设计与实现

0 引言
    自主式水下机器人(Autonomous Underwater Vehicle，AUV)代表着未来水下机器人的发展方向，因而是世界各国研究的热点。而便携式AUV由于使用方便，可执行环境评估、水文地理、辅助水道测量、港口安全、岩屑区域绘图等工作以及可以用在未来战争中，将是未来AUV发展的重点。
    本文主要论述了便携式AUV控制软件的设计及其实现，该软件主要用于监视AUV在水下运行时的状态信息以及控制AUV的运行。AUV在水下运行时的状态信息包括位置信息、航向、舱内温湿度、推进器转速、舵的方向角以及在水面时GPS传感器数据等信息，该软件将这些信息显示到界面上最终实现对AUV的监控和导航。

1 便携式AUV系统简介
    该小型AUV由两个密封舱组成，前舱安置了传感器系统，后舱安置了AUV推进器以及方向舵的控制系统。两个密封舱中间放置的一个垂直推进器用来控制AUV的上下运动，后舱安放了用于控制AUV水平方向的水平推进器和方向舵。系统搭载了AHRS、数字罗盘、GPS等传感器，这些传感器采集到的数据用于AUV的导航。AHRS传感器用来测量AUV的航向角、俯仰角、横滚角、3个方向的速度、加速度；数字罗盘测量AUV的航向角等信息控制软件对一串口进行操作，该串口连接与AUV进行通信的无线模块。将从无线模块接收到的数据经过惯性导航算法处理，根据协议将惯性导航算法处理结果发送到AUV，最终实现对AUV的控制。

2 串口通信
    串口在做文件处理时，简单的应用可以采用查询方式或定时方式，复杂的可以采用事件驱动的方式。所谓事件驱动，即当串口有数据进入输入缓冲区时，自动执行接收程序。利用WinAPI读／写串口操作可以有同步方式与异步方式。所谓同步方式是指发出写命令时，直到有数据写入到输出缓冲区写函数才返回。异步方式的重叠方式是指发出写操作命令后，不管写操作是否完成，写函数马上返回，写操作在后台继续进行，写操作完成后通过某种方式通知调用写操作的线程。这样避免了主线程被挂起，提高了程序的工作效率。
2．1 串口通信设置
    在实现串口通信时，首先在界面上设置串口号、波特率、校验等信息。单击按钮打开串口，进入命令响应函数OnBtnOpen()，利用API函数打开并对串口进行配置。最后使用API函数CreateThread创建一个线程。由于软件工作过程中需要传送的数据量不大，所以仅仅打开一个串口。
    主线程打开串口具体流程图如图1所示。

    在主线程中打开串口的代码如下：
    m_hCom=CreateFile(m_port，GENERIC_READ|GENERIC_WRITE，0，NULL，OPEN_EXISTING，FILE_FLAG_OVERLAPPED，NULL)
    在串口操作线程中使用API函数ReadFile用于读取串口数据ReadFile(hCom，buf，19，＆Length，＆Eol)；而在该线程中向AUV发送控制指令时使用：
    fState=WriteFile(m_hCom，buf，19，＆m_bytes，＆m_os Write)[page]

2．2 串口通信协议
    串口通信必须遵守一定的通信协议，才可实现该控制软件与AUV的正常通信。串口通信数据格式如图2所示，图中Data0，Datal，Data2 …代表一个字(2 B)。

    发送或接收的一帧数据最长为19 B，Data0中第1个字节代表指令(0xA1)、请求(0xB2)或者正常应答(0xC3)等含义；Data0中第2个字节代表具体指令、请求何种信息或者某种信息的应答。Data1，Data2，…代表发送或者接收到的数据。开关机指令长度为19 B，第19字节控制8个继电器，1，0分别表示开、关第零位控制总电源。开机、关机指令前18 B分别是：
    ～A16613579BDF02468ACE13579BDF02468A
    ～A166DF985713CE8A4602DF9B5713CE8A46
    开机指令的第19个字节根据需要选择相应的继电器开启或关闭；关闭指令第19个字节为0x00，所有的继电器关闭。

[page]

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

[page]

4 控制软件界面及实验结果
4．1 软件界面
    本文设计的软件界面左侧上半部分和右侧主要实现对AUV的控制，界面左侧中下部分的3个仪表盘和TAB页控件显示AUV的各个状态信息。
    单击开机、关机按钮将实现AUV的开启与关闭；单击询问AUV按钮，此时应答情况为AUV存在，表示监控软件与AUV的通信正常，否则应该检查无线模块和AUV。单击前舱参数、GPS经纬度、GPS时间、推进器状态、AUV航向角等按钮将持续获得AUV相应的信息；步进电机控制按钮用于实现方向舵的调整，进而实现AUV方向的调整。为了防止步进电机失步，这里还特意设计了步进电机的微调按钮，目的是在步进电机失步时将方向舵调整回原位置。
    该控制软件还以仪表盘的方式显示推进转速、罗盘、温、湿度等信息。
    以速度仪表盘为例，当从串口接收到的数据中提取出水平推进器或垂直推进器速度信息时，将速度信息存放到成员变量m_Spd1或者m_Spd 2。利用API函数得到控件IDC_STATIC_SPD的区域坐标rect2，调用API函数InvalidateRect(＆rect2)重绘，将进入函数CDspsockDlg：OnPaint()重绘。利用MFC中的函数Pie，Ellipse，SetBkColor，TextOut画出仪表盘背景。最后通过下列两个公式将速度值转换成对话框上的坐标值，调用函数画一条连接该区域中心位置到该点(a1，b1)的直线，最终实现仪表指针随速度值的变化。坐标(a1，b1)计算公式如下：
    b1=60sin((m_Spd1×3／25+150)π／180)
    a1=60cos((m_Spd1×3／25+150)π／180)
4．2 实验结果
    软件运行期间界面显示如下。图6显示了温、湿度分别是32°，51．5°；单击复位按钮、温湿度指针将分别指向-30°，20°位置处；图6还显示了2个推进器的速度信息，其中水平推进器速度为1 180 r／min，垂直推进器速度为0．此时AUV在水平方向运动垂直方向静止。

5 结语
    介绍了20 kg级便携式AUV控制软件的实现，该软件利用串口通信技术、多线程编程技术在传输数据量不很大的情况下表现了良好的性能。软件以稳定的性能、友好的界面及简单的操作方法满足了对便携式AUV的监视和控制。