高小鹏
(西安精准测控有限责任公司,陕西 西安 710075)
捷联惯性导航系统(SINS)具有自主导航能力,不需要任何外界电磁信号就可以独立给出载体的姿态、速度和位置信息,抗外界干扰能力强。但是SINS定位误差随时间的延续不断增大,即误差积累、漂移大[1]。
全球导航卫星系统(GNSS)具有全球、全天候、高精度、实时定位等优点,它的误差不随时间积累,但是GPS存在信号容易被遮挡的缺陷,特别是当车辆行驶到高大建筑物下或者隧道里面等地方,车辆GPS接收机就接收不到卫星信号,从而使定位误差瞬间增大甚至不能获得定位信息。
航位推算(Dead Reckoning, DR)是一种自主定位技术,在提供给它确定的初始位置之后,通过陀螺仪和里程仪推算出车辆的瞬时位移增量,从而推算出该车辆当前的具体位置。但是该方法的定位误差会随着时间的积累而积累,时间过长将失去定位效果[2]。
因此,本文提出了一种基于惯导/卫星信号/里程仪组合的车载导航系统[3],可以充分发挥各自系统的优点,克服缺点,实现在高动态和短时强电子干扰的环境下实时、高精度的导航定位。
系统算法框架如图1所示。首先由IMU经过纯惯性解算得到车辆的位置+速度和姿态信息,里程仪通过惯导的姿态矩阵经过航位推算得到位置。在卫星信号有效时,将接收机与SINS各自输出的位置+速度作差构成位置+速度观测量,同时将里程计推算的位置与GPS的输出位置作差形成位置量测;在卫星信号无效时,将里程计推算的位置与SINS的输出位置作差形成位置量测。经过卡尔曼滤波后得到SINS的误差估计和里程计的误差估计,进而分别对SINS和里程计进行反馈校正。
图1 SINS/GPS/OD组合导航原理框架
本系统采取模块化设计,采用积木式搭建,生产维修便捷。首先把主要选取的元器件做一个简要介绍。主处理器选用意法半导体的STM32F767IGK6,该处理器自带DSP和FPU的高性能ARM Cortex-M7 MCU,具有1 MB FLASH、216 MHz CPU、ART加速器、一级缓存、SDRAM、TFT、JPEG编解码器和DFSDM,非常适合在空间体积受限,接口要求多,计算精度、速度、功耗都有一定要求的场合。IMU选用挪威SENSONOR公司的STIM300高性能MEMS惯性测量单元,其陀螺的零偏稳定性优于0.3( °)/h,加速度表的零偏稳定性优于50 μg,性能非常优越。采用RS 422接口,易于集成到新设计中。卫星接收机选用加拿大诺瓦泰公司的OEM718D,OEM718D采用555跟踪通道设计,支持所有GNSS系统,包括GPS、GLONASS、BEIDOU、Galileo。小尺寸外形设计、全新硬件平台、高抗干扰算法、支持双天线输入等,为用户提供厘米级差分定位和高精度航向测量,低功耗、重量轻、接口灵活等特点尤其适合各领域集成应用。里程计信号直接读取汽车总线的原始信息。其硬件结构如图2所示。
图2 SINS/GNSS/OD组合模块硬件结构
产品上电后微处理器自动进行端口配置,等待STIM300信息接入,对接收机及千寻4G模块进行自检,当产品各个模块自检完成后,连接4G网络,等待接收机差分定位完成之后跳转至正常工作模式。在正常工作模式下,以100 Hz的解算频率读取IMU输出的增量信息和卫星高精度差分定位定向信息。车载ODB接口输出的车载总线信息经过多信号融合算法,得出当前车辆的姿态、位置和速度信息,并通过DMA接口模块进行帧格式的打包及对外接口的DMA控制器数据发送。系统软件流程如图3所示。
图3 系统软件流程
惯性导航角速度关系及比力方程如下:
载体静止时ωn en和非常小可以忽略,考虑陀螺和加速度计测量误差远小于有用信号时,式(1)、式(2)分别改写为:
初始对准一般是在运载体相对静止的环境下进行的,那么重力矢量和地球自转角速度矢量在地理坐标系(初始对准参考坐标系)的分量如下:
式中:L,g和ωie分别表示当地纬度、重力加速度大小和地球自转角速率大小;且记地球自转角速度的北向分量ωN=ωiecosL和天向分量ωU=ωiesinL。
根据双矢量对准原理,选择(-gn)作为主参考矢量,可得姿态阵估计:
将式(5)代入式(6),得:
设载车的行驶速度为vD,它指向载车的正前方,写成矢量形式为vb=[0 vD0]T。因此,载车的速度在n系的表达形式为[4-5]:
将惯性导航失准角误差和航位推算误差合并在一起,同时考虑车体坐标系(m系)和IMU坐标系(b系)的俯仰角偏差αθ和方位角偏差αφ,里程仪刻度系数误差δkD,可以得到以速度位置误差作为观测量的惯导/卫星/里程仪的组合导航状态空间模型如下:
普通卡尔曼滤波是在线性高斯情况下利用最小均方误差准则获得目标估计,适应于过程和测量都属于线性系统,且误差符合高斯分布系统。但是实际上很多系统都存在一定的非线性,这种情况下就不能使用一般的卡尔曼滤波。扩展卡尔曼滤波模型可以实现非线性系统的近似线性化,将其转化为线性问题,可进一步提高求解精度。
实验载体是别克英朗小轿车,如图4所示。系统包括:车顶前后放置的2根GNSS天线(与高精度定位定向板卡配套,提供高精度GNSS RTK定位定向解算结果),车顶中间放置设备,后备箱盖上放置千寻4G天线,设备同时接入汽车OBD总线。
图4 实验设备安装图
为验证本文算法的效果,进行相应的跑车试验。车辆在城市内办公园区绕行1周,根据实际采集数据产生一条轨迹。
从实验结果图5和图6可以看出,GPS系统定位结果基本能够反映出车辆行驶的真实轨迹,但是定位曲线不够平滑,存在较多的野值点,甚至某些时刻没有定位。如图7所示,在仿真轨迹中选取一段城市建筑比较密集的地段进行局部放大,可以看出在该路段GPS定位结果中出现很多跳点,而采用惯导/卫星/里程仪组合导航系统的定位结果明显改善了这一现象,能够很好地跟踪行车轨迹,定位曲线更趋平滑。
图5 跑车实验路线
图6 GPS和组合后轨迹对比
图7 GPS和组合后轨迹对比局部放大图
为了进一步产品性能,用一套接收机板卡作为对比,在中途跑车进行中(1 920~2 117 s),人为拔掉惯组外接天线,对比纯惯性和惯导/里程计组合后位置误差。
从图8可以看出,在200 s连续丢星情况下惯组和里程仪组合定位曲线和卫星定位结果趋势一致,表明惯组和里程仪组合导航系统能有效抑制导航精度随时间的发散。由于里程计提供只是相对定位,从图9中看出组合后位置误差随着时间和距离增大趋势逐渐增大。从图10得出,定位精度为2.28/1 225=0.001 8,也就是0.18%,表明这种组合方式在人行天桥、高架、 隧道等场景和城市复杂环境时能提供较高精度的定位。
图8 GPS和组合后轨迹对比
图9 北向位置误差曲线
图10 总位置误差随行驶距离变化曲线
通过实验结果验证,在卫星信号遮挡或者丢失情况下,惯导/卫星/里程计组合导航系统能够平滑行车轨迹,较好地抑制惯导误差发散,并将其误差限制在较低的水平,提高该组合导航系统的精度与可靠性。