【WORD格式论文底稿】基于对偶四元数的未知几何结构的目标的位姿确定1

1、基于对偶四元数的未知几何结构的目标的位姿确定1冯国虎，吴文启 国防科技大学机电工程与自动化学院，长沙410073 E-mail HYPERLINK mailto:yantai_fgh126 ：yantai_fgh126 摘要：为了解决摄影测量中目标体坐标和相机坐标系之间的相对位置、姿态确定问题，用 单摄像机作为观测设备，用对偶四元数描述目标体坐标和相机坐标系之间的变换，用透视投 影描述目标直线在相机坐标系中的投影变换，将表征目标位置和姿态以及运动线速度和角速度加上目标未知的直线特征作为状态量，图像二维坐标作为观测值，建立扩展卡尔曼滤波器，得到目标位置、姿态和运动的估计值。仿真说明，估计值和真实

2、值是一致的，减小了对目标 几何尺寸准确描述的要求，为摄影测量中的目标定位找到了一个具有工程应用的方法。 关键词：单目视觉；对偶四元数；扩展卡尔曼滤波；几何特性；位姿确定中图分类号：U61引言确定两个坐标系之间的相对位置、姿态以及相对运动在机器人、计算机视觉、摄影测量 等领域是一个重要问题。为了解决这一问题，在大多数实际应用中利用单摄像机的连续图像。 当目标物体几何特征未知时，通常还要求估计物体的几何尺寸。多使用单摄像机是因为标准 摄像机本钱低，安装和标定简单，物理占用空间小，可靠性高，同时数字化和处理图像的硬 件本钱低。通过图像测量确定目标位置和姿态存在一定困难，目标物体的三维特征投影到二 维

3、图像上是非线性过程。刚体运动变换的传统算法是用四元数或方向余弦阵(DCM)表示旋转，向量表示平移。这 种算法要求四元数更新速度必须足够快以减小平移因素带来的影响。与质点运动的纯位移不 同，刚体一般运动包括绕空间轴的旋转和沿该轴的平移，两者必须联合起来分析。纯四元数 在表示坐标变换时会发生变换断裂1，也就是不连续的情况，这是在刚体一般运动中一直困 扰我们的问题。这一问题因应用对偶四元数得到圆满解决。对偶四元数能够同时描述物体的 转动和平移，是描述一般运动最为简洁完整的数学工具，利用较少的测量值图像二维坐标 等，在不破坏坐标变换中位移和转动的完整性和融合性blend的根底上，可以同时获 得相对位置

4、和姿态，这是四元数或 DCM 方法等传统算法难以做到的。在应用对偶四元数确定目标位置、姿态方面，James2提出观测目标直线的 line-point 坐 标来确定物体坐标系相对相机坐标系的位置、姿态，而 Wu_xuedong3在此根底上对其中的 数值计算作了更为详实的研究。这些研究的前提是目标几何尺寸准确，如果目标几何尺 寸有误那么难以准确估计目标的位置、姿态。实际情况通常是目标局部几何尺寸，其余尺 寸未知，在这种情况下如何准确确定目标的位置、姿态正是本文要解决的问题。2根底知识 对偶四元数对偶四元数属于几何代数的一种，由实部和对偶局部组成，q = r + s ，r, s 为四元数，其中 0,

5、 2 = 0 。根据 Kotelnikov 的转移原理(Principle of Transference)4，对偶四元数完2*全继承了四元数的性质。对偶四元数范数定义为 q= qq，其中 q= r + s为共轭对偶1本课题得到高等学校博士学科点专项科研基金工程编号 20069998009的资助。四元数。对偶四元数可以同时描述刚体的转动和平移，q = q + t q ，其中 q, t 分别表示刚2体的纯转动和纯平移。 坐标系转换对偶四元数可实现坐标系变换。如图 1 所示，坐标系 O 绕空间矢量 l 旋转 角度，同时沿 l 平移 d 形成新坐标系 N，该过程是刚体的一般空间运动，可用单位对偶四元

