信息与控制
Information and Control
1999年　第28卷　第5期　Vol.28　No.5　1999



由2D点对应进行刚体运动参数估计的鲁棒算法

陈海峰　何铁军　陈维南

　　摘　要：本文提出一种根据刚体运动前后生成的2D点对 应数据集合估计其运动参数的方法，考虑到集合中存在部分匹配错误的对应点，文中利用最 小截平方和估计和递归残差分析的方法来剔除这些出格数据，从而获得刚体空间运动的正确 参数，并且不需要任何先验知识．实验结果表明了算法的有效性． 
　　关键词：刚体运动，参数估计，最小截平方和，遗传算法，鲁棒性，出格 点．递归残差
　　中图分类号：TP13　　　　　　文献标识码：B

ROBUST ESTIMATION OF RIGID BODIES' MOTION PARAMETERS
FROM 2-D POINT CORRESPONDENCE
CHEN Hai-feng1 HE Tie-jun2 CHEN Wei-nan1
(1. Institute of Automation Southeast University 210096; 2. Research Institute of Intelligent Transportation System, Southeast University)
Abstract: Given the 2-D point correspondence set of a scene containing moving rigid object, this article proposes a method to estimate its m otion parameters. Because there exists several incorrect correspondence in the s et, the least trimmed square estimator and recursive residuals analysis is adopt ed to deal with the outliers and get the correct motion parameters. It does not need any prior knowledge. Computer simulation results demonstrate the effective ness and efficiency of the algorithm.
Key words: rigid motion, parameter estimation, least trimmed sq uares, genetic algorithm, robust, outliers, recursive residuals

