鱼雷状小型无人艇路径跟踪控制系统研制

翁 昱,曾庆军,李 维,李 昂,戴晓强

(1.江苏科技大学 自动化学院,江苏 镇江 212100; 2.江苏科技大学 计算机学院,江苏 镇江 212100)

无人艇[1]是依靠遥控或自动控制方式在水面航行的无人智能化平台。因其具有成本低、机动性好、体积小等特点,在军民用领域有着较为广泛的应用前景[2]。国外,英国研制的“水獭”无人艇,长约2 m,宽约1.08 m,最大时速6 kn,搭载多波束回声测深仪等传感器,可应用于湖泊、河流测深等场景[3];国内,哈尔滨工程大学研制的“天象一号”无人艇,具有自主导航、自主避障等功能,可应用于水文、气象监测等领域[4]。近年来,随着无人艇自动化和智能化水平的提高,无人艇智能系统引起越来越多研究者的关注[5]。

无人艇路径跟踪控制系统[6]是实现无人艇自主化的基础和核心,是无人艇安全航行的重要保障。赵梦铠等[7]针对固定双桨驱动的无人艇,采用模糊PID控制方法设计了一种自主直线路径跟踪系统,该系统运行稳定,有效降低了目标路径的跟踪误差;柳晨光等[8]为了解决环境干扰下的精度、可靠路径跟踪问题,设计了一种自适应路径跟踪控制系统,有效提高了系统的可靠性和跟踪精度。

本文为了提高无人艇水域作业时的航向跟踪精度,设计了一款鱼雷状小型无人艇路径跟踪控制系统。首先搭建了软硬件系统框架,然后设计了LOS-MPC路径跟踪控制算法,在岸基控制系统中进行坐标系转换,用采集视线法计算期望航向;在艇载控制系统中将航向误差作为MPC控制器[9]的输入,实时输出舵机PWM信号以调整航向;最后开展湖试实验验证系统的可靠性和跟踪精度。

1 控制系统概述

鱼雷状小型无人艇控制系统具有手动控制和自主巡航2种模式,由上位机和下位机控制系统组成。其中,上位机控制系统为岸基控制系统,下位机控制系统为艇载控制系统。图1是鱼雷状小型无人艇控制系统组成图。岸基控制系统由岸基端计算机(岸基通信终端)和数传电台等设备组成,利用数传通信方式接收GPS、惯导、温湿度等数据并进行显示,通过4G信号接收图传画面信息,艇载控制系统以STM32F429芯片为主控芯片,通过RS232、RS485等通信方式对GPS、惯导等设备数据进行采集和解算,将自身状态回传至岸基控制系统。

无人艇路径跟踪控制系统的核心是艇载主控软件和岸基端计算机,岸基端计算机根据期望路径信息与无人艇GPS测得的当前位置信息,利用视线(Line of Sight,LOS)制导算法实时计算出期望航向并下达指令;在艇载主控系统中建立预测模型,结合预测模型计算出预测时域内系统状态序列、输出序列和控制序列,通过在线求解基于状态量误差和控制量的目标函数,输出舵机控制信号调整无人艇航向。

2 系统硬件设计

艇载控制系统采用12 V锂电池包供电,由单片机模块控制,模块以STM32F429芯片为主控芯片,该芯片主频为180 MHz,具有性价比高、配置灵活、功耗低等特点[10]。丰富的通信外设为控制系统[11]提供了很大的便利,也为导航模块提供了完整的通信条件,鱼雷状小型无人艇控制系统框架如图2所示。

鱼雷状小型无人艇控制板如图3所示,由最小系统、电压转换电路、驱动电路、通信电路等组成。最小系统[12]主要包括MCU运行所需的一些基本外围电路,为控制系统提供基本条件;电源电路由12 V转 5 V和5 V转3.3 V的电压转换电路组成,为芯片和外设提供稳定的电压;驱动电路主要由隔离电路等组成,通过控制芯片定时器产生PWM信号,经光耦隔离输出稳定的控制信号,驱动推进器和舵机。

