基于多传感器的波浪能滑翔器导航策略研究*

史 健，龚 威，齐占峰，贾立娟，张 森，秦玉峰

（1.天津城建大学 计算机与信息工程学院，天津 300384;2.天津城建大学控制与机械工程学院，天津 300384;3.国家海洋技术中心 深海室，天津 300112）

0 引言

波浪能滑翔器是一种多功能的海洋自主运动平台，其主要包括2方面结构:舟行上浮体和水下滑翔，舟行上浮体搭载北斗、电子罗盘、GPS以及波高仪和流量计等传感器采集导航信息;同时搭载多种科学仪器，采集海洋数据，完成海洋监测任务［1］。水下滑翔机主要结构为舵机，根据不同位置角度的偏差值，实现打舵转弯，从而控制波浪能滑翔器朝预设位置运动。近十年来，美国相继开发了多型波浪能滑翔器作为海洋观测平台，并投入了海洋水文资料探测和军事应用，水下航行器观测平台包括高机动性的自主水下机器人（AUV）和低机动性水下滑翔器都得到较好的应用，而我国在海洋自主监测方面的研究才刚刚起步，国家海洋技术中心联合清华大学，天津城建大学等多所高校和科研单位致力于国家公益性行业科研基金专项“全球气候变化海洋CO2自航监测平台研发与应用示范”的研究与开发，取得了重大突破［2］。

1 平台嵌入式控制与导航系统

1.1 基本结构

平台嵌入式控制与导航系统包括平台嵌入式控制器、平台航行与传感系统、能源管理系统、通信设备等，具体系统结构连接示意图如图1所示。由于滑翔器工作环境复杂且恶劣，所以，器件均需选择低功耗、高可靠性的工业级芯片，降低系统功耗，提高系统的可靠性。LPC2478具有较高主频和同步动态随机存取器SRAM，具有速度高、能耗低的特点。因此，采用以LPC2478为微处理器的Mini ARM嵌入式核心板，中央处理器采用ARM7TDMI芯片。同时，为提高任务调度的实时性，预装μC/OS-II操作系统;预留外设接口连接导航设备、传感器和通信系统等，如图2所示。

图1 嵌入式平台导航系统Fig 1 Embedded platform navigation system

图2 嵌入式控制器Fig 2 Embedded controller

1.2 导航传感系统组成

1.2.1 传感器

综合考虑导航传感器的精度和波浪能自航行平台对导航精度的要求，同时考虑到舵机转向的机动性和方向矫正的能耗，设计了基于电子罗盘和GPS的组合导航系统;同时选用波高仪和流量计作为辅助导航传感器，确保导航精度。

1.2.2 执行控制器

波浪能滑翔器在海上运动，只依赖海洋中的波浪提供动力，不依赖外部动力系统。因此，只需在水下滑行机尾部安装尾舵机构进行控制，LPC2478采集GPS的经纬度和电子罗盘的航向角数据，经数据处理后注入导航算法，与预设航向比较得到航向角偏差值，控制舵机打舵偏角，从而实现了波浪能滑翔器在海上朝预设位置自主运行。

2 导航算法的分析与实现

波浪能自航行平台载有GPS和电子罗盘，利用当前位置的经纬度和航向角作为输入，通过高斯大地线算法计算出2个坐标点的距离长度和航向角偏差值，通过PID算法控制舵机打舵角度，使波浪能滑行器到达预设位置坐标，从而实现视向跟踪，如图3所示;到达预设位置坐标半径内50 m后，在设定时间内保持波浪能滑翔器原地45°转圈，采集该区域内的海洋数据，完成海洋监测任务，实现虚拟锚泊，如图3所示。

图3 导航算法图解Fig 3 Navigation algorithm illustration

2.1 视向跟踪算法

波浪能滑翔器在海洋上运行是按照预设轨迹进行移动，通过实时采集GPS和电子罗盘的数据，由高斯大地线算法计算当前位置与预设路径的距离长度和偏移角度，从而调整滑翔器舵机的打舵角度，使其朝预设位置移动。为了保证波浪能滑翔器移动的精度和准确性，同时考虑到GPS和电子罗盘的精度要求，预设轨迹点的密度应保持适中，一般以500 m为单位设定一处轨迹点。

2.2 高斯大地线算法

通过GPS可获得波浪能滑翔器在海面上的经度和纬度基于高斯大地线算法，以经纬度作为输入参量，计算出波浪能滑翔器当前位置与预设位置的距离长度，作为波浪能滑翔器航行的距离;同时计算出预设路径的航向角，实现对波浪能滑翔器的角度校正。高斯大地线算法是基于高斯平均引数来进行大地主题反解的迭代算法［3］，如图4所示。

图4 导航算法流程图Fig 4 Flow chart of navigation algorithm

令

则高斯平均引数正解公式可以写成下面的形式

由上式可得

2.3 PID 算法

模拟PID控制器的系统原理如图5所示，其控制规律如公式（3）描述。

图5 PID控制器Fig 5 PID controller

由于ARM平台LPC2478是通过软件的算法执行PID控制，因此不能用模拟方式进行连续控制，只能根据采样时刻的偏差值来计算输出的速度。所以，必须将模拟PID控制器进行离散化和增量式处理。在连续时间t内进行K次连续采样，每次采样周期为T，则得到模拟PID控制器的控制规律的离散变换如下所示［4］:

其中，uk为第k次采样时刻输出的舵机打舵角度;ek为第k次采样时刻输入的航向角偏差，其值为电子罗盘测量得到的当前航向角rk与高斯大地线算法计算得到的预设航向角Amk的差值，即ek=Amk-rk;KP为比例系数，TI为积分系数，TD为微分系数，令 Δuk=uk-uk-1，Δek=ekek-1，则离散化后的增量式算法表述为

