RT3000惯性GPS组合导航系统实现车辆运动高精度测量

（江苏省交通技师学院，江苏 镇江 212006）

RT3000惯性GPS组合导航系统实现车辆运动高精度测量

刘 斌

（江苏省交通技师学院，江苏 镇江 212006）

本文以RT3000惯性GPS组合导航系统为研究对象，进行了组合导航的技术介绍，介绍了SINS（捷联惯性）/GPS组合导航系统工作原理，采用四元数法进行姿态描述，通过捷联惯性导航计算导航参数，利用卡尔曼滤波进行修正。通过实验发现，在这些技术的支持下，SINS/GPS组合导航系统实现地面车辆的精确导航。

RT3000；SINSGPS组合导航；四元数；卡尔曼滤波

1 惯性/GPS组合导航技术

为了获得移动载体的实时位置和姿态信息，已经提出和采用了多种导航方式。其中，以惯性导航系统（Inertial Navigation System，INS）和全球卫星导航系统（以Global Positioning System，GPS为典型代表）应用最为广泛。INS不仅能够提供载体位置速度参数，还能提供载体的三维姿态参数，是完全自主的导航方式,在航空、航天、航海和陆地等几乎所有领域中都得到了广泛应用。

随着惯性技术与卫星导航定位技术的发展，由GPS/INS不同程度组合而成的定位定姿传感器已成为移动测图系统中确定载体轨迹和平台姿态的重要工具，其中GPS多用于定位而INS则用于测姿。随着城市建设的飞速发展和人民生活水平的日益提高，车辆在人们的生活中起着越来越重要的作用，其发展速度也越来越快。因此，如何有效的对其指挥和管理己成为交通运输和安全管理等部门面临的一个重要问题。由惯性/GPS组成的车载导航系统有着广泛的应用前景。本文重在介绍RT3000惯性GPS组合导航系统（以下简称RT3000）以及内部先进技术。

2 RT300简介

RT3000由Oxford Technical Solutions公司研发，目的是实时地对车辆，飞机和船只等的运动做高精度的测量。

为了获得高精度的测量，RT使用了为战斗机导航系统开发的数学算法。一个由三个加速度计和三个陀螺仪（角速度传感器）组成的惯性传感器组块用来计算所有的输出。当从高级的运动GPS获得的测量数据刷新由惯性传感器导航的位移和速度时，一个WGS-84型捆绑导航算法用来补偿地面的弯曲，旋转和科里奥利加速。采用捷联惯性（SINS）导航方式。

图1 RT300

图2 RT300内部组成

这个独创性的方案使RT3000比只使用GPS的系统具有了一些独特的优势：（1）RT3000有一个很高（100Hz）的刷新率和很宽的带宽；（2）输出可以被访问的反应时间很低，只有3.9ms；（3）所有的输出可以持续被访问，即使GPS中断期间，例如，车辆在桥下行驶时；（4）RT3000可 以辨识GPS位置的跳动并忽略它；（5）GPS进行的位置和速度测量中的高频噪声可以被消除；（6）RT3000可以进行许多GPS不能作的测量，例如，加速度，角速率，航向，俯仰，侧倾等。

图3 RT3000工作原理图

图4 捷联力学编排计算流程

和常规的惯性导航系统不同，RT3000用GPS校正所有的测量值。GPS测量位置，速度和（双天线）航向，但是，通过这些测量，RT3000可以使其它量，如俯仰，侧倾非常精确。当GPS起作用时RT3000进行测量时就没有漂移。

标准的RT3000系统可以实时处理数据。实时结果可以通过RS232串口输出，通过10/1000Base-T以太网通过一个UDP无线电广播，和一个选项，CAN总线。输出带有时间的印记并参考GPS时间，1PPS时间同步器可以给不同系统提供很精确的时间同步，惯性测量和GPS时钟同步。内部数据日志使数据可以在任务后再处理。数据可以在盒子里收集，用“ftp”下载，在计算机上处理，也可以通过用户软件查看。

3 RT3000的SINSGPS组合导航系统工作原理