由于导航传感器的外设接口不同,单片机控制芯片不能产生通信所需要的电平信号,因此设计了如图4所示的通信电路,分别将单片机输出的TTL电平信号转为RS232电平信号和RS485差分信号,其中输出的RS232电平信号与GPS接口相连,采集定位数据;输出的RS485差分信号与惯导设备接口相连,采集载体运动时的姿态数据,MCU将采集到的数据进行解析处理,通过数传电台上传至岸基控制系统。

图4 通信电路

3 路径跟踪系统软件设计

无人艇路径跟踪软件主要有GPS和惯导数据解算与存储、路径跟踪算法的实现、舵机驱动控制等功能。图5是路径跟踪算法流程图。在上位机中执行LOS制导算法[13]程序,在下位机中进行MPC控制算法的处理。首先导入地图,设置目标跟踪点,进行坐标系转换,利用LOS制导方法生成接纳圆,计算期望航向,给下位机下达控制指令。接着下位机接收到指令后,采集当前状态信息进行卡尔曼滤波处理,上传位置信息,同时将航向误差作为MPC控制器输入,输出舵角控制指令并生成相应PWM信号,驱动舵机,调整航向;最后上位机实时计算当前位置与期望位置的距离,当达到接纳圆后,进行路径更新,当达到终点后,下达停止指令。

图5 路径跟踪算法流程图

3.1 上位机算法设计

上位机软件是基于QT5平台开发的,采用C++语言编写,上位机软件按照预先约定的协议通过RS232串口通信方式与下位机进行信息交互[14]。

上位机路径跟踪子程序主要有坐标系转换、LOS制导算法。其中,坐标系转换程序是调用百度地图的JS交互接口,根据起始点坐标经纬度和结束坐标经纬度,在地图上标注目标跟踪点,考虑到百度地图提供的是基于BD09坐标系下的经纬度信息,而GPS所需的是基于WGS84坐标系下的经纬度信息,因此进行坐标系转换处理。

LOS制导算法[15]根据距离的远近来跟踪期望位置,再由自身位置计算出期望航向角,具有较好的实时性。图6是LOS制导算法原理图。

图6 LOS制导算法原理图

图6中:P为无人艇当前位置;Δ为无人艇可视距离;Pn+1为当前目标航向点;ψlos为期望航向。

LOS制导算法具体步骤如下。

① 计算期望航向,公式为

(1)

式中:(xlos,ylos)为LOS点位置;(x,y)为无人艇当前位置。

② 更新目标路径点。判断无人艇是否在以Pn+1为圆心,R为半径的接纳圆内,若满足,则跟踪下一目标路径点Pn+2,公式为

[xn+1-x(t)]2+[yn+1-y(t)]2≤R2

(2)

式中:R为接纳圆半径,一般取3～5倍无人艇的长度。

3.2 下位机算法设计

Keil5是一款兼容单片机的C语言软件开发系统,提供了包括C编辑器、库管理等在内的完整开发方案,拥有强大的软件仿真调试工具和方便易用的集成环境。因此下位机软件基于Keil5软件开发,采用C语言编写[16]。

下位机路径跟踪子程序主要是MPC控制程序,MPC控制程序过程如图7所示。

图7 MPC控制过程

具体算法步骤如下。

① 建立舵角小幅度操纵模型作为预测模型,本文采用的是一阶野本(Nomoto)模型,其数学表达式为

(3)

式中:δ为舵角;ψ为航向角;r为转首角速度;K、T为操纵性指数。

② 当下位机接收到期望航向信息后,采集当前航向信息,进行卡尔曼滤波,并计算期望航向误差作为MPC控制器的输入。

③ 设置系统约束。由于无人艇的推进器驱动机构和舵机受机械性能影响,其运动范围和速率有限。因此,为了无人艇的作业安全,需对控制量输入进行约束,公式为

umin

(4)

式中:umin为控制量输入下限;umax为控制量输入上限;u(t+i)为t+i时刻的控制量。

④ 基于状态量误差和控制量,构建在t时刻的模型预测控制目标函数,进行寻优求解,公式为

(5)

式中:Np为预测步长;x(t+i)为t+i时刻的预测状态;u(t+i-1)为t+i-1时刻的控制量;xo为目标点状态信息;Q为权重矩阵;R为权重参数。

