组合定位导航技术

组合定位导航技术（共9篇）

组合定位导航技术 篇1

1. 引言

智能交通系统 (ITS) 已被公认为解决消防部队在突发事故发生时如何快速抵达事故现场问题的有效途径, 它是在关键基础理论研究的前提下, 将先进的信息技术、数据通信技术及电子控制技术等有效地综合运用于地面交通运输体系, 从而建立起一种大范围、全方位发挥作用、实时、准确、高效的交通运输系统。

车辆定位导航技术是ITS中的关键技术之一。车辆导航定位系统的首要功能是能够提供车辆的位置、速度和航向等信息, 而精确、可靠的车辆定位则是实现导航功能的前提和基础。常用的车辆定位技术主要有:航位推算技术 (DR) 、卫星定位技术 (GPS) 、惯性导航技术 (INS) 、地图匹配技术 (MM) 等等。由于基于任何一个单独的定位技术的系统都有本身无法克服的短处, 因此出现了组合导航系统。本文根据智能交通系统的特点, 提出了GPS、航位推算技术与地图匹配技术相结合的组合导航系统。

2. GPS定位技术

全球定位系统 (Global Positioning System-GPS) [1]是当前全球定位系统中技术最成熟, 应用也最为广泛的系统。它可以全天候连续为全球范围陆、海、空军民用户提供定位导航信息, 用户设备的定位精度优于20m, 时间准确度达到ns量级。具有全天候, 定位迅速, 精度高, 可连续提供三维位置 (精度、纬度和高度) 、三维速度和时间信息等一系列优点[2], 主要应用于单点导航定位与相对测地定位两个方面, 是当今车辆定位导航的主流。

GPS系统包括三大部分:

(1) 空间部分——GPS卫星星座

由24颗在轨卫星和3颗备份卫星组成, 部署在高达20200km的轨道上, 在地球上和近地空间任何一点均可连续同步地观测4颗以上卫星, 从而实现全球、全天候连续导航定位。

GPS的空间卫星星座如图1所示:

(2) 地面控制部分——地面监控系统

地面控制部分是整个系统的中枢, 由美国国防部管理, 它包括1个主控站, 5个监控站。主控站负责对地面监控站的全面控制。监控站内装备有接收机、原子钟、气象传感器及数据处理计算机, 其任务是追踪及预测GPS卫星轨道, 控制GPS卫星状态及轨迹偏差, 维护GPS系统的正常运作。

(3) 用户设备部分——GPS信号接收机

用户部分则是适用于各种用途的GPS接收机, 其主要功能是接收GPS卫星播发的定位信息, GPS用户接收机是由主机、电源和天线组成。主机的核心部件是信道电路、基带处理电路和中央处理器, 在专用软件的控制下, 进行作业卫星选择、数据搜集、加工、传输、处理和存储, 其天线则接收来自各方位的导航卫星信号。GPS接收机接收到从卫星传来的连续不断的编码信号后, 再根据这些编码辨认相关的卫星, 从导航电文中获取卫星的位置和时间, 然后计算出接收机 (即用户) 所在的准确地理位置。

三者的关系如图2所示:

GPS导航利用GPS模块接受导航卫星信号, 然后计算出汽车的经纬度、速度、行驶方向、时间等信息, 它具有全球性、全天候、低成本、高精度、实时三维的测定位置和速度的能力, 因而有很大的优势。

但是, GPS导航也有其本身所固有的弱点[3], 主要是非自主性、易受干扰、动态性能较差, 卫星信号因在有些地方受遮挡会导致丢失信号而影响定位, 定位精度容易受电子欺骗等因素影响。更致命的是城区内地物特征复杂, 当卫星信号被树木、城市高层建筑、隧道和桥梁等遮挡或GPS接收机接收不到四颗及以上的卫星信号时, GPS导航系统便不能提供连续导航信息, 其定位误差将增大, 甚至可能出现不定位的现象。

单一的卫星定位方式由于可能会受到使用环境的影响, 不能很好的完成定位功能, 但是即使是使用卫星定位系统之间的组合定位:如GPS与GLONASS (俄罗斯的“格拉纳斯”卫星定位导航系统) 之间的组合而成的GNSS, 以及GPS, GLONASS与欧盟的伽利略卫星的组合定位, 这些组合定位技术都提高了定位的可靠性和精度。但是在高架、立交、停车场等卫星定位号接收不良的情况下, 定位精度仍然难以保证。并不能从根本上解决卫星定位系统的固有缺陷。

3. INS (Inertial Navigation System惯性导航系统) 技术

惯性导航系统 (INS) 是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统。其工作环境不仅包括空中、地面, 还可以在水下。其基本工作原理是以牛顿力学定律为基础, 通过测量载体在惯性参考系的加速度, 将它对时间进行积分, 且把它变换到导航坐标系中, 就能够得到在导航坐标系中的速度、偏航角和位置等信息。在现代航空、航天和航海载体的导航系统中, 惯性导航系统是技术相对成熟、应用最为广泛的一种导航设备之一。

惯性导航系统是一种完全自主式的导航系统, 导航过程中不向外辐射电磁信号, 和外界不发生任何光、电的联系, 因此有很好的隐蔽性和强大的抗干扰能力, 工作不受气象条件限制, 可全天侯、全球工作与空中、地球表面乃至水下。惯性导航系统能同时输出位置、速度、姿态、加速度和角速度等导航信息, 所产生的导航信息连续性好而且噪声低, 可单独完成导航功能。同时具有数据更新率高、短期精度和稳定性好等优点。

但是, 惯性导航系统不能给出时间信息, 又由于导航信息经过积分而产生, 定位误差会随时间而增大, 长期工作精度变差, 因此难以长时间独立工作。而且每次使用之前需要较长的初始对准时间。另外, 惯性测量设备的价格高昂, 这也是导致它在车辆导航系统中的运用受到很大的限制。

4. DR (Dead Reckoning航位推算) 技术

航位推算的现代定义是由Cotter提出的, 其定义“为从一己知的坐标位置开始, 根据航行体 (船只、飞机、陆地车辆等) 在该点的航向、航速和航行时间, 推算下一时刻坐标位置的导航过程就称为航位推算”。DR基本原理是利用方向和速度传感器来推算车辆的位置。由于车辆的运动可以看成是二维平面上的运动, 因此如果知道车辆的起始位置和起始方位角, 通过实时的测量车辆的行驶距离和航向角度的变化, 就可以实时的推算车辆的位置。

DR导航技术完全自主, 既不发射信号, 也不接收信号, 不存在电磁波传播问题, 成本低, 只需利用自身的测量元件的观测量, 推求位置、速度等导航参数, 不受外界环境及其它政策性人为因素的影响, 在短时间内能保持较高的精度。且机动灵活, 无论是涵洞还是水下, 只要载体 (车、船、飞机、潜艇) 能够到达的地方就能导航定位。

但是DR系统只能确定相对位置。且误差随时间的延长而积累, 导致定位误差随时间延长而迅速增长的问题, 因此DR方法不能单独、长时间地使用, 而常常作为一种辅助的定位技术得到应用。

5. 组合定位技术

每一种定位技术各自都有其优点和特色, 但也存在固有的不足, 其精度及可靠性都有一定的限制。将各种定位技术综合起来, 组成组合导航系统, 将能达到取长补短、综合发挥各种导航系统特点的目的, 并能提高导航信息精度, 更好地满足载体对导航系统的要求。并且, 组合后的系统具有冗余功能, 增加了导航系统的可靠性。

在导航应用领域中, 一般采用全球定位系统 (GPS) 和惯性导航系统 (INS) 的组合导航[4], 它们能分别独立地获取信息, 并快速推算出地理位置信息。然而, 价格昂贵制约了高精度INS的使用。高性能INS通常用于军事和民航领域, 而不适合车辆导航这类一般的应用领域。

GPS/DR车载组合导航[5]是一种很好的解决方案, GPS与DR存在很强的互补关系, 一方面, GPS可以为DR提供推算定位所需的初始点的绝对位置信息, 并进行误差校正, 避免DR信息因传感器的漂移和噪声而产生的误差积累。另一方面, DR的推算结果可以弥补GPS信息在短期内因受高楼、树荫阻挡而无法正常定位的缺陷, 用于补偿部分GPS定位中的随机误差, 平滑定位轨迹。另外, GPS/DR组合导航方式性价比高, 组成实用的车辆导航系统, 因而在民用低成本车载导航系统中广为采用。

GPS/DR车载组合导航采用以GPS定位为主、航位推算为辅的组合导航定位方式可以很好的保证车辆定位连续性和可靠性。利用微处理器将各个传感器有机的结合在一起, 并利用最优估计理论与方法进行多种导航信息的综合处理, 计算出精确的位置信息。整个组合导航系统在工作时, 导航计算机同时接收来自GPS定位系统和DR航位推算系统的数据, 根据组合导航系统的数学模型进行两种定位结果的信息融合, 得到最优的定位结果, 从而获得最好的定位精度。当GPS信号丢失, 无法正常工作时, 能够利用DR系统的自主定位结果, 以维持正常导航。此外, 当GPS定位由于可见星少于四颗而定位精度较低时, 还可以利用DR系统在一定的距离内的较高精度来改善GPS的定位精度。

GPS/DR组合导航系统原理如图3所示:

但是, 这种车载组合导航系统也有自己的局限, 主要反映在:

(1) 它用里程计采集位移信号, 这需要改动汽车电气线路, 并且汽车型号不一样, 里程计感应元件的参数也不一样, 安装不方便, 并且导航精度受里程计的精度限制。

(2) 长时间接收不到GPS的情况下, 单靠这种DR系统积累的误差将会很大, 方位信息已经不准确, 不能正确的导航。

(3) 汽车行驶期间, 不能感应汽车的姿态, 汽车机动模型是建立在水平面的假设基础上的, 而实际情况却不是这样, 当汽车行驶在坡度路面上时, 这样推算出来的结果必然和实际情况有误差。

(4) 微处理器运算速度慢, 不能实时的输出航位信息, 有滞后现象。

6. 地图匹配 (Map Matching) 技术

