自动驾驶技术研发中,行驶车辆的水平位置(经度、纬度)和航向角度是两大关键信息,主要为车辆控制系统的横向控制(转向盘控制)和纵向控制(制动、加速控制)提供参考数据,满足自动驾驶车辆定位导航的综合需求[1]。自动驾驶技术对车辆航向角测量精度要求非常高。对高速行驶的汽车而言,航向角轻微偏差都会导致汽车偏离原来的行驶路线。对农业机械而言,虽然对农机作业速度要求不高,但对导航作业精度要求很高(特别是播种时需要达到cm级),航向角的轻微偏差都会对导航作业精度产生很大影响。因此,提高车辆航向角的测量精度是非常必要的。车辆航向角测量方法主要有磁阻传感器法、双天线全球导航卫星系统(Global navigation satellite system,GNSS)定位定向法、单天线GNSS定位定向法和陀螺仪测航向法等。磁阻传感器受周围磁场环境影响大,精度和可靠性不高,在高压线等有磁场干扰的作业环境下容易受到影响[2-5];双天线GNSS定位系统测航向,虽然测量精度高,但动态响应特性差,成本高[6-7];单天线GNSS定位系统可输出航向角度信息,但是随机噪声大,且速度越低,噪声越大[8-11];陀螺仪测航向一方面需要航向角度初始化,另一方面随机漂移误差会出现累积发散现象[ 12-15]。上述几种测量方法测得的航向角,均不能很好地满足车辆自动驾驶的精度要求。
本文采用单天线GNSS定位和微电子机械系统(Micro electro mechanical system, MEMS)陀螺仪相结合的方式,通过融合算法实现车辆航向角的测量。提出基于卡尔曼滤波器的车辆航向角估计模型,把实时动态–全球导航卫星系统(Real time kinematic-GNSS,RTK-GNSS)测量出来的经纬度和高程经过高斯投影变换为导航平面坐标,与陀螺仪测量的车辆角速度经过积分得出的航向角做融合处理,得到更为精准的航向角。该方法克服了GNSS因更新频率低出现的数据延迟和MEMS陀螺仪因随机漂移引起的累积误差等问题,不仅能够得到更为精准的航向角数据,还能保证良好的实时性。
1 传感器组合本文测量车辆航向角采用的导航传感器主要有Trimble®BD970 GNSS嵌入式板卡和内置于Xsens MTi-300微型姿态参考系统的MEMS陀螺仪。
1.1 Trimble®BD970 GNSS嵌入式板卡Trimble®BD970 GNSS嵌入式板卡是一款紧凑型的多星接收机板卡,专为满足各种精确到cm级的定位精度应用需求而设计。系统模块不仅支持GPS L1/L2、L2C、L5,而且支持GLONASS L1/L2 信号在内的各种卫星信号。该板卡易于集成且坚固可靠,支持因特网、USB、RS232 和CAN 等多种接口,串口输出波特率最高达115 200 bps,可实现高达50 Hz的原始测量与定位输出。基准站输出支持CMR、CMR+、RTCM 2.1、2.2、2.3、3.0、3.1等协议格式;定位数据输出支持ASCII:NMEA-0183 GSV、AVR、RMC、HDT、VGK、VHD、ROT、GGK、GGA、GSA、ZDA、VTG、GST、PJT、PJK、BPQ、GLL、GRS、GBS以及二进制:TrimbleGSOF。低延时RTK定位模式的水平定位精度可达±(8 mm+1 ppm)RMS,垂直定位精度可达±(15 mm+1 ppm)RMS,延迟时间小于20 ms,最大输出频率50 Hz。GNSS板卡物理特性如下,尺寸:100 mm×60 mm×11.6 mm;电源:3.3 V DC(−3%~5%);典型功耗:1.4 W (L1/L2 GPS)或1.5 W (L1/L2 GPS和G1/G2 GLONASS);质量:62 g;连接器I/O:24排针转接口和6排针转接口;天线:MMCX插座;工作温度:−40~75 ℃;储存温度:−55~85 ℃;振动限值:随机8 g RMS。
1.2 Xsens MTi-300微型姿态航向参考系统Xsens Technologies B.V.公司研发的MTi-300微型姿态航向参考系统内部包括:3D速率陀螺、3D加速度计和3D磁场感应计。运行于DSP上的卡尔曼滤波算法融合上述传感器信息,给出运动载体的精确3D姿态角度[8]。系统通过RS232接口按设定格式输出3D姿态角度。MTi内置的3D速率陀螺的测量范围可达±300°/s,零偏稳定性为1°/s,随机游走系数0.05°/(s·Hz),校准误差0.1°,带宽40 Hz,A/D分辨率16位,更新速率最大为120 Hz。本文利用3D速率陀螺中的Z轴陀螺实现车辆航向角的角速率累积测量。
2 航向角估计算法设计 2.1 GNSS定位数据预处理GNSS定位数据预处理主要将Trimble®BD970 GNSS板卡测量得到的WGS-84大地坐标系的经度、纬度和高程向大地导航坐标系转换,通过高斯投影将WGS-84大地坐标转换为与WGS-84椭球对应的高斯平面坐标,这种转换是为了使GNSS板卡输出的WGS-84大地坐标定位数据能够用于车辆的导航控制系统。
本文使用的Gauss-Kruger投影坐标系的主要参数[12]包括:中央经线为114.000 000 (3度带);水平偏移量为500 km;地理坐标系为GCS_WGS_1984;大地参照系为D_WGS_1984;参考椭球体为WGS_1984;椭球长轴为6 378 137.000 000;椭球扁率为0.003 352 810 7。
2.2 卡尔曼滤波器设计设定k时刻车辆本体的真实航向角度为ψk',车辆本体的真实前进速度是vk',则
$ \psi _{{k}}' = {\psi _{{k}}} + {\varepsilon _{{{\psi k}}}} + {\xi _{{{\psi k}}}}\text{,} $ | (1) |
$ v_{{k}}' = {v_{{k}}} + {\xi _{{{vk}}}}\text{,} $ | (2) |
式中,ψk、vk分别指航向角度、前进速度的测量值,εψk指航向角度的测量偏差值,ξψk、ξvk分别指航向角度和前进速度的随机测量误差。
基于航位推算原理,建立车辆导航控制点在2D平面坐标系下的运动方程:
$ {x_{{{ck}}}} = {x_{{{ck}} - 1}} + v_{{k}}' \cos \theta _{{k}}' {\rm{d}}t\text{,} $ | (3) |
$ {y_{{{ck}}}} = {y_{{{ck}} - 1}} + v_{{k}}' \sin \theta _{{k}}' {\rm{d}}t\text{,} $ | (4) |
式中,xck、yck为k时刻车辆本体的高斯投影平面坐标,xck−1、yck−1为k−1时刻车辆本体的高斯投影平面坐标,dt为航位推算的时间间隔。
将(1)和(2)式代入上述表达式,得到:
$ \begin{split} {x_{{ck}}} = & {x_{{{ck}} - 1}} + {v_{{k}}}\cos {\psi _{{k}}}{\rm{d}}t + {\varepsilon _{{{\psi k}}}}\cos {\psi _{{k}}}{\rm{d}}t - \\ &{v_{{k}}}{\varepsilon _{{{\psi k}}}}\sin {\psi _{{k}}}{\rm{d}}t\text{,} \end{split} $ | (5) |
$\begin{split} {y_{ck}} =& {y_{{{ck}} - 1}} + {v_{{k}}}{\rm{sin}}{\psi _k}{\rm{d}}t + {\varepsilon _{\psi {{k}}}}\sin {\psi _{{k}}}{\rm{d}}t + \\ &{v_k}{\varepsilon _{\psi k}}\cos {\psi _k}{\rm{d}}t\text{。} \end{split} $ | (6) |
将上述等式以卡尔曼滤波器状态转移方程的形式表示为:
$ {{{X}}_{{k}}} = {{{A}}_{{k}}}{{{X}}_{{{k}} - 1}} + {{{b}}_{{k}}} + {{{u}}_{{k}}}\text{,} $ | (7) |
式中,Xk=[xck,yck,εψk],表示k时刻的状态空间向量;Xk−1=[xck−1,yck−1,εψk−1],表示k−1时刻的状态空间向量;
$ {{{A}}_{{k}}} = \left[ {\begin{array}{*{20}{c}} 1&0&{\left( {{\rm{cos}}{\psi _{{k}}} - {v_{{k}}}\sin {\psi _{{k}}}} \right){\rm{d}}t}\\ 0&1&{\left( {{\rm{sin}}{\psi _{{k}}} + {v_{{k}}}\cos {\psi _{{k}}}} \right){\rm{d}}t}\\ 0&0&1 \end{array}} \right]\text{,} $ |
是k时刻状态转移矩阵,由陀螺仪累积航向角度和前进速度的测量值实时更新;
$ {{{b}}_{{k}}} = \left[ {\begin{array}{*{20}{c}} {{v_{{k}}}\cos {\psi _{{k}}}{\rm{d}}t}\\ {{v_{{k}}}\sin {\psi _{{k}}}{\rm{d}}t}\\ 0 \end{array}} \right]\text{,} $ |
$ {{{u}}_{{k}}} = \left[ {0,\;\;0,\;\;{\xi _{\psi {{k}}}}} \right]\text{,} $ |
是状态转移方程的白噪声序列;系统过程噪声协方差矩阵为Qk,表示状态转移方程的误差大小,本文中Qk设定为常数矩阵,在仿真和试验过程中整定矩阵参数。
以GNSS天线在大地导航坐标系下的定位坐标作为观测向量,得到卡尔曼滤波器的测量方程如下:
$ {{{Z}}_{{{gk}}}} = {H_k}{X_k} + {{{\nu}} _k} $ | (8) |
式中,
$ {{{Z}}_{{{gk}}}} = \left[ {\begin{array}{*{20}{c}} {{x_{{{gk}}}}}\\ {{y_{{{gk}}}}} \end{array}} \right]\text{,} $ |
$ {{{H}}_{{k}}} = \left[ {\begin{array}{*{20}{c}} 1&0&0\\ 0&1&0 \end{array}} \right]\text{,} $ |
$ {{{\nu}} _{{k}}} = \left[ {\begin{array}{*{20}{c}} {{\xi _{{{gxk}}}}}\\ {{\xi _{{{gyk}}}}} \end{array}} \right]\text{,} $ |
式中,xgk、ygk为GNSS天线处的定位坐标,Hk为卡尔曼滤波器k时刻的测量矩阵,ξgxk、ξgyk为OEM GNSS板卡定位在水平面坐标系下的随机定位误差。
测量向量的噪声方差矩阵为:
$ {{{R}}_k} = \left[ {\begin{array}{*{20}{c}} {{r_{xk}}^2}&0\\ 0&{{r_{yk}}^2} \end{array}} \right]\text{,} $ | (9) |
式中,rxk2、ryk2分别为ξgxk、ξgyk的方差统计值。
综合上述推导,采用线性离散卡尔曼滤波器的递归差分方程进行状态向量预测和测量向量校正:
预测方程组为:
$ {\hat x_k} = {{{A}}_k}{\hat x_{k-1}} + {{{b}}_k}\text{,} $ | (10) |
$ {{{P}}_k} = {{{A}}_k}{{{P}}_{k - 1}}{{{A}}_k}^T + {{{Q}}_{k - 1}}\text{,} $ | (11) |
式中,
校正方程组为:
$ {{{K}}_k} = {{{P}}_k} {{{H}}_k}^T{({{{H}}_k}{{{P}}_k} {{{H}}_k}^T + {{{R}}_k})^{ - 1}}\text{,} $ | (12) |
$ {\hat x'_k} = {\hat x_k} + {{{K}}_k}\left( {{{{Z}}_{gk}} - {{{K}}_k}{{\hat x}_k}} \right)\text{,} $ | (13) |
$ {{{P}}_k} = \left( {I - {{{K}}_k}{{{H}}_k}} \right){{{P}}_k} \text{。} $ | (14) |
式中,Kk为k时刻的卡尔曼滤波增益,
采用GNSS板卡和MEMS陀螺仪在轮式拖拉机平台上进行原始数据采集。GNSS天线安装于车辆后轮轴中心点的正上方。内置MEMS陀螺仪的MTi尽可能安装于车辆质心位置处,以减少车辆颠簸晃动对MTi的干扰。
轮式拖拉机的行驶路线分直线型和S型2种情况。用C++编程语言开发卡尔曼滤波器和原始数据仿真测试的程序。最后将测得的数据以文本文件的方式导入Matlab程序中,测得的曲线图如图1、2所示。
图1为拖拉机直线行驶时,GNSS、陀螺仪和卡尔曼滤波融合后得到的3条航向角度对比曲线。GNSS航向误差幅度超过5°,陀螺仪累积航向的偏移在300 s左右超过2°,融合后的航向角度都在38°左右,偏移不超过1°,较原始GNSS航向角度的精度提高80%以上。
图2为拖拉机以S型轨迹行驶时,GNSS、陀螺仪和卡尔曼滤波融合后得到的3条航向角度对比曲线。融合后的航向角度可以跟踪拖拉机180°换向的转弯动作,曲线既保持了GNSS航向的整体变化趋势,也较GNSS和陀螺仪所得结果更为平滑,符合拖拉机实际运动状态。
从图1、2中可看出,未经处理的陀螺仪累积航向角度和GNSS定位测量的航向角度有较大波动,经卡尔曼滤波融合后,有效抑制了陀螺仪累积航向的发散,减少了零偏和随机漂移带来的误差。融合后的航向角度曲线既保持了GNSS航向的整体变化趋势,也保持了陀螺仪航向的细部变化趋势,且较GNSS和陀螺仪所得曲线更为平滑。
4 结论本文采用卡尔曼滤波器对RTK-GNSS、MEMS陀螺仪所得的拖拉机航向角进行融合处理,得出了更为精确的航向角融合估计结果,仿真测试结果表明本文所用方法可用于在线测量拖拉机航向角。
RTK-GNSS航向角和MEMS陀螺仪累积航向角在采样频率方面,分别属于低频型和高频型;在误差特性方面,分别为零均值随机误差和偏移型缓变误差。2种传感器互补性强,研究结果表明多传感器融合的方法能够很好地弥补这2种传感器单独测量数据时存在的噪声误差。
[1] |
别韦苇, 卢飞腾, 胡进, 等. 基于多器件的航向角优化算法[C]// 2018中国汽车工程学会年会论文集. 上海: 中国汽车工程学会, 汽车环境保护技术分会. 2018: 104-107.
(0) |
[2] |
卢志才, 米东, 徐章遂, 等. 基于改进巨磁阻传感器的地磁定向系统[J]. 传感技术学报, 2011, 24(2): 304-307. DOI:10.3969/j.issn.1004-1699.2011.02.028 (0) |
[3] |
李秉玺. 基于系统芯片的捷联式定向系统研究[D]. 西安: 西北工业大学, 2004.
(0) |
[4] |
HE P, CARDOU P, DESBIENS A, et al. Estimating the orientation of a rigid body moving in space using inertial sensors[J]. Multibody Syst Dyn, 2015, 35(1): 63-89. DOI:10.1007/s11044-014-9425-8 (0) |
[5] |
LI Y, EFATMANESHNIK M, DEMPSTER A G. Attitude determination by integration of MEMS inertial sensors and GPS for autonomous agriculture applications[J]. GPS Solut, 2012, 16(1): 41-52. DOI:10.1007/s10291-011-0207-y (0) |
[6] |
刘永彪. 基于GNSS双天线的定向研究[D]. 南昌: 南昌大学, 2011.
(0) |
[7] |
夏佩, 王峰, 黄祖德, 等. 基于双天线的RTK-GNSS定向方法[J]. 中国新通信, 2018, 20(22): 58-60. DOI:10.3969/j.issn.1673-4866.2018.22.045 (0) |
[8] |
甘雨, 隋立芬, 戚国宾, 等. 陆地导航中GNSS/陀螺仪组合实时测姿方法[J]. 武汉大学学报(信息科学版), 2015, 40(3): 379-383. (0) |
[9] |
HE J, XIE J, HE X, et al. Analytical study and compensation for temperature drifts of a bulk silicon MEMS capacitive accelerometer[J]. Sens Actuators A Phys, 2016, 239: 174-184. DOI:10.1016/j.sna.2016.01.026 (0) |
[10] |
陈亮, 杨柳庆, 肖前贵. 基于梯度下降法和互补滤波的航向姿态参考系统[J]. 电子设计工程, 2016, 24(24): 38-41. (0) |
[11] |
姚卓. 基于双天线的车载GNSS/INS组合导航技术研究[D]. 武汉: 武汉大学, 2017.
(0) |
[12] |
张智刚, 罗锡文, 胡炼, 等. 4种DGPS模块动态定位精度测试与分析[J]. 华南农业大学学报, 2010, 31(1): 102-107. DOI:10.3969/j.issn.1001-411X.2010.01.025 (0) |
[13] |
MOUSAZADEH H. A technical review on navigation systems of agricultural autonomous off-road vehicles[J]. J Terramechanics, 2013, 50(3): 211-232. DOI:10.1016/j.jterra.2013.03.004 (0) |
[14] |
NAGASAKA Y, SAITO H, TAMAKI K, et al. An autonomous rice transplanter guided by global positioning system and inertial measurement unit[J]. J Field Robot, 2010, 26(6/7): 537-548. (0) |
[15] |
CECHOWICZ R. Bias drift estimation for MEMS gyroscope used in inertial navigation[J]. Acta Mechanica Et Automatica, 2017, 11(2): 104-110. DOI:10.1515/ama-2017-0016 (0) |