基于机器视觉的最大似然位姿估计算法

屈也频，张超然，吕余海

(海军研究院，上海 200235)

引言

利用相机对位置已知的控制点进行拍摄，通过拍摄得到的二维图像解算相机在世界坐标系下的位置和姿态，称为位姿估计问题，是机器人导航、计算机视觉等领域[1-3]的核心问题之一。位姿估计根据计算方法分为2种方式[4]：第一种一般通过解算一组与旋转参数和平移参数相关的多项式，进而获得位姿估计结果[5]，这类方法对于噪声敏感[3]，估计性能有限；另一种通常称为迭代类算法，这种算法通过建立代价函数[6]，将位姿估计问题转换为非线性最小二乘优化问题，然后利用Gauss-Newton[7]、Levenberg-Marquardt等方法进行位姿参数的估计，这类算法能够综合利用多点的冗余信息[8]，增加了估计的鲁棒性，迭代算法中还有一种称为正交迭代(orthogonal iteration, OI)算法，该算法将物空间共线性误差的最小化视为求解绝对定向问题，然后利用迭代方式进行参数估计[9]，这种方法计算量较小，速度较快[10]，并保证了全局收敛性，但是性能略逊于传统迭代算法。

传统迭代算法是从信号的测量误差出发[11]，利用误差最小化的原则建立代价函数，从而得到估计值。这一类算法是一种最小二乘估计，试图使采样得到的信号和无噪声情况下的数据之差的平方达到最小，这一类方法对采样数据不做任何的统计假设，性能取决于噪声的特性，并且往往不是最佳估计，而由于没有对信号做任何统计假设，估计的统计性能是无法评价的[12]。由于相机拍摄的不确定性，图片采样所得的像素信号应视为随机信号，本文从随机信号的统计特性出发，根据采样信号的概率密度推导了最大似然位姿估计的一般形式。从理论上证明了利用单幅图像，在各向同性高斯噪声情况下传统迭代算法等效于最大似然估计，因此在理论上传统迭代算法只能应用于各向同性高斯噪声，对于复杂噪声，将会产生模型误差，导致算法失效。本文的最大似然算法将是推导复杂噪声下的位姿估计的基础；另外，根据信号的Fisher信息阵推导了位姿估计的克拉美-罗界(Cramér-Rao bound, CRB)，作为任何无偏估计的方差下界，克拉美-罗界可以作为位姿估计方法有效性的评价标准，并且根据所得结果可知，通过适当增加采样个数可以有效提高估计的性能。这是由于通过采样信号个数的增加，可以更好地估计随机噪声的统计特性，从而减小随机噪声对于估计的影响。

1 信号模型

(1)

式中：k=1,2,…,K,其中：

m(k)= [u1(k),v1(k),u2(k),v2(k),…,

un(k),vn(k)]T

(2)

图1 位姿估计问题的几何示意图Fig.1 Geometric sketch of pose estimation

世界坐标与相机坐标存在以下关系：

(3)

由共线方程可以得到：

(4)

(5)

式中：tx(k)、ty(k)、tz(k)分别为t(k)从上到下的3个元素；r1(k)、r2(k)、r3(k)分别为R(k)的从上到下的3个横向量；R可以用3个欧拉角表示。假设在K次拍摄过程中，相机的姿态、控制点的世界坐标保持不变，即m不随时间变化，则采样信号可以表示为

s(k)=m+n(k)

(6)

2 最大似然估计

最大似然估计是一种使用了信号概率模型的方法，其基本思想是参数的估计值应是使观测信号概率最大的值。假设信号为高斯随机变量，噪声的均值为零，则m即为观测量s(k)的均值，根据概率论可知，k时刻观测量的概率密度为[13]

(7)

式中：det(·)表示矩阵的行列式；Qs为随机信号s的方差。与待估计参数无关，待估计参数表示为

θ=[α,β,γ,tx,ty,tz]T

(8)

则m(θ)表示为θ的函数，假设K次拍摄是统计独立的，则K次拍摄观测值的联合密度函数为

(9)

对等号两边求自然对数，去掉常数项并除以K，得到似然函数为

(10)

(10)式的第二项可以表示为

(11)

式中：tr{·}表示取矩阵的迹；Cs(θ)定义为信号相关矩阵：

(12)

于是得到似然函数：

(13)

(13)式为最大似然估计的一般形式，使似然函数最大的参数θ即为位姿估计的最大似然估计，可以通过搜索或者更高效的迭代算法计算。

(14)

由于似然函数取最大值的必要非充分条件为

(15)

得到：