在上述PID控制算法中，当有较大幅度的扰动或大幅度改变给定值时，由于此时有较大的偏差，以及系统有惯性和滞后，故在积分项的作用下，往往会产生较大的超调量和长时间的波动。为此可以采用积分分离措施，将航向角偏差值设一个上限emax，若|ek|＜|emax|，采用 PID控制;若|ek|≥|emax|，采用PD控制，具体算法描述为

其中，k=2，3，4，…。

综合高斯大地线算法和PID算法可构建出波浪能滑翔器导航算法的数学控制模型为

其中，Δuk为第k次输出的舵机打舵角度;Amk为在第m个位置的第k次由高斯大地线算法计算得到的预设方位角;rk为第k次由电子罗盘测量得到的当前波浪能滑翔器的方位角;T为采样周期;KP，TD，TI分别为PID控制算法的比例系数，微分系数和积分系数。其在导航系统中的应用流程图如图6所示。

3 导航系统的实现流程

波浪能滑翔器在海洋上运动，嵌入式控制系统（包括ARM—LPC2478控制器和多路传感器，可参照表1描述），实时采集导航数据，获得波浪能滑翔器当前的经纬度，航向角数据以及用于辅助导航的波浪高度和排水流量数据。由于在实际采集的导航数据中含有噪声杂质，干扰导航精度，因此，利用卡尔曼滤波算法对原始数据进行降噪处理［5，6］;同时，考虑到导航数据的采集存在一定误差，对原始数据进行整合，对多次采样的数据做平均处理。对滤波与整合后的经纬度和航向角注入高斯大地线算法计算出当前位置与下一预设位置的距离长度和预设航向角角度值。通过采集海洋中波浪的高度和滑翔器的排水流量［7］，计算得到波浪能滑翔器的当前的运行速度，从而得到波浪能滑翔器从当前位置达到下一预设坐标位置的航行距离和航行时间。预设航向角与电子罗盘采集的实时航向角计算差值，得到的航向角偏差值作为输入量注入到PID控制算法中，输出舵机控制器的打舵角度，从而实现波浪能滑翔器按照预设轨迹运行。当当前位置与预设位置的距离长度小于设定值时，保持波浪能滑翔器在所设定海域内的45°转角，采集海洋数据，完成海洋监测任务。其具体流程图如图7、图8所示。

图6 PID算法的应用Fig 6 Application of PID algorithm

图7 导航流程图Fig 7 Navigation flow chart

图8 流程图详解Fig 8 Detailed annotation of flow chart

4 实验结果与分析

对波浪能滑翔器在青岛进行实地海试，使用上位机软件显示波浪能滑翔器的航迹图。图9为第一次海试时，未使用传感器，采用遥控打舵的方式控制平台运行的航迹图;图10为第二次海试采用多传感器控制系统，并使用高斯大地线算法和PID算法混合算法得到的平台自运行航迹图。其中设置采样间隔为5 min，设置预设轨迹位置的间隔为500 m，预设轨迹形状为直接三角形，当时实际海况为2～3级，平均有效航速1.05 m/s。由实际海试结果得出采用多传感器并多算法混合的系统得到的运行轨迹与预设轨迹符合度更高，基本达到了实验要求。验证了波浪能滑翔器导航系统的可行性。图11、图12为波浪能滑翔器自航行过程中通过气象传感器采集到的温度与风向数据。

图9 遥控打舵方式得到的海试航迹图Fig 9 Tracking map by remote control rudder way

图10 多传感器与混合算法方式得到的海试航迹图Fig 10 Tracking map by mode of multi-sensor and mixing algorithms

图11 温度数据Fig 11 Temperature datas

图12 风向数据Fig 12 Wind direction datas

5 结束语

本文设计了基于嵌入式ARM平台LPC2478的波浪能滑翔器的导航系统，在硬件上，选用多路高精度传感器:GPS和电子罗盘作为主传感器，波高仪和流速仪作为辅助导航传感器，既提高了导航的精度，又保证了导航系统的低功耗和高可靠性;软件设计中，采用高斯大地线算法和PID控制算法相结合的算法，建构出一种新的导航算法，通过实际应用，验证了算法的可行性和有效性;同时在任务调度中，移植μC/OS-II操作系统，实现了对多路传感器不同任务的合理调度，保证了系统的实时性。整个系统中，嵌入式控制系统的合理搭建和多路传感器的同时使用，以及多种算法的有效结合，提高了波浪能滑翔器在海洋运行中的导航精度，降低了导航误差。

［1］Graver J G，Underwater gliders:Dynamics，control and design［D］.New Jersey:Princeton University，2003.

［2］Kraus N，Bingham B.Estimation of wave glider dynamics for precise positioning［C］//Oceans'11，MTS/IEEE KONA，2011.

［3］赵长胜.高斯平均引数计算大地坐标主题反解的迭代算法［J］.测绘通报，2004（10）:11-12.

［4］冯 城，叶文华.基于ARM处理器的数字PID伺服电机调速系统［J］.电工电气，2010（3）:47-48.

［5］Mahmoudian Nina.Efficient motion planning and control for underwater gliders［D］.Blacksburg，Virginia，USA:Virginia Polytechnic Institute and State University，2009.

［6］Daniel T，Manley J，Trenaman N.The wave glider:Enabling a new approach to persistent ocean observation and research［J］.Ocean Dynamics，2011，61:1509-1520.

［7］Manley J，Willcox S.The wave glider:A new concept for deploying ocean instrumentation［J］.IEEE Instrumentation ＆ Measurement Magazine，2010，13:8-13.