第五章 GPS-DR组合导航
位置-姿态并行滤波的DR GPS组合导航算法
ＧＰＳ／ＤＲ组合导航系统是解决移动车辆在ＧＰＳ卫星信号受遮挡时，仍能有效地定位问题的主要手段之一。
早期的ＧＰＳ／ＤＲ组合导航算法没有考虑航向角的估计问题［１］，只是综合ＧＰＳ和ＤＲ的导航参数信息得出定位结果，一旦ＧＰＳ失效航位推算系统的航向角偏差将导致位置推算精度较差。
随着算法的深入研究将航向角或航向角误差作为状态量参与滤波估计，但是其状态方程是非线性的，线性化过程会引入模型误差［２，３］，而且以往的航位推算往往因为是二维定位而忽略了俯仰角的计算。
虽然较小的坡度对定位精度的影响远小于陀螺等惯性器件的漂移对定位造成的影响，但车辆在大坡度或城市高架桥等地况行驶时车体的俯仰对航向角的测量影响不可忽视。
本文就如何利用低成本的ＤＲ和ＧＰＳ进行姿态、位置组合进行了研究，以卡尔曼滤波为基础，提出了带反馈的位置、姿态角并行滤波器结构，其状态方程都是线性的，既简化了滤波方程，又满足了各子系统对滤波周期的要求。
并针对各子系统的特点分别设计滤波算法，根据姿态角滤波器观测方程的非线性和观测量的冗余性，设计了噪声方差阵自动加权扩展卡尔曼滤波算法。
文章最后给出了静态和动态试验结果，验证了该方法的有效性。
１位置－姿态并行滤波器结构多传感器组合导航系统的主系统一般都是自主式导航，虽然有误差积累，但不受外界环境的干扰，可以连续定位。
而卫星导航系统定位精度高，无误差积累，但易受外界环境干扰，信号一旦丢失将无法定位，因此选其为辅助导航系统。
本文选取ＤＲ为主系统，以ＧＰＳ作为辅助导航系统采用直接滤波法，即以主导航系统输出的导航参数（例如东向位置，北向位置等）为滤波器状态变量，当ＧＰＳ接收机能够正常工作时，辅助导航系统输出的导航参数作为观测量，滤波器输出的导航参数估计值就是系统最终的定位结果；当ＧＰＳ接收机受到屏蔽不能正常工作时，主导航系统仍然能通过推算提供导航信息。
本文滤波器采用位置、角度分别滤波的并联滤波器结构。
一般来说１Ｈｚ的采样频率可以满足位置对数据更新率的要求，但远不能满足角度测量要求，本文角速率的采样频率为１０Ｈｚ，同时为避免量测相关问题要求位置滤波周期要大于ＧＰＳ的位置时间相关常数，因此采用位置、角速度并行滤波方式，位置子滤波器以较长的滤波周期处理位置数据，角度子滤波器以较短的周期处理姿态角数据，既满足了位置和角度对滤波周期要求，又便于根据各子系统的特点设计相应的滤波算法。
基于GPS-DR-MM组合导航AGV定位系统研究
基于GPS-DR-MM组合导航AGV定位系统研究基于GPS/DR/MM组合导航AGV定位系统研究摘要：随着自动化技术的不断发展，自动导引车（AGV）在物流、制造业等领域中得到了广泛应用。
AGV的准确定位是实现其自主导航的关键技术之一。
本文基于GPS/DR/MM组合导航技术对AGV定位系统进行了研究。
关键词：AGV；定位系统；GPS；DR；MM；组合导航1. 引言自动导引车（Automated Guided Vehicle，AGV）是一种能够提供自主导航功能的无人车辆，广泛应用于物流、制造业等领域。
AGV的自主导航依赖于准确的定位技术，而传统的单一定位技术由于受制于环境因素，难以达到高精度定位的要求。
因此，采用多种定位技术的组合导航成为解决AGV定位问题的有效方法。
2. GPS定位技术全球定位系统（Global Positioning System，GPS）是一种采用卫星定位原理的导航系统，具有全球覆盖、高精度等特点。
AGV可通过接收GPS卫星信号获取自身的经纬度信息，从而得到位置信息。
然而，GPS定位存在误差，尤其在城市峡谷、高楼大厦等复杂环境中定位精度会降低。
3. 惯性导航技术惯性导航（Dead Reckoning，DR）是通过测量车辆自身的加速度和角速度来估计位移和方向。
DR技术不依赖于外部参考信号，可以实现实时定位。
然而，DR技术存在漂移问题，随着时间的推移误差会逐渐积累，导致定位精度下降。
4. 微波测距技术无线微波测距（Microwave Ranging，MM）技术是一种通过测量无线微波信号的传播时间来确定距离的技术。
MM技术可以提供高精度的距离信息，用于辅助定位。
然而，MM技术对于环境的要求较高，在有干扰或反射环境下精度会受到影响。
5. GPS/DR/MM组合导航技术为了克服单一定位技术的不足，本文提出了基于GPS/DR/MM组合导航技术的AGV定位系统。
系统首先利用GPS获取初始位置信息，然后通过DR技术计算位移和方向信息，再利用MM技术获取辅助距离信息，最终通过无迹卡尔曼滤波算法融合得到高精度的定位结果。
GPSDR车载组合导航定位系统研究的开题报告
GPSDR车载组合导航定位系统研究的开题报告一、选题背景和意义随着现代社会交通运输的快速发展，车辆导航定位系统成为人们在出门旅行时比较重要的工具之一。
在现代城市中，车辆驾驶人员需要根据导航系统提供的路线规划来完成车辆行驶任务。
因此，车辆导航定位系统的精度、速度和可靠性是车辆驾驶人员选择该系统的重要前提条件。
与此同时，为了满足人们对车辆安全和驾驶体验的需求，车载导航系统功能不断普及和深化，不仅能为车辆驾驶人员提供导航引导功能，而且能提供车辆状态实时监测和智能分析等服务。
现今比较成熟的导航定位技术主要有GPS、GLONASS、BD（北斗）等卫星导航系统。
然而，在城市中心和建筑密集地区，信号延迟、信号干扰和多径效应等导航问题就会多出现，因此这些卫星导航系统的定位精度会受到很大的影响。
因此，发展可靠的车载组合导航定位系统已成为解决城市车辆导航定位精度问题的一个重要途径。
二、研究目的和内容本研究旨在通过对车载组合导航定位系统的分析和研究，提高城市车辆导航定位的精度和可靠性。
具体目的和内容如下：1. 分析车载组合导航定位系统的基本原理和功能，探讨车载组合导航定位系统与传统卫星导航系统的区别和优势；2. 研究车载组合导航定位系统的传感器选择和集成，优选合适的传感器进行组合定位，提高车辆定位精度和稳定性；3. 分析车辆在城市中行驶的特点和定位难度，提出适用的全局和局部定位算法，优化车辆定位效果；4. 开展车载组合导航定位系统的性能测试和数据分析，验证车辆定位效果，为车辆定位系统的实际应用提供参考和指导。
三、研究方法和实现计划本文采用文献调研、理论分析和实验测试相结合的方法进行研究。
实现计划如下：第一阶段：对车载组合导航定位系统的研究和分析。
综合比较不同车载组合导航定位系统的原理和功能，研究车载组合导航定位系统在城市中行驶的定位难度和优化策略。
预计用时2周。
第二阶段：优选传感器进行组合定位。
通过对车载组合导航定位系统的传感器进行选择和集成，提出一种比较适应城市道路的定位方法。
GPS概论第五章GPS卫星定位基本原理PPT课件
线性化后：
i
X ( 0 0)x ii dX Y(0 0)yii dY Z (0 0)zii dZ (0)i NctV RctV S (Vio)niVtrop
x(i0X )i0dX y(i 0)Yi0dY z(i0Z )i0dZ (0)iNctV RctV S (Vio)niVtrop
误差方程为：
• 定义
– 单独利用一台接收机确定待定点在地固坐标系中绝对位 置的方法
• 定位结果－与所用星历同属一坐标系的绝对坐标
– 采用广播星历时属WGS-84
– 采用IGS – International GPS Service精密星历时为 ITRF – International Terrestrial Reference Frames
~ N 0 Int ( ) Fr ( )
• 整周计数 Int
载波相位观测值
• 整周未知数（整周模糊度） N 0
19
载波相位测量的观测方程
原始形式：
(iN i) ictR V ctS V (V i o)in V trop i iN i ictR V ctS V (V i o)in V trop
接收机根据自身 的 钟 在 tR时 刻 所 接 收 到 卫 星 在 tS 时刻所发送信号 的相位
(tS)
tR tS
R
R
理想情况
实际情况
18
载波相位观测值 ti
• 观测值
Fr i Int() i N 0
首次观测：
0 Fr ( ) 0
t0
以后的观测：
Fr 0 N0
i Int ( ) i Fr ( ) i 通常表示为：
载波波长为原来波长的一半，信 号质量较差（信噪比低，降低了 30dB）
基于DSP的车载GPS／DR组合导航系统硬件设计
基于DSP的车载GPS／DR组合导航系统硬件设计2010-03-07 23:38:06 来源：作者：北京市北京理工大学信息科学技术学院杜萌付梦印王美玲针对低成本组合导航技术发展的需要，结合主要传感器特点，本文介绍了以浮点DSPTMS320VC33为组合导航算法实现的核心处理器，利用TL16C554进行通信口扩展的GPS/DR组合导航系统的设计方案，给出了系统硬件的设计方法。
所设计的系统具有体积小、成本低、实时性好、可靠性高、扩展性好等特点，具有广泛的应用价值。
0引言目前，差分GPS水平定位精度已经达到3~5m，完全满足车辆定位精度的要求。
但是，由于在城市高建筑群中或穿过立交桥时，常常会出现GPS信号遮挡问题，导致GPS不能正常定位。
航位推算（DR）是常用的车辆定位技术，但方向传感器随时间积累误差较大，不能单独、长时间地使用。
采用组合导航系统能够利用GPS系统提供的位置和速度信息对DR 系统的误差进行实时的校正和补偿；当GPS信号失锁时，又可通过DR系统完成航位推算，提高了组合导航系统的可靠性。
组合导航系统除了要完成大量运算处理工作外，还要实现惯性测量单元IMU（陀螺仪和加速度计）和GPS等传感器的数据采集、与外部系统的通信、时序逻辑控制和人机接口等功能。
在这种情况下，如果仅用一片DSP芯片，则系统的实时性能较差，因此多数组合系统都采用两个或多个DSP或是由一个或几个通用的微处理器MPU加上一个DSP构成主从式多处理器系统的方案。
而目前利用DSP与FPGA结合的方案来处理高速的数字信号越来越被广泛采用。
1 GPS/DR组合导航系统组成该GPS/DR组合系统具有接收和处理里程计信息、电子罗盘信息、惯性测量单元IMU以及GPS的信息的功能，其系统组成主框图如图1所示。
GPS提供的绝对位置信息可以为DR提供推算定位的初始值并进行误差校正；另一方面，DR的推算结果可以用于补偿部分GPS定位中的随机误差，从而平滑定位轨迹。
位置-姿态并行滤波的DR／GPS组合导航算法
ａ ａ ｔ ｅｙＳ ｍｕ ａｉ ｎ ｒ ｓ ｌ ｎ ｉ ａｅ ａｌ ｓ ｎ ｏ ｓ ｎｏ ｍａｉ ｎ ｃ ｎ ｂ ｓ ｄ ｅ ｅ ｔ ｅｙ ａ ｄ ｇｗ ｅ ｆ ｃ ｅ ｕ ｔ ｄ ｐ ｉ ｌ ． ｉ ｌｔ ｕ ｔ ｉ ｄ ｃ ｔ １ ｅ ｓ ｒ ’ｉ ｆｒ ｔ ａ ｅ ｕ ｅ ｆ ｃｉ ｌ ． ｎ ｌ ｐ ｒｅ ｔ ｒ ｓ ｌ． ｖ ｏ ｅ ｓ ｏ ｖ Ｋｅ ｗｏ ｄ ： ｄ ａ ｅ ｋ ｎ ｎ ＧＰ ｉ ｔｇ ａｅ ａ ｉａｉ ｎ ｓ ｓｅ ， ＫＦ ｙ ｒｓ ｅ ｄ ｒ ｃ ｏ ｉｇ， Ｓ， ｅ ｒ ｔ ｄ ｎ ｖｇ ｔ ｙ ｔ ｍ Ｅ ｎ ｏ
驶时车体的俯仰对航 向角的测量影 响不可忽视 。 本文就如何利
用 低 成 本 的 Ｄ 和 Ｇ ｓ进行 姿 态 、 置 组 合 进 行 了研 究 ， 卡 Ｒ Ｐ 位 以
系统 最 终 的定 位结 果 ： 当 Ｇ Ｓ接 收 机 受 到 屏 蔽 不 能 正 常 工 作 Ｐ 时 ． 导航 系统 仍 然 能 通 过 推 算 提 供 导 航 信 息 。 主
本 文 滤 波 器 采 用 位置 、角 度 分别 滤波 的并 联 滤 波 器 结 构 。
一
和Ｄ Ｒ的导航参数信息得 出定位结果 ， 一旦 Ｇ Ｓ失效航位 推算 Ｐ
般 来 说 １ 的 采样 频 率 可 以满 足位 置 对 数 据 更 新 率 的 要 求 ， Ｈｚ 同时 为 避 免 量 测相 关 问题 要 求 位 置 滤 波 周 期 要 大 于 Ｇ Ｓ的 位 Ｐ 置时 间 相 关 常 数 ， 因此 采 用 位 置 、 角速 度 并 行 滤 波方 式 ， 置 子 位 滤波 器 以较 长 的滤 波周 期 处 理 位 置数 据 ， 度 子 滤 波 器 以较 短 角 的周 期 处 理 姿 态 角 数 据 ， 既满 足 了位 置 和 角 度 对 滤 波 周 期 要
第五章 GPS-DR组合导航
（2）传感器数据的集中处理导致滤波器的 容错性能较差。当一种传感器的数据出现 较大偏差时,该错误将会被传播到所有观 测量和状态变量的估计中去,导致状态污 染,滤波精度和稳定度下降。
分散式滤波技术就是为了解决集中式kalman滤 波器的上述缺陷而提出的, 它将单独的Kalman滤波 器用一个主滤波器和一组局部滤波器来代替,相应 的数据处理过程也分为两个阶段。在第一阶段中, 每个传感器的信息被送入对应的局部滤波器中处理, 产生局部最优状态估计;在第二阶段,主滤波器将局 部滤波器输出的局部状态估计进行融合处理,并产 生最终的全局状态最优估计。在分散滤波过程中, 由于不同传感器的数据被单独和并行处理,因而减 少了计算量,计算效率也大大提高。
第五章
GPS/DR组合导航
5.1 GPS/DR组合方法简介 5.2 联邦滤波算法
5.1 GPS/DR组合方法简介
GPS定位的方法在车辆定位与导航中应用 日趋广泛,但是由于GPS容易受到外界环境的 干扰,尤其是在高楼林立的城市,或者车辆通 过隧道及立交桥时,GPS 卫星信号将很差甚 至中断而无法定位。要得到连续可靠的定位 信息就需要其他的辅助手段。DR是典型的独 立定位技术,在短时间内能够保持较高的精 度,且其有效性不受外界影响,但该方法仅能 确定相对位臵,且误差将随推算过程而累加。
利用kalman滤波实现多个传感器的信 息融合有两种途径:一种是标准的集中式 kalman滤波,另一种是分散式kalman滤波。 采用集中式kalman滤波,理论上可以获得 系统状态的最优估计,但是在实际应用中 存在着以下缺陷:
（1）采用严格最优估计的方法对系统所有 的测量量进行集中处理,系统状态维数高, 计算负担重,严重影响了滤波器的动态性 能和实时性。
因此,GPS和DR存在很强的互补关系。 一方面,GPS提供的绝对位臵信息可以为 DR提供推算定位的初始值并进行误差校正; 另一方面,DR的推算结果可用于补偿部分 GPS定位中的随机误差,从而平滑定位轨迹。 所以,利用适当的方法将两种系统组合起 来,充分利用其定位信息的互补性,就能够 获得比单独使用任何一种方法时都要高的 定位精度和可靠性。
AGV用GPS-DR组合导航信息融合
2011年1月第27卷第1期 沈阳建筑大学学报(自然科学版)Jo urnal o f Shenyang Jianzhu U n i v ersit y (N atura l Sc i ence)Jan. 2011V o l .27,N o.1收稿日期:2010-09-20基金项目:辽宁省自然科学基金项目(20072009)作者简介:张晓霞(1959 ),女,教授,博士,主要从事高温材料性能、结构分析研究.文章编号:1671-2021(2011)01-0173-05AGV 用GPS/DR 组合导航信息融合张晓霞,汪传龙,韩刘柱,李晓琳(沈阳建筑大学交通与机械工程学院,辽宁沈阳110168)摘 要:目的研究一种满足室外自动导引小车(AGV )导航要求的高精度、低成本的组合导航系统.方法应用信息融合技术,通过航位推算系统辅助GPS 定位,并采用卡尔曼滤波的融合算法,克服了GPS 信号的遮蔽与中断问题,使组合系统能够较好地满足AGV 导航定位的需要.用VC++编写融合算法以及实验控制程序,实现了实验数据的采集以及对AGV 的运动控制.结果给出了GPS 单独导航和GPS /DR 组合导航两种工作状态下的位置误差.从对比中可以看出,组合导航系统的定位精度比GPS 单独导航的定位精度提高了50%.结论组合导航系统可以减小甚至消除推算定位系统随时间积累的误差,并在任何情况下能较好地实现AGV 的实时、可靠、准确定位.关键词:信息融合;AGV;定位技术;GPS /DR 组合定位;卡尔曼滤波中图分类号:V 249 32+8 文献标志码:AOn the GPS /DR Integrated Navigati on Syste m for AGVZHANG X iaoxi a,WANG Chuan l ong,HAN L i uzhu,LI X iao li n(Schoo l o f T raff i c and M echan ica l Eng i neering,Shenyang Jianzhu U niver sit y,Shenyang,Ch i na ,110168)Abst ract :A GPS /DR i n tegra ted nav igati o n sy ste m is i n vestigated to m ee t the outdoo r auto m ati c gui d ed ve h icle(AGV )nav i g ation require m ents such a s h igh accuracy and l o w co s.t In t h is paper ,the Dead Reckon i n g Sy ste m is assisted w ith the GPS po siti o n,and the proble m o f GPS si g nal shie l d i n g and i n terrupti n g is over com e through the Ka l m an F ilter .B y tak i n g full adv antages o f the i n for m a ti o n fusi o n theory ,the i n teg rated syste m can be w e ll satisfi e d w ith the de m ands o fAGV nav igation o rienta ti o n.M o reo ver ,the AGV m ov e m en t contro l and experi m enta l data acquisition are obta i n ed by usi n g VC++fusi o n and experi m en tal contro l pro gra m.The nav igation po sitioning erro rs o f t h e GPS and i n tegra ted G PS /DR w ere co m puted as w e l.l The po sitioning accuracy o f the integ rated nav i g a ti o n sy ste m increases by 50%in com parison w ith t h at of t h e G PS nav i g ation a l o ne .Therefo re ,the i n teg rated nav igation can actua lize rea l ti m e ,credible and ex act AGV po si ti o n i n g w ith a m i n i m um position i n g erro r ,and decrease or eli m i n ate t h e reckon i n g po siti o n i n g sy ste m erro r accum ulated w ith ti m e .K ey w ords :infor m ati o n fusion;AGV;po sitioning techno logy ;integ rated G PS /DR nav igation ;ka l m an filter 全球定位系统(G PS)可以提供全天候、连续、实时的高精度定位信息,而且其具有误差不随时间累积、使用成本低等优点,因此在车辆导航系统中被普遍采用.但是GPS 接收机在城市中受到高大建筑物、桥梁、隧道、树木等的遮挡时,会引起信号的失锁,此时就不能够为用户提供导航服务.174 沈阳建筑大学学报(自然科学版)第27卷DR(D ead-R eckoning )是一种常用车辆导航定位技术,它通过一个方向传感器和一个速度传感器推算出车辆的瞬时位移增量,在用户初始位置已知的情况下求解出用户的位置.由于方向传感器的误差随着时间的延长而积累,所以DR 不能够单独、长时间为用户提供导航服务.因此将G PS 与DR 组合,是一种比较理想的组合导航方式.在车辆GPS /DR 组合定位系统中,卡尔曼滤波是应用最为广泛的数据融合算法[1-4].K a l m an 滤波方法用于G PS /DR 组合定位中,就是将G PS 和DR 的信息同时用于定位解的求解过程中,使DR 系统的状态在滤波过程中不断得到修正,组合定位的输出又可以提供较为准确的初始位置和方向信息,从而即使在G PS 失效,单独使用DR 推算定位时也能长时间保持较高精度,同时利用K a l m an 滤波可以有效地消除G PS 和DR 传感器定位测量中的随机误差,系统的可靠性得到了提高[5-10].目前国内外学者对室外AGV 的导引技术的研究还比较欠缺,因此笔者利用GPS /DR 组合导航系统和卡尔曼滤波数据融合算法的优点,应用到室外AGV 定位导引技术中,结果使AGV 定位精度比G PS 单独定位精度提高58 3%,同时使AGV 能够适应室外大范围的作业.1 G PS /DR 组合导航的卡尔曼滤波器定位设计在组合导航系统中采用多传感器信息融合技术可以扩展整个系统的时间和空间覆盖率,增加系统的信息利用率,提高融合数据的置信度和精度,增强系统的容错性和可靠性.卡尔曼滤波器是一个最优和高效的传感器数据融合技术.卡尔曼滤波器应用于AGV 定位将GPS /DR 导航组合的定位问题陈述为传感器数据融合问题.本系统G PS /DR 信息融合结构如图1所示.图1 G P /DR 信息融合结构图F ig 1 G P /D R i nfo r m ati on fusi on structure2 G PS /DR 组合卡尔曼滤波器定位技术的信息融合2 1 里程计位姿估计模型简介2 1 1 差动驱动AGV 的里程计位姿估计模型对平面移动的差动驱动AGV 车体,导航应用要求AGV 车体的位置和姿态(简称位姿).在AGV 运动平面上建立的现场坐标下,可以用以下向量表示p =[x y ]T.(1)式中:x,y 为AGV 在现场坐标系下xy 平面的位置; 为AGV 车体在现场坐标系下对x 轴的姿态角度.对于差动驱动的AGV 来说,后续位姿可以依据初始位置和姿态,依据两个驱动车轮的行进路程推算得出.对具有固定采样间隔 t 的离散系统,行走距离的增量( x, y, )为x = s co s( + /2),(2) y = s si n ( + /2),(3) = s r - s lb ,(4) s=s r + s l2.(5)式中: s l , s r 分别为采样间隔时间内左右轮行走的距离;b 为差动驱动AGV 两个轮子之间的距离.位置更新进一步得到AGV 位姿更新的基本方程:p =f(x,y, , s r , s l ).(6)2 1 2 差动驱动AGV 的里程计位姿估计误差模型在误差模型的建立中,主要考虑了差动驱动AGV 两个驱动轮行走距离引入的误差.并且假设AGV 在初始的位姿协方差矩阵 p 为已知.对运动增量( s r , s l ),假定协方差矩阵 为=co v ar ( s r , s l )=k r | s r |00k l | s l|.(7)式中: s r , s l 为各轮行走的距离,k r 和k l 为误差系数,代表马达驱动和轮子地面交互的非确定性参数,其值可以通过实验取得.本实验编码器误差系数为0 1~0 3.假定采样周期前一时刻位姿p 与 s rl =( s r , s l )不相关,对式(6)线性化(一阶泰勒展第27卷张晓霞等:AG V用G PS/D R组合导航信息融合175开)后利用误差传播定律有[11-14]P = p f p p f T+rl frlf T.(8)式中: p为前一步 p 给定.2 2 G PS/DR组合导航的卡尔曼滤波器定位(1)AGV位置预测根据AGV老位姿(在时间步k)和由于控制输入u(k)产生的运动,通过DR系统预测AGV 在时间步k+1的位姿和预测误差方差阵p^(k+1|k)=f(p^(k|k),u(k)),(9)p(k+1|k)= p f p(k|k) p f T+ u f u(k) u f T.(10)由差动驱动的AGV,式(9)可以由式(6)推导得出,其中u(k)= s r l=( s r, s l).(2)观测(量测值)G PS输出的是以经纬度为坐标的协议地球参考系(W GS-84),坐标原点在地球质心,为了便于使用,需要进行高斯-克吕格投影计算,转化为平面直角坐标系.如果将G PS接收机看作AGV的一个位姿传感器,那么输出的导航定位数据就是AGV的位姿的一组观测数据.在时刻k+1得到GPS输出的AGV的位姿数据z(k+1)及其协方差 R(k+1).即:z(k+1)=(x G PS y G PS GPS)T,(11)R(k+1)= 2xG PS xyG PS0yxGPS yG PS000GPS.(12)由于GPS解算位置和解算速度采用不同原理,故其位姿方差阵中姿态角与位置不相关.(3)GPS接收机输出数据有效性的判断在G PS/DR导航组合中,G PS的观测值序列的位置数据易出现大的突变和跳跃(其中某些观测数据无效);转化后的GPS输出坐标首先利用阈值法根据机器人里程计的定位数据剔除野值,阈值的选取可以根据实验观测数据来设定.通过DR系统在短时间和短范围内具有较高的位姿的精度,在到达新的位置后,依据M aha l a nobis距离来确立门限判断GPS数据的有效性.针对G PS输出的定位数据和速度数据的非相关性,分别计算G PS到DR的位置和姿态M ahalanobis距离r xy(k)、r (k)计算如下:2r xy(k)2=(X GPS(k)-X0(k))T( 0(k)+G PS(k+1))-1(X GPS(k)-X0(k)),(13)XDR(k)=(xDR(k),yDR(k))T,(14)D R(k)=xD R(k)2 xyDR(k)xyDR(k) yDR(k)2,(15)G PS(k)=xG PS(k)200 yG PS(k)2,(16) 2r DR(k)2=( GPS(k)- D R(k))T( DR(k)2+ G PS(k)2)-1( G PS(k)- DR(k)).(17)式中:X DR(k), D R(k), DR(k), DR(k)为DR的位置和姿态数据以及方差误差;X GPS(k), G PS(k), G PS(k), G PS(k)为G PS接收机输出的位置和航向转角数据及误差.根据试验确定M aha lanob is距离的门限值[11],通常情况下位置M aha lanob is距离门限值r xy th=1 2,位置M aha lanob is距离门限值r th=1 6[12-15].当计算得到的马氏距离大于阈值时,将G PS接收机输出数据视为错误值舍去;小于阈值则加以利用.分别判断G PS位置和姿态,修正G PS位姿如下:a.r xy(k) r xy th当r (k) r th时,说明GPS接收机的位置数据和航向角数据均有效.p GPS(k)=[x GPS(k),y G PS(k), G PS(k)],(18) GPS(k)=2xGPS(k)000 2yG PS(k)000 2 GPS(k).(19)当r (k)>r th时,说明GPS接收机的位置数据有效,航向角数据无效,式中:G PS(k)=0, G PS=0.b.r xy(k)>r xy th当r (k) r th说明G PS接收机的位置数据无效,航向角数据有效:p GPS(k)=[0,0, G PS(k)],(20)GPS(k)=00000000 2 G PS(k).(21)当r (k)>r th说明G PS接收机的位置和航向角数据均无效,式中: G PS(k)=0, G PS=0.(4)测量预测和匹配通过DR系统的位置预测,直接得到了AGV 导航定位的位姿预测z^(k+1).176沈阳建筑大学学报(自然科学版)第27卷^z(k+1)=H(k+1) p^(k+1|k).(22)式中:H(k+1)=I(单位阵).在G PS/DR导航组合中,DR系统的数据采集和GPS导航数据的采集在时间上保持同步,所以认为G PS观测位姿在z(k+1)和DR系统的测量预测^z(k+1)是匹配的.从配对的G PS观测位姿z(k+1)和DR系统的测量预测^z(k+1)可以计算修正v(k+1).v(k+1)=z(k+1)-^z(k+1).(23)修正的协方差 I N(k+1)可以通过误差传播率求得:I N(k+1)=H(k+1) p(k+1|k)H T(k+1)+ R(k+1).(24)(5)卡尔曼滤波位姿估计对G PS输出的位姿数据作判断和修正后,得到了相对可靠的位姿数据.接下来根据DR的位置预测和在k+1时刻的GPS观测,计算AGV位姿的最佳估计p^(k+1|k+1).卡尔曼滤波位姿估计的增益矩阵:K(k+1)= p(k+1|k) H(k+1) -1I N(k+1).(25)卡尔曼滤波位姿估计:p^(k+1|k+1)=p^(k+1|k+1)+K(k+ 1) v(k+1).(26)卡尔曼滤波位姿方差估计:p(k+1|k+1)= p(k+1|k)-K(k+ 1) I N(k+1) K T(k+1).(27)以上各式实现了GPS/DR导航组合的卡尔曼滤波位姿估计和方差更新.3 G PS/DR组合导航试验为了验证笔者所设计的定位方法的定位精度和有效性,以博创旅行家II型轮式移动机器人为平台,在沈阳建筑大学进行了实验.由于受到G PS 差分信号转发范围的限制,实验在足球场上进行.令AGV行走100m,时速为0 2m/s.导航定位结果如图2~5所示.图2和图3中分别给出了AGV实际运行轨迹线,GPS单独滤波和GPS/DR 组合滤波的变化曲线.比较图2和图3可知,G PS 单独滤波定位精度为1 2m,采用基于卡尔曼滤波的GPS/DR数据融合后,定位精度为0 5m,比GPS单独滤波定位精度提高了58 3%.并且输出轨迹的平顺性有一定提高.图2 G PS单独滤波曲线图F ig 2 F ilter curve o fG PS a l one图3 G P S/DR组合滤波曲线图F i g 3G P S/DR com b i nati on filter curve s图4和图5分别给出了沿直线运动的AGV 姿态角在融合前后的效果.图4 G PS原始数据AGV姿态角F i g 4 AG V attitude ang les based o n the raw G PS da ta图5 G PS/DR数据融合以后AG V姿态角F i g 5 AG V attitude ang les after G PS/DR da t a fusi o n 比较图4和图5可见,通过数据融合以后姿态角有了明显的改善,但是融合以后的姿态角仍然有一定偏差(准确偏差为0).存在偏差的原因是G PS的姿态角是由GPS输出的运动速度数据第27卷张晓霞等:AG V用G PS/D R组合导航信息融合177转换得到,同时G PS数据本身就存在一定偏差,故姿态角必然存在偏差.但采用更高精度的G PS 接收机可以进一步减小偏差.4 结 论试验结果表明,AGV用GPS/DR组合导航系统是一种高精度的实时导航系统.在该系统中,采用低成本的航位推算DR传感器辅助GPS定位,克服了GPS单独使用时因信号中断而引起的系统失效,具有较高的性价比.同时笔者利用DR 数据,依据M ahalanobis距离来确立门限值以判断G PS数据.提高了系统中G PS数据的有效性,也就提高了系统的精度.参考文献:[1] 张男,王莹,张尔扬.G PS/I N S组合导航系统设计及实现方案[J].遥感技术与应用,2004,17(2):70-72.(Z hang N an,W ang Y i ng,Z hang E ry ang.The desi g n and rea liz i ng sche m e o f G PS/I N S com bi ned nav igati on sy ste m[J].R e m o te Sensi ng T echno l o gy andA ppli ca tion,2004,17(2):70-72.)[2] 刘巍巍,赵红.遗传算法在自动化立体仓库路径调度问题中的应用[J].沈阳建筑大学学报:自然科学版,2008,30(3):339-340,360.(L iuW ei w e,i Z hao H ong.A ppli ca tion o f geneti c a l go r it hm i n pa t h scheduli ng o f autom a ted w areho us e[J].J o urnal o f Shenyang Jianzhu U niversity:N a tura lSc ience,2008,30(3):339-340.)[3] W h itaker W R.Po siti on and o rien tati on da t a fo r autonom ous nav i ga ti on[J].G PS W o rld,2006(9):71-75.[4] H iroka w a R,Sato K,Is h i hara R,et a.l T hread i ng them aze G P S/I N S,l and m ark s ensi ng and ob stac l e avo i dance[J].G PS W o rd,2004(11):20-27.[5] M aughan F G,L e w isH J.AGV co ntro lled FM S[J].A S HRA E T ran s ac ti ons,2003,109(1):27-35.[6] M i chi ko W,M asash i F.Inte lli g ent AGV driv i ng tow ard an autonom ous decen trali zed m anufacturi ngsy ste m[J].R o bo tics and C om puter Integ ra ted M anufac t ur i ng,2001,17(2):57-64.[7] K o ch K R,Y ang Y.R obust K al m an filt e r fo r rankdefic i ent o bserv ati o n m ode ls[J].Jo urnal o f G eo desy,1998,72:7-8.[8] 徐景硕,秦永元,彭蓉.自适应卡尔曼滤波器渐消因子选取方法研究[J].系统工程与电子技术,2004,26(11):1552-1554.(X u Ji ng s huo,Q i n Y ongy uan,P eng R ong.N ewm e t hod fo r se l ec ting adapti v e ka l m an f ilter fadi ngfac t o r[J].Sy stem s Eng i neer i ng and E l ec tron ics,2004,26(11):1552-1554.)[9] 万德钧,房建成,王庆.G P S动态滤波的理论、方法及其应用[M].南京:江苏科学技术出版社,2000:73-80.(W an D ejun,F ang Jiancheng,W ang Q i ng.T heory and m e t hod o f G P S dyna m ic filteri ng and its appli cati on[M].N an ji ng:Jiang su Sc ience and T echno l o gyPub lish i ng H o us e,2000:73-80.)[10]许丽佳,陈阳舟,崔平远.G PS/IN S组合导航系统中的信息融合算法研究[J].计算机仿真,2004,21(5):20-23.(X u L iji a,C hen Y ang zhou,Cu i P i ngyuan.S tudy o ni nfo r m ati on fusi on techno l o gy i n G PS/I N S i n teg ratednav iga tion sy st em[J].C om put e r S i m ul a ti on,2004,21(5):20-23.)[11]Sun S L.M ulti sen s o r op ti m a l i nfo r m ati on fusi o nK a l m an filter w it h applicati o n[J].A ero space Sc ience and T echno l o gy,2004,8(1):57-62.[12]M o ham ed A H,Schw a rz K P.A dap tive kal m an f ilter i ng fo r I N S/G PS[J].Journa l of G eodesy,1999,73(4):1024-1030.[13]史忠科.最优估计的计算方法[M].北京:科学出版社,2001.(Shi Z hongke.C alculati on o f opti m al esti m ati o n [M].B e ijing:Sc i ence Publis h i ng Ho use,2001.) [14]武汉大学测绘学院测量平差学科组.误差理论与测量平差基础[M].武汉:武汉大学出版社,2003.(Survey ing and M appi ng R esearch G ro up o f W uhanU niver sity.Erro r t heo ry and fo undati on o f surv ey i ngad j ust m ent[M].W uhan:W uhan U niversity P re ss,2003.)[15]C o llier M.A m i cro AG V fo r f l ex ible m anufacturi ngi n s m a ll enterpr ises[J].Integ ra ted M anufacturi ngSy ste m s,2003,14(5):442-448.。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
与此同时,局部滤波器的存在也使整个多传感器 融合系统的容错能力有所 提高。分散滤波理论已经 发展了20多年,Person，Speyer， Willsky,Bierman,Kerr, Carlson等人先后对其进行 了改进和提高。在所有这些分散滤波方法中,由 Carlson 提出的联邦滤波器以其设计的灵活性、计 算量小和容错性好而受到了广泛重视。目前利用联 邦式滤波器实现多传感器信息融合已成为国际上导 航和制导技术领域内的一个研究重点,并被美国空军 列入其发展新一代容错导航系统的"公共Kalman滤波 器"计划。
另外,DR所需要的初始信息由前一时 刻的有效GPS数据提供,这使得GPS数据中 的误差被直接传播到了DR推算的结果中, 造成DR系统的定位精度下降,从而缩短了 其能够满足定位精度要求的有效工作时间。 另外,该方法只能解决卫星遮挡问题,而对 于其他原因如多径效应等造成的GPS定位 误差则无能为力。
kalman滤波方法用于GPS/DR的组合定 位中,就是将GPS和DR的信息同时用于定位 解的求解过程中,使DR系统的状态在滤波 过程中不断得到修正,组合定位的输出又 可以提供较为准确的初始位臵和方向信息, 从而,即使在GPS失效、单独使用DR推算定 位时也能长时间保持较高的定位精度。第来自章GPS/DR组合导航
5.1 GPS/DR组合方法简介 5.2 联邦滤波算法
5.1 GPS/DR组合方法简介
GPS定位的方法在车辆定位与导航中应用 日趋广泛,但是由于GPS容易受到外界环境的 干扰,尤其是在高楼林立的城市,或者车辆通 过隧道及立交桥时,GPS 卫星信号将很差甚 至中断而无法定位。要得到连续可靠的定位 信息就需要其他的辅助手段。DR是典型的独 立定位技术,在短时间内能够保持较高的精 度,且其有效性不受外界影响,但该方法仅能 确定相对位臵,且误差将随推算过程而累加。
（2）传感器数据的集中处理导致滤波器的 容错性能较差。当一种传感器的数据出现 较大偏差时,该错误将会被传播到所有观 测量和状态变量的估计中去,导致状态污 染,滤波精度和稳定度下降。
分散式滤波技术就是为了解决集中式kalman滤 波器的上述缺陷而提出的, 它将单独的Kalman滤波 器用一个主滤波器和一组局部滤波器来代替,相应 的数据处理过程也分为两个阶段。在第一阶段中, 每个传感器的信息被送入对应的局部滤波器中处理, 产生局部最优状态估计;在第二阶段,主滤波器将局 部滤波器输出的局部状态估计进行融合处理,并产 生最终的全局状态最优估计。在分散滤波过程中, 由于不同传感器的数据被单独和并行处理,因而减 少了计算量,计算效率也大大提高。
实现GPS/DR组合的核心问题是数据融 合方案的设计,即采用何种方法来融合两 种定位系统的信息以获得最优的组合定位 结果。在车辆导航定位中有两种常用的组 合方案:切换式组合和Kalman滤波。
切换式组合方案有两种工作状态:GPS模式和 DR模式,系统工作在何种模式取决于GPS信号的有 效性:当GPS观测卫星数较多、卫星几何分布结构 较好时工作于GPS模式,同时利用GPS的定位输出 刷新DR系统的初始推算位臵;一旦GPS定位数据失 效或者GDOP增大到预定门限,则切换到DR模式。 该方法的优点是简单易行,系统承担的计算量很 小,可以解决在卫星信号失效时短时间的定位问 题。但是该方法并没有将GPS和DR两种系统的信 息融合在一起,不能完全发挥两者的优点。
根据利用GPS信息的不同,基于Kalman 滤波的GPS/DR组合方案又分为松组合和紧 组合两种:松组合直接利用GPS接收机输出 的定位解与DR进行数据融合;紧组合利用 GPS接收机输出的伪距、伪距率等原始信 息与DR所需要的信息(车辆行驶距离和方 位角 的变化)进行数据融合。
5.2
联邦滤波算法
因此,GPS和DR存在很强的互补关系。 一方面,GPS提供的绝对位臵信息可以为 DR提供推算定位的初始值并进行误差校正; 另一方面,DR的推算结果可用于补偿部分 GPS定位中的随机误差,从而平滑定位轨迹。 所以,利用适当的方法将两种系统组合起 来,充分利用其定位信息的互补性,就能够 获得比单独使用任何一种方法时都要高的 定位精度和可靠性。
利用kalman滤波实现多个传感器的信 息融合有两种途径:一种是标准的集中式 kalman滤波,另一种是分散式kalman滤波。 采用集中式kalman滤波,理论上可以获得 系统状态的最优估计,但是在实际应用中 存在着以下缺陷:
（1）采用严格最优估计的方法对系统所有 的测量量进行集中处理,系统状态维数高, 计算负担重,严重影响了滤波器的动态性 能和实时性。