(16)

将上式代入似然函数，去除常数项并除以2n，似然函数化简为

L(θ)=-lntr{Cs(θ)}

(17)

根据Cs(θ)的定义，求似然函数的最大值等效于求下式的最小值：

(18)

利用单幅图像时，上式与传统的迭代算法等效，因此，利用单幅图像时，在各向同性高斯噪声情况下传统迭代算法与最大似然算法等效。值得注意的是，上述结果是在(16)式的情况下得到的，即位姿估计是在噪声功率估计的基础上实现的，所以增加信号的采样，提高对噪声统计特性的估计，有利于提高参数估计的性能。

3 克拉美-罗界分析

为了了解参数估计的潜在性能，可以利用克拉美-罗界作为评价标准。克拉美-罗界给出了参数所有无偏估计的方差极限[14]，将估计误差的方差表示为C(θ)，对于所有无偏估计，有关系式：

C(θ)≥CCR(θ)

(19)

式中CCR(θ)表示克拉美-罗界，克拉美-罗界可以通过求解Fisher信息阵J的逆矩阵求得：

CCR(θ)=J-1

(20)

Fisher信息阵J中的元素为

(21)

通过对(13)式求偏导并取期望可以得到[J]i,j通用的形式(推导可参见文献[15])：

(22)

(22)式为Fisher信息阵的一般化形式，是许多克拉美-罗界推导的起点，需要注意的是，通常方差Qs是一个未知矩阵，因此上式应该包含方差的未知参数。

在位姿估计问题中，由于(22)式的第1项只与方差所含未知参数有关，第2项只与待估计参数有关，可以将Fisher信息阵分块：

(23)

(24)

(25)

(26)

根据矩阵分块求逆原理，噪声功率估计的克拉美-罗界为

(27)

(28)

定义

(29)

由于θ=[α,β,γ,tx,ty,tz]T，待估计参数的克拉美-罗界为

(30)

从(30)式中可以看出，参数估计的克拉美-罗界随拍摄数量的增加线性下降，随噪声功率的增加线性增加。

4 仿真

利用计算机仿真对最大似然估计和克拉美-罗界进行分析，并与传统迭代算法以及OI算法进行比较。

仿真参数：相机焦距f=35 mm，单个像素尺寸dx=dy=62.5 μm，主点在图像中心，控制点从相机坐标系的[-2 2]×[-2 2]×[4 9] m3的长方形区域内随机选取，相机坐标系相对于世界坐标系的欧拉角为[20 10 30]°，平移矩阵为[2 3 10] m。图2(a)是随机选取的控制点在相机坐标系下的坐标，图2(b)中圆圈表示相机拍摄控制点时实际成像位置(像素坐标系)，圆圈旁边的10个点为噪声功率为1 dB时，由拍摄的10张图片提取的坐标点。

图2 控制点在相机坐标系和成像平面上的位置Fig.2 Positions of control points in camera coordinate system and image plane

图3是传统迭代算法、OI算法、最大似然方法在不同的噪声功率下的参数估计均方根误差(root-mean-square error, RMSE)，在各噪声级下完成500次独立实验，其中最大似然方法利用了10张图片。从仿真结果中可以看出，3种方法在噪声功率较小的情况下都趋向于克拉美-罗界，传统迭代算法性能略优于OI算法，而最大似然方法的性能明显优于其他两种方法。

图3 不同噪声功率下各算法的参数估计均方根误差Fig.3 RMSEs of different methods with different noise powers

图4是最大似然估计在不同的图片数下的参数估计均方根误差，噪声功率为2 dB，每个图片数完成500次独立实验，可以看出参数估计的RMSE随着图片数的增加而减小，说明适当地增加拍摄数量可有效提高估计的性能。

图4 不同图片数下参数估计的均方根误差Fig.4 RMSEs with different snapshot numbers

5 结论

给出机器视觉位姿估计的信号模型，并从随机信号的角度出发，推导了基于机器视觉的最大似然位姿估计以及相应的克拉美-罗界，从理论上证明了利用单幅图像时，在各向同性高斯噪声情况下传统迭代算法与最大似然算法等效，通过仿真分析，可知在图像数量适当增加的情况下，估计性能得到了明显改进，适用于更高精度的位姿估计。并且推导的位姿估计克拉美-罗界作为任何无偏估计的方差下界，是位姿估计的性能极限，可以作为位姿估计方法有效性的评价标准；下一步，针对复杂的噪声情况，应在此基础上讨论各向异性噪声以及相关噪声情况下的位姿估计方法及其克拉美-罗界。