6、数表示：q = cos ( 2)sin ( 2)l1 其中 l = l + ( p l ) ， p 为坐标系 O 的原点到旋转轴上一点的向量。从图 1 可以看出，只要给定对偶矢量 l 和对偶角 ，对偶四元数就可以非常简洁地表示坐标系 O 到 N 的变换。图 1 由坐标系 O 变换到坐标系 NFig.1 Transformation from coordinates O to N设坐标系内过 p 点的 l 方向直线 Plu c ker 直线 l 可以由 (l, m) 表示，其中 m = p l 。 对偶四元数算子 q ( ) q* 可以表示空间矢量的任意变换。于是，同一 Plu c ker 直线

7、 l 在两个坐标系中分别表示为： l = l+ m OOOl = l+ m且有： NNNl + m= q + t q (l+ m) q + t q *2NN2 OO 2 式2完成了坐标系变换，即同一矢量在不同坐标系中的表示。 对偶四元数更新对偶四元数的动力学方程5为q = 1 q2式中 为刚体运动的角速度斜对称矩阵，当刚体运动角速度为恒定值时，可解得1 3泰勒展开近似得q (tk +1 ) = e 2q (tk ) 1 q (t1 ) = cos I +sin q (t) = Qq (t )4k + 2 2ktrank一般情况下式中 并非常值，可将更新算法中的时间系数选得足够小，使得 在足够小

8、的时间间隔内可当作常量来处理。3目标位姿估计 系统模型状态量 s 定义为Ts ( k ) = txt ytzq0q1q2q3VWl1mm1mlnmmnm5式中 ti 和 qi 分别是线性平移量和转动四元数，V ,W 分别是线速度和角速度，为简化问题， 假设在充分小的时间间隔内为匀速运动，即TTV (tk ) = V (tk 1 ) ,V = vxvyvz ,W (tk ) = W (tk 1 ) ,W = xyz lim , mim 分别是描述目标直线的对偶四元数的实部和对偶部，n 为直线条数，目标的几何特 征不随时间变化而改变，因此 lim , mim 为常量。系统方程sk = k 1 (

9、sk 1 ) + wk 1式中 w 为系统噪声，假设为高斯白噪声，其协方差矩阵为 Qk 1 ，状态转移函数为6Tk ( s ) = tx + vxt y + vytz + vzQtran q (tk )VWl1mm1mlnmmnm式中 Qtran 根据式4定义。 观测方程观测方程为zk = hk ( sk ) + k7式中 为观测噪声，假设为高斯白噪声，其协方差矩阵为 Rk ，观测量为目标直线在像平面投影的 line-point 坐标2，即zk =x1lpy1lpxnlpTynlp8图 2 二维像平面上 line point 的定义Fig.2 The definition of line po

10、int on the 2D image plane目标体坐标中直线投影到相机坐标中像平面上是一个 3-D 到 2-D 的非线性过程。为方便分析分两步研究：首先是坐标系变换，目标直线由在体坐标中表示变换成在相机坐标系中 表示，这一个过程由对偶四元数完成；然后是投影变换，将空间直线投影到 2-D 像平面，这 一过程用透视投影描述。相机坐标系中点 ( xc , yc , zc ) 投影到像平面坐标 ( xi , yi ) 为记 222xi 0= xcf, yi 0zcf= yczc9ri = xi 0 + yi 0 ，考虑到透镜畸变后像点坐标为ii 0i 0 i 0 2kc (3) x y+ kc

11、( 4) (r 2 + 2x2 ) xi = (1 + kc (1) r 2 + kc ( 2) r 4 ) xi 0 + i 0 i 0ii 0 ii yi yi 0 kc (3) ( r 2 + 2 y 2 ) + 2kc ( 4) x y 式中畸变系数 kc 通过相机标定获得。空间向量在像平面上的投影位于空间直线和相机原点确定的平面上，该平面方程为mx x + my y + mz z = 0该平面交像平面于 z = f ，得到投影直线方程mx xi + my yi = mz f10那么投影直线的方向向量为Tli = mym2 + m2mx0m2 + m2 xyxy投影直线用对偶四元数表示