1　前言
　　运动视觉研究的是如何从变化场景一系列不同时刻的图像中，提取出有关场景中物体的形状，位置和运动的信息，它是机器人视觉的一个重要研究领域．其研究方法大体上可分为基于特征的方法和基于光流场的方法两类．本文所研究的是基于特征的运动参数估计方法， 其问题表达为：给定不同时刻t1＜t2时的两幅图像P1(x,y)和P2(x,y)，要求图中刚 体的运动参数(R,t)，其中R是刚体的旋转矩阵，t是其平移向量．
　　文献中已有很多解决该问题的成熟算法，但这些算法仅考虑了有限的噪声干扰和没有图像对应点匹配错误的情况．然而实际问题中由于各种偶然因素，不仅存在噪声干扰，还会使刚体运动前后两幅图像的对应点发生匹配错误，甚至混入一些非刚体上的点．对此，人们提出了各种各样的鲁棒算法，如Haralick提出的M估计方法[7]，但该方法在出格 点百分比大于20％时失效；庄新华提出一种模型匹配的估计算法[3]，孙学宁提出 了引入参数初始值的鲁棒算法[6]，它们都需要一些参量的初始值；朱嘉琳、吴立 德等提出基于最小平方中值的估计算法[5]，能很好地处理数据中的出格点,但该方 法随机生成初始数据的时间较长．本文拟尝试利用最小截平方和估计(Least Trimmed Squ ares LTS)和递归残差(Recursive Residuals)分析的方法来剔除出格数据，进行正确的参 数估计．为了提高算法的效率，文中采用遗传算法寻优的手段生成其基本数据．从我们的实 验中看出，用该方法估计刚体运动能够有效的克服由于出格点产生的误差，从而得到运动参 数稳定的估计而不需任何先验知识．
2　刚体运动估计的线性算法
　　由n组对应点求单刚体的运动，文献中已经有很多成熟的算法，其中较有影响的是Tai & Huang的线性算法[1]．对摄像机采用透视投影模型，设焦距f=1，那么摄像机中心坐标系(X,Y,Z)和图像坐标系(x,y)之间的关系为x=X/Z,y=Y/Z,．令(X,Y,Z)和(Xt,Yt, Zt)分别为运动前后刚体上的特征点，则刚体的运动可以用3×3的旋转矩阵R和平移向量t 来表示：
　　　　　　　　　(1)
引入一反对称矩阵， 并令， 很显然有 T.t=0，并对任意向量u，有uTTu=0．我们在方程式(1)两边同时乘以(X′,Y′, Z′).T，则最终整理得
(x,y,l).E.(x′y′l)T=0　　　　　　　　　　　　(2)
　　线性方程(2)是齐次的，因而独立参数只有8个，这里不防设，h9=-1,那么(2)式可以表示成：
αTβ=1，其中α=(xx′,yx′,x′xy′,yy′,y′x,y)T，β=(h1,h2, h3，…,hs)T.　　　(3)
　　可见对于每一个特征点对(xi,yi)(x′i,y′i)，对应一数据矢量αi，其满足线性方程αTiβ=1，要求得未知参数β，至少需8组对应点．用最小二乘法求出向量β后，就很容易求得刚体的运动参数(R,t)[2]．
　　上述线性算法所用的运动前后图像特征点对一般都是有误差的．这些误差分为两类，一类是特征点本身定位的误差，它们一般比较小，可以用正态噪声来表示，并通过最小二乘法 就能得到有效地抑制．而另一类误差对算法的计算结果影响比较大，它是由于出现假的特征 点或者是特征点之间产生错误的匹配而造成的．它们不服从正态分布，在统计中称为“出格点”，出格点的存在对基于正态模型的最小二乘方法带来了很大的问题．为此，统计学中研究出许多鲁棒方法来解决这一问题，其中不少已引入运动视觉，得到运动分析的鲁棒性方法[3-7]．本文中我们采用一种最小截平方和(LTS)的估计方法，并将它与上面 的线性方法相结合，给出无需任何先验知识的，能很好处理特征点对中出格数据的运动参数 鲁棒估计方法．
3　刚体运动估计的鲁棒算法
3.1 利用LTS获得基本数据
　　对于回归模型
yi=xi1β1+xi2β2+…+xipβp+εi　　　　　　　　　　　　　　　　(4)
残差ri(β)=yi-ri1β1-…ripβp,对r2i(β)进行排序0≤(r 2(β))1：n≤(r2(β))2：n≤…≤(r2(β))n: n．其中(r2(β))k:n表示排序后的第k个残差平方，那么最小截平方(LT S)解β=(β1,β2,…,βp)T使得下面的函数最小：
　　　　　　　　　　　　(5)
　　LTS方法(5)式只取残差平方的前项之和，而不予考虑其余项较大的残差，这样只要 合理地选择参数，就能消除观测数据中出格点的影响．因此LTS算子是一个具有较强鲁 棒性的估计算子，当h=n时，LTS方法就等于常规的最小二乘法．在刚体的运动估计模型 中，我们将特征点集合Q分为Q=A∪W,其中A表示刚体运动的特征点集，它们对应于正常数据 矢量{αi}，满足线性方程αTi=1．而W表示出格的特征点集．文[12]中指出， 即使在出格数据W占所有数据Q的比例接近50％时，LTS方法仍能有效地估计出其参数， 此时h可以取，,[ ]表示取整运算．
　　由于(5)式中函数g(β)是非凸的，用常规的优化方法容易陷入其局部极小，[13]中提 出用p子集的方法来估计使g(β)最小的β．在集合Q中任取p个特征点对，它们对应的数据矢 量构成p条线性方程(3)，一般情况下，这p条方程构成的方程组存在唯一解β．那么，对于 含n个数据矢量的集合，共存在Cpn个可能的解，构成集合βJ，βJ中的每一个参数 β′对应于由集合A中的p个特征点对．p子集方法用在集合βJ中使得g(β)最小的参数β′来逼近使(5)式最小的．这样将p维寻优空间减少为在Cpn个参数空间中进行搜索， 即求：
　　　　　　　　　　　(6)
　　但是在参数集βJ中求g(β)的最小也非易事，对于n=100,p=8，那么搜索空间大小为1.9×1011，如果用穷举法进行搜索效率太低，并且鲁棒性不强．而遗传算法是一种基于生物进化思想的优化算法，它利用随机化技术来指导对一个被编码的参数空间进行高效搜索，并具有较强的全局搜索性能，因此我们采用遗传算法求解(6)式．
　　遗传算法是Holland在70年代受生物进化论的启发提出的优化问题求解方法．它模拟了自然界胜者生存的竞争机制．首先定义染色体的适应度，并通过交叉，突变等基本遗传操作算子逐代优化，使得染色体的优势得到积累，直到在某一世代中产生适应度足够高的满意解．
　　为了将遗传算法用于最小截平方和估计问题，这里设计了相应的染色体表示方式，适应度函数的选取及两种新的遗传操作算子．
　　(1) 染色体的表示：在Holland的原始文献[9]中，染色体都使用二进制数0和1组 成的串表示，但作为优化问题的解，不限于仅使用二进制数，可以使用其它符号集或直接使 用数值表示．在刚体运动估计问题中，我们将集合Q中的n个特征点对从1到n进行编号，在计 算时从Q中随机地挑选出p个特征点对，这p个点对的编号组成了遗传算法的一条染色体S=(i 1,i2,…,ip)，在染色体S中，这些编号的次序是无关紧要的．
　　(2) 染色体适应度的求取：
a)对于染色体S=(i1,i2,…,ip)中的每一个编号l,l∈(i1,i2,…,ip)对应 一个特征点对(xl,yl)(x′l,y′l)，它构成数据矢量αl，其满足约束aT lβ′=1，联立并求解S中所有编号对应的约束方程，解表示为β′．
b)求残差，并将r 2i(β′)排序
0≤(r2(β′))1:n≤(r2(β′))2:n≤…≤(r2( β′))n:n
求前h项之和，并定义它为染色体S的适应度．
　　(3) 部分互补交叉算子：根据上述染色体表示形式的特点，我们采用一种新的交叉算子 ．当两条父代染色体S1和S2在交叉算子的作用下繁殖子代个体C1，C2时，C1，C 2中都保存S1和S2交集部分的编号，此外对于S1和S2的互异部分，C1和C2从 中随机地挑选同样数目的编号，最后分别组成含p个编号的集合，称这种算子为“部分互补 交叉算子”．
　　(4) 随机替换突变算子：算子对于一条含有p个编号的染色体，从中选取随机数目的编号将其删去，再从编号1～n中抽取同样数目的尚不包含在染色体中的编号添加其中，但是变异的数目不应过大，使这样既保留原有的优势，又发生了变异． 
通过上述过程，我们可以获得属于正常特征点集A的p个基本点对(对应于池中适应度最高的染色体的p个编号)及对应的参数β．但仅仅利用p个特征点对来估计运动参数并不能有效地抑制Gauss噪声的影响，为了充分地利用Q中的正常数据．我们接着采用递归残差分析的手段，完成正常数据子集A的生长过程，从而将最小二乘方的优点和最小截平方和的优点结合在一起．
3.2 特征点子集A的生长过程
利用遗传算法获得特征点集Q的p个基本特征点对，它们属于子集A，对应的矢量{αi, i=1,2,…,p}满足基本线性方程组：
Xpβ(p)=1　　　　　　　　　　　　　　　　　　(7)
　　其中β(p)表示由p个属于子集A的特征点数据矢量所构成的方程组的解．子集生长过程的基本思想是将Q中的n-p个剩余数据矢量依次添加至基本方程组(7)中，然后分析各自的残差，当其小于某一阈值时就认为它属于子集A，否则将其剔除．
　　在回归模型的残差分析中，标准残差，学生化残差[15]等都不是严格地不相关 的．在出格点检测过程中，我们希望找到一种不相关的残差，并且能够通过递推公式进行计 算．Brown等人提出的递归残差[10]满足了我们的要求．如果找到了j-1个属于子集A 的特征点数据矢量Sj-1={ai|ai∈A,i=1,2,…,j-1;j-1≥p},那么它们满足相同的 模型aiβ(j-1)=1， 联立j-1条方程并求解获得估计参数j-1．为了 观察矢量aj是否满足同Sj-1中矢量相同的线性模型，定义“递归残差”．[10]中指出，如果aj,aj+1, …,an满足方程组(7)， 那么wj,wj+1,…wn为零均值，方差为σ2的随机变量．为此我们可以根据的w j的值来完成对A的数据添加过程．如果wj小于阈值α，就将矢量aj对应的特征点对添至子集A中，并对(7)中的(j-1)进行更新，最终获得A的全部特征点数据及其对 应的运动矢量．
结合[14]中迭代更新公式，我们将子集A的生长过程实现如下：
　　for (j=p+1;j<n;j++)
　　　
　　
　　　　　　
　　　　　　　Add aj into A;}
　　else {(j)=(j-1);
　　(XTjXj)-1=(XTj-1Xj-1) -1}
}

