基于多传感器融合的陆空两栖机器人移动控制系统设计

洪向共, 钟地长, 赵庆敏

(南昌大学信息工程学院，南昌 330031)

陆空两栖机器人是指既可在地面行驶又可在空中飞行的机动机器人，同时兼具无人车和无人机的机动能力，可为安防、勘探、救援等领域提供多种优势，或将对未来部队作战带来重大影响[1]。文献[1]提到美国“黑骑士”军用陆空两栖平台在部队作战的装备投送、伤员救援、急救物资补给等方面具有较大作用，其经济性高于直升机，但采用的是两套驱动系统控制。文献[2]总结了当前新型陆空两栖平台的技术特点和发展趋势，对两栖平台功能、用途分类阐述清晰。文献[3]提出使用履带式机械结构设计，具有很好的机动性，但系统在陆空两种情况下运动控制阐述不够清晰。新型平台对陆空两栖移动控制算法和传感器数据融合进行了清晰的描述，系统采用多传感器数据融合处理方式实现两种驱动模式控制一体化，具有成本低、效益高、集成度高、可扩展性强等特点，可用于地质勘探、巡视侦查、物资投送等领域。

1 系统总体构建

主要从机械结构到硬件电路设计，再到软件算法控制实现来完成系统的功能。

1.1 系统设计方案

考虑到两栖机器人移动控制的安全性、可靠性，以及控制系统移动的便捷性等因素，同时在进行机械结构设计时需要考虑到机器人自身质量、承载能力和控制难易度，而轮式控制结合了无人车的设计特点，减震、易控制，载重能力也比较好，在不平地面上的越障能力较强，因此将其作为地面行驶运动机构。旋翼具有可垂直起降，可悬停，而且结构简单，机动性好，可操纵性较好，可作为空中飞行的运动机构[4]。最终，两栖机器人结构采用两驱四轮式和四旋翼式结构设计作为该控制系统的测试样机。各部分结构如图1所示。

图1 陆空两栖机器人系统结构Fig.1 Structure of air-ground amphibious robot system

1.2 系统控制电路设计

1.2.1 主控制器

为了更好地实现陆空两栖机器人的控制效果，并考虑到系统的集成化、降低成本、可操作性、稳定性等，将飞行控制和行驶控制集成到一块主控制板，兼容使用相同的传感器数据的思路去设计。控制主板的具体硬件电路如图2所示。设计选用了意法半导体的STM32作为CPU。使用STM32F405RGT6型号芯片作为CPU控制器，32位高性能ARM Cortex-M4处理器，时钟高达168 MHz。强大的时钟系统使其具有高效的运行效率，能满足陆空两栖系统的稳定运行。

图2 系统硬件电路框图Fig.2 Hardware circuit block diagram of the system

1.2.2 内部感知模块

(1)姿态传感器采用ICM20602型号的6轴运动处理传感器。它集成了微机电系统(microe-lectro mechanical system，MEMS)传感器、3轴的陀螺仪、3轴的加速度计，更精确，噪声更小。它还可以获得32 kHz的陀螺读数，相较MPU6500型号的芯片在性能上有很大提升，因此对于具有更高处理能力的未来飞行控制器将是一个很好的选择，可以选用I2C(inter-integrated circuit)总线或SPI(serial peripheral interface)总线接口连接一个第三方的数字传感器，比如磁力计。型号为AK8975的是一个3轴电子罗盘，是具有高灵敏度的霍尔传感器。

(2)气压计选用歌尔推出的型号SPL06-001数字压力传感器，它是一款具备SPI总线接口的新一代高精度气压传感器。

1.2.3 地面执行模块

(1)针对轮式机器人调控操作，选用了调速范围大，过载、启动、动转矩大，易于控制，可靠性高的直流电机。

(2)直流电机驱动器采用了工作电流比较大，驱动能力较强的A4950电机驱动芯片。

1.2.4 外部感知模块

为使系统能够更好地运行，采用主要定位和测距模块作为外部感知模块。

根据上述设计，最终设计的系统控制主板如图3所示。

2 运动学建模

针对陆空两栖两种驱动方式进行运动学建模，将进行两种模式下的受力分析，模型线性化，建立模型公式，建立坐标系转换，方便主控处理器更好地处理两栖机器人的位姿[5]。

2.1 空中飞行

动力学建模在实际应用中，模型复杂度的增加通常并不能带来准确性的提高，相反会导致算法实时性的降低，因此，模型简化是一种非常重要的手段。