⑤ 将第一个控制分量作为舵角控制指令,生成对应的PWM信号作用于无人艇。

4 湖试实验

4.1 实验平台设计

利用QT等软件建立无人艇路径跟踪实验平台。该平台主要有5大功能区域,分别为当前状态信息显示区、地图显示区、巡航信息显示区、按钮区、画面显示区。其中当前状态信息显示区用于显示无人艇的当前状态,包括经度、纬度、航向角等数据;地图显示区用于显示目标点位置和无人艇当前位置;巡航信息显示区用于显示自主巡航模式下起始点、目标点等信息;按钮区用于手动模式下无人艇的运动控制;画面显示区用于实时显示图传画面。无人艇航行时将实时采集的数据信息通过数传电台发送给上位机,上位机将采回的数据存入设计好的数据库中,路径跟踪控制器根据预先设置的控制参数结合MPC控制算法得出舵角控制信号传送给执行结构,通过改变无人艇的航向实现路径跟踪控制。

4.2 路径跟踪实验

为了验证本文设计的无人艇路径跟踪系统在真实场景下的有效性和可靠性,研制了一款如图8所示的鱼雷状小型无人艇实体样机,为了满足系统需要,艏段安装有探照灯,电子舱段安装有GPS、惯导等设备,艉段安装有舵机、推进器等设备,垂推舱安装了2个备用推进器,在特殊情况下能实现下潜抵近监测等功能。为了保证无人艇作业的安全性,在电子舱内装有电极式水浸传感器,当传感器探头接触到水时,上位机将发送暂停自主巡航任务的指令并发出警报提示返航。该无人艇具有远程控制、自主巡航控制、数据存储等功能。

图8 鱼雷状小型无人艇实体样机

湖试实验开始前,将无人艇放入水池进行调试,验证所研制的无人艇能否满足水密安全性等设计要求,测试无人艇通信是否正常,导航数据是否接收正常。调试结束后,在室外某水域进行路径跟踪实验,无人艇需要先将自身的位置导航到设定的路径上,在上位机界面的地图上设定目标点进行跟踪。无人艇初始位置A的经纬度是(120.559 783 9,31.891 803 74),起始点B的经纬度是 (120.559 860 2,31.891 815 19),目标点C的经纬度是(120.560 150 1,31.892 097 47),图9为无人艇湖试实验情境。

图9 无人艇湖试实验情境

在执行路径跟踪任务时,多种传感器互相配合进行导航,实时记录惯导和GPS数据。图10为湖试实验结果,表1为无人艇部分实验数据。

表1 无人艇部分实验数据

图10 湖试实验结果

由图10(a)可以看出,由于无人艇存在一定的拐弯半径且GPS有定位误差,无人艇的跟踪轨迹与直线路径不完全吻合,与PID控制算法相比,本文控制方法下的轨迹曲线波动较小。

由图10(b)可以看出,与PID控制算法相比,本文控制方法下航向变化曲线波动较小,虽然刚开始航向角有较小的超调,但系统稳定后,航向角误差波动始终保持在较小范围内。

为了进一步分析本文所述控制系统的控制性能,给出如表2所示的2种控制方法的平均跟踪误差和航向角绝对值的平均误差。其中,平均跟踪误差的计算公式为

表2 控制性能指标对比

(6)

式中:q(i)为第i时刻距离的偏差。

综上可见,与PID控制方法相比,本文算法的平均跟踪误差小且航向角绝对值的平均误差也较小。因此,本文设计的路径跟踪控制器的路径跟踪精度优于PID控制器。

5 结束语

为了提高无人艇在水域作业时的航向跟踪精度,设计了一款鱼雷状小型无人艇路径跟踪控制系统。艇载控制系统以STM32F429为主控芯片,硬件上搭建串口通信等电路,采集自身状态数据,利用数传电台与岸基控制系统进行信息交互,接收远程指令完成运动控制;软件上在上位机中采用LOS制导算法,下位机中采用MPC控制算法设计路径跟踪程序;通过湖试实验表明系统整体运行稳定。所设计的控制系统既能为无人艇航行提供安全保障,也可为其他无人艇路径跟踪控制的应用提供参考。