单纯的GPS技术是无法满足汽车导航需要的，因此，必须采用其它技术与GPS进行组合，应用信息融合技术对导航信息进行处理，获得具有高精度和高可靠性的导航信息。而低成本IMU的陀螺仪精度不足以感受地球自转角速率，无法获得航向信息，只能引入外部航向信息，利用GPS所测速度进行匹配对准，或引入其它方位测量方法，如磁航向或双天线GPS定向系统等来获得初始航向信息。

SINS（捷联惯性系统）也就是将惯性敏感元件（陀螺仪和加速度计）直接“捆绑”在运载体的机体上，从而完成制导和导航任务的系统。在捷联式惯导中，惯性平台的功能由计算机完成，故有时也称作“数学平台”，它的姿态数据是通过计算得到的。

图5 RT3000的捷联导航系统

IMU测得机体相对于惯性空间的角速度和加速度，再通过捷联式惯导（SINS）力学编排算法，实时计算导航信息，然后利用SINS算法得到导航参数。卡尔曼滤波对误差进行估计出并校正系统，DSP（数字信号处理器）包含高精度的校正矩阵，可以进行数字抗锯齿滤波器和圆锥积分/划船效应补偿算法，保证了测量精度精确到0.01°。其工作原理如图3。

4.1 四元数法的姿态描述

姿态描述的四种方法:欧拉角、欧拉轴旋转参数、方向余弦和四元数法。RT使用四元数微分方程，消除了欧拉方程的奇异性，修正姿态组合信息的数值漂移。计算效率也远远优于其他三种方法。姿态描述最常用的方法是欧拉角。非惯性坐标系相对于惯性坐标系可以用三个欧拉角表示：ψ、θ和φ,分别表示方位角、俯仰角和倾斜角。将惯性坐标系按绕相应的轴依次旋转ψ、θ和φ,可使惯性坐标系与非惯性坐标系重合。,但是欧拉角表示两个坐标系之间的关系,当俯仰角为±90°时,描述姿态角与角速度关系的运动学方程的系数矩阵出现奇异。避免出现这种奇异情况发生的有效办法就是在座标变换中用四元数代替欧拉角。

4 RT3000内部技术

这四个参数就是著名的Euler-Rodrigues对称参数,又称四元数。推导得出任一向量v在体轴系中的分量与在地轴系中的分量关系见公式2：

该方程避免了欧拉方程的奇异性。四元数的另外一个优点是便于采用卡尔曼滤波实现四元数最优估计,也正是基于这个原因TR3000采用四元数进行姿态控制。

4.2 SINS（捷联惯性系统）的计算

捷联式惯性导航系统在开始导航之前，必须进行初始对准，也就是确定导航计算的初始条件。捷联式系统数学平台的水平基准是计算机根据加速度计所测量到的重力加速度水平分量用数学计算方法确定。在完成水平基准的确定以后，根据陀螺仪跟随地球转动所测量的信息，利用与平台式惯导系统计算罗经法相同的关系确定出数学平台所处的方位，也就完成了捷联式惯导系统的初始对准。

RT3000直接获得载体在地心直角坐标系（如WGS84）中的三维直角坐标（X,Y,Z），直接用于修正INS，适合与GPS数据进行组合处理，且省去了反复的坐标变换，程序更简洁和模块化。

SINS力学编排即利用陀螺仪所得旋转角速度计算出b系至e系坐标变换矩阵；利用该矩阵将所测比力变换至e系并两次积分得速度增量。计算分两个环路：1）角速度积分环路：将测得的b系对i系的旋转角速度，减去e系对i系的地球自转角速度，得所需的b系对e系的旋转角速度；利用该角速度按四元数积分法修正b系至e系坐标变换矩阵。2）加速度积分环路：用上述所得坐标变换矩阵，将比力观测值转换至e系，并从中补偿掉重力加速度和科里奥里加速度；对转换并补偿后所得比力值进行两次积分可得位置增量,由此可提取出载体的姿态速度与位置信息。如图4所示

图6 输出校正与反馈校正示意图

图7 倾斜测试路面侧倾和俯仰角度

