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-05
AGV 用GPS/DR 组合导航信息融合
张晓霞,汪传龙,韩刘柱,李晓琳
(沈阳建筑大学交通与机械工程学院,辽宁沈阳110168)
摘 要:目的研究一种满足室外自动导引小车(AGV )导航要求的高精度、低成本的组合导航系统.方法应用信息融合技术,通过航位推算系统辅助GPS 定位,并采用卡尔曼滤波的融合算法,克服了GPS 信号的遮蔽与中断问题,使组合系统能够较好地满足AGV 导航定位的需要.用VC++编写融合算法以及实验控制程序,实现了实验数据的采集以及对AGV 的运动控制.结果给出了GPS 单独导航和GPS /DR 组合导航两种工作状态下的位置误差.从对比中可以看出,组合导航系统的定位精度比GPS 单独导航的定位精度提高了50%.结论组合导航系统可以减小甚至消除推算定位系统随时间积累的误差,并在任何情况下能较好地实现AGV 的实时、可靠、准确定位.关键词:信息融合;AGV;定位技术;GPS /DR 组合定位;卡尔曼滤波中图分类号:V 249 32+
8 文献标志码:A
On the GPS /DR Integrated Navigati on Syste m for AGV
ZHANG 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 structure
2 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 l
b ,(4) s=
s r + s l
2
.(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 |
00
k 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
f
rl
f 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)=
!2
xG PS
!xyG PS 0!yxGPS
!yG PS 0
!
GPS
.
(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 )-X 0(k ))T
( 0(k )+
G PS (k +1))
-1
(X GPS (k)-X 0(k )),
(13)X DR (k )=(x DR (k ),y DR (k ))T
,(14)
D R (k )=!xD R (k )2
!xyDR (k )!xyDR (k )!yDR (k )
2
,(15)
G PS (k )=
!xG PS (k )
2
00
!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)=
!2
xGPS (k)
000!2
yG PS (k )00
!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 )=
0000
0!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) -1
I 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,比G
PS单独滤波定位精度提高了58 3%.并且输出轨迹的平顺性有一定提高.
图2 G PS单独滤波曲线图
F ig 2 F ilter curve o f
G PS a l one
图3 G P S/DR组合滤波曲线图
F i g 3
G P S/DR com b i nati on filter curve s
图4和图5分别给出了沿直线运动的AGV 姿态角在融合前后的效果.
图4 G PS原始数据AGV姿态角
F i g 4 A
G V attitude ang les based o n the raw G PS da ta
图5 G PS/DR数据融合以后AG V姿态角
F i g 5 A
G 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 iga
ti on sy ste m[J].R e m o te Sensi ng T echno l o gy and
A 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 l
Sc 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 au
tonom 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 the
m aze G P S/I N S,l and m ark s ensi ng and ob stac l e a
vo 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 to
w ard an autonom ous decen trali zed m anufacturi ng
sy ste m[J].R o bo tics and C om puter Integ ra ted M anu
fac 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 rank
defic i ent o bserv ati o n m ode ls[J].Jo urnal o f G eo de
sy,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 ew
m e t hod fo r se l ec ting adapti v e ka l m an f ilter fadi ng
fac 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 ca
ti on[M].N an ji ng:Jiang su Sc ience and T echno l o gy
Pub 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 n
i nfo r m ati on fusi on techno l o gy i n G PS/I N S i n teg rated
nav 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 n
K a l m an filter w it h applicati o n[J].A ero space Sc i
ence 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 ilte
r 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 uhan
U niver sity.Erro r t heo ry and fo undati on o f surv ey i ng
ad 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 ng
i n s m a ll enterpr ises[J].Integ ra ted M anufacturi ng
Sy ste m s,2003,14(5):442-448.