对两栖无人机飞行时的气流进行受力分析，建立系统飞行模型，如图4所示,并对实际控制情况进行分析，确定各个环节的传递函数[6]。

图3 系统主控制板Fig.3 System main control board

图4 系统飞行模型Fig.4 System flight model establishment

四旋翼无人机的沿轴平移运动需要在导航坐标系n中分析,假设四旋翼除重力和升力外不受其他力，四旋翼外力仅有螺旋桨提供的升力为

(1)

式(1)中：F为4个桨叶的合力；Ω1、Ω2、Ω3、Ω4分别为螺旋桨1、2、3、4提供的升力；b为转换系数。

(2)

通过升降、俯仰、横滚、偏航4种运动模型的计算，得出旋翼转速与升降、俯仰、横滚和偏航4种运动的控制量之间的关系为

(3)

式(3)中：F1、F2、F3、F4分别是升降、俯仰、横滚、偏航运动方向的合力；U1、U2、U3、U4分别是升降、俯仰、横滚、偏航4个输出控制量，这4个控制量直接与程序算法输出量相关联；c、b为模型的转换系数。

2.2 地面行驶

在无人驾驶系统中，合理的车体模型的选择对规划、控制模块的复杂度和性能有很大的影响。为了能够尽可能准确描述车体运动，需要建立复杂的微分方程组，并用多个状态变量来描述其运动[7]。

系统对车体位姿(地面行驶的位置和航向)，首先在大地坐标系下，设定[x,y,θ]为车体模型的状态量(描述车体的位姿Vehiclepose)，[v,δ]为车体模型的控制量。v代表车体前进车速，δ代表车体的前轮偏角。建立图5所示的车体位姿信息模型。

Vehiclepose=[x,y,θ]

(4)

而后根据单自由度车体位姿模型计算得二自由度车体运动学模型公式为

(5)

最后根据转向位姿估计模型，如图6所示，给定车体的初始定位信息。图5、图6中(XG,YG)为世界坐标系，(XL,YL)为车体局部坐标系，OL为车体后轴中心，OLXL为车体前进方向。车体位姿用向量表示为(x0,y0,θ0)T，(x0,y0)为车体位置，θ0为车体航向角。根据车体后轴左、右轮的单位移动距离估计车体的位姿(位置和航向)信息，得出轮速计位姿估计表达式为

(6)

图6 转向位姿估计模型Fig.6 Steering pose estimation model

3 软件算法控制

一般的比例积分微分(PID)控制算法是根据反馈后的控制效果来不断整定合适的参数，而没有对控制对象作细致的分析，因而对简单的系统控制过程比较简单[8]。但对于控制对象的滞后和时间常数很大、干扰作用强而频繁的系统来说，很难通过调整最佳参数来获得高质量的控制效果。而串级PID控制目的在于通过设置副变量来提高对主变量的控制质量，抗干扰作用强，改善控制质量，提高系统工作频率，且自适应负荷能力较强。另外对于姿态测量传感器进行了数据融合，采用不同的融合算法能够实现在不同模式下反馈值给PID控制器，输出不同驱动量驱动机体的桨叶和轮子。传感器数据融合处理与两种模式之间的联系如图7所示。

图7 多传感器数据融合后的反馈Fig.7 Feedback after multi-sensor data fusion

3.1 空中飞行控制算法

飞行过程中系统通过惯性导航单元(inertial measurement unit，IMU)数据融合，一般IMU包括加速度计、陀螺仪、磁力计，然后进行解算实时的姿态角，反馈到飞行姿态控制系统中。根据PID的控制原理，输入期望与反馈值作差，通过比例、积分、微分等运算，输出四路的脉冲宽度调制(pulse width modulation，PWM)波形信号来控制电机转速，实现系统稳定飞行[9]。

系统使用串级PID控制策略能够提高飞行系统的抗干扰性和适应性。如图8所示，根据串级PID的特性和文献[10-11]的分析可知，串级PID分为外环和内环两个控制环，其中外环为角度环，其作用是对姿态角进行控制，内环为角速度环，其作用是增强系统的稳定性，使系统的控制更加精确。外环期望姿态角与解算后反馈的实时姿态角作差，经过外环比例控制器运算输出的是角速度值，并作内环期望角速度，与由陀螺仪测得的实时角速度的偏差作为内环的输入，最终内环输出通过控制量转换，得到不同的PWM波信号控制电机转速，实现机器人的各种姿态变化运动。

图8 空中飞行姿态控制流程Fig.8 Process of attitude control in air flight