GPS和航位推算法 (DR) 系统的组合导航虽然能够提高导航系统的精度和提高组合导航系统的可靠性, 但导航数据仍然存在一定的误差, 并且在GPS信号长期丢失的条件下, DR系统的误差也会因为长时间得不到校正而积累变大。而移动目标的精确定位正是系统的关键, 这就要求人为对其进行校正, 在实际系统中通常采用地图匹配方法[6]来提高DR和GPS系统的精度。地图匹配方法是借助地理信息系统数据库存储的高精度道路数据来提高车载导航系统的定位精度, 使导航数据和道路数据相一致。地图匹配功能在现代车辆定位与导航系统中起着重要作用, 它能使系统的定位功能更加准确可靠, 从而为导航功能的实现提供良好的基础。

地图匹配是一种基于软件技术的定位修正方法, 其基本思想是将车辆定位轨迹与数字地图中的道路网信息联系起来, 并由此相对于地图确定车辆的位置, 原理参见图4。

地图匹配应用是基于以下两个假设条件:

(1) 车辆总是行驶在道路上。

(2) 采用的道路数据精度要高于车载定位导航系统的定位精度。

当上述条件满足时, 就可以把定位数据和车辆运行轨迹同数字化地图所提供的道路位置信息相比较, 通过适当的匹配过程确定出车辆最可能的行驶路段以及车辆在该路段中的最大可能位置。如果上述假设不成立, 则地图匹配将产生错误的位置输出, 并可能导致系统性能的严重下降。一般认为用于匹配的数字地图误差不应超过15米 (真实地面距离) 。由于陆地车辆在除进入停车场等之外的绝大多数时间内都位于公路网络中, 因此使用地图匹配技术的条件是满足的。

7. 小结

本文对组合导航技术在智能交通系统中的应用应用作了初步的探讨。介绍了几种常用的导航定位技术, GPS定位、惯性导航技术与航位推算技术。这几种技术都有自身的优势与不足, 而这GPS和航位推算技术的优势与不足是互补的, 根据两者的互补性提出了两种技术结合的组合导航技术, 并在此基础了讨论了进一步通过地图匹配技术来提高导航精度。

参考文献

[1]胡风珍.汽车GPS卫星导航全球定位系统技术研究与应用[J].石油仪器, 2007, 21 (4) :46~48.

[2]G.Mintsis, S.Basbas, P.Papaioannou.etc al, Applications of GPStechnology in the land transportation system[J], European Journal ofOperational Research.2004, 152 (2) :399~409

[3]Elliott, D.Kaplan著.邱致和, 王万义译.GPS原理与应用[M].北京:电子工业出版社, 2002.

[4]陈红英, 罗文田.GPS/INS组合导航系统的鲁棒滤波研究[J].中国民航飞行学院学报, 2006, 17 (2) :35~38.

[5]赵超凡, 付梦印, 张继伟.低成本组合导航系统滤波算法的研究[J].微计算机信息, 2007, 23 (4) :215~216.

[6]孙棣华, 王春丽.基于模糊模式识别的车辆定位地图匹配算法[J].计算机工程与应用, 2007, 57 (25) :227~230.

组合定位导航技术 篇2

北斗/罗兰组合导航系统中伪距导航定位解算新算法研究

为提高北斗/罗兰组合导航系统伪距导航定位解算的.精度和改善对动态目标的实时跟踪,本文提出了一种新方法,采用镜像映射法解具有病态的矛盾方程组以提高精度,以及将伪距导航定位解算方程模型转换为具有贯序输入输出数据的系统辨识模型,然后用递推最小二乘法,对所求的北斗/罗兰组合导航系统伪距导航定位信息-接收机的三维位置参数和时钟误差参数进行动态快速实时估计.

作 者：王明福 王仕成 罗大成 张安京 WANG Ming-fu WANG Shi-cheng LUO Da-cheng ZHANG An-jing 作者单位：第二炮兵工程学院301教研室,西安,710025刊 名：电光与控制 ISTIC PKU英文刊名：ELECTRONICS OPTICS & CONTROL年，卷(期)：13(4)分类号：V474.2关键词：北斗/罗兰组合导航系统 伪距 实时 镜像影射 递推最小二乘法

一种组合星座定位的导航终端设计 篇3

关键词：北斗二代导航卫星系统,全球定位系统,组合星座,导航终端

0引言

北斗卫星导航系统是新兴的卫星导航系统,也是世界上继全球定位系统(GPS)、全球导航卫星系统(GLONASS)之后的第3个卫星导航系统,具有高精度、实时和全天候等特点。目前,北斗二代导航系统(COMPASS)共有12颗卫星[1],在局部地区已经能够正常使用。计划2020年发射卫星数量达到35颗,使用范围可以覆盖全球。

采用组合星座定位形式的导航终端既可以实现COMPASS单星座定位和GPS单星座定位[2],又可以实现COMPASS和GPS组合星座定位,大大提高了定位终端的可靠性和可用性。

1总体设计

1.1导航终端的总体框架

导航终端由北斗基带处理单元、GPS基带处理单元、定位解算单元和显示单元等3部分组成,其总体结构如图1所示。

在接收卫星信号良好的条件下,北斗基带处理单元和GPS基带处理单元分别将各自的原始观测量发送给定位结算单元,由定位解算单元统一处理后,将可用的定位结果上报显示单元。

1.2定位解算单元

如图1所示,定位解算单元由DSP实现。DSP的设计采用C语言描述,选用TI系列器件TMS320C6747ZKB[3]。

DSP设计由北斗接收模块、GPS接收模块、串口发送模块、控制模块、输入数据缓存单元、解算模块和输出数据缓存单元等模块组成,如图2所示。

工作过程为:北斗接收模块接收到北斗基带处理单元串口数据发来的信号,GPS接收模块接收到GPS基带处理单元串口数据发来的信号,控制模块检测上述2种信号语句开始和结束标志字符,并检查语句格式的正确性,将正确的语句存入输入数据缓存单元,然后由解算模块来进行定位结算并进行调整,再将结果存入输出数据缓存单元。控制模块可以控制输出数据缓存单元的数据传送到串口发送模块,然后由串口发送模块输出可用定位信息。

1.3控制模块

在COMPASS系统和GPS系统都正常工作的情况下,通过控制模块,可选择COMPASS单星座定位数据、GPS单星座定位数据和组合星座定位数据中的任何一种数据的输出。同时控制模块也可以判断COMPASS系统或GPS系统任一系统不能正常工作时,立即切换到另一系统进行工作。

1.4解算模块

解算模块的任务是提取北斗基带处理单元和GPS基带处理单元发来的原始观测量,并对其进行解算。解算内容包括3个方面:① 解算COMPASS单星座定位数据;② 解算GPS单星座定位数据[4];③ 解算出COMPASS和GPS组合星座定位数据。

2关键技术

2.1单星座伪距定位算法

为了确定导航终端在地球上的位置,还需获得导航终端对卫星的相对距离[5]。但是,由于导航终端时钟与卫星时钟不能达到完全同步,导航终端通信链路得到的导航终端到卫星的距离应记为伪距ρ,它包含从卫星到导航终端的几何距离和由导航终端时钟与卫星时钟之间的差异而造成的偏移,其中导航终端时钟偏移tu也是未知数。因此,确定导航终端位置需解算4个未知数:位置坐标(x,y,z)和时钟偏移tu,为此至少需要4个方程式,即至少需要观测到4颗卫星。解算过程如下:

规定下标为卫星号,则产生定位的基本方程组:

undefined

。 (1)

这里忽略了诸如大气层效应等误差源。

将式(1)进行微分,做线性化处理,并将导航终端的近似位置坐标(x0,y0,z0)和时间偏差估计值t0作为初始值代入,得到

undefined。 (2)

式中,undefined个坐标分量的系数是近似用户位置指向第s号卫星的矢量的方向余弦。第一次计算定位时,如无近似位置可用,可将(0,0,0)作为初始值,也可由用户手动输入导航终端的近似位置坐标作为初始值代入以减少迭代次数。如果在不久前的工作中已获得位置,则可用所存储的位置作为初始值进行迭代,从而减少迭代次数,缩短解算时间。

采用下述变量以简化上述公式:

undefined

。 (3)

组成伪距定位的基本方程为:

undefined

。 (4)

采用矩阵表示为:

Δρ=[dρ1 dρ2 dρ3 dρ4]T 伪距,

undefined

状态矩阵,

Δx=[dx dy dzcdt]T 未知数。

则式(4)变为:

H×Δx-Δρ=0。 (5)

对式(5)求解,可得到惟一解:

Δx=H-1×Δρ。 (6)

这样便可算出导航终端位置坐标(x,y,z)和导航终端时钟偏移tu。

undefined

。 (7)

如果这样计算出的结果还不够精确,则将解算出的点坐标作为新的初始估计值代入式(2),再重新按上述步骤求解,一般迭代2～3次便可达到可接受的精度要求。

2.2组合星座的定位技术

组合星座定位[6]时,接收到的卫星数量增多,可采用上述伪距定位算法进行解算,这时获得的伪距既有导航终端对COMPASS卫星的相对距离,又有导航终端对GPS卫星的相对距离。由于COMPASS卫星钟和GPS卫星钟之间存在差异,利用GPS星座位置进行定位解算时,导航终端时钟偏移tu还要考虑两系统间的钟差,这一差值可从COMPASS卫星播发的导航星历中获得。因此,要确定导航终端位置仍需解算4个未知数:位置坐标(x,y,z)和时钟偏移tu (当观测量为COMPASS伪距时tu=tuBD,当观测量为GPS伪距时tu=tuGPS+tBD-GPS),具体算法可参见上述单星座伪距定位算法。

采用组合星座定位时,卫星的几何布局优于单星座定位,既提高了定位精度,又保持了导航定位的精度及连续性。

导航终端工作在组合星座定位方式时,由于GPS星座有24颗卫星均匀覆盖全球,COMPASS系统服务范围内可见的GPS星数为8颗以上。因此由原来的单COMPASS系统最多7～8颗星到目前最多可至15颗星,大大降低了几何因子(PDOP),从而大大提高了定位精度。

定位解算中,位置/时间解的精度最终可表示为几何因子和伪距误差因子之积,表示为:

定位误差(E)=几何因子(PDOP)×伪距误差因子(δ)。

式中,几何因子表示卫星/导航终端的相对几何布局对定位误差的复合影响。伪距误差包含影响伪距值实际精度的多种误差分量,计算时取标准差,伪距误差因子为:

undefined。