4　实验结果
　　为了验证算法的有效性，进行了下列实验．这里用刚体围绕三个坐标轴X,Y,Z的旋转角φ,θ ,ψ来表示旋转矩阵R[5].取(,θ,ψ)=(6°,9°,12°)，平移向量t=(6,9,3)T.
随机生成刚体运动前的数据集合：
B1={(X,Y,Z)T|Xi∈(-20,20),Yi,Zi∈(10,20), i=1,2,…,N1},
运动后的三维坐标由(1)式算出．对于运动前后的三维坐标进行透视投影，便得到两维投影坐标．这样我们获得运动前后的2D点对应集合A，其大小N1=40．在集合A中的运动后两维投影坐标中加均值为0的Gauss噪声，信噪比SNR=40．
　　分不同的情况来验证我们算法的鲁棒性．
　　在Q=A∪W中不加任何出格点，即集W=集，分别比较线性算法和鲁棒算法的准确性．对线性算法有(1,1,1)=(6.03°,9.09°,11.94°)，平移向量1=(5.97,8.91,3.13)T, 对于鲁棒算法有(2,2,2 )=(6.08°,8.95°,11.91°)，平移向量2=(6.04,8.93,3.07)T．可见在无出格点情况下，线性算法和鲁棒算法结果相近，它们抑制Gauss噪声的能力是相似的．
　　接着我们在集合Q中加入出格点，它是随机生成的2D坐标集合W={(xi,yi)(x′i,y′i)|i=1,2,…,N2}，分别在W在Q中所占比例为10%, 15%, 20%, 25%, 30%,35%, 40%的情况下比较线性算法和鲁棒算法的结果，如图1所示．其中图1(a)表示估计的平移 向量与真实平移向量之间的夹角随着出格点比例变化的结果，图1(b),1(c),1(d)分别表 示旋转角,θ,ψ的估计误差随着出格点比例变化的结果．在图1(a)～(d)中，上面的曲 线对应的是线性算法的结果，下面的曲线对应的是最小截平方和算法的结果．很明显，在存 在出格点的情况下，线性算法失效，而鲁棒算法则能准确地估计出其运动参数．
　　　　　　　　　　
　　(a)　平移向量方向误差Δα 　　　　　　　　　　(b) 旋转角误差Δφ
　　　　　　　　　　　
　　(c)　旋转角误差Δθ　　　　　　　　　　　　　　 (d) 旋转角误差Δψ
图1　实验结果

