简化UKF算法在摄像机标定中的应用_陈益
基于UKF的机器人末端执行器位姿实时估算方法
窄带随机㊂频率范围为190~210H z,功率谱密度为0.1g 2/H z ;频率范围为384~420H z ,功率谱密度为0.025g 2/H z㊂振动条件曲线见图8㊂图7振动试验装置图图8 振动条件曲线图振动测试打印曲线如图9所示,图中测试点曲线为此装置传感器的振动测试扫描曲线㊂可以得出,加入橡胶减振器后,加速度均方根值(R M S(a)加橡胶减振器垂直方向振动曲线(b)未加橡胶减振器垂直方向振动曲线(c)加橡胶减振器水平方向振动曲线(d)未加橡胶减振器水平方向振动曲线图9 振动曲线对比值)明显减小了,垂直方向由23.1g 减小到10.0g ,水平方向由11.4g 减小到4.8g ,大大降低了由振源传递来的振动量值,取得了良好的减振效果㊂图10为光电吊舱实际挂飞过程中光学载荷 可见光电视在7000m 高空锁定跟踪汽车的截图,可以看出光学载荷成像效果良好,跟踪精度满足技术指标要求㊂图10 可见光电视在7000m 高空锁定跟踪汽车截图4 结语为了克服传统减振器普遍存在的体积大㊁质量偏大的缺点,结合机载光电吊舱光学载荷减振的需要,根据实际使用环境的要求,设计了一种轻小型的减振器㊂依据减振设计原理,确定了减振器本身的材料属性及结构尺寸㊂该减振器具有体积小㊁重量轻等优点㊂振动试验表明,应用减振器后的加速度值均方根值明显减小,减振效果明显,有效地实现了特定环境下对外部振动的隔离,对于机载吊舱内光学载荷减振具有良好的减振效果,保证了光学载荷的成像质量和光电吊舱的跟踪精度㊂参考文献:[1] Y a n g H T ,C a o JZ ,F a nZY ,e t a l .T h eR e s e a r c ho f t h eH i g hP r e c i s i o nU n i v e r s a l S t a b l eR e c o n n a i s s a n c e P l a t f o r mi nN e a rS pa c e [J ].P r o c .S P I E ,2011,8196:111‐116.[2] H a d d e nS ,D a v i sT ,B u c h e l e P ,e t a l .H e a v y L o a dV i -b r a t i o n I s o l a t i o nS y s t e mf o rA i r b o r n eP a yl o a d s [J ].P r o c .S P I E ,2001,4332:171‐182.[3] 李玉龙,何忠波,白鸿柏,等.角振动对光学成像系统传递函数影响分析[J ].中国机械工程,2012,23(15):1784‐1788.L i Y u l o n g ,H e Z h o n g b o ,B a iH o n g b a i ,e t a l .A n a l ys i s o f I n f l u e n c e o fA n g u l a rV i b r a t i o no n M T Fi nO p t i -c a l I m a g i n g S y s t e m s [J ].C h i n a M e c h a n i c a lE n g i -n e e r i n g,2012,23(15):1784‐1788.[4] 赵鹏,杨牧,张葆.航空机载光学设备的振动分析及座架减震器的设计[J ].光学精密工程,1997,7(3):58‐63.㊃1131㊃机载光电吊舱橡胶减振器的设计与应用刘家燕 程志峰 王 平Copyright ©博看网. All Rights Reserved.Z h a oP e n g ,Y a n g M u ,Z h a n g B a o .T h eV i b e r a t i o nA -n a l y s i s a n dD a m p e rD e s i g no fO p t i cI n s t r u m e n t i n F l y i n g P l a t f o r m [J ].O p t i c sa n dP r e c i s i o nE n g i n e e r -i n g,1997,7(3):58‐63.[5] 刘树峰,白鸿柏,李玉龙,等.金属橡胶光电吊舱的减振器设计研究[J ].新技术新工艺,2011(3):87‐89.L i uS h u f e n g ,B a iH o n g b a i ,L iY u l o n g ,e ta l .D e s i gn a n dA n a l y s i s o fD a m p e r f o rO p t o e l e c t r o n i cP o d [J ].N e w T e c h n o l o g y &Ne wP r o c e s s ,2011(3):87‐89.[6] 李玉龙,何忠波,白鸿柏,等.无人机载光电平台无角位移减振器研究进展[J ].航空制造技术,2011(18):78‐85.L iY u l o n g ,H eZ h o n g b o ,B a iH o n g b a i ,e ta l .R e c e n t D v a n c e o fN o n ‐A n g u l a rV i b r a t i o nA b s o r b e r f o rO p -t o ‐E l e c t r o n i c P l a t f o r m o f U A V [J ].A e r o n a u t i c a l M a n u f a c t u r i n g T e c h n o l o g y,2011(18):78‐85.[7] 甘至宏.光电吊舱内框架减振系统设计[J ].光学精密工程,2010,18(9):2036‐2042.G a nZ h i h o n g .D e s i g no f I n n e rF r a m eV i b r a t i o nA b -s o r b i n g S y s t e m f o r O p t o e l e c t r o n i cP o d [J ].O pt i c s a n dP r e c i s i o nE n g i n e e r i n g,2010,18(9):2036‐2042.[8] 朱石坚,楼京俊,何其伟,等.振动理论与隔振技术[M ].2版.北京:国防工业出版社,2008.[9] 闻邦椿,刘树英,张纯宇.机械振动学[M ].2版.北京:冶金工业出版社,2008.(编辑 王艳丽)作者简介:刘家燕,女,1968年生㊂中国科学院长春光学精密机械与物理研究所航空光学成像与测量重点实验室副研究员㊂主要研究方向为航空成像与测量方面的结构设计㊂发表论文3篇㊂程志峰,男,1979年生㊂中国科学院长春光学精密机械与物理研究所航空光学成像与测量重点实验室副研究员㊂王 平,男,1982年生㊂中国科学院长春光学精密机械与物理研究所航空成像与测量技术部助理研究员,长春理工大学光电工程学院博士研究生㊂基于U K F 的机器人末端执行器位姿实时估算方法项筱洁1 何庆稀1 应 征21.温州职业技术学院,温州,3250002.浙江省特种设备检验研究院,杭州,310022摘要:为了能在工业机器人运动过程中快速准确地估算出末端执行器的位姿,提出了一种基于u n s c e n t e d 卡尔曼滤波器(U K F )的末端执行器位姿实时估算方法,并将该方法应用于以激光跟踪仪作为反馈系统的工业机器人中㊂首先,在工业机器人运动过程中实时获取各个关节运动参数,并结合工业机器人的结构参数计算末端执行器的位姿初值,然后借助于激光跟踪仪实时跟踪测量固定在机器人末端执行器上的一个测量点,运用U K F 融合以上两类数据,估算出末端执行器的实时位姿㊂计算机仿真验证了该方法的有效性与实时性,同时表明该方法具有易于实现㊁计算速度快和精度高等优点㊂关键词:工业机器人;U K F ;数据融合;激光跟踪仪中图分类号:T P 241.2 D O I :10.3969/j.i s s n .1004-132X.2014.10.008R e a l ‐T i m eP o s eE s t i m a t i o n f o r I n d u s t r i a lR o b o t E n dE f f e c t o rB a s e do nU K FX i a n g X i a o j i e 1 H eQ i n g x i 1 Y i n g Z h e n g21.W e n z h o uV o c a t i o n a l&T e c h n i c a l C o l l e g e ,W e n z h o u ,Z h e j i a n g,3250002.Z h e j i a n g P r o v i n c i a l S p e c i a l E q u i p m e n t I n s p e c t i o na n dR e s e a r c h I n s t i t u t e ,H a n gz h o u ,310022A b s t r a c t :T o e s t i m a t e t h e p o s e o f i n d u s t r i a l r o b o t e n de f f e c t o r q u i c k l y a n da c c u r a t e l y,a r e a l ‐t i m e e s t i m a t i o nm e t h o dw a s p r o p o s e db a s e do nU K F .T h i sm e t h o dw a s a p p l i e d t o i n d u s t r i a l r o b o tw i t h l a -s e r f e e d b a c k c o n t r o l s y s t e m.I n t h e i n d u s t r i a l r o b o tm o v i n g p r o c e s s ,a r o u g h e s t i m a t i o n v a l u e o f e n d e f -f e c t o r p o s ew a s a c q u i r e d b y u s i ng f o r w a r d p o s i t i o n a n a l y s i s a n d k i n e m a t i c s p a r a m e t e r s o f i n d u s t r i a l r o -b o t i n r e a l t i m e .A n dth e p o si t i o no f a m e a s u r e d p o i n t f i x e do ne n de f f e c t o rw a so b t a i n e db y a la s e r t r a c k e r .T h e n ,U K Fw a s e m p l o y e d t o i n t e g r a t e t h e p r e v i o u s r o u ghe s t i m a t i o nv a l u e a n d t h em e a s u r e d p o i n t p o s i t i o n .T h e a c c u r a t e p o s e o f i n d u s t r i a l r o b o t e n d e f f e c t o rw a s e v a l u a t e db y u t i l i z i n g t h i sm e t h -o d .C o m p u t e r s i m u l a t i o n r e s u l t s s h o wt h a t t h e p r e s e n t e dm e t h o d i s a c h i e v e d e a s i l y ,a n dh a s f a s t c a l c u -l a t i o na n dh i g ha c c u r a c y.K e y wo r d s :i n d u s t r i a l r o b o t ;u n s c e n t e dK a l m a n f i l t e r (U K F );d a t a f u s i o n ;l a s e r t r a c k e r 0 引言工业机器人具有较强的通用性㊁柔软性㊁自动收稿日期:2014 03 11基金项目:国家高技术研究发展计划(863计划)资助重大项目(2011A A 04A 101)性与准确性等特点,因而较多地应用于柔性制造系统(F M S )㊁计算机集成制造系统(C I M S )㊁工厂自动化(F A )等领域中[1],特别是因其具有较高的重复定位精度而广泛应用于焊接㊁喷涂㊁搬运等工作中㊂与重复定位精度相比,工业机器人的绝对㊃2131㊃中国机械工程第25卷第10期2014年5月下半月Copyright ©博看网. All Rights Reserved.定位精度较低,限制了其在航空㊁航天制造与机械加工领域等的应用㊂为了提高工业机器人的定位精度,国内外目前使用较多的方法是在末端执行器上添加反馈系统,使用较多的反馈系统有三类:光学测量系统[2](如激光跟踪仪㊁i G P S等);视觉检测系统[3];力检测系统[4]㊂本文针对采用激光跟踪仪作为反馈系统的工业机器人提出了一种末端执行器姿态实时估算方法㊂W a n g等[5]使用激光跟踪仪实时补偿工业机器人末端三轴动力头的运动误差,但为了降低系统复杂性与成本,该方法只能在三自由度的机器人上使用㊂刘万里[6]设计了一种能够在复杂型面的被测对象表面上运动的小型轮臂复合式激光制导测量机器人,该机器人能够通过激光跟踪仪与视觉系统的数据确定自身的姿态㊂V i n c z e等[7]提出了一种采用激光跟踪仪与视觉系统实时测量机器人位姿的方法,其中机器人的位置信息由激光跟踪仪获取,机器人的姿态由激光跟踪仪与视觉系统共同获取㊂以上定位方法均需要其他测量系统辅助完成,或是无法同时完成位置与姿态的估计㊂本文针对工业机器人的特点,提出了一种末端执行器位姿估算方法㊂首先,在机器人运动过程中通过机器人各个运动关节旋转角度计算末端执行器的位姿初值,同时,利用一台激光跟踪仪实时跟踪测量位于工业机器人末端执行器上的一个测量点;然后,运用u n s c e n t e d卡尔曼滤波器(u n-s c e n t e dK a l m a n f i l t e r,U K F)对以上两类数据㊁信息进行融合,计算得到末端执行器的位姿㊂1 机器人末端执行器位姿实时估算系统组成机器人末端执行器位姿实时估算系统结构如图1所示㊂系统由一台激光跟踪仪㊁一台工业机器人与一台计算机组成㊂激光跟踪仪的反射球安装在末端执行器上,图中安装在距离腕关节比较近的位置㊂在机器人运动过程中激光跟踪仪实时跟踪反射球,同时将反射球的位置信息发送给现场计算机,机器人也实时地将自身的各个关节的角度位置发送给现场计算机,现场计算机以激光跟踪仪与工业机器人的数据为依据实时估算工业机器人末端执行器的位姿㊂图1中,O X Y Z为全局坐标系㊂本文以I R B1400工业机器人为例描述估算方法㊂I R B1400工业机器人结构如图2所示㊂该机器人是一种6R机器人,机器人的尺寸参数如图3所示㊂图1 机器人末端执行器位姿实时估算系统示意图图2 I R B1400工业机器人结构示意图图3 I R B1400工业机器人尺寸参数2 工业机器人末端执行器位姿初值计算工业机器人末端执行器位姿初值通过机器人的正运动学分析得到㊂机器人末端执行器与法兰固定连接,故计算末端执行器位姿的问题可以转化为计算法兰姿态的问题㊂I R B1400工业机器人各个部件的自身坐标系定义如图4所示㊂每个部件自身坐标系的Z轴与旋转轴方向一致,为了方便计算,将机器人底座坐标系O0X0Y0Z0与全局坐标系O X Y Z重合㊂㊃3131㊃基于U K F的机器人末端执行器位姿实时估算方法 项筱洁 何庆稀 应 征Copyright©博看网. All Rights Reserved.图4 I R B 1400各部件坐标系定义相邻部件位姿的变换关系(i -1)T i 采用广义变换矩阵表示,根据坐标系的定义与I R B 1400的尺寸参数,计算出具体的(i -1)T i 如下:(0)T 1=c θ1-s θ100s θ1c θ100001193.5éëêêêêêùûúúúúú0001(1)(1)T 2=0010c θ2-s θ20150s θ2c θ20281.5éëêêêêêùûúúúúú001(2)(2)T 3=c θ3-s θ300s θ3c θ306000010éëêêêêêùûúúúúú001(3)(3)T 4=010c θ4-s θ40120s θ4c θ400éëêêêêêùûúúúúú0001(4)(4)T 5=s θ5c θ5000010c θ5-s θ50720éëêêêêêùûúúúúú0001(5)(5)T 6=00180c θ6-s θ600s θ6c θ600éëêêêêêùûúúúúú0001(6)其中,c θi 与s θi 表示部件绕轴i 旋转角度θi 的正弦值s i n θi 与余弦值c o s θi ㊂末端执行器在全局坐标系下的位姿广义变换矩阵T 6为T 6=an g l e 2T (θ)=(0)T 1(1)T 2(2)T 3(3)T 4(4)T 5(5)T 6(7)θ=[θ1θ2θ3θ4θ5θ6](8)其中,a n g l e 2T (θ)表示根据θ计算T 6的函数㊂反射球中心在末端执行器下的位置为(6)po =(x o ,y o ,z o )T ,则该点在全局坐标下的位置(0)po 为(0)p o =d e h o m o (po éëêêùûúú1)(9)p o éëêêùûúú1=T 6(6)p o éëêêùûúú1(10)其中,d e h o m o 函数的作用是将齐次坐标转换为普通坐标㊂机器人末端执行器的位姿用向量x =(x ,y ,z ,φ,θ,ψ)T表示,其中,x ㊁y ㊁z 表示末端执行器在O X Y Z 下的位置,φ㊁θ㊁ψ表示末端执行器在O X Y Z 下的姿态,采用R P Y (r o u p i t c h y a w )角[8]表示,分别表示绕X ㊁Y ㊁Z 轴旋转量㊂x 与T 6的相互转换采用如下函数表示:T 6=tr a n s (x )=c φc θc φs θs ψ-s φc φc φs θc ψ+s φs ψx sφc θs φs θs ψ+c φc ψs φs θc ψ-c φs ψy -s θc θs ψc θc ψz éëêêêêêùûúúúúú0001(11)x =i n v t r a n s (T 6)(12)其中,式(12)表示式(11)的逆变换㊂根据工业机器人末端执行器位姿初值以及激光跟踪仪的测量值构造实时估算系统的状态方程与观察方程如下:x k +1=~x k +1+d i a g (~x k +1-x k )v k (13)~x k +1=x k +u k (14)u k =i n v t r a n s (a n g l e 2T (θk +1))-i n v t r a n s (a n g l e 2T (θk ))(15)z k =d e h o m o (t r a n s (x k )(6)po éëêêùûúú1)+w k (16)式中,v k 为服从高斯分布的系统噪声,其方差矩阵为Q k ;z k 为激光跟踪仪实时跟踪的测量点在绝对坐标系下的位置矢量;w k 为激光跟踪仪的测量噪声,亦服从高斯分布,其方差矩阵为R k ㊂3 基于U K F 的位姿估算算法U K F 是一种针对非线性系统的卡尔曼滤波,与卡尔曼滤波类似,它也是一种递推最小方差估计,其基础为U T 变换(u n s c e n t e dt r a n s f o r m )㊂U T 变换是一种计算随机状态变量经过非线性变换后统计量的方法[6],其实现原理为:在非线性变换之前,通过一定的规则在状态变量上采集一些带权值的点,这些点也被称为s i g m a 点㊂s i g m a 点通过非线性变换后得到相应点集㊂根据该点集计算出状态变量变换后的均值与协方差[9]㊂假设已知n 维随机变量为h ,其均值与方差分别为h ㊁p h h ㊂经过非线性变换y =f (h )得到随机变量y ㊂从h 中抽取2n +1个带权值的点H i 作为s i g m a 点,计算公式如下:㊃4131㊃中国机械工程第25卷第10期2014年5月下半月Copyright ©博看网. All Rights Reserved.H 0=hH i =h +((n +λ)p h h )i H i +n =h -((n +λ)ph h )üþýïïïïi (17)H i 的权重W i 计算公式如下:W m0=λ/(n +λ)W c 0=λ/[2(n +λ)]+(1-α2+β)W m i =W ci =1/[2(n +λüþýïïïï)](18)λ=α2(n +k )-n其中,上标m ㊁c 分别对应均值和方差计算的权值,((n +λ)p h h )i 是矩阵(n +λ)ph h 的均方根的第i 行或第i 列,该均方根的求解可用C h o l e s k y 分解得到㊂α为一个正的缩放参数,k 为一个可正可负的调节参数,β为用于调节第0个s i g m a 点计算方差的参数,一般β=2㊂H i 经非线性变换得到Y i ,通过Y i 可以估算出y 的均值y 与方差p y y ㊂计算公式如下:Y i =f (H i )(19)y ≈∑2ni =0W mi Yi(20)p y y ≈∑2ni =0Wci(Y i -y )(Y i -y)T(21)根据式(13)~式(16)构造U K F ,其过程如下㊂(1)初始化:^x 0=E [x 0](22)^p0=E [(x 0-^x 0)(x 0-^x 0)T ](23)(2)状态量预测,根据^x k ㊁^pk 结合U T 变换预测x k +1|k ㊁p k +1|k ㊁z k +1|k ㊂x k +1|k ㊁p k +1|k ㊁z k +1|k 这三个变量分别表示在第k 次迭代得到的计算结果基础上计算第k +1次迭代状态向量x ㊁p 与z 的中间量㊂首先对状态变量进行扩维,得到n (α)=15,即为15维的状态变量:x (α)k =[^x T k v T kw T k +1]T (24)p (α)k=^P k 000Q k 000R k +éëêêêùûúúú1(25)其中,^x T k 为^P k 的方差㊂将随机变量x (α)k视为式(17)中的h ,根据式(17)㊁式(18)得到第k 次迭代的X (α)i㊁W mi与Wc k ,i,分别记为X(α)k ,i㊁W m k ,i 与W ck ,i ,i =0,1, ,2n (α),将X (α)k ,i 分解为三部分:Xk ,i ㊁V k ,i ㊁W k +1,i ,它们分别是与^x k ㊁v k ㊁w k +1对应的分量㊂根据式(19)可以计算出:X k +1|k ,i =X k ,i +u k +V k ,i (26)将X k +1|k ,i 代入式(16)得到Z k +1|k ,i =d e h o m e (t r a n s (X k +1|k ,i )(6)p o éëêêùûúú1)+W k +1,i (27)将X k +1|k ,i 与W mk ,i 代入式(20)得到x k +1|k =∑2n (α)i =0Wm k ,iX k +1|k ,i (28)将X k +1|k ,i 与W ck ,i 代入式(21)得到p k +1|k =∑2n (α)i =0Wck ,i(X k +1|k ,i -x k +1|k )(X k +1|k ,i -x k +1|k )T(29)z k +1|k =∑2n (α)i =0Wmk ,i Zk+1|k ,i (30)(3)结合最新的观测值z k +1更新状态变量,得到p z k +1|k =∑2n (α)i =0Wck ,i(Z k +1|k ,i -z k +1|k )(Z k +1|k ,i -z k +1|k )T(31)p x k +1|k z k +1|k =∑2n (α)i =0Wck ,i(X k +1|k ,i -x k +1|k )(Z k +1|k ,i -z k +1|k )T(32)K k =p x k +1|k z k +1|k (p z k +1|k )-1(33)^x k +1=x k +1|k +K k (z k +1-z k +1|k )(34)^p k +1=p k +1|k -K k p z k +1|k K T k (35)4 仿真实验为了验证基于U K F 估计方法的性能,实验结果采用均方根误差(R M S E )表示,并且与基于正运动学分析得到的末端执行器位姿初值x p 进行比较,分别计算其平移分量与旋转分量的均方根误差如下:R M S E (^x t )=(^x t -x t )(^x t -x t )T (36)R M S E (^x r )=(^x r -x r )(^x r -x r )T (37)R M S E (^x p ,t )=(^x p ,t -x t )(x p ,t -x t )T (38)R M S E (^x p ,r )=(^x p ,r -x r )(x p ,r -x r )T (39)式中,x t 与x r 分别为末端执行器位姿的平移分量与旋转分量的真实值;^x t 与^x r 分别为基于U K F 方法估算出的末端执行器位姿的平移分量与旋转分量;x p ,t 与x p,r 分别为基于正运动学分析得到的末端执行器位姿的平移分量与旋转分量㊂仿真初始条件如下:角度单位为(°),长度单位为mm ,机器人从θ0=(0,0,0,0,0,0)匀速运动至θ1=(10,10,10,10,10,10)㊂系统方差矩阵Q =d i a g(0.012,0.012,0.012,0.012,0.012,0.012),跟踪仪测量方差矩阵R =d i a g(0.0052,0.0052,0.0052),x 0=[-900-9009501195],p0=d i a g(0.0012,0.0012,0.0012,0.0012,0.0012,㊃5131㊃基于U K F 的机器人末端执行器位姿实时估算方法项筱洁 何庆稀 应 征Copyright ©博看网. All Rights Reserved.。
相机模型与标定（七）--LM算法在相机标定中的使用
相机模型与标定（七）--LM算法在相机标定中的使⽤LM算法在相机标定的应⽤共有三处。
（1）单⽬标定或双⽬标定中，在内参固定的情况下，计算最佳外参。
OpenCV中对应的函数为findExtrinsicCameraParams2。
（2）单⽬标定中，在内外参都不固定的情况下，计算最佳内外参。
OpenCV中对应的函数为calibrateCamera2。
（3）双⽬标定中，在左右相机的内外参及左右相机的位姿都不固定的情况下，计算最佳的左右相机的内外参及最佳的左右相机的位姿矩阵。
OpenCV中对应的函数为stereoCalibrate。
本⽂⽂阅读前提是你已经对LM(Levenberg-Marquardt)算法有⾜够的了解。
因为本⽂主要是分析LM算法在相机标定中应⽤。
本⽂的分析是基于OpenCV的源码，所以可参见OpenCV的源码阅读此⽂。
0变量设置设标定板上⾓点数为m，标定过程中拍摄n幅视图（对双⽬标定⽽⾔，左右相机各抓取n幅视图）。
关于相机的成像模型和畸变模型，我这⾥就不占空间了，详见OpenCV官⽅⽂档或相关论⽂。
我⽤如下函数表⽰：其中，(u,v)是像素坐标，(X,Y,Z)是世界坐标，R=(r1, R2, R3)T是旋转外参，T=(T1, T2, T3)T是平移外参，A=(fx, fy, cx, cy)T是投影内参，D=(k1, k2, p1,p2, k3, k4, k5, k6, s1, s2, s3, s4, a, b)T是畸变内参。
纵所周知，(u,v)是存在畸变的（后⽂称之为畸变坐标），我们⽤(uu,vv)表⽰(u,v)对应的⾮畸变的坐标（后⽂称之为标准坐标）。
我们⽤findChessboardCorners提取的⾓点坐标就被当作是标准坐标。
1计算最佳外参在内参固定的情况下，我们需要计算最佳外参。
于是成像模型简化为：对于m个⾓点则有如下⽅程：其中(Xi, Yi, Zi)是已知项。
计算雅可⽐矩阵如下：由J可得J和J T J。
新型正交消隐点的摄像机标定方法_卢津
Matlab标定工具箱在摄像机定标中的应用
T -T -1
h1 K K h2=0 .
（3）
T -T -1
T -T -1
h1 K K h1=h2 K K h2 .
（4）
收 稿 日 期 ：2010－01－07；修 回 日 期 ：2010－02－11 作者简介：孙玉青（ 1982- ） ，女，山东潍坊人，在读硕士，主要从事图像处理与摄像机定标研究，E- mail：sun_yuqing99@。
和平移向量，
H= h1 h2 h3 λK r1 r2 t ，
r1=
1 λ
K-1h1，
r2=
1 λ
K-1h2
.
（2）
T
根 据 旋 转 矩 阵 的 性 质 ， 即 r1 r2=0 和 ‖ rq‖ = ‖r2=1‖ ， 每 幅 图 像 可 以 获 得 以 下 两 个 对 摄 像 机 内 参 数矩阵的基本约束，
在图像测量过程以及机器视觉应用中， 常常会 涉及到这样一个概念， 那就是利用摄像机所拍摄到 的图像来还原空间中的物体。 从摄像机获取的图像 信息出发计算三维空间中物体的几何信息， 并由此 重建和识别物体， 而空间物体表面某点的三维几何 位置与其在图像中对应点之间的相互关系是由摄像 机成像的几何模型决定的， 这些几何模型参数就是 摄像机参数。 在大多数条件下这些参数必须通过实 验与计算才能得到， 这一过程被称为摄像机定标[1]。 1 Matlab 简述
本文摄像机定标的基本原理为的平面上其中为模板上点的齐次坐标为模板平面上点投影到图像平面上对应点的齐次坐标分别是摄像机坐标系相对于世界坐标系的旋转矩阵和平移向量每幅图像可以获得以下两个对摄像机内参数矩阵的基本约束atlab标定工具箱在摄像机定标中的应用孙玉青冀小平简述了atlab标定工具箱以及摄像机定标的概念原理和意义给出了摄像机定标中所用的三种坐标系以及它们之间的关系推导出摄像机参数标定算法公式在此基础上利用atlab标定工具箱仿真了棋盘网格的角点标定算法并给出了仿真结果以及误差分析得出了atlab标定工具箱在摄像机定标方面运用的有效性及算法的正关键词
一种基于OpenCV的简易摄像机标定方法
图像像素坐标系， 该坐标系的单位是像素 ， 坐标原点 ０ 在图像的左上角。由于 （ 只表示像素位于数 。 ， ） 组中的列数与行数， 并没有用物理单位表示出该像 素在 图像 中的位置， 因此， 需要再建立 以物理单位 （ 毫米 ） 表示 的图像坐标系 ， 如图 ｌ 所示 ， Ｙ 是图 （ ） ， 像物理坐标 ， 光心 ０ 在图像的中心点上 。
图 １ 图像坐标系
由图 １ 可得， 图像中任意一个像素在两个坐标
系下 的坐标 有 如下 的关 系 ：
＝
孚＋ ‰
（） １
（） ２
用齐 次坐标 与矩 阵表示为 ：
１ Ｏ
ｕ。
图 ３ 摄 像 机模 型
图 ３ ０为摄像机光心 ， 中， ０ 为图像坐标原点 ，
（） ３
①读取 实验所 需 的图像 。
②使用 ｃＦ ｄ ｈｓ ｏｒＣｒ ｒ 函数提取棋盘 ｖｉ Ｃ ｅ ｂａ ｏ ｅ ｎ ｓ ｄ ｎｓ
模 板上 的角点 。
１ｙ ，） ｋ， ＋， ＋ ｐ（ ）） ２ｌ ］ ６（ Ｙ ：ｚ（ ，） ［２３ ， ＋ｐ ｙ ｘ ） ＋２ ｘ
（） ７
③使用 ｃＦｎＣ ｒ ｒ ｂ ｉ ｖｉ ｏ ｅ ｕＰ ｄ ｎ Ｓ ｘ函数对上步 中所提 取 出来的角点进行进一步的细化， 以得到更精确 的 角 点位 置 。 ④亚像素精确化后， 再使用 ｃＤａ Ｃ ｅ ｂａ — ｖ ｒ ｈｓ ｏｒ ｗ ｓ ｄ
界坐 标 系。
收 稿 Ｅ期 ：２ １ ０ ｔ ０ １— ７—０ ４
取三维空间信息的前提和基础。标定结果的好坏直
接影 响 着 三 维 测 量 的 精 度 和 三 维 重 建 结 果 的 好 坏Ｈ ， 因此研究 摄像 机 标定 方 法 具有 重 要 的理 论 意 义 和实 际应用 价值 。
一种新的基于灭点的相机标定方法
图 1 灭点的确定
Fig. 1 Definition of vanishing points
当直线与水平方向倾角 < 45 时, 直线两端 点的横坐标的变化对灭点的影响很小, 可忽略不 计, 这样, 式( 1) 可简化为
( xj - xV ) v yi + ( xV - x i ) v yj + ( yj - yi ) dxV +
校出像主点及焦距的误差变化.
图 4 仅当角度 v 变化时焦距 f 及主点 ( x 0, y 0) 的误差曲线 Fig. 4 Error curve of f and ( x 0, y 0) obtained when the angle v
ch anged
由图 4 可见: 角度 v 越大, 焦距 f 的中误差 mf 就越低, 但在 40 与 70 之间趋于平缓; x 0 的中误 差 mx0 随角度的增大而增加, 而 y 0 的中误差 my0
xZ ! = - GZ ! cos !- oGsin !;
yZ ! = GZ ! sin !- oG cos !;
( 3)
其中: GX ! = f cot sec ; oG = f t an ;
oY ! = f cot ; GZ ! = f tan sec .
图 6 平面透视图
Fig. 6 Plane scenograph
a31 1 0 a34 a35 a36 dy 0 a41 0 1 0 a45 a46 d
( 4)
a51 1 0 a54 a55 a56 d
中图分类号: X703
文献标识码: A
文章编号: 0367- 6234( 2003) 11- 1384- 04
New method of camera calibration with vanishing points
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
—274— 简化UKF算法在摄像机标定中的应用 陈 益，赵高鹏，刘 娣 (南京理工大学自动化学院，南京 210094) 摘 要：提出一种基于简化无迹卡尔曼滤波(UKF)算法的摄像机标定方法。将平面靶标图像上的不同特征点坐标视为同一个特征点在不同时刻的运动坐标。为避免欧拉角描述法带来的奇异问题，用单位四元数描述世界坐标系和摄像机坐标系之间的变换关系，选取摄像机内外参数作为系统状态变量。结合实际应用背景，简化标准UKF算法，将其用于摄像机参数估计，在保证标定精度的前提下降低运算复杂度。仿真结果表明了该方法的有效性。 关键词：摄像机标定；平面靶标；简化UKF算法
Application of Simplified UKF Algorithm in Camera Calibration
CHEN Yi, ZHAO Gao-peng, LIU Di (College of Automation, Nanjing University of Science and Technology, Nanjing 210094)
【Abstract】This paper proposes a camera calibration method based on simplified Unscented Kalman Filtering(UKF) algorithm. The feature pointsin the planar target images are considered as motion coordinates of one feature point at different time. To avoid the singular problem from Eulerangle description, the quaternion is used to represent the transform relation between world coordinate system and camera coordinatesystem. Theintrinsic and extrinsic camera parameters are taken as system state variables. According to the application background, it simplifies the original UKFalgorithm and uses it to estimate camera parameters, and reduces the calculation complexity while the calibration precision is kept. Simulationresults show that this method is effective. 【Key words】camera calibration; planar target; simplified Unscented Kalman Filtering(UKF) algorithm
计 算 机 工 程Computer Engineering第35卷 第19期
Vol.35 No.19 2009年10月
October 2009
·开发研究与设计技术·文章编号：1000—3428(2009)19—0274—03文献标识码：A
中图分类号：TP391
1 概述 根据摄像机获取的图像信息计算三维空间中物体的几何信息，即实现三维场景在二维平面上的投影，并由此重建和识别物体是计算机视觉研究的主要对象之一[1]。在计算机视觉中，必须解决摄像机标定问题。摄像机标定是获取摄像机内部空间几何特性和光学特性(摄像机内部参数)、摄像机坐标系相对某个世界坐标系的三维位置和方向关系(外部参数)近似值的过程[2]。 利用一幅或多幅高质量靶标图像，采用非线性优化算法进行摄像机参数估计是目前常用的标定方法。近年来，随着非线性滤波理论的发展，很多学者研究了如何将滤波算法应用于摄像机标定。文献[2-3]利用扩展卡尔曼滤波(Expanded Kalman Filtering, EKF)分别对摄像机的内外参数和外参数进行估计，但在扩展卡尔曼滤波方法中，状态分布用高斯随机变量来近似，由于对非线性系统模型的一阶线性化，使得解析的传递状态分布在此过程中不可避免地引入了线性化误差，降低了滤波精度甚至导致滤波器不稳定。因此，文献[4]提出采用无迹卡尔曼滤波(Unscented Kalman Filtering, UKF)方法，通过一组确定的、具有不同权值的sigma点逼近待估计状态，较好地解决了上述问题，取得了良好的滤波性能。 在标准UKF算法中，需要对系统状态变量进行扩展处理，即将系统原状态变量、输入噪声和观测噪声合并为新的系统状态。此处理在维数较高的情况下将极大增加算法复杂度。标准UKF算法将无迹变换同时应用于状态方程和测量方程，而不考虑它们是否为非线性方程。在实际应用中，噪声通常是加性的，且状态方程和测量方程并非都是非线性的。对于此类混合系统，可以在保证滤波器性能的前提下，对标准UKF算法进行简化以降低其运算复杂度。 2 摄像机模型 为定量描述摄像机成像过程，需用到如图1所示的3种坐标系，即图像平面坐标系、世界坐标系和摄像机坐标系。
iOiuiv
cxcycO
cz
O wx w
y
wzP摄像机 ),(iivuf
xtzt图像平面
Pty
图1 3种坐标系 3种坐标系具体定义如下： (1)图像平面坐标系iiiOuv−。原点iO取为摄像机光轴与图像平面(靶面)的交点，iu轴和iv轴分别平行于图像像素的
行与列。 (2)摄像机坐标系ccccOxyz−。原点cO取为摄像机的光心，
作者简介：陈 益(1983－)，男，博士研究生，主研方向：光电测量技术；赵高鹏、刘 娣，博士研究生 收稿日期：2009-04-30 E-mail：cy19830408@163.com —275—
与iO点的距离为摄像机焦距f，cz轴沿光轴指向摄影方向，cx轴、cy轴分别与图像坐标系的iu轴、iv轴平行。 (3)世界坐标系wwwOxyz−。在环境中选择的一个基准坐标系，用来描述摄像机位置，可以根据具体环境选取。 设wPX(,,)wwwPPPxyz表示世界坐标系中某物点P的三维坐标，cPX(,,)cccPPPxyz是同一点P在摄像机坐标系中的坐标，则 123456789010001111cwwxpppcwwypppTcwwzppprrrtxxxrrrtyyyrrrtzzz⎡⎤⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥==⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦⎣⎦RT (1) 其中，T是3×1平移矩阵，Txyzttt⎡⎤=⎣⎦T；R是3×3正交旋转矩阵，为避免欧拉角描述姿态问题的奇异现象，由单位四元数描述如下： R=2222012312031302222212030123230122221302230101232()2()2()2()2()2()qqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqq⎡⎤+−−+−⎢⎥−−+−+⎢⎥⎢⎥+−−−+⎣⎦ (2) 单位四元数的参数0123,,,qqqq满足如下约束： 222201231qqqq+++= (3) 设物点P在图像平面上的投影点坐标为iPX(,)iiuv，由针孔成像相机模型的透视原理可知式(4)成立。 1icpiuzv⎡⎤⎢⎥⎢⎥⎢⎥⎣⎦=0000001ucPcvPcPfudxxfvydyz⎡⎤⎢⎥⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎢⎥⎢⎥⎣⎦=0000001cuPcvPcPauxavyz⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦ (4) 其中，/uuafdx=, /vvafdy=是摄像机在x方向和y方向上的有效焦距；00(,)uv为主点坐标。 式(1)~式(4)表征了二维图像坐标与三维世界坐标间的基本关系，若已知物点的世界坐标，则利用该变换关系可以求出相应的理想图像坐标。若知道像点的图像坐标和上述变换关系，则可以确定过摄像机光心的一条空间射线。摄像机标定是指获取模型参数ua, va, 0u, 0v, 0q, 1q, 2q, 3q, xt, yt, zt的值。 3 基于SUKF的摄像机参数估计 3.1 状态方程 获取标定靶上的特征点图像坐标，将图像平面上不同特征点的图像坐标视为同一个特征点在不同时刻的运动坐标，取摄像机的内外参数为状态变量，则k=x[ua,va,0u,0v, 0q,1q,2q,3q,xt,yt]T，可以建立如下系统状态方程[5]： |11kkkk−−=xxΦ (5) 其中，kx,1k−x分别为k时刻和1k−时刻的状态变量；|1kk−Φ为1k−时刻到k时刻的状态转移矩阵，对同一幅图像而言，不同时刻的状态变量是不变的，因此，|1kk−Φ是11×11单位矩阵。 3.2 测量方程 选取标定靶上特征点的图像坐标iPX(,)iiuv为观测量ky，由于其对应的世界坐标wPX(,,)wwwPPPxyz已知，因此综合式(1)~式(4)可得： ky=T12[(),()]kkkhhv+xx (6) 其中，kv为系统观测噪声，一般主要来源于成像过程中的几何失真、图像的量化误差以及特征点提取误差等，本文认为其为零均值高斯白噪声，且方差为kR。
121078452078()()wwppxkiuww
ppz
wwppykivww
ppz
rxrythuaurxrytrxrythvavrxryt⎧++==+⎪++⎪⎨++⎪==+⎪++⎩x
x (7)
应注意，四元数不是刚体姿态的最小参数描述，它给出了一个额外自由度，因此，每次运算后必须将四元数规范为单位量。 3.3 摄像机标定中的SUKF算法推导 在式(5)~式(7)组成的系统中，状态方程是线性的，测量方程是非线性的，且观测噪声kv为加性高斯噪声。对于此类
混合系统，文献[6]证明在进行UKF滤波运算时，无需对系统状态进行扩展，且可以使用和卡尔曼滤波方法相同的一步预测方程，而其他非线性部分则仍然保持UKF算法的特征。将上述UKF和KF的混合滤波称为简化的UKF(Simplified UKF, SUKF)算法，则对于本文中的摄像机标定系统，推导SUKF算法流程如下： (1)Sigma采样点权值设定 采用比例修正对称采样产生增广状态的Sigma点集aχ，
定义其权值0mW,0cW,miW,ciW形式如下：
020/()/()(1)1/[2()]1,2,,2mc
mcii
WLWLWWLiL
λλλλαβλ
⎧=+