式中,vTv=[vv]=(vundefined)2+(v2k)2+…+(vnk)2,n为在tk时刻观测到的COMPASS卫星和GPS卫星的个数。

根据上述分析可知,在PDOP≤4时,定位精度可保持在10 m以内(95%)。

3结束语

经综合分析,导航终端的设计采用组合星座的定位技术,能够有效地利用COMPASS系统和GPS系统的卫星,可用的卫星数目大大增加,同时组合星座的定位技术有效地提高了定位精度。使用DSP作为导航终端解算模块的控制和数据处理核心,基于DSP的设计也使系统稳定性更高,维护和升级更方便。导航终端的稳定性、可靠性、定位精度和导航连续性得到显著提高,各项指标处于领先水平。 

参考文献

[1]戴邵武,马长里,李宇,等.基于“北斗二代”的卫星星座设计[J].海军航空工程学院学报,2010,25(1):5-9.

[2]徐绍铨,张华海,杨志强,等.GPS测量原理及应用[M].武汉:武汉大学出版社,2001.

[3]江思敏,刘畅.TMS320C6000DSP应用开发教程[M].北京:机械工业出版社,2005.

[4]李征航,黄劲松.GPS测量与数据处理[M].武汉:武汉大学出版社,2005.

[5]周婧,张怡,张航.全球定位系统的差分伪距定位的算法研究[J].科学技术与工程,2008,8(4):178-181.

组合定位导航技术 篇4