图8中φ、θ、ψ为姿态解算后的3个姿态角(俯仰、横滚、偏航)，φe、θe、ψe是期望的姿态角度，经过外环PID控制器运算，输出3个姿态角速度pe、qe、re作为内环PID的输入，陀螺仪测量的角速度p、q、r作为反馈量。

3.2 地面行驶控制算法

系统地面行驶的控制难点在于对机器人运动速度和方向控制的融合，其中方向的控制是系统控制策略的难点。根据文献[7，12]的描述，采用最优曲率法。其控制思路如下：模型车在一个控制周期内的运动轨迹可以近似为一段圆弧，整个运动轨迹可以看作由小段圆弧连接成的曲线。在假设线速度恒定的前提下，给定路径上的一个点就能确定出模型车到达该点所应具有的舵机角度[12-13]。根据该点的信息得到车体实时位姿情况。

建立地面运动坐标系，以车体的两后轮之间中点为坐标系原点，车体的中轴前进方向为y轴正方向，设(cx,cy)为路径上某一点在该坐标系中的坐标，则控制量之间的计算公式为

(7)

ω=arctan(L/rb)

(8)

R=ωm

(9)

式中：rb为后轮的转弯半径；L为车体的前桥与后桥的距离；m为将ω转换为R的系数。

集合上述算法的控制思想和地面运动学模型，设计出特定的串级PID控制算法，将控制器分为内、外环控制，外环控制为小车行驶位置和方向角度PID控制，控制输出角速度，再将角速度转换成线速度，由运动学方程进行转换后，作为内环的期望值输入，内环控制为小车加速度PID控制，内环控制输出经过线性映射转换成控制电机的PWM波，如图9所示。

图9 地面行驶位姿控制流程Fig.9 Process of position control forground driving

4 系统测试与分析

两栖机器人控制系统在陆空两种模式下分别进行了测验，其稳定性和可操作性已实现，并且通过MATLAB对系统输出数据进行绘图，更直观地看到控制的效果。

在空中飞行模式下，以陀螺仪Z轴采集数据为例进行传感器数据融合实际测试。如图10所示，红色部分为传感器数据融合后的值，蓝色部分为传感器采集的原始数据。由图10可知，数据经过本文系统的数据融合算法处理，明显减少了大部分的噪声干扰，使得数据对系统控制更加稳定和可靠。如图11所示，以加速度计y轴的数据融合前后对比为例，蓝色是加速度原始采集的数据，红色是融合处理后的数据。从图11可以看出，融合的速度和原始数据几乎贴合，而且对数据的尖峰做了一定的滤波限制处理，提高数据柔和性，避免飞行出现大幅度抖动。

图10 陀螺仪z轴数据融合前后对比Fig.10 Comparison of z-axis data fusion for gyroscope

图11 加速度计y轴数据融合前后对比Fig.11 Comparison of y-axis data fusion for accelerometer

传感器数据作为串级PID闭环反馈控制量，是输出飞行姿态角重要的数据来源。处理后的传感器数据通过姿态解算输出姿态角，如图12所示，其中蓝色为俯仰角，黑色为横滚角。姿态角在实际测试过程中收敛较快，灵敏度高，具有较好的控制效果和操作性。

图12 飞行姿态角度测试Fig.12 Flight attitude angle test

图13 地面行驶速度控制测试Fig.13 Ground velocity control test

对系统地面行驶进行测试，如图13所示，蓝色实线为期望的速度曲线，绿色实线为系统控制输出的曲线，通过遥杆的作用测试，系统在地面行驶下的速度曲线和期望的曲线基本相近，绿色曲线出现的±0.01 m/s的毛刺误差，是由于系统定时器计数测量引起的误差。实际速度和理想速度控制效果基本相近，体现该两栖机器人系统的地面运动可控制性较强。

系统实物测试如图14所示。

图14 实物测试Fig.14 Physical testing

5 结语

设计的陆空两栖无人机系统控制主板是以STM32为核心，集成了无人机和无人机车测量姿态传感器，同时兼容外部测距、定位等传感器。主控板设计使用有刷和无刷电机驱动，并预留可扩展传感器功能接口。代码开源，算法清晰易懂，可靠性高，可操作性强，系统可根据测距模块采集信息实现避障功能。系统的串级PID 算法虽然增加了控制系统的工作频率，减少了时间常数，但达到了更佳的控制效果。最后制作了样机，通过试验，验证了设计的合理性和可靠性，两栖系统运行正常。今后可在此基础上展开水陆空三栖控制系统的研发。