对于测量出有误差角速度，RT3000先进行偏差修正，再进行比例因子的修正。为避免地球每分钟0.25°的旋转需要减去地球自转速率。受地球自传速度的影响，物体运动的水平速度需按正比例进行修正。通过四元数法角速度和卡尔曼滤波将整合产生方位角、俯仰角和倾斜角度。（采用RT可以工作在任何角度下，不会产生任何奇异。）地球的自转给坐标系方向上加速度，通过偏差修正再通过旋转矩阵，（利用卡尔曼滤波）加速度的误差修正，使得重力场消失并且消除科氏加速度的影响，从而整合加速度产生速度值，进而得到位移值。RT3000的捷联导航系统见图5。

4.3 卡尔曼滤波的修正

在组合导航系统，利用卡尔曼滤波进行导航参数估计，可较好实现测量数据的混合。根据滤波器状态选取的不同，估计方法分直接法和间接法两种。直接法直接以各种导航参数X为主要状态，滤波器估值的主要部分就是导航参数估值间接法以组合导航系统中某一种导航系统（经常采用惯导系统）输出的导航参数X1的误差ΔX为滤波器主要状态，滤波器估值的主要部分是导航参数误差估值用去校正X1。

利用直接法进行估计时，惯导的系统方程和量测方程是非线性的。利用间接法进行估计时，系统方程中的主要部分是导航参数误差方程，由于这种方法的误差很小，通常可忽略二阶小量，所以间接法的系统方程和量测方程一般都是线性的，故而组合导航系统的综合滤波器常采用间接法。利用状态估值 Xˆ

Δ 对系统进行校正也有两种方法，输出校正和反馈校正的原理框图如图6所示。

5 RT3000倾斜路面的实车测试

车辆在有一定坡度或倾斜的测试路面测量得到的侧倾角、俯仰角、车身加速度和滑动角有些反常。对俯仰角和侧倾角的影响最明显。这些反常的测试值，必须加以补偿修正，以满足正常行驶需要。

倾斜测试路面上坡部分俯仰角是正的，下坡部分是负的。同样的问题出现在侧倾角上，车辆在坡顶处为负的，在坡底处为正的。如图7所示。

利用RT3000中产生新的“俯仰角平面”和“侧倾角平面”通过CAN总线（CANbus）技术得以输出，补偿了测试路面面的倾斜角。Configuration Software（组态软件）用来测量路面的倾斜角度，并将数据记录到测试系统中，从而也可对路面不平度进行统计测量。

补偿路面倾角后，RT3000输出的相对新的“俯仰角平面”和“侧倾角平面”的侧倾角和俯仰角，而不是相对于重力的。但是水平方向的侧倾角和俯仰角的真值仍然正常输出，不受影响。图8给出了车辆在循环加速测试下，对其侧倾角修正的效果图。俯仰角也是同样变化。可见通过RT3000的修正，提升了测量的精度。更好了满足精确导航的需要。

结语

本文进行了组合导航的技术介绍，针对RT3000分析了其内部先进的技术：采用四元数法进行姿态描述，通过捷联惯性导航计算导航参数，利用卡尔曼滤波进行修正。在这些先进技术的支持下，确保了RT3000测量的精确性，以更好的满足导航的需要。并且通过倾斜路面的实车测试，发现RT3000能补偿了道路倾斜角度，提高了测量的精度。

[1]RT3000产品手册[M]．Oxford Technical Solutions， 2006．

[2]John Miles．Roll and Pitch on inclines[J]．Vehicle Dynamics International，2008．

[3]张海燕．惯性/GPS/GLONASS组合导航系统在地面车辆上的应用[J]．2001年飞行器惯性器件学术交流会论文集，2011．

[4]丁文娟，李岁劳，熊伟．捷联惯导系统/里程计自主式车载组合导航系统研究[J]．计测技术，2006．

[5]钟乐，胡延霖，陈永明，王翌臣．一种基于DSP的SINS/GPS组合导航系统实现[J]．舰船电子工程，2011（02）．

图8 侧倾角的修正比较

V249 < class="emphasis_bold"> 文献标识码：A

A