针对车载定位定向系统中存在的缺陷,设计了一种新的车载定位系统.完成了由低成本、高可靠性的`捷联惯性参考系统与高分辨率里程仪组成的捷联惯性/里程仪组合定位、定向系统的方案设计及仿真计算.仿真结果表明,系统对姿态角、航向角等误差具有极高的在线对准及标定精度.组合系统无需停车,可进行零速校正.

作 者：吉翠萍 雷宏杰 冯培德 JI Cuiping LEI Hongjie FENG Peide  作者单位：吉翠萍,JI Cuiping(西北工业大学,西安,710072)

雷宏杰,冯培德,LEI Hongjie,FENG Peide(飞行自动控制研究所,西安,710065)

刊 名：弹箭与制导学报  PKU英文刊名：JOURNAL OF PROJECTILES, ROCKETS, MISSILES AND GUIDANCE 年，卷(期)：2008 28(4) 分类号：V249.322 关键词：捷联惯性参考系统   里程仪   定位定向  

★ 《惯性》的课件

★ 大型客机航空电子系统研发关键技术分析及建议

★ 仪表工作总结范文

★ 软件工程关键技术探讨论文

★ 方案设计模板

★ 移动数据库关键技术论文

★ 个人仪容仪表

★ 仪表销售工作计划

★ 仪表自动化年终总结

组合定位导航技术 篇5

关键词：组合定位系统,无迹卡尔曼滤波,扩展卡尔曼滤波

GPS全球定位系统(Gobal Position System,GPS)和DR航位推算(Dead Reckoning,DR)组合定位方式是目前车辆定位的一种常用定位方式,这种定位方式既解决了GPS单独定位时,定位信号受遮挡不能定位的问题,又可以消除DR单独定位时误差累积。因此如何有效地将GPS和DR两者定位信息进行有效融合成为关键。目前,GPS/DR组合定位方式中,使用最为普遍的融合方式是利用扩展卡尔曼滤波器(Extended Kalman Filter,EKF)进行融合滤波,EKF是一种典型的非线性系统进行粗略的一阶线性化近似的滤波算法,所以当系统非线性度较严重时,会引起线性化误差的增大,导致滤波器的滤波误差增大甚至发散;其次应用EKF时需要计算雅可比矩阵,而雅可比矩阵的计算量很大,造成此算法在实际应用中难以实施。无迹卡尔曼滤波器[1,2](Unscented Kalman Filter,UKF)是一种新型的滤波器,UKF对于非线性系统则不需要进行线性化近似,因此可以达到比EKF更高的滤波估计精度,而且UKF对滤波参数不敏感,鲁棒性强,还有就是UKF滤波算法无需计算雅克比矩阵,因此用UKF代替传统的EKF,可以有效地提高GPS/DR组合定位系统的滤波精度和可靠性。

1 UKF滤波算法

设系统的状态方程和量测方程为

X(k+1)=f[k,X(k)]+W(k) (1)

Z(k)=h[k,X(k)]+V(k) (2)

其中X(k)是系统k时刻的状态变量,均值为$\widehat{X}$(k),协方差为P(k);Z(k)是系统的量测向量;过程噪声W(k)和量测噪声V(k)为互不相关的高斯白噪声序列,其统计特性满足

E[W(k)]=0,E[W(k)WT(j)]=Q(k)δkj。

E[V(k)]=0,E[V(k)VT(j)]=R(k)δkj,∀k,j;

E[W(k)VT(j)]=0。

UKF算法的计算步骤如下:

① 初始化$\widehat{X}{}_0=E[X{}_0]$;

${\rm P}{}_0=E[(X{}_0-\widehat{X}{}_0)(X{}_0-\widehat{X}{}_0){}^{\text{Τ}}]$。

② 计算(2n+1)个sigma采样点χi和相对应的权值Wi:

其中κ是一个尺度参数,可以为任意数值,只要(n+κ)≠0,一般取(n+κ)=3。$[\sqrt{(n+\kappa ){\rm P}}]$为(n+κ)P的均方根矩阵的第i行或第i列,n为状态变量X的维数。

③ 时间更新

χi(k,k-1)=f[k,χi(k,k-1)] (5)

$\widehat{X}(k,k-1)={\displaystyle \sum _{i=0}^{2n}W}{}_i\chi {}_i(k,k-1)$ (6)

${\rm P}(k,k-1)={\displaystyle \sum _{i=0}^{2n}W}{}_i\text{Δ}X{}_i(k,k-1)\text{Δ}X{}_i^{{\rm T}}(k,k-1)+Q(k-1)(7)$

ζi(k,k-1)=h[k,χi(k,k-1)],i=0,…,(2n+1) (8)

$\widehat{{\rm Z}}{}_2(k,k-1)={\displaystyle \sum _{i=0}^{2n}W}{}_i\zeta {}_i(k,k-1)(9)$

其中$\text{Δ}X{}_i(k,k-1)=\chi {}_i(k,k-1)-\widehat{X}(k,k-1)$。

④ 量测更新

${\rm P}{}_{{\rm Z}{\rm Z}}=R{}_2(k)+{\displaystyle \sum _{i=0}^{2n}W}{}_i\text{Δ}{\rm Z}{}_i(k,k-1)\text{Δ}{\rm Z}{}_i^{\text{Τ}}(k,k-1)(10)$

${\rm P}{}_{X{\rm Z}}={\displaystyle \sum _{i=0}^{2n}W}{}_i\text{Δ}X{}_i(k,k-1)\text{Δ}{\rm Z}{}_i^{\text{Τ}}(k,k-1)(11)$

K(k)=PXZ·P${}_{{\rm Z}{\rm Z}}^{-1}$ (12)

P(k)=P(k,k-1)-K(k)PZZKT(k) (13)

$\widehat{X}(k)=\widehat{X}(k,k-1)+{\rm K}(k)[{\rm Z}(k)-\widehat{{\rm Z}}(k,k-1)](14)$

其中$\text{Δ}{\rm Z}{}_i(k,k-1)=\zeta {}_i(k,k-1)-\widehat{{\rm Z}}(k,k-1)$。

2 系统状态方程的建立

选取系统状态变量为

$X=\left[\begin{array}{cccccc}s{}_e,& v{}_{\text{e}},& a{}_{\text{e}},& s{}_n,& v{}_{\text{n}},& a{}_{\text{n}}\end{array}\right]{}^{\text{Τ}}$ (15)

式(15)中se和sn分别为东向和北向的位置分量,单位为m;ve 和vn分别为东向和北向的速度分量,单位为m/s;ae和an分别为东向和北向的加速度分量,单位为m/s2。

车辆的运动模型选用“当前”统计模型[3],系统的状态方程为:

X(k+1)=Φ(k+1)X(k)+U(k)+W(k) (16)

$\Phi (k+1)=\text{d}\text{i}\text{a}\text{g}\left[\Phi {}_{\text{e}}(k+1),\Phi {}_{\text{n}}(k+1),\Phi {}_{\delta }(k+1)\right]$ (17)

将(18)式中的τe换成τn可得Φn(k+1),其中T为采样周期,τe和τn分别为东向和北向的机动时间常数的倒数。

利用“当前”统计模型的自适应滤波算法[3],可将式(16)简化为

X(k+1)=Φ1(k+1)X(k)+W(k) (19)

其中:

$\Phi {}_1(k+1)=\text{d}\text{i}\text{a}\text{g}[\begin{array}{cc}\Phi {}_{1\text{e}}(k+1),& \Phi {}_{1\text{n}}(k+1)]\end{array}$;

系统噪声协方差矩阵$Q(k)=E\left[W(k)W(k){}^{\text{Τ}}\right]=\text{d}\text{i}\text{a}\text{g}$$\left[2\text{τ}{}_{\text{e}}\text{σ}{}_{\text{e}}^2\text{Q}{}_{\text{e}}(\text{k}),2\text{τ}{}_{\text{n}}\text{σ}{}_{\text{n}}^2\text{Q}{}_{\text{n}}(\text{k})\right]‚\text{σ}{}_e^2$和σ${}_n^2$分别为东向和北向机动加速度的方差。为了简化计算,在计算Q(k)时用Φ1(k)代替Φ(k),可求得

3 系统量测方程的建立

将GPS接收机的东向位置信息seob(单位:m)、北向位置信息snob(单位:m)、角速率陀螺的输出ω和里程计的在一个采样周期内的输出距离s(单位:m)作为观测量[4],角速率陀螺的理想刻度因子取为1,观测量与系统状态变量之间的关系为:

$s{}_{\text{e}\text{o}\text{b}}=s{}_{\text{e}}+v{}_1,s{}_{\text{n}\text{o}\text{b}}=s{}_{\text{n}}+v{}_2,s{}_{\text{n}}=\psi {\rm T}\sqrt{v{}_{\text{e}}^2+v{}_{\text{n}}^2}+v{}_s$。

$\omega =\frac{\partial }{\partial t}\left[\arctan \left(\frac{v{}_{\text{e}}}{v{}_{\text{n}}}\right)\right]+\epsilon +v{}_{\omega }=\frac{v{}_{\text{n}}a{}_{\text{e}}-v{}_{\text{e}}a{}_{\text{n}}}{v{}_{\text{e}}^2+v{}_{\text{n}}^2}+\epsilon +v{}_{\omega }$

上式可写为:

即:

${\rm Z}(t)=h\left[t,X(t)\right]+V(t)$ (21)

其中v1和v2分别为GPS接收机输出的东向位置和北向位置的量测噪声,分别是(0,σ${}_1^2$)和(0,σ${}_2^2$)的高斯白噪声;vω为角速率陀螺随机漂移中的高斯白噪声分量(0,σ${}_{\omega }^2$);vs为里程计的量测噪声,是(0,σ${}_s^2$)的高斯白噪声。

对式(20)进行离散化处理,便可得到系统的离散化量测方程为

$\begin{array}{l}{\rm Z}(k)=\left[\begin{array}{c}s{}_{\text{e}\text{o}\text{b}}(k)\\ s{}_{\text{n}\text{o}\text{b}}(k)\\ \omega (k)\\ s(k)\end{array}\right]=\left[\begin{array}{c}s{}_{\text{e}}(k)\\ s{}_{\text{n}}(k)\\ \frac{v{}_{\text{n}}(k)a{}_{\text{e}}(k)-v{}_{\text{e}}(k)a{}_{\text{n}}(k)}{v{}_{\text{e}}^2(k)+v{}_{\text{n}}^2(k)}+\epsilon (k)\\ \psi (k){\rm T}\sqrt{v{}_{\text{e}}^2(k)+v{}_{\text{n}}^2(k)}\end{array}\right]+\\ \left[\begin{array}{c}v{}_1(k)\\ v{}_2(k)\\ v{}_{\omega }(k)\\ v{}_s(k)\end{array}\right](22)\end{array}$

即${\rm Z}(k)=h\left[k,X(k)\right]+V(k)$。

4 仿真结果分析[5]

为了说明UKF的滤波性能,仿真以下几种情况。

① 假设车辆作匀速直线运动,为了消除初始状态误差对滤波精度的影响,取$X(0)=\widehat{X}(0|0)$,而初始协方差${\rm P}(0|0)$则不考虑匹配性。

车辆的初始状态:

$X(0)=\left[\begin{array}{ccccccccc}0& 10& 0& 0& 10& 0& 0& 0& \end{array}\right]{}^{\text{Τ}}$

滤波初始状态:

$\begin{array}{l}\widehat{X}(0|0)=\left[\begin{array}{ccccccccc}0& 10& 0& 0& 10& 0& 0& 0& \end{array}\right]{}^{\text{Τ}}‚\\ {\rm P}(0|0)={\rm I}{}_{8\times 8}。\end{array}$

Monte Carlo仿真次数n=30,采样周期T=0.1 s,采样点数N=5 000,仿真时间为500 s,σ1=σ2=15 m,σω=0.4°/s,σs=1 m,σε=0.015°/s,σψ=0.005,τae=τan=τε=100 s,选取统计指标RMSE为仿真指标。图1和图2为采用EKF和UKF算法滤波后东向和北向的位置RMSE曲线,表1为采用EKF和UKF滤波算法的RMSE均值和RMSE方差比较表。

从仿真结果可以看出UKF算法和EKF相比,位置的估计精度有了很大提高,这不仅表现在RMSE均值减小,而且RMSE方差也明显减小,原因主要是基于UKF的算法采用UT变换而非EKF的线性化近似,因此基于UKF的算法对非线性系统的后验均值和协方差的估计精度高于EKF。

② 非线性度因子对滤波性能的影响。为了说明UKF的非线性滤波性能,引入非线性度因子g,将量测

方程(22)改为

$\begin{array}{l}{\rm Z}(k)=\left[\begin{array}{c}s{}_{\text{e}\text{o}\text{b}}(k)\\ s{}_{\text{n}\text{o}\text{b}}(k)\\ \omega (k)\\ s(k)\end{array}\right]=\left[\begin{array}{c}s{}_{\text{e}}(k)\\ s{}_{\text{n}}(k)\\ \frac{v{}_{\text{n}}(k)a{}_{\text{e}}(k)-v{}_{\text{e}}(k)a{}_{\text{n}}(k)}{g{}^{*}v{}_{\text{e}}^2(k)+v{}_{\text{n}}^2(k)}+\epsilon (k)\\ \psi (k){\rm T}\sqrt{g{}^{*}v{}_{\text{e}}^2(k)+v{}_{\text{n}}^2(k)}\end{array}\right]+\\ \left[\begin{array}{c}v{}_1(k)\\ v{}_2(k)\\ v{}_{\omega }(k)\\ v{}_s(k)\end{array}\right](23)\end{array}$

取g=1.5分别利用EKF和UKF两种滤波算法,仿真结果如图3所示。

从图3可以看出在引入非线性度因子后EKF的滤波结果已经表现出了有偏性,而UKF仍表现出了很好的滤波性能,原因主要是引入了非线性度因子g后,系统非线性度增强,EKF的引入的线性化误差增大,导致滤波结果恶化。

5 结语

基于UT思想的UKF是完全不同于传统的EKF的一种新型滤波算法,它具有滤波精度高、计算量小易于工程实现等优点,对于GPS/DR组合定位方式,用无迹卡尔曼滤波算法UKF代替传统的扩展卡尔曼滤波算法EKF。仿真结果表明定位精度得到显著提高,而且UKF对由于系统非线性所引起的滤波误差有很好的抑制作用,因此UKF算法对于要求高精度、低成本和高可靠性的定位系统来说是一种值得推广的滤波算法。

参考文献

[1]Julier S J,Uhlmann J K.Anewextension of the Kalman filter nonlin-ear systems.SPIE,1997;3068:182—193

[2]Julier S J,Uhlmann J K.A new method for the nonlinear transforma-tion of means and covariances in filters and estimators.IEEE Trans on AC,2000;45(3):477—482

[3]周宏仁,敬忠良,王培德.机动目标跟踪.北京:国防工业出版社,1991

[4]房建成,申功勋,万德钧,等.GPS/DR组合导航系统自适应扩展卡尔曼滤波模型的建立.控制理论与应用,1998;15(3):385—389

组合定位导航技术 篇6

1 GPS、北斗卫星导航系统定位原理

在基于GPS、北斗卫星导航系统中, 其主要由空间部分、地面控制管理部分与用户终端组成[1]。其中对于空间部分而言, 是由2颗地球静止卫星及1颗在轨备份卫星组成, 主要工作在卫星无线电定位业务频段内, 上行是L频段, 下行是S频段。在系统的地面控制中, 主要由1个中心控制站与若干标校站组成, 中心控制站内可同时与2颗工作卫星施行双向通信, 并有效完成对每个用户的精确定位。在系统的用户终端部分, 用户需根据出站信号中的帧时标发射定位申请, 并通过中心控制站将定位数据发往指定用户, 从而实现有效的卫星定位通信功能。

2 单点定位模型

基于GPS的精密单点定位模型中, 主要包括传统模型、Uof C模型以及无模糊度模型[2]3种, 将其应用于卫星导航系统, 可提高卫星导航的精度、简化用户端系统、提高GPS精密定位操作的灵活性。

单点定位模型中, 对于传统模型而言, 其就是由双频GPS伪距及载波相位观测值中无电离层组合的观测模型。常用于对定轨精度要求较低的系统中, 可有效减弱电离层影响, 其单点定位的函数模型[9]如式 (1) 所示

单点定位模型中的Uof C模型, 与传统模型不同, 其中不仅采用无电离层相位组合外[3], 还采用了L2和L1频率码和相位平均, 也可有效降低电离层的影响, 能应用于实时性要求较高的系统中, 其模型形式如下

单点定位模型中的无模糊度模型, 采用无电离层伪距组合观测值以及历元间差分的载波相位观测值求差[4], 无需考虑估计模糊度, 可应用于实时性要求较高的系统中, 其观测模型形式如下

基于GPS卫星导航系统可将3种模型分别应用与实际系统中, 对流层延迟误差具有较好的改善。所采用的3种模型, 均各有优点与不足[5], 根据具体情况选择合适的模型方法, 不仅能提高卫星导航定位的精度, 还可提升卫星导航系统的应用效率。

3 定位算法验证

在北斗卫星导航定位系统中, 对用户发送连续的导航电文, 采取单点定位的方式, 提高系统精度[6]。在系统中, 其定位过程首先由地面中心对卫星连续发射X波段以及C波段载波, 其中的数据流有测距信号及地址电文等信息, 当这些询问信号经卫星变频及放大、转发到测站内, 之后由测站来接收询问信号, 当地面中心站接收到应答电文后, 就可得到其测站的坐标和交换的电报信息, 再由中心站将系统处理后的信息传输给测站, 而测站最终将收到所需信息。在北斗卫星定位系统的组合单点定位滤波算法实现中, 由于北斗卫星定位系统是有源工作方式, 可采取间断组合模式, 避免暴露用户目标, 在卫星定位接收机中, 在接收到定位信息的同时发送定位申请, 再由卫星导航系统中的接收机接收定位信息, 并将数据进行卡尔曼滤波[7], 将最优滤波值进行校正, 由此可最终获得较为精确的状态。而对新的状态进行估值, 施以逐次迭代, 经若干次迭代逼近[8]后, 便可得到准确定位。在卫星导航系统中的定位算法仿真时, 对每个接收机的伪距测量误差, 当接收机经定位后, 同时在X、Y和Z轴方向上进行误差定位。如图1~图4所示。

对于卫星导航系统中的接收机精确位置进行计算, 利用气压高度测量得到高程和地心距测量, 对估计值、估计误差及在实际定位解算中[6,9], 利用伪距测量量以及使用迭代的方式求解, 并最终得出准确定位。在进行迭代计算中, 若相邻2次假设定位时使用的测量量无误, 然而在仿真结果中却有误差, 则是由计算的截断误差与模型误差所造成的。定位卫星在伪距测量中, 对误差较大的仿真结果, 可得到定位开始或结束时的自位置坐标, 同时会对接收机时钟及本振频率进行校正。

在基于GPS、卫星导航系统中, 采用单点定位方式, 可通过修正精确的误差模型来进行, 或利用天线的相位中心位置偏差及计算误差等方式, 实现对卫星导航系统的高精度定位。定位解算位置参数误差, 对于接收机钟差平均约为68 m。且在接收机的观测误差中, 除观测的分辨率外, 还包括对接收机天线相对测站点位置的误差, 其约为信号波长的1%。此外, 在基于GPS、卫星导航系统在实测数据计算结果方面, 对于北斗卫星定位结果更接近标准值, 卫星定位的精度更高。在北斗卫星导航系统中, 在X、Y、Z方向上的均方差均<15 m, 因此满足了对中高精度的卫星导航定位用户的需求。

4 结束语

综上所述, 基于GPS、北斗双星定位系统, 采用组合单点定位模式, 并设计低阶滤波算法方案, 可在线根据北斗双星位置信息对中低精度的激光陀螺误差进行有效地估计与补偿, 其不仅可提高组合导航系统的精度, 且在工程实现中还具有良好的应用价值。

摘要：北斗卫星定位系统可为用户提供快速定位, 以及简单数字报文通信的高精度卫星定位, 其不仅可满足用户对中高精度的导航定位需求, 且算法设计简单实用, 可为GPS导航系统定位提供有效辅助。文中在对GPS和北斗卫星导航系统组合单点定位原理分析的基础上, 建立了二者组合的定位模型, 并验证了算法的有效性。

关键词：GPS导航系统,北斗定位系统,单点定位模型算法

参考文献

[1]马茹, 胡建伟, 杨立成.GPS信号和其干扰的Matlab仿真[J].电子科技, 2011, 24 (5) :17-22.

[2]王海军, 魏冲, 祝益芳.北斗卫星导航系统的发展与思考[J].信息与电脑:理论版, 2010 (8) :54-56.

[3]于华明.北斗卫星是怎样导航定位的[J].计算机测量与控制, 2003 (6) :21-25.

[4]秦加法.我国自主卫星导航定位系统:北斗卫星导航定位系统综述[J].全球定位系统, 2003 (3) :32-35.

[5]李建, 谢小荣, 韩英铎, 等.北斗卫星导航系统与GPS互备授时的分布式相量测量单元[J].电网技术, 2005 (9) :11-15.

[6]赵楠, 王永生.北斗卫星定位算法研究[J].电子科技, 2011, 24 (10) :102-103.

[7]朱伟康, 张建飞, 傅俊璐.北斗卫星系统在远洋船舶上应用的研究[J].无线电工程, 2008 (9) :111-114.

[8]程翔, 陈恭亮, 李建华, 等.基于北斗卫星导航系统的数据安全应用[J].信息安全与通信保密, 2011 (6) :77-81.

组合定位导航技术 篇7

随着国家十二五规划纲要关于建设智能坚强电网要求的提出,智能电网信息化、数字化、网络化已经成为当前电网智能化发展的一个热点研究领域。目前,承载着电网输变电环节的变电站,多采用人工巡检方式监控变电设备的运行状态,这一传统巡检方式费事费力。因此,基于智能机器人的无人值班变电站巡检技术已然成为智能化变电站发展的一个创新性热点话题,而机器人实现自主巡检任务的关键及难点在于如何构建机器人自主导航定位系统。

目前,常用的导航系统多种多样,导航原理、定位精度及成本造价也存在较大差别。常用的导航定位系统包括黑白线识别导航、磁导航、G P S/惯性组合导航、视觉图像导航、激光雷达/惯性组合导航等。而较为成熟的机器人黑白线识别导航技术,通过激光对地面黑白线进行反射接收识别,保证机器人始终沿预设白线行走,该方法简单易行,但施工较大,且易受大雪天气影响;磁导航技术则利用磁传感与测量技术,通过在地面铺设磁条,保证机器人始终沿预设磁航道行走,该方法虽然解决了大雪天气遮挡黑白线的问题,但其成本较大,且变电站长期强磁干扰容易导致磁条失磁,降低其灵敏度,最终可能导致导航失效。D G P S/惯性导航系统[1]定位精度可以到达亚米级,定位灵活方便,但G P S受天气、变电站强电磁干扰等外界环境因素影响较大,考虑系统的容错性,需要增加额外独立的导航子系统配合使用,而该系统成本造价高、经济性较差。视觉图像导航定位系统借鉴人体视觉导航原理,利用图像识别及神经网络技术,通过机器人预先对巡检环境自主学习,建立基于当前环境模型下的知识库与规则库,即利用已有学习经验来实现自主巡检任务,但该方法技术难度较大,有待进一步深化研究。

本文提出的惯性/激光雷达组合导航定位技术,利用高精度激光雷达测距技术,在预设全局路径[2]的前提下,通过多点全向扫描测距、信息融合与滤波处理技术,对机器人当前运行环境进行视觉建模,同时利用多点测距技术实现实时定位,该方法成本低廉,定位精度高,但对现场环境的依赖性相对较大,易受外界随机干扰,需要优化程序算法,减小外界的随机干扰误差等。

文献[3]基于激光雷达测距信息进行机器人周边环境建模,提出了一种时变势场算法,并通过对比实验引入了一种改进型算法—多分辨率势场法,该算法可以有效地实现机器人导航与避障,实时性非常好。文献[4]讨论了基于激光雷达的机器人实时位姿估计方法,通过基于Hough变换的切线角度直方图算法和迭代切线加权最近点算法,解决了传统方法中未能解决的局部最小值、类孔径及大计算量问题,该方法精度高,实时性好,并且对随机噪声、遮挡和类孔径问题具有高鲁棒性特点,适用性广。上述算法对本文中激光雷达的数据程序处理方法有一定的借鉴作用。

本文基于D R/L M S的组合导航定位技术,特别地,提出了一种基于卡尔曼滤波算法的定位精度优化估计方法,通过D R子系统与L M S子系统的相互补充与修正,对原有系统定位及导航精度(定位精度可以达到cm级)加以改进与优化,进一步提高了导航定位的稳定性与精确性。文献[5]通过对外界随机干扰噪声的统计与建模,对Kalman滤波理论做了详细的推导与误差分析,同时提出了改进型的非线性卡尔曼滤波模型与算法,并给出了实例分析与验证。

2 DR导航定位系统原理

我们这里提出的DR(Dead-Reckoning)航位推算是一种基于机器人车体运动模型的、自主式导航定位系统,可以实现二维平面内的航位推算。本系统采用里程计与激光雷达的组合式传感器定位系统。里程计主要用于采样机器人左右轮的行走路程VSk,而机器人航位角θk的采样计算则是高精度激光雷达的应用之一(激光雷达测距系统同时实现外部测距定位与航向角的采样计算)。基于激光雷达的机器人偏转航向角θk测量系统的优点是有效解决了强电磁干扰条件下电子罗盘失效以及光纤陀螺累积误差带来的定位精度大幅降低等一系列难题,但不足的是其需要依赖于外部参考测距对象,独立性较差。

2.1 DR导航定位系统原理图简介

DR导航定位系统航位推算原理如图1所示。已知初始点A的定位坐标(x,y)、导航测量角θk及光电位移传感器测量里程VSk可以计算出系统下一时刻在B点的坐标估计值。

其中,VSk=k S0,S0为车轮旋转一周的传动里程,k为旋转光电位移传感器的采样脉冲计数(车轮每旋转一周进行一次脉冲采样),VSk则为程序采样周期内,机器人的行驶距离。θk表示机器人与道路方向(Y轴方向)的夹角大小,其值由激光测距系统采样间接计算得到。因此,在已知机器人初始位置(x0,y0)的前提下,由式(1)可以求解得到机器人在X、Y轴方向的行驶位移。

通过上式,可以实时推算机器人当前所在坐标。但鉴于机器人车体本身存在机械不对称性,里程计存在一定的测量误差,并考虑左右轮电机出力的不同步性,其定位精度经现场测试,可以达到80cm～100cm左右,虽然可以基本满足现场应用,但由于惯性D R导航精度具有随时间发散的特性,即长期稳定性差,必须辅助外在定位系统进行及时的校正。

2.2 DR导航定位系统程序流程图

D R导航定位系统程序流程图见图2所示。

3 激光雷达测距导航定位原理

本系统采用了SICK公司生产的LMS激光雷达全向测距传感器,该测距系统采用双脉冲快速测距技术,以50hz的扫描频率实现360度、20m范围内障碍物的快速扫描、高精度(m m级)测距需求。测距系统与上位机L A B V I E W软件系统通过以太网接口实现相互通信,数据传输及处理速度能够满足实际工业需求。

机器人通过发送采样相应请求指令,对周围360度、20m范围内障碍物进行距离采样,采样角度间隔为0.5度或0.25度可选。那么,对于同一障碍物或标记物,实际中会得到多个采样距离值,因此需要对这些采样距离值进行均值滤波等一系列数据处理与合并。而如何保证滤波后的距离值与实际障碍物或标记物的一一映射关系,即实现采样数据的识别与验证则是整个定位系统的关键。

本定位子系统误差来源:(1)道路两侧给定参考标记物的地理坐标测量误差;(2)实测目标距离值滤波合并后的处理误差。

实现的功能:(1)机器人通过测量其距离道路两侧标记物(物理坐标给定)的各距离值,通过圆或三角形方程求解,粗略计算其所在的地理坐标(x,y)。(2)机器人利用扫描道路两侧标记物得到的扫描角度间隔,并结合道路方向与机器人车头方向(即扫描中线角度值),精确计算(误差相互抵消)得到机器人的实际行走方向,即航向角α。而机器人正常行走的二维控制参数就是上述计算得到的x坐标(横向偏移)与导航角α。

3.1 LMS导航定位系统原理图简介

LMS导航定位系统原理图见图3所示,其定位与导航计算式分别见式(2)(3):

其中,(xref1,yref1),(xref2,yref2)分别为参考点1,2的测量坐标值;1r,2r分别为机器人距离参考点1,2的激光测距半径值;(x,y)则为需要求解的机器人实际坐标值。

该定位原理简单,但重点、难点在于如何对多点扫描距离值进行滤波处理,包括多点合并、滤波等效以及干扰值的验证与排错等诸多环节,其预处理流程图见图4。只有保证测距目标与实际距离的一一对应,以及多点滤波合并后的高精度性,才能进一步提高定位精度。其定位精度经现场测试,可以达到20～40cm,可以满足现场工业应用需求。

航向角计算式如下:

其中,r1、r2表示采样预处理后,机器人距离当前两参考目标1,2的有效半径值;Vα12表示半径r1、r2之间的扫描间隔角度;表示经激光测距得到的参考点1,2之间的等效距离值(这里,考虑到1,2参考点间距d12的人工量测误差,我们以高精度激光测距系统的理论计算值为计算标准);β表示距离机器人最近的参考目标1或2分别与机器人定位点、次近参考目标2或1连线的夹角计算值;k1、k2分别表示参考目标坐标点1,2连线的斜率、道路方向的斜率(考虑到道路方向,即Y轴方向斜率为无穷大,这里我们将X,Y坐标轴进行调换求两直线的夹角);γ表示道路方向与参考目标点1,2连线的夹角;α表示r1、r2中的较小者对应的扫描角度值;△α测量表示机器人实际车头方向与α之间的间隔角度(其中π是车头方向的扫描角度,固定不变);△α理论表示机器人理想前进方向(即Y轴道路方向)与α之间的间隔角度(三角形求解);θk表示机器人实际车头方向与道路方向的偏角计算值,即我们要求的机器人航向角。

本算法实现相对简单,只需要通过激光测距系统对当前环境参数进行识别、采样即可快速实现在线计算与控制。其中∆α测量、∆α理论作差抵消了多点扫描带来的绝对误差,有效提高了航向角精度。经现场测试,可以达到0.1度甚至更高,可以满足实际工业应用需求。此外,本算法的特别之处在于,当选取的参考点1,2关于道路左右对称,即时,由式(3)中(b)(e)(g)可以看出,航向角仅与当前采样参数有关,而与目标参考点的坐标选取无关,进一步提高了系统的独立性。

3.2 LMS导航定位系统的程序流程图

L M S导航定位系统程序流程图如图4所示。

4 卡尔曼滤波在组合导航定位系统中的应用

卡尔曼滤波本质上是一种建立在时域空间内的线性最小方差估计算法,具有时域状态递推性,适用于对多维随机过程(平稳、非平稳)进行状态估计,包括了连续与离散两类算法。其算法简洁高效,便于在计算机上实现,而卡尔曼滤波在实际组合导航系统中是其较为成功的一个典范。本文以随机线性离散卡尔曼滤波为理论基础,实现了变电站智能机器人的实时精确导航与定位。

4.1 卡尔曼滤波在DR/LMS组合导航定位系统中的现场应用

基于Kalman滤波的DR/LMS组合导航定位系统整体结构图如图5所示:

上述两导航定位子系统各有其优缺点:D R系统自主导航定位能力较强,但位移传感器测量精度较低,使得系统定位精度随时间发散,即长期稳定性差;LMS系统虽然其精度高,但受外在环境影响较大,主要表现在参考点的识别与交替更新在某些不可预测条件下的误判现象,从而导致定位错乱等。为了保证系统的定位精度、提高系统的稳定性,这里我们引入一种基于卡尔曼滤波的组合导航技术,即D R/L M S组合导航技术,通过最优线性估计算法,使两个定位子系统相互校正,取长补短,既可以及时校正DR航位推算随时间带来的航位漂移,又可以有效地修正L M S系统对参考点的错误识别,极大地改善了整个系统的动态稳定性与准确性。

基于卡尔曼滤波模型,并考虑到导航角本身的计算精度可以满足需求,因此,这里我们仅对D R/L M S组合导航系统的X、Y定位坐标作K a l m a n滤波,方程如下:

状态方程:

观测方程:

考虑到前后两次采样时间极小,故认为θk-1≈θk,因此,这里我们取θk-1表示k-1状态到k状态过渡过程的等效航向角。上式中,kx、yk分别表示机器人在X轴、Y轴方向的状态位置坐标;θk-1表示状态控制量,即计算航向角;VSk,k-1表示各采样阶段的转移控制系数;W(k-1)x、W(k-1)y表示D R推算过程噪声序列;Γ(k,k-1)x、Γ(k,k-1)y表示D R推算过程噪声输入系数;zkx、zyk分别表示激光测距计算得到的机器人定位点的x、y坐标测量等效值;Vkx、Vky表示L M S测量系统等效噪声序列。

根据变电站现场环境,系统过程噪声序列与观测噪声序列可以看做是均值恒定的高斯白噪声随机序列,并设系统过程噪声(即估计噪声)方差为kQ,系统观测噪声(即LMS测量噪声)方差为kR。且在整个滤波过程中,系统过程噪声序列与观测噪声序列不相关,系统初始状态0x、0y与过程噪声序列、观测噪声序列也不相关。系统定位坐标初始状态0x、0y由激光测距系统给定,滤波误差初始值P0已知。具体的噪声建模及参数确定,由于篇幅限制不作详述,可以参考文献[5]。

依据Kalman滤波求解步骤,DR/LMS系统的滤波估计求解过程如下:

I.状态一步预测:

V.滤波估计误差:

4.2 DR/LMS组合导航系统现场运行结果分析

(1)基于Matlab的主要导航参数波形分析

机器人视觉区域范围内对参考标记点的自主识别与更新曲线如图6所示,图(1)绿色曲线表示机器人视觉范围内距离参考标记点的距离值变化曲线,即随着机器人的前进路径方向,其距离参考标记点的距离值逐渐减小,直至进入下一段参考域时,参考标记将会发生更新替换,此时参考距离值会发生瞬时突变,即上图中绿色曲线由小及大的突变,也就是说机器人将根据测量距离(绿色)这一视觉参量来及时更新参考标记点,以便进入下一参考域的定位导航阶段。图(2)蓝色曲线表示与测量距离相对应的测量角度变化趋势,其主要用于视觉识别中干扰物的查找、验证及纠错等。中图绿色曲线表示突变脉冲,即参考距离值发生指定裕度内的突变时,输出记为布尔值1,反之,记为0。相应地,对于中图每次的突变脉冲,图(3)则对应显示了该时刻机器人参考标记点的依次替换、更新过程。

在本系统中,机器人实现理想直线行走的关键在于两个控制量的计算:X坐标偏移量V x和导航角θ。由于取Y轴方向为机器人理想行驶方向,故X坐标能实时记录机器人与中心路径(X=a)的偏移量V x。

图7显示了机器人沿Y轴方向行驶过程中纵坐标Y的变化曲线。其中蓝色曲线(一部分与红色曲线相互重合)表示未经限幅滤波的轨迹变化,参杂了外界噪声的干扰,而红色曲线则添加了限幅滤波,过滤掉了实际过程中不可能出现的坐标变化量,该试验曲线线性度非常好,完全符合现场运行轨迹。经过卡尔曼滤波后的Y坐标变化曲线线性度与此红色曲线类似,区别仅仅在于相对挺高了定位精度。

图8显示了组合导航系统X坐标定位值的精度变化,图中道路中心线方向的直线方程为X=3.3附近。其中,黄线代表机器人实际行走过程中X定位值的正常变化趋势,即X坐标的理想变化曲线;蓝线代表D R/L M S组合导航定位系统原始定位坐标X的变化曲线;红线代表组合导航系统经限幅滤波、均值滤波等一系列常用经典滤波后的滤波定位坐标X的变化曲线;绿线则代表组合导航系统引入Kalman滤波后的X值变化曲线。因此,我们可以明显地看出,与X值理想定位变化趋势相比,经过Kalman滤波后的定位波形具有更高的精度与可信度。经现场测试,其定位精度完全符合实际中的运行轨迹,精度可以达到10cm左右,引入模糊控制后的系统参数可调裕度大,运行效果非常理想。

(2)基于Labview的导航系统现场轨迹试验波形分析

图9对变电站现场地理环境进行了直观的描述,坐标系的选取如图中所示。这里,取道路方向为Y轴方向,道路宽度介于X=1.7～4.7之间。其中,方块表示参考电气附件(杆塔),各个电气附件的Y坐标如图中所示,而将道路左侧电气附件的X坐标统一定义为0,道路右侧电气附件的X坐标根据现场测量结果统一定义为6.8,这样也就建立了导航系统的参考坐标知识库。根据机器人现场运行轨迹,这里我们用图中蓝色曲线近似模拟机器人的实际行走路线。在Y=4 0.1的标记杆塔之前,机器人有偏离道路中心线向右行走的趋势,偏离幅度在10cm～20cm之间,而经过该杆塔之后,在控制系统的反馈调节作用下,机器人则重新回到道路中心线,保持理想前进方向。

图10基于Labview数据处理与图形显示软件,对本导航系统的主要导航参数之一:实时定位坐标(X,Y)进行了直观化地显示。定位坐标同时也是整个控制系统的参数之一。图中红色曲线表示经过卡尔曼滤波后的机器人现场定位的轨迹路线测试图。对比图10与图9,我们可以明显地看出,机器人实际运行轨迹始终保持在X=3.2附近,且轨迹变化趋势基本符合上述模拟条件,定位精度可以保持在20cm左右,完全可以满足现场运行环境的要求。

图1 1则显示了本导航系统的另一个主要导航参数:实时航向角θ。导航角也给出了控制系统的另一主要参数,对机器人行驶方向与道路中心线方向偏离程度进行了量化。图中绿色曲线直观地显示了机器人航向角的偏离程度,根据左偏角度为负,右偏角度为正的定义,在Y=40.1前后两个阶段,导航角的总体趋势由正变到0度附近,表明了机器人先右偏后左偏最终回到道路中心线的趋势,符合现场运行条件,导航精度完全可以现场运行条件。

5 结束语

通过在500KV超高压变电站现场测试及结果分析,有效地验证了本文算法实现的有效性。实践证明,DR-L M S组合导航系统实现方便,原理相对简单,且不易受变电站强电磁环境的干扰,稳定性高,实时性好(更新速率可以达到10Hz甚至更高),经济性强,即在大幅度降低导航系统成本的同时,其定位精度完全可以达到厘米级,完全不亚于同等精度条件下的其他导航系统,完全可以满足现场工业应用需求,在智能变电站建设中具有广阔的应用前景与研究价值。

摘要：本文介绍了一种基于DR航位推算与LMS高精度激光雷达全向测距技术的组合式导航定位系统,旨在应用于强电磁干扰环境下的变电站智能机器人自动巡检环境。DR/LMS组合式导航定位系统原理相对简单、实现方便、实时性强,且与其他组合式导航系统相比,在满足同样定位精度的前提下,成本低廉,现场运行效果良好。特别地,本文提出了一种基于卡尔曼滤波方法的定位精度优化估计方法,对原有定位及导航精度(定位精度可以达到cm级)加以改进与优化,进一步提高了导航系统的稳定性与精确性。整个系统模型及控制策略的实现基于工业用PC控制板的上位机LABVIEW软件控制系统,并结合MATLAB软件进行仿真与数据分析,经现场运行测试,定位及导航精度完全可以满足现场工业环境的应用需求.

关键词：变电站,机器人,DR(航位推算),LMS(激光测距系统),组合导航定位,卡尔曼滤波

参考文献

[1]魏鹏,张志强,张春熹等.无人值守变电站巡检机器人导航系统研究[J].自动化与仪表,2009,(12):5-8,25.

[2]孟偲,王田苗.一种移动机器人全局最优路径规划算法[J].机器人,2008,27(3):217-222.

[3]杨明,王宏,何克忠,张钹.基于激光雷达的移动机器人环境建模与避障[J].清华大学学报(自然科学版),2000,40(7):112-116.

[4]杨明,董斌,王宏等.基于激光雷达的移动机器人实时位姿估计方法研究[J].自动化学报,2004,30(5):679-687.

组合定位导航技术 篇8

1) 惯导系统 ( SINS) 是一种完全自主的导航系统, 可提供速度、位置和姿态等信息, 但导航误差随时间积累［1］。

2) 全球定位系统 ( GPS、BD2) 具有全天候、覆盖面广、基本不受时间限制和高精度的优点, 但数据输出频率低, 在动态环境中可靠性差, 而且依赖GPS将在非常时期完全受制于人。

3) 多普勒计程仪 ( DVL) 测得的速度精度高, 使用方便, 但单独使用不能给出载体的绝对地理位置。

因此, 上述的哪一种导航方式都没有办法满足长时间且高精度导航定位的要求。为了克服单一导航方式的缺点, 本文利用联邦滤波器理论将SINS、 GPS、BD2和DVL四者结合在一起, 构成了具有高精度、长时间导航系统。以下对SINS/GPS/BD2/ DVL联邦滤波器的进行设计。

1 SINS /GPS /BD2 /DVL联邦滤波器设计方案

本文设计的联邦滤波器如图1所示, 包括一个主滤波器 ( MF) 和三个子滤波器 ( LF) 。SINS为公共参考系统。SINS与GPS构成子滤波器LF1, 进行位置信息和速度信息的组合。SINS与BD2构成子滤波器LF2, 进行位置信息和速度信息的组合。 SINS与DVL组成子滤波器LF3, 进行速度信息的组合。

LF1、LF2 、LF3都是卡尔曼滤波器, 将它们经过滤波得到的数据送入MF进行最终的数据融合, 其中主滤波器采用联邦滤波。主滤波器的信息不反馈给子滤波器, 因而不同的子滤波器之间没有相互影响, 故此方案容错性能较好, 满足该船舶导航系统对可靠性的要求。

2 SINS /GPS /BD2 /DVL联邦滤波器设计

2. 1各导航方式的误差模型

2. 1. 1 SINS误差模型［2］

1) 惯性仪表误差

捷联惯导系统 ( SINS) 工作在水平阻尼状态时的主要仪表误差源包括北向、东向加速度计零位误差 ΔAN, ΔAE和东向、北向及方位陀螺漂移误差 εE、 εN、εU。这5个误差源都能够近似表述为“随机游动+ 白噪声”的形式, 即

上述式中 ΔAES、ΔANS、εES、εNS、εUS和wAE、 wAN、wεE、wεN、wεU为白噪声。把随机游动 ΔAEC、 ΔANC和 εEC、εNC、εUC列为状态量, 把白噪声 ΔAES、 ΔANS、εES、εNS、εUS作为系统噪声。

2) 导航参数误差

其中位置信息误差方程

下标E、N、U代表地理坐标的东、北、天, δL、δλ 为惯导系统输出的经纬度。

速度信息误差方程

式 ( 2) 中 Ω 表示地球的自转角速率, Ω = 15°/h。k1、 k2、k3为阻尼系数。

姿态误差角

外速度误差

δL、δλ 为惯导系统输出的经纬度; δVE、δVN为东向、北向速度误差; δVE*、δVN*为外速度误差。

2. 1. 2 GPS误差模型

1) GPS的位置信息误差方程:

相关时间 τLG= τλG= 1 s, RLG ( 0) = RλG ( 0) = 0. 005, δLG、δλG为GPS系统的经纬度。

2) GPS速度信息误差方程为:

相关时间 τVGE= τVGN= 1 s, 均方差RVGE ( 0) = RVGN ( 0) =0. 01 m/s, δVGE, δVGN为GPS的东向、北向误差。

2. 1. 3多普勒计程仪的误差分析

1) 相关误差

相关误差可以用一阶马尔可夫过程表示:

2) 刻度系数误差

刻度系数误差认为是随机常数, 均方值为0. 000 1。

2. 2各子滤波器的状态方程、量测方程

2. 2. 1状态方程

该子滤波器的状态方程为, 即

2. 2. 2量测方程

惯导位置、速度信息以及GPS位置、速度信息分别为

2. 3 SINS /BD2位置速度信息子滤波器［3］

2. 3. 1状态方程

上述式中: 下标E 、N分别代表地理坐标的东向和北向; L为纬度, λ 为经度; φE、φN、φU代表着东、北、天方向的姿态角误差; εbxεbyεbz代表东、北、 天方向的等效陀螺漂移误差, !ax!ay!az代表东、北、 天方向的加速度计漂移误差。

2. 3. 2量测方程

2. 3. 3 SINS / DVL速度信息子滤波器

SINS/DVL子滤波器的状态方程为

SINS / DVL子滤波器量测方程如下

3 SINS /GPS /BD2 /DVL联邦滤波器算法

3. 1子滤波器算法

为了方便在计算机中联邦滤波的实现, 将各子滤波器连续形式的状态方程和量测方程离散化, 得到如下形式的差分方程［4］:

3. 1. 1时间更新

各个子滤波器根据状态方程进行时间更新, 如下:

3. 1. 2量测更新

当有量测值时, 各子滤波器进行量测值的修正, 即量测更新, 算法如下:

3. 2主滤波器算法

3.2.1信息分配[5]

在初始时分配一次信息。由于惯导系统同时参与三个子滤波器的滤波, 惯导系统的状态为公共的状态, 其按信息守恒原理在各个子滤波器之间进行信息的分配:

式 ( 15) 中, βi为信息分配系数, βi< 1 , 并且满足∑ βi= 1; Q为公共状态的过程噪声方差强度阵, 下标c表示为SINS状态; g表示全局估计［6］。

3. 2. 2全局融合算法

在各子滤波器计算出各自的局部估计之后, 由主滤波器把各子滤波器的公共状态X·ci、Pci ( i = 1, 2) 按如下的公式进行信息的全局融合, 得到全局估计［7］:

4 SINS /GPS /BD2 /DVL联邦滤波器的仿真

对以上设计的SINS/GPS/BD2/DVL联邦滤波器, 本文进行计算机仿真, 首先仿真了船舶的航行轨迹, 仿真时间1 000 s。

仿真初始条件: 初始纬度32°, 经度118°平台初始误差角取为东向10', 北向10'; 速度、位置信息误差分别为1 m / s, 5 m, 陀螺漂移误差为0. 1 ( °) / h, 等效加速度计零偏为10－ 4g, 陀螺相关时间为0. 1 ( °) / h, 陀螺白噪声漂移误差为0. 1 ( °) / h。初始航向90°, 速度0 m/ s。对上述航行轨迹进行仿真分析, 仿真后的航行轨迹、导航参数误差曲线分别如图2 ~ 图4所示。

由图4可以看出, 经过本文所设计的SINS/ GPS / BD2 / DVL联邦滤波器滤波后各导航参数误差均得到极大降低, 本文所设计的滤波器滤波效果十分显著, 满足系统高精度导航定位的要求, 具有重要的工程价值。

参考文献

[1]吴铁军, 马龙华, 李宗涛.应用捷联惯导系统分析.北京:国防工业出版社, 2011:1—2

[2] 黄德鸣.惯性导航系统.北京:国防工业出版社, 1986

[3] 林雪原, 刘建业, 范胜林.三星定位/SINS伪距组合导航系统的研究.中国空间科学技术, 2002; (6) :6—11

[4] Carlson N A.Federated square root filter for decentralized parallel processes.IEEE Transactions on AES, 1990;AES-26 (3) :517—527

[5] Carlson N A, Berarducci M P.Federated Kalman filter simulation results.Navigation, 1994;41 (3) :297—321

[6] 杨常松, 徐晓苏, 汪丽云, 等.信息融合技术在INS/GPS/DVL组合导航中的应用研究.中国惯性技术学报, 2006; (10) :4—6

组合定位导航技术 篇9

建立可靠的井下人员定位系统, 对改善矿山安全生产管理有着十分重要的意义[1-2]。现有的井下人员定位系统主要包括2类:基于射频识别 (RadioFrequency Identification, RFID) 技术的井下人员定位系统 (简称RFID系统) 和基于无线传感器网络 (Wireless Sensor Network, WSN) 技术的井下人员定位系统 (简称WSN系统) [3]。其中, RFID系统通过读取所安装的读卡器信息对人员进行登记, 以读卡器的位置作为井下人员位置信息, 但该系统无法实时地报告井下人员的具体位置和实现语音通信, 只能用于井下人员区域定位和考勤登记管理。WSN系统通过在井下广泛布置的传感器监测点收集井下人员随身携带的识别卡信息, 从而对人员进行定位。该系统与RFID系统相比, 大大提高了人员定位性能, 但其研究主要集中在局部区域上, 构建一个全矿井WSN还存在组网困难、数据库难以建立及硬件设备笨重等问题。

综上分析, 目前井下人员定位系统虽然在定位性能上有了较大的提升, 但对实时全局定位和发生矿难后搜救方案等方面尚未展开研究。随着微机电系统 (Micro Electro Mechanical System, MEMS) 的微惯性器件出现, 极大推动了微惯性测量组合 (Micro Inertial Measurement Unit, MIMU) 的发展[4]。MIMU系统是一种新型的捷联惯导系统 (Strapdown Inertial Navigation System, SINS) , 它继承了SINS的自主性和抗干扰性强、隐蔽性好等传统特性的同时又具有尺寸小、成本低和便于安装调试等优点。因此, 将MIMU系统运用到炼矿井下环境中, 进行实时全局定位具有重要意义。然而, 由于微惯性器件的随机漂移导致其误差随时间逐渐积累, 长时间运行必将导致客观的误差积累, 因此, 需要另一种定位系统来对其定位误差进行校正。WiFi定位技术的覆盖范围广、定位精度高, 适合长时间定位, 且没有误差积累, 但其定位精度极易受到环境因素的影响。鉴此, 本文设计了一种MIMU/WiFi井下人员组合定位系统。

1 MIMU系统工作原理

MIMU系统由三轴微陀螺仪和三轴微加速度计分别同时测量3个正交方向上的角速度和加速度, 由此六自由度信息经导航算法计算来获得载体的三维运动轨迹。MIMU系统将微惯性器件直接固连于载体上, 通过对三轴微陀螺仪的输出值进行积分计算来获得井下人员自身的姿态信息, 对三轴微加速度计的输出值进行2次积分来获得井下人员的速度和位置信息, 进而实现对井下人员的定位功能。MIMU系统工作原理如图1所示。

图1中, 定义n系为导航坐标系 (本文选取东-北-天地理坐标系作为导航坐标系) ;b系为载体坐标系;i系为惯性坐标系[5];表示在b系的比力值;表示在n系的比力值;表示b系相对于i系的转动角速度在b系下的投影;表示b系相对于n系的转动角速度在b系下的投影;表示n系相对于i系的转动角速度在b系下的投影。

姿态矩阵可通过四元数法求解姿态矩阵微分方程来获得, 姿态矩阵的方向余弦阵为

式中:θ为俯仰角, 其定义域为 (-90°, 90°) ;ψ为航向角, 其定义域为 (-180°, 180°) ;γ为横滚角, 其定义域为 (0°, 360°) 。从Cnb中可求得载体的姿态角, 即θ=arcsin T32, ψ=arctan (-T12/T22) , γ=arctan (-T31/T33) 。

位置矩阵可以通过求解位置矩阵微分方程来获得, 位置矩阵的方向余弦阵为

式中:λ为载体所处的经度, λ=arctan M33;L为载体所处的纬度, L=arcsin (M33/M31) 。

2 WiFi定位原理

在WiFi定位中, 常用的定位算法有基于到达时间差 (Time Difference of Arrival, TDOA) 定位算法和基于接收信号强度指示 (Received SignalStrength Indicator, RSSI) 定位算法, 但这2种定位算法都必须保证至少有3个接入点 (Access Point, AP) 同时接收到同一井下工作人员随身携带的定位终端的信号, 使得井下巷道中AP数量将大大增加, 同时给组网带来了麻烦。因此, 本文采用基于RSSI的位置指纹识别算法[6], 其定位原理如图2所示。位置指纹识别依靠表征目标特征的数据库进行识别, 其定位过程主要分为训练阶段和定位阶段。

训练阶段的目的在于建立一个位置指纹识别数据库, 通过采集不同基站RSSI值, 将相应的MAC地址与参考点的位置信息记录在数据库中。为了克服RSSI不稳定对定位的影响, 通常将每个参考点上的多次测量求平均值作为该参考点的RSSI值。定位阶段是在给定数据库后, 根据接收到AP的RSSIi值, 依据一定的匹配算法与数据库中的已有数据RSSIj进行比较, 通过匹配判断, 计算出待测人员位置的估计值。常用的匹配算法有最近邻法 (Nearest Neighbor, NN) 、K- 邻近法 (K-NearestNeighbor, KNN) 、神经网络等[7], 本文选用NN匹配算法。

3 组合定位系统设计

将MIMU与WiFi构成组合定位系统[8]可充分发挥两者各自的优势。利用WiFi提供的可靠位置信息来给定MIMU最初的位置信息并校正MIMU的位置, 从而可放宽对MIMU系统的精度要求, 并且弥补了MIMU误差随时间累积的缺点[9-12];利用MIMU的实时全局定位来弥补WiFi的局部定位。此外, 二者都具有体积小、成本低的优点, 因而可以构成性价比优越的微型组合定位系统。

在井下需要进行人员跟踪的区域和巷道中布置若干WiFi基站, 井下人员携带含MIMU/WiFi模块的定位终端。随着人员的移动, 定位终端中的MIMU系统对井下人员进行定位。当人员进入WiFi基站工作区域时, 定位终端的WiFi模块被激活, 根据获得的信号强度与数据库相对比获得人员的位置信息, 并以此信息对MIMU系统的定位误差进行校正, 进而与MIMU系统进行组合定位。定位终端还通过WiFi网络将自身的位置信息包传送到相应的数据库中, 地面计算机接收到基站信号后, 从数据库中读出相关采集信息, 实现对人员信息的采集记录和分析处理。

3.1 组合定位系统硬件设计

MIMU系统由三轴微陀螺仪、三轴微加速度计及相关辅助电路构成。将WiFi系统硬件嵌入到MIMU系统中, 构成硬件一体化组合系统。MIMU/WiFi组合定位系统硬件结构如图3所示。

数据接收及滤波处理、系统控制及定位坐标绘制通过开发平台中ARM处理器实现, 本文选用高性能ARM芯片STM32F103VET6, 该芯片含有64kB片内SPAM, 512kB片内Flash, FSMC和SDIO接口及外设接口, 具有容量大、体积适中等特点。利用该芯片内基于卡尔曼滤波的组合定位算法程序计算定位信息数据;数据经嵌入式μC/OS-II操作系统平台上的窗口应用程序处理, 在液晶屏上绘制出移动终端的三维空间运动轨迹。

微惯性器件选用MPU-6050模块, 该模块同时具有三轴微陀螺仪和三轴微加速度计2个微惯性器件, 其供电电源为3~5V, 芯片内置16bit的AD转换器, 具有16位数据输出能力。该模块以数字输出6轴的旋转矩阵、四元数、欧拉角公式的融合演算数据。其中三轴微陀螺仪测量范围为±250、±500、1 000 (°) /s, 三轴微加速度计测量范围为 ±2g、±4g、±8g、±16g (g为重力加速度) , 引脚间距为2.54mm。三轴微陀螺仪运作电流为5mA, 待命电流为5μA;三轴微加速度计运作电流为350μA, 省电模式电流为20μA。在2.4GHz通信领域中, 选用NRF24L01 作为WiFi定位模块, 使系统具有2Mbit/s、1Mbit/s和250kbit/s三种可选传输速率的无线数据通信能力。MAX8902A是一款低噪声、高稳定性线性稳压器, 能够在很宽的输入范围内保持稳定的电压输出。MAX8902A可通过芯片端口选定输出电压, 其输入输出电压范围完全满足系统应用需求。

设计组合滤波器时应注意, 当WiFi或MIMU某一个子系统不能正常工作时, 系统应能自动重构并转换成单独WiFi或MIMU数据处理器。因此, 通过对2种定位系统的组合能够提高可靠性和定位精度, 并且组合定位系统的体积、电耗等指标均明显下降。

3.2 组合定位系统软件设计

将WiFi系统的输出信息与MIMU系统的微惯性测量信息传送到载体计算机中, 首先利用相应的软件进行2套数据的坐标统一, 然后利用卡尔曼滤波器进行最优组合处理[13]。有关校正计算可以在计算机上实现, 无需反馈回子系统的硬件。本文选用集中卡尔曼滤波器进行系统组合, 利用间接法对组合定位系统进行估计, 滤波器的状态为定位参数误差。组合定位系统的信息融合如图4所示, 在WiFi系统测量输出量的基础上, 利用卡尔曼滤波器估计MIMU系统的各种误差状态, 并采用闭环校正法 (即用误差状态的估计值) 去校正MIMU系统, 以达到系统组合的目的。

MIMU系统定位处理软件主要包括MIMU输出信号的采集及预处理、导航参数的计算、误差校正等部分。程序的核心部分是姿态矩阵计算和卡尔曼滤波方程的建立。姿态更新算法是影响MIMU系统能否正常工作的决定性因素, 本文选用四元数法来获取MIMU系统的姿态矩阵信息, 并采用四阶龙格-库塔法来对四元数微分方程进行求解, 其中, 初始对准为四元数算法提供初始姿态, 通过三轴微陀螺仪测得的角速度增量对姿态矩阵的更新进行计算。在信息融合过程中, 采用位置为观测信息, 因此, 组合定位系统的卡尔曼滤波方程的误差状态变量只取MIMU系统的误差状态变量, 系统的测量值包含位置测量差值, 该差值是由MIMU系统给出的位置信息与WiFi系统获得相应位置信息间的差值。WiFi系统为组合定位系统提供初始位置信息, 三轴微加速度计输出经坐标变换和积分计算获得速度和位置信息。基于MIMU/WiFi组合定位解算流程如图5所示。

4 搜救方案

一旦矿难发生, 搜救人员可携带基于MIMU/WiFi组合定位系统的搜救仪下井进行搜救。该搜救仪含有接收模块、报警模块和显示模块, 能够接收到定位终端和基站传输的信息, 并在显示屏上显示出被困人员的位置信息。

矿难发生后, 如果基站损坏, 井下人员携带的定位终端无法接收到基站的信号强度, 进而无法对人员进行定位, 更不能通过WiFi向井上传输定位信息。但MIMU系统不受基站影响, 可以自主进行定位。因此, MIMU系统的定位信息将存储在定位终端中, 随着被困人员离开事故现场, 定位终端将MIMU定位系统的定位信息发送出去, 一旦搜救人员接收到信息, 就能够在显示屏上显示出被困人员的位置。如果被困人员逃离到基站没有损坏的区域, 这时定位终端发送出去的位置信息可通过基站间的传输进行传送, 一旦搜救人员搜到该区域时, 基站传送的信息将被搜救人员携带的搜救仪接收到, 获得被困人员的信息, 进而缩短了搜救时间, 提高了搜救效率。

5 结语