　　实验结果表明，基于遗传寻优的最小截平方和估计算法，在估计运动参数时具较强的鲁棒性，并且不需任何先验知识，是一个比较理想的估计算法．

作者简介：陈海峰，1975年生，博士生．研究领域为图像处理和机器视觉．
　　　　　何铁军，1974年生，硕士．研究领域为模式识别在智能运输系统中的应用．
　　　　　陈维南，1937年生，博士生导师．研究领域为智能控制和机器视觉．
作者单位：陈海峰、陈维南：东南大学自动化所　南京　210096;
　　　　　何铁军：东南大学智能交通运输研究所

参考文献
1　Tai R Y,Huang T S. Uniqueness and Estimation of 3D Motion Parameters of Rigid Bodies with Curved Surfaces, IEEE trans. PAMI, 1984,6(1): 13～27
2 Weng J, Huang T S, Ajuja N. Motion and Structure from Two Perspective Views: Algorithms, Error Analysis, and Error Estimation. IEEE trans. PAMI, 1989, 11(5):451～476
3 Zhuang X, Wang T,Zhang P. A Highly Robust Estimator Through Partially Likel ihood Function Modeling and its Application in Computer Vision. IEEE Trans. PAMI , 1992, 14(1):19～35
4 梁学斌，吴立德. 用两维点对应数据分割和估计多刚体运动，自动化学报,1998, 24(2):186～191
5 朱嘉琳，吴立德. 用2D点匹配恢复刚体运动参数的鲁棒估计，计算机学报,1994, 17(5):388～392
6 孙学宁等. 一种三维运动参数的鲁棒估计方法. 计算机学报,1990,13(7): 481～488
7 Haralick R M, Jee H, Lee CN, Zhuang X et al. Pose Estimation from Corres ponding Point Data, IEEE trans. PAMI, 1989, 19(6):1426～1446
8 Ruddph G. Convergence Analysis of Canonical Genetic Algorithms. IEEE trans.N eural Network, 1994, 5(1): 96～101
9 Holland J H. Adaption in Natural and Artificial Systems. Ann Arbor:The Univ. of Michigan Press.1977
10 Brown R L, Durbin J, Erans J M. Techniques for Testing the Constancy of Regr ession Relationships Over Time. J. Roy. Stastic. Soc. B, 1975,37:149 ～192
11 陈国良等. 遗传算法及其应用. 人民邮电出版社. 1996
12 Rousseeuw P J, Levoy A. Robust Regression and Outlier Detection. New York:Jo hn Wiley,1987
13 Rousseeuw P J,Gilbert W, Bassett J R. Robustness of the P-subset Algor ithm for Regression with High Breakdown point. Directions in Robust Stastics a nd Diagnostics. Part 2. 1991:185～194
14 KianiFard F, Swallow W H. A Review of th Development and Application of Recu rsive Residuals in Linear Models. JASA. 1996,91:391～400
15 韦博成等. 统计诊断引论. 东南大学出版社,1991
收稿日期：1998-08-25
