郭鹏杰1，2，唐莉萍1，2
（1.东华大学 信息科学与技术学院，上海 201620；2.东华大学 数字化纺织服装技术教育部工程研究中心，上海 201620）
摘要：讨论了一种通过地磁进行智能系统导航控制的方法，通过地磁传感器获得智能系统的行驶状态，并对地磁导航角进行误差校正。综合考虑车辆速度、转向角大小等因素对智能系统行驶状态的影响，提出一种适用于内核为Cortex_M4的智能小车转向控制策略。
关键词：地磁导航；智能系统；行驶状态；转向控制
0引言
21世纪，智能交通系统的飞速发展引发了智能导航技术的不断进步。机器视觉由于其具有检测范围广、信息容量大、类似于人类驾驶决策且成本低廉等诸多优势，在国内外智能导航的研究中得到了广泛应用［1］。但是机器视觉是通过视觉识别道路和障碍物，进而规划路径实现导航控制，控制结果容易受天气状况、光照等因素影响。全球定位系统(Global Positioning System，GPS)是另一种广泛应用的导航技术，它可以为全球用户提供三维位置、速度等信息［2］。但是GPS导航具有一定的局限性，它在使用时要求用户实时连接网络或者存储高精度的数字地图，否则其导航精度和实时性难以保证［3］。
地磁场是地球的固有资源，它能为航天、航海、航空提供天然的坐标系［3，4］。由于地磁场的矢量性，通过地磁传感器感知测量即可确定系统当前的运动状态信息［5，6］。与其他导航方式相比，地磁导航具有无源、全天候、全地域、低能耗等优势，在工业部门、航空航天等诸多领域发挥了重要作用，成为近年来智能导航学术界关注的对象［7］。
本文在参考文献［812］的基础上，对基于地磁传感器自动导航的智能小车控制方法进行探讨，结合系统硬件框图，对基于地磁导航的控制策略进行研究，并采用PID控制导航。对通过地磁传感器获得的当前航向角度，判断其与目标角度的夹角的偏差，给出转向角度指令，调节舵机，实现小车自动路径行驶。最后在研究的理论算法基础上，考虑硬件平台限制，给出优化方案，并通过实践证明该优化方案的可行性。
1系统设计总体方案
地磁导航小车在自动路径运动中不断通过地磁传感器获得当前行进方向的角度信息，与预设路径的航向角比较，通过所得结果的偏差角度调整舵机的偏转方向，从而控制智能小车的行驶轨迹。系统的硬件主要模块如图1所示。其中地磁传感器采用意法半导体公司的MEMS数字集成芯片LSM303DLHC。
2地磁导航角度计算
图5系统控制流程地磁传感器只能提供当前所在区域的磁场量，要获取当前小车的导航角，必须计算地球磁场和导航角之间的关系并进行标定。
地球的磁场总是由地磁南极指向地磁北极，在极点处磁场与当地水平面垂直，在赤道处与当地水平面平行，如图2所示。对于地球上绝大部分区域来说，当地地磁场均可视为与水平面平行。由于地磁场是一个矢量，任意一点的地磁场都可以分解为两个平行于当地水平面的分量和一个垂直于当地水平面的分量。如果保持地磁传感器和当地的水平面平行，那么就有图3所示的地磁传感器的3个轴和这3个分量对应关系。
图3磁场向量分解［5］图3中，Hnorth和Hz分别为Hearth在水平面和垂直面的分量，Hx和Hy为Hnorth在相对坐标系X轴和Y轴的分量。
在水平方向上磁场两个分量的矢量和总是指向磁北。地磁传感器中的航向角（Azimuth）是当前方向和磁北的夹角，即：
当地磁传感器保持水平，航向角只需要用传感器水平方向两轴（通常为X轴和Y轴）的数据就可以由式（1）计算得到。当地磁传感器水平旋转时，航向角在0°~360°之间变化。如果设备不是水平放置，那么设备与水平面存在一个夹角θ，如图4所示。这个夹角将影响航向角的计算精度，需要借助加速度传感器进行倾角补偿。先通过测量3个轴上重力加速度的分量，得到：
再将地磁传感器测的得数据（XM，YM，ZM）通过γ和θ转化为Hx、Hy：
Hy=YMcos(θ)+XMsin(θ)sin(φ)－ZMcos(φ)sin(θ)（4）
Hx=XMcos(φ)+ZMsin(φ)（5）
结合式（1）即可计算出航向角ψ。
图4设备与水平面之间存在夹角［5］
3基于地磁导航的控制策略
3.1智能系统控制模型
智能导航控制的实质是对小车前进方向角度与设定目标角度偏差的控制。主要控制算法是基于自适应的PID控制。系统控制流程图如图5所示。
导航控制的具体过程如下：导航控制器根据接收到的智能车运行航向角信息ψ与设定目标角度进行比较，计算出航向偏差Δφ。当Δφ较大时，转向控制器通过收到的角度信息和反馈的角速度信息计算出合适的转向角θ，系统经过大角度转向、预判目标、回转等一系列操作后，以较小的偏差角度切入目标航线；当Δφ→0时，直接经过PID控制算法生成角度控制指令u，然后通过导航控制器不断调整，系统进入直线行驶控制状态。
3.2转向控制器设计
转向控制器采用PID控制方法，设定量和控制量都是角度值，反馈量为智能系统的转向角速度ωw。
本地磁导航系统以方向角为预瞄条件，其中车辆系统的二自由度运动模型如图6所示。
图6中，θ为智能系统相对于相对坐标系x轴的方向角；φ为舵机转角；R为智能系统的转弯半径；l为车辆轴距；v和ω分别为智能系统在点（x，y）处的速度和前轮转向角速度。由图6所示的车辆运动几何关系可以得到车辆的运动模型如下：
为了使轨道误差最小，根据智能系统当前状态下的理想侧向速率(t)与理想侧向加速度(t)，由系统理想运动模型，此时智能车所需理想侧向加速度为：
同时，在不考虑车辆的滞后情况下，其运动简单地服从Acklman几何关系。
式（8）表明智能车的曲率1/R与方向盘转角δ成正比，即智能车转弯半径R与方向盘转角δ成反比。汽车的侧向加速度同时与车速v和舵机转角φ有关。智能车通常行驶在低速状态下，系统通过调整转向角度即可获得良好的转弯半径。考虑到切入目标角度和转弯半径的约束，必须首先输出一个较大的转向角度，然后达到此角度后舵机进行回转，以达到切入目标角度时角度偏差最小的目的。
由于转向系统中方向偏角是不断积累的过程，故PID控制器采用比例控制和积分控制就能实现控制目的。这里采用位置式PID算法。
uf(k)=uf(k－1)+Kp［δe(k)－δe(k－1)］
+KiTδe(k－1)（9）
其中，uf为控制信号量，T为系统采样周期，δe(k)为角度偏差，Kp为比例系数，Ki为积分系数。
3.3导航控制器设计
导航控制器采用PID控制算法，设定值为目标角度，反馈量为当前航向角ψ；当系统直线行驶或者与目标角度的偏差较小时，系统进入直行导航阶段。为了防止系统调节时出现振荡现象，当地磁传感器所得到航向角与目标角度的偏差在±2°以内时，系统不进行调整，运动方向不发生改变。当偏差达到±3°时，系统采用PID控制方法调整智能车方向。
为了提高系统响应速度，算法采用比例控制方法，控制率为：
δ=αθ（10）
其中，δ代表转向角指令，α为比例系数。
4实验结果与改进
本文设计的智能小车其核心是Cortex_M4，嵌入式设备由于硬件限制，其运算能力不及PC，如果按照常规的控制算法进行自动导航，系统必然出现较大延时；如果要提高系统响应速度，则会影响系统转向角的控制精度。因此需要对第3节讨论的方法进行改进，以期在时间和精度之间得到一个较优的平衡。在智能小车自动导航过程中，可以将目标角度进行区域划分，针对不同的车速和角度区域，根据经验值设置不同的转角指令，将智能车的转弯曲线轨迹尽可能拟合为理想转弯轨迹，以达到良好的转弯效果。
在图4所示的理想运动系统模型中，Δφ是系统导航控制的关键因素。在自动导航实验中， 取Δφ≤2°角度作为导航控制器判断的阈值时，系统具有良好的响应速度，但是系统偏角响应处理很快，系统振荡周期较短，以至于系统出现明显的左右摆动。取Δφ>5°时，系统有明显的响应延时，造成明显的路径偏移。根据实验结果取Δφ=±3°作为导航控制器和转向控制器的阈值。
PID参数的选择是影响系统性能的重要因素，对PID参数的整定比较繁琐，需要进行复杂的积分、微分计算。这些运算如果在嵌入式硬件平台上实现，由于计算工作需要占用大量的时间，系统的实时控制能力将下降。为此，对PID参数可以采用查表法进行选择。首先建立一个模拟系统，计算出系统在不同角度与不同车速下的PID参数，再通过实验不断调整优化，最终建立PID参数经验表。
优化过后的系统导航控制过程如图7所示。
导航控制流程如下：MCU通过地磁传感器获得数据并计算出当前行驶角度v以及与目标角度的偏角差值Δφ，通过查找经验表获得适合当前车速与偏角Δφ的PID控制系数，然后调节PWM的占空比调节舵机转向角度大小。
5结论
实践证明：通过经验表代替系统复杂的积分微分运算，在几乎不影响系统的运行稳定性的情况下，提高了系统的响应速度，此优化方案具有可行性。
参考文献
［1］ 郭才发,胡正东,张士峰,等.地磁导航综述［J］.宇航学报,2009, 30(4): 1314-1319.
［2］ 尤亚男.智能车辆导航技术［J］.金田,2013(8): 346，323.
［3］ STORMS W, SHOCKLEY J, RAQUET J. Magnetic field navigation in an indoor environment［C］.Ubiquitous Positioning Indoor Navigation and Location Based Service (UPINLBS), 2010， IEEE, 2010: 1-10.
［4］ 李兴城,张慧心,张双彪.地磁导航技术的发展现状［J］.飞航导弹,2013, 10(10): 80-83.
［5］ 陈伟韬. 基于MEMS传感器的电子罗盘系统的设计［D］.广州：广东工业大学，