12、为 Li = li + mi ， mi = pi li 其中 pi 为投影直线上的点，满足mi =fxym2 + m2m ，根据 line point 的定义，有x = l m = fmx mz, y = l m = fmy mz11lpiy izm2 + m2lpix izm2 + m2xyxy 扩展卡尔曼滤波由式6和7知，系统动态过程和测量方程都是非线性的，采用扩展卡尔曼滤波EKF 进行状态估计，滤波过程需要对状态转移函数和观测函数线性化。状态转移函数线性化 k ( s k( ) = k ( s )ss = s k ( ) It0 I0000 0Qtran0Qw000 00Iv0000 =

13、 000I00012k 0000Il ,m00 00000% 0 000000Il ,mQtran q (tk )式中 Q =观测函数线性化 =kH k ( s k( ) = h ( s )ss = s k ( ) nm nm 式中 H k 为 2in 矩阵，i 为观测目标直线条数，n 为状态量维数xlpx=lpmx + xlpmy + xlp mzsimxsimysimzsi13ylpy=lpmx + ylpmy + ylp mzsi对式11求 m 的偏导，得mxsimysimzsixfmfm2mlp = z 2 x z xxyxym(m2 + m2 )(m2 + m2 )2xlpm= 2f

14、m x my mz(m2 + m2 )2yxyxlpm=fmx(m2 + m2 )zxyylpm= 2fm x my mz(m2 + m2 )2xxyyfmfm2m= 2lp z m(m2 + m2 )(y z22 )2yxymx + myylpmfmy= (m2 + m2 )zxy对偶四元数描述直线由目标体坐标系到相机坐标系的变换为l + m =( r + s ) (lm + mm ) (r* + s* )式中直线在相机坐标系中的对偶部 m 为*1* *1*m = rmm r写成四元数矩阵联乘形式，有+rlm r t2+trlm r2+1 + +m = M r M r*mm + M t +

15、M2 t* M r M * lmr14对式14求状态量 s 的偏导，有+ + m = 1 M t + M t* M M lti2tirr* m + + m M r M r* =1 + M r M * r=qqmm + M t + M t* qlmii2 im = 0, m = 0vimi+T mT+m = M r M r* , l= M t + M t* M r M r* mm 将对式11和14的偏导代入式13，即可得到观测矩阵 H k 的解析表达式。4实验及结果分析以目标直线投影的 line-point 为观测量，通过 EKF 估计目标位姿、速度的流程如图 3 所 示。如果目标几何特征，即

16、lim , mim ，状态量相应降维s ( k ) = txt ytzq0q1q2Tq3VW 15状态转移函数和观测函数也作相应简化。如果目标几何特征完全未知，那么对应状态量维数很 大，难以通过单纯的 EKF 获得准确的目标位姿、速度信息。通常情况下，目标几何特征是 局部的，可以利用的尺寸和相应图像测量，结合透视投影估算目标未知尺寸，作为 相应状态量的初始值，通过 EKF 获得目标位姿、速度信息。以平面四边形为目标进行研究，体坐标选择和目标直线标注如图 4 所示。根据文献6 的结论：如果平行四边形的两组边长均未知，那么无法确定平移向量的大小；当知道平行四边 形任一组边长时，旋转矩阵和平移向量均

17、可以唯一确定。假设直线 2，4 之间的距离，那么 l2 m , m2 m 和 l4 m , m4 m ，直线 1，3 平行于 X b 轴，l1m , l3m ， m1m , m3m 未知。由直线2，4 之间的几何尺寸和直线 2，4 和 1，3 在像平面上投影之间距离，根据透视投影可以算 出直线 1，3 之间的几何尺寸，因为目标体坐标和相机坐标系之间存在旋转，计算出的尺寸 存在误差。为了方便比拟，在相同条件下对同一目标的位姿、速度用不同方法进行估计。目标几何尺寸为 200cm80 cm，直线 1，3 之间尺寸估算误差 10。初始状态量，误差 均方差和系统噪声如表 1 和 2 所示。假设相机是理想

18、模型，焦距为 12mm，像点定位误差mm，那么观测噪声矩阵 R 的对角线元素为mm2。仿真总时间 30s，滤波周期 s。表 1 真实初始状态量和估计初始状态量Tab.1 True and assumed initial state状态量平移量m四元数线速度m/sec角速度rad/sec真实初始量10 10 10001 0 0 05 5 00 0 0.05估计初始量0 0 9900.9998 0.01 0.01 0.010 0 00 0 0表 2 误差协方差初值和系统噪声矩阵的对角线元素Tab.2 The diagonal elements of initial error covariance

19、 and process noise matrix噪声均方差平移量m2四元数线速度m/sec2角速度rad/sec2误差均方差初值1000.01100系统噪声10-510-510-510-6图 3 目标位姿、速度估计流程示意图图 4 体坐标和目标直线标注Fig.3 Functional block diagram of the pose andFig.4 The object reference frame and motion estimationlines label on the object文献2中的方法仿真结果如图 5 所示图 5 文献2中的方法对目标位姿、速度信息的估计Fig.5

20、Simulation results of pose and motion estimation using Jamess2method从图 5 可以看出，当用有误差的估计值代替未知的目标几何尺寸时，目标的位姿、速度估计 也存在误差。要准确对目标定位，必须事先知道目标几何尺寸的准确值，这在摄影测量中难以满足。本文方法的仿真结果如图 6 所示图 6 本文方法对目标位姿、速度信息的估计Fig.6 Simulation results of pose and motion estimation using our method比拟图 5 和 6 发现在目标几何尺寸估计误差相同的条件下，相对于文献2中

21、的方法， 本文方法提高了目标位姿、速度估计精度，从而减少了对目标几何尺寸准确描述的要求。对目标未知几何尺寸的估计量如图 7 所示，从图 7 可以看出，目标尺寸的估计量向真实 值趋近，但没有收敛于真实值。计算可观性矩阵相应的奇异值，发现这些奇异值很小，说明 这些状态量的可观测度低，解释了尺寸估计值没有收敛于真实值的原因。5总结图 7 对目标未知几何尺寸的估计Fig.7 The estimation of objects unknown geometric description本文解决了在目标几何尺寸不是完全情况下对目标位置、姿态准确估计问题。对本 文方法的仿真结果说明目标位姿的估计值迅速收敛于

22、真实值，相对于 James2的方法，本文 方法提高了目标位姿估计的精度，降低了对目标几何尺寸准确描述的要求，符合工程应用的 实际情况。仿真发现目标未知的几何尺寸没有收敛于真实值，为提高目标位置、姿态估计的精度，需要给出准确的初值。大量仿真说明，目标未知尺寸估计值误差在 5之内时，目标位置、姿态估计误差在 3之内。如何结合其他方法将目标未知尺寸估计值误差控制在 5之内是 下一步研究方向。同时，相机的真实模型以及系统噪声和观测噪声真实特征的准确描述也是 下一步研究方向。参考文献1 S C. Dual quaternions for rigid transformation blendingC/ p

23、roceedings of the Symposium on Interactive 3D Graphics and Games, 2007.2 S GJ. Pose and motion estimation from vision using dual quaternion-based extended kalman filteringDD. Knoxville: Tennessee, 1997.3 Xue-dong W. An application of unscented Kalman filter for pose and motion estimation based on mo

24、nocular visionC/ proceedings of the IEEE International Sysmposium on Industrial Electronics, 2006.4 P KA. Screw calculus and some applications to geometry and mechanicsJ. Annuals of Imperial University ofKazan, 1895.5 武元新. 对偶四元数导航算法和非线性高斯滤波研究DD. 长沙: 国防科学技术大学, 2005.6 孙风梅, 王卫宁. 基于单个平行四边形单幅图像的物体定位J. 自动

25、化学报, 2006,32(5): 746-752.Pose Estimation with Unknown Object Geometry based-onDual QuaternionFeng Guohu, Wu WenqiCollege of Mechatro-Eletronic and Automation, NUDT, Changsha (410073)AbstractDetermination of relative three-dimensional position and orientation between object reference frameand camera reference frame is an important problem in photogrammetry. A s