CN1804541A - 一种摄像机空间三维位置姿态测量方法 - Google Patents

Abstract

本发明属于测量技术领域，涉及对机器视觉导引系统中摄像机相对于合作目标的空间三维位置姿态测量方法的改进。具体步骤是：在标定阶段：设定合作目标，固定摄像机与合作目标，拍摄摄像机标定图像，提取目标特征点，标定摄像机内部参数；在测量阶段：固定合作目标，移动摄像机(或者移动合作目标，固定摄像机)，拍摄合作目标的图像，提取合作目标上的目标特征点，计算摄像机的三维空间位置姿态，连续测量摄像机在2个不同位置的三维位置姿态计算出移动物体的线速度和角速度。本发明可以获得高质量的目标特征点，简化了合作目标，所获得的目标特征点精度高，可靠性高，具有更大的适应范围。

Description

一种摄像机空间三维位置姿态测量方法

技术领域

本发明属于测量技术领域，涉及对机器视觉导引系统中摄像机相对于合作目标的空间三维位置姿态测量方法的改进。

背景技术

在可视化跟踪或者伺服控制的闭环控制环路中，基于机器视觉的三维位置姿态测量技术是实现智能机器系统(如工业自动装配机器人、水下机器人、无人飞行器、空间飞行器等)的自主导引的最有效途径之一。机器系统的导引控制信号的一部分主要来自于机器系统相对于合作目标的绝对位置和方向信息。因此，在机器系统的闭环控制环路中，精确地估计机器系统的位置和姿态，对实现机器系统的自主导引起着重要作用。机器系统的位置姿态估计主要由导引系统完成，现有的导引方法有：全球定位系统(GPS)、惯性导引系统(INS)、光电导引系统、视觉导引系统以及各种方式的组合导引系统。视觉导引是三维视觉技术的一个重要内容，以计算机视觉为理论基础的视觉导引系统，具有大视场、非接触、速度快、信息丰富以及精度适中等优点，特别适合于估计机器系统相对于合作目标的位置姿态。自主控制系统根据视觉导引系统获得的导引信息，控制机器系统相对于合作目标的相对运动。作为导引控制环路的控制信号获取传感装置，视觉导引系统能够单独估计机器系统的运动状态，也可以作为其他导引系统的辅助传感装置，更加精确地估计机器系统的运动状态，并作为对其他传感系统的信息补充或者进行误差校正。

一种被广泛采用的非接触导引方法是基于特征点的视觉导引方法，其基本原理是，将摄像机装载在机器系统上，并在合作目标平面上预先设置已知的特征点，通过提取特征点对应的图像坐标，根据摄像机的透视投影模型，实时估计出机器系统上摄像机相对于合作目标的位置姿态。由此可知，基于特征点的视觉导引方法的关键问题是解决摄像机的三维位置和姿态的测量。2001年，Sharp CS等人在文章“一种无人航空飞行器视觉着陆系统”(A vision system for landing an unmanned aerialvehicle)IEEE会议录《机器人和自动化国际会议》，第21～26页，2001年(Proceedings of 2001 IEEE，International Conference of Roboticsand Automation，2001)提出基于以正方形顶点为合作目标特征点的摄像机位置姿态测量方法，该方法以5个小四边形和1个大四边形为基本合作目标，通过四边形的不同空间分布实现特征点的自动排序，且特征点具有透视投影不变性，能够获得较高精度。该方法需要至少6个四边形的合作目标，由于四边形顶点的图像特征容易受到光照、噪声影响，使得提取精度下降，因此该方法的可靠性和适应范围受到很大限制。

发明内容

本发明所要解决的技术问题是：提供一种精度较高，基于双圆合作目标的摄像机三维位置姿态实时测量方法，以简化合作目标，提高目标特征点的自动探测的鲁棒性和图像坐标提取精度，增强视觉导引方法的适应能力。

本发明的技术解决方案是：一种摄像机空间三维位置姿态测量方法，其特征在于，测量过程分标定和测量两个阶段，具体步骤是：

1.1、标定阶段：

1.1.1、设定一个合作目标，它为一个二维平面，在合作目标平面上有2个圆，圆的半径为10～10000mm，其精度为0.01mm～1mm，圆与背景的灰度相反，如背景为黑色，则圆为白色，或反之，2个圆心之间的距离为其半径的3倍以上，在其中一个圆的中心设定1个任意形状的小块区域，该区域的灰度与背景相同，面积为大圆的1/5～1/3；

1.1.2、将合作目标和摄像机固定好，摄像机和合作目标的距离由摄像机的拍摄范围确定，要求摄像机能够完整观测到合作目标内的2个圆，打开摄像机电源；

1.1.3、在摄像机的视场范围内，自由、非平行移动合作目标至少5个位置，每移动一个位置，拍摄一幅图像，称为摄像机标定图像，并保存到计算机中，要求合作目标的2个圆包含在图像内；

1.1.4、合作目标上的2个圆的透视投影图像为2个椭圆，以密集度即面积和周长平方的比值为描述特征，自动分割出标定图像中的2个椭圆，提取每一个位置的摄像机标定图像中2个椭圆的所有8个公切点，求出不同公切点的连线交点，公切点和连线交点共21点，称为目标特征点，将目标特征点的图像坐标及其对应世界坐标，存储到计算机中；

1.1.5、利用所有位置的目标特征点图像坐标及其对应的世界坐标用来标定摄像机内部参数，包括摄像机有效焦距和主点坐标，保存到系统参数文件中，以备测量阶段调用；

1.2、测量阶段：

1.2.1、设定工作模式，包括2种工作模式：摄像机移动模式和合作目标移动模式，摄像机移动模式指合作目标固定，摄像机移动；合作目标移动模式指合作目标移动，摄像机固定。在2种模式下，要求摄像机能观测到合作目标上的2个大圆，以下假定测量阶段工作在摄像机移动模式，合作目标移动模式类同；

1.2.2、移动可移动物体到空间一个位置，拍摄合作目标的图像，称为测量图像；

1.2.3、在测量图像中，提取合作目标上的21个目标特征点的图像坐标并与其世界坐标对应；

1.2.4、根据所建立的摄像机的运动模型和透视投影模型，利用摄像机内部参数、目标特征点的图像坐标及对应世界坐标，由所建模型计算摄像机的三维空间位置姿态，显示测量结果；

1.2.5、重复1.2.2至1.2.4的步骤，连续测量摄像机在2个不同位置的三维位置姿态，由摄像机在不同时刻的位置和姿态分别计算得到摄像机的线位移和角位移参数，并由2个位置的运动时间，由摄像机的运动模型，计算出移动物体的线速度和角速度。

本发明的优点是：本方法以双圆合作目标的公切点以及不同公切点连线的交点作为目标特征点，根据所建立的摄像机透视投影模型和运动模型，计算摄像机的三维位置和姿态，并可以进一步求出摄像机的线位移、角位移、线速度以及角速度等参数，从而为实现机器系统的自主导引提供控制信号。与现有的采用四边形顶点为目标特征点的摄像机位置姿态测量方法相比，即便在圆有部分遮挡，或者受光照影响，圆边缘图像部分受到污染的情况下，仍然可以获得高质量的目标特征点。因此该方法简化了合作目标，所获得的目标特征点精度高，可靠性高，具有更大的适应范围。特别适合为现场机器系统的自主导引提供控制信号。

附图说明

图1是基于目标特征点的视觉导引流程图。

图2是双圆合作目标。

图3是双圆合作目标的世界坐标系定义及目标特征点示意图。1～4和18～21为公切点，4～17为公切点连线的交点，1～21称为目标特征点。

图4是摄像机运动和透视投影模型示意图。

图5是双圆目标特征点图像坐标提取过程实例。

图6是仿真试验中生成的带双圆目标的合成图像。

图7是本发明算法对高斯白噪声的敏感性仿真试验结果。

具体实施方式

下面对本发明方法做进一步详细说明。本发明使用双圆合作目标实现摄像机三维位置姿态以及运动参数的测量。机器系统的导引控制信号的一部分主要来自于机器系统相对于合作目标的绝对位置和方向信息。因此，在机器系统的闭环控制环路中，精确地估计机器系统的位置和姿态，对实现机器系统的自主导引起着重要作用。视觉导引是三维视觉技术的一个重要内容，以计算机视觉为理论基础的视觉导引系统，具有大视场、非接触、速度快、信息丰富以及精度适中等优点，特别适合于估计机器系统相对于合作目标的位置姿态。基于目标特征的视觉导引流程如图1所示，自主控制系统根据视觉导引系统获得的导引信息，控制机器系统相对于合作目标的相对运动。作为导引控制环路的控制信号获取传感装置，视觉导引系统能够单独估计机器系统的运动状态，也可以作为其他导引系统的辅助传感装置，更加精确地估计机器系统的运动状态，并作为对其他传感系统的信息补充或者进行误差校正。

基于特征点的视觉导引方法基本原理是，将摄像机装载在机器系统上，并在合作目标平面上预先设置已知的特征点，通过提取特征点对应的图像坐标，根据摄像机的透视投影模型，实时估计出摄像机相对于合作目标的位置姿态。

摄像机运动及投影模型：

假定载有摄像机的机器系统的运动是相对连续的，机载摄像机与合作目标的基本关系如图2所示。π_w为位于合作目标上的靶标平面，π_c为摄像机的图像平面。t时刻的摄像机坐标系为o_tx_ty_tz_t，o_n为主点(光轴与图像平面的交点)，图像坐标系为o_ux_uy_u。假定在靶标平面上的三维世界坐标系为o_wx_wy_wz_w。定义o_tx_t//o_ux_u，o_ty_t//o_uy_u和o_tz_tπ_c。

设π_w上任意特征点P_i在π_c上的透视投影点为p_i。设P_i在o_wx_wy_wz_w下的齐次坐标为 ${\tilde{p}}_{\mathrm{wi}}={(x_{\mathrm{wi}},y_{\mathrm{wi}},z_{\mathrm{wi}},1)}^T,$ 在o_tx_ty_tz_t下的齐次坐标为 ${\tilde{p}}_{t,i}={(x_{t,i},y_{t,i},z_{t,i},1)}^T,$ 相应的图像齐次坐标为 ${\tilde{p}}_{\mathrm{ui}}={(x_{\mathrm{ui}},y_{\mathrm{ui}},1)}^T.$ 则机载摄像机的投影模型可以表示为

$\mathrm{\ρ}{\tilde{p}}_{\mathrm{ui}}=A\left[\begin{array}{cc}{R}_{w,t}& T_{w,t}\end{array}\right]{\tilde{p}}_{\mathrm{wi}},\mathrm{\ρ}\≠0---\left[1\right]$

其中 $A=\left[\begin{array}{ccc}{f}_x& 0& u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right]$ 为摄像机内部参数矩阵，f_x，f_y为摄像机在x、y方向上的有效焦距，u₀，v₀为摄像机的主点坐标，ρ为任意不为0的比例因子。R_w，t为3×3单位正交矩阵，表示世界坐标系到t时刻摄像机坐标系之间的旋转。T_w，t为世界坐标系到t时刻摄像机坐标系之间的平移。

由于位于靶标平面上的特征点的世界坐标z_wi＝0，这里仍然采用 ${\tilde{p}}_{\mathrm{wi}}={(x_{\mathrm{wi}},y_{\mathrm{wi}},1)}^T$ 表示平面上特征点的二维世界坐标，则由式(1)得到：

$\mathrm{\ρ}{\tilde{p}}_{\mathrm{ui}}=H{\tilde{p}}_{\mathrm{wi}}---\left[2\right]$

其中H＝A[r₁ r₂ T]＝[h₁ h₂ h₃]，h_i(i＝1～3)为H的第i列，r_i为矩阵R_w，t的列向量。H矩阵表示了靶标平面和图像平面之间的单应性变换，由式(2)可知，需要至少4个非共线对应点，就可以求出带比例因子的H矩阵。若摄像机经过标定，则A已知，由Zhang Z.(张政友)的文章“一种灵活的摄像机标定新技术”(A Flexible New Technique for CameraCalibration)，第7届计算机视觉国际会议，第一期，666～673页，1999年(In 7^th International Conference on Computer Vision，No.1，666～673，1999)中的算法，可以得到R_w，t和T_w，t：r₁＝ρA^-1h₁ r₂＝ρA^-1h₂ r₃＝r₁×r₂ T＝ρA^-1h₃ ρ＝1/‖A^-1h₁‖＝1/‖A^-1h₂‖    [3]

由式[3]计算出的R_w，t求出的R_w，t，T_w，t，易受噪声的影响，为了获得更可靠的结果，由摄像机的透视投影模型(公式[1])，由Levenberg-Marquardt非线性优化方法求得最优R_w，t，T_w，t。Levenberg-Marquardt算法，参见《最优化理论与方法》，(袁亚湘、孙文瑜著，科学出版社，1999年)。

根据以上分析可知，由4个以上的靶标平面上的特征点及其对应图像坐标，就可以估计世界坐标系到t时刻摄像机坐标系的三维变换，即

${\tilde{p}}_{t,i}=\left[\begin{array}{cc}{R}_{w,t}& T_{w,t}\\ 0^T& 1\end{array}\right]{\tilde{p}}_{\mathrm{wi}}=M_{w,t}{\tilde{p}}_{\mathrm{wi}}---\left[4\right]$

其中 $M_{w,t}=\left[\begin{array}{cc}{R}_{w,t}& T_{w,t}\\ 0^T& 1\end{array}\right],$ 则(R_w，t)^T和-(R_w，t)^TT_w，t表示了t时刻摄像机的位置姿态。

同理可得：

${\tilde{p}}_{t-1,i}=\left[\begin{array}{cc}{R}_{w,t-1}& T_{w,t-1}\\ 0^T& 1\end{array}\right]{\tilde{p}}_{\mathrm{wi}}=M_{w,t-1}{\tilde{p}}_{\mathrm{wi}}---\left[5\right]$

其中M_w，t-1表示了t-1时刻世界坐标系到摄像机坐标系的变换，则摄像机坐标系到世界坐标系的变换为：

$M_{t-1,w}=\left[\begin{array}{cc}{\left(R_{w,t-1}\right)}^T& -{\left(R_{w,t-1}\right)}^T{T}_{w,t-1}\\ 0^T& 1\end{array}\right]$

${\tilde{p}}_{t,i}=M_{w,t}{M}_{t-1,w}{\tilde{p}}_{t-1,i}=M_{t-1,t}{\tilde{p}}_{t-1,i}---\left[6\right]$

其中 $M_{t-1,t}=\left[\begin{array}{cc}{R}_{t-1,t}& T_{t-1,t}\\ 0^T& 1\end{array}\right]$ 表示了t-1时刻的摄像机坐标系到t时刻的摄像机坐标系的变换，即表示了摄像机的相对运动。

由参考文献《机器视觉》(贾云得编著，科学出版社，2000年)，旋转矩阵R_t-1，t可以表示成欧拉角，即用θ_x，θ_y和θ_z来描述从t-1时刻到t时刻摄像机在x、y和z方向的相对角位移，T_t-1，t描述了摄像机从t-1时刻到t时刻的线位移，即用t_x，t_y和t_z摄像机在x、y和z方向的相对位移。设时间间隔为Δt，则可以直接计算出相应的摄像机的角速度和线速度。计算公式为：

${\mathrm{\ω}}_x=\frac{{\mathrm{\θ}}_x}{\mathrm{\Δt}},{\mathrm{\ω}}_y=\frac{{\mathrm{\θ}}_y}{\mathrm{\Δt}},{\mathrm{\ω}}_z=\frac{{\mathrm{\θ}}_z}{\mathrm{\Δt}}---\left[7\right]$

$v_x=\frac{t_x}{\mathrm{\Δt}},v_y=\frac{t_y}{\mathrm{\Δt}},v_z=\frac{t_z}{\mathrm{\Δt}}---\left[8\right]$

由此可知，如果能够获得至少4个靶标平面上的特征点及对应t时刻图像坐标，根据式(2)和(4)就可以估计摄像机在t时刻的位置姿态。分别估计出t-1时刻和t时刻的摄像机位置姿态，由式(5)就可以估计摄像机的相对运动。如果时间间隔足够小，就可以根据相对运动，估计出摄像机的相对角速度和线速度，即能够获得摄像机的运动参数。

根据摄像机的运动和投影模型和本发明的测量原理，摄像机三维位置姿态的非接触测量分为摄像机内部参数的标定和根据测量模型进行测量2个阶段。

摄像机模型参数的标定具体步骤如下：

摄像机的内部参数包括焦距和主点，共4个参数。

1、设定一个合作目标，它为一个二维平面，在合作目标平面上有2个圆，圆的半径为(10～10000)mm，其精度为(0.01mm～1mm)。圆与背景的灰度相反(如背景为黑色，则圆为白色，或反之)。2个圆心之间的距离为3倍半径以上。在其中一个圆的中心设定一个任意形状的小块区域，其灰度与背景相同，面积约为大圆的1/5～1/3。

2、将合作目标和摄像机固定好，摄像机和合作目标的距离由摄像机的拍摄范围确定，要求摄像机能够完整观测到合作目标内的2个圆，打开摄像机电源。

3、在摄像机的视场范围内，自由、非平行移动合作目标1至少5个位置，每移动一个位置，拍摄一幅图像，称为摄像机标定图像，并保存到计算机中，要求合作目标的2个圆包含在图像内。

4、合作目标上的2个圆的透视投影图像为2个椭圆，以密集度(面积和周长平方的比值)为描述特征，自动分割出标定图像中的2个椭圆，提取每一个位置的摄像机标定图像中2个椭圆的所有8个公切点，求出不同公切点的连线交点，公切点和连线交点共21点，如图3的1～21所示，称为目标特征点，其中1～4和18～21为公切点，5～17为由公切点连线交点产生的目标特征点。合作目标的世界坐标系定义为：带有小圆的圆心为原点o_w，x_w轴水平向右，y_w轴向下，z_w由右手法则确定。

由图1可知，图像特征信息的提取是视觉导航的关键步骤，特征提取的速度、精度和算法的鲁棒性直接决定了导航系统的性能。针对本发明所设计的靶标图案，图像处理的基本任务是实时提取摄像机在不同时刻拍摄的靶标图像中的特征点并将其与世界坐标对应。双圆合作目标的21个特征点的世界坐标，由给定圆的半径和中心可以求出。非退化二次曲线(如2个圆或者2个椭圆)公切线的求解算法，参见M.Pollefeys的演讲报告“三维模型的图像获取指南”，欧洲计算机视觉会议，2000年(Tutorial on 3D Modeling from Images，European Conference onComputer Vision 2000)。

求得双圆的公切线后，由公切线与二次曲线的交点获得公切点的世界坐标，由公切点可以求得其他特征点。所有公切点按图3所示顺序排列。

特征点的图像坐标需要从投影的椭圆中求得，而且图像特征点必须和相应的世界坐标对应。本发明提出的双圆特征图像坐标的提取及标记算法主要包括图像阈值化、轮廓跟踪、分割双椭圆及求解参数方程、双椭圆公切点及排序和全部特征点标记等。具体过程如下：

1)、采用自适应阈值化方法对图像进行阈值化处理，获得二值图像。具体算法参见《图像分割》，(章毓晋著，科学出版社，2001年)。

2)、在二值图像中，采用8连通方法跟踪轮廓，获得链码表示的轮廓。把像素值为0(或者1)的部分分离出来，将0像素值连接起来组成0连通区域，1像素值连接起来组成1连通区域。

3)、分割着陆靶标上的双椭圆。首先排除断开轮廓，保留封闭连通轮廓；排除周长小于T_L(轮廓长度阈值)的轮廓；计算封闭轮廓的密集度C＝4π·A/P²，A为封闭轮廓面积，P为周长。考虑到透视投影的特性和图像噪声的影响，实际椭圆的密度集小于1，设置阈值T_C＝0.7～1.0，T_C从大到小设置。保留密度集大于T_C的封闭轮廓。如果保留的封闭轮廓数量小于2，减小T_C，重新进行以上过程。

4)、分别将获得的2个封闭轮廓数据，采用最小二乘方法拟合成2个椭圆。求出2个椭圆的公切点，记为p_ui(i＝0～7)。

5)、分别取2个椭圆中心的9×9邻域，并计算邻域内的平均灰度值，设灰度值大的邻域对应的椭圆中心为o₁，另一中心为o₂，则o₁位于o_w的透视投影点的附近，

对应于 的投影。计算

与图像x轴的夹角，设为θ。将

和8个公切点旋转θ，使得

与x轴对准，按旋转后的公切点，很容易排定8个公切点的对应顺序，由此也就确定了8个公切点图像坐标与世界坐标的对应关系，利用已知的8对公切点，就可以获得其余13个特征点的对应关系，从而完成了特征点图像坐标的提取以及标记。图5为双圆图像特征提取和标记算法所获得的结果。

将目标特征点的图像坐标及其对应世界坐标，存储到计算机中。

5、利用所有位置的目标特征点图像坐标及其对应的世界坐标用来标定摄像机内部参数，包括摄像机有效焦距和主点坐标，保存到系统参数文件中，以备测量阶段调用。

完成标定以后，只要不调整摄像机的镜头焦距和光圈，可以对不同位置时的摄像机三维位置姿态进行连续测量。

测量阶段：

6、设定工作模式。包括2种工作模式：摄像机移动模式和合作目标移动模式。摄像机移动模式指合作目标固定，摄像机移动；合作目标移动模式指合作目标移动，摄像机固定。在2种模式下，要求摄像机能观测到合作目标上的2个大圆。以下假定测量阶段工作在摄像机移动模式，合作目标移动模式类似；

7、当摄像机空间移动到一个位置时，拍摄合作目标的图像，称为测量图像。

8、在测量图像中，提取合作目标上的21个目标特征点的图像坐标并与其世界坐标对应。目标特征点的图像坐标提取与标定阶段的步骤4相同。

9、根据摄像机的运动模型(见公式[5])和透视投影模型(见公式[1])，利用摄像机内部参数、目标特征点的图像坐标及对应世界坐标，根据公式[2]到公式[5]，计算摄像机的三维空间位置姿态，显示测量结果。

10、由以上测量方法连续测量摄像机在2个不同位置的三维位置姿态，由公式[6]计算得到摄像机的线位移和角位移参数，并由2个位置的运动时间，由公式[7]和公式[8]，计算出摄像机的线速度和角速度。

11、重复步骤7～10，进行新的摄像机三维位置姿态测量。

实施例

采用摄像机从不同角度拍摄本发明所设计的双圆合作目标，并利用所提出的图像特征提取及标记算法进行了大量试验，所采用的摄像机为WAT-902H(CCIR)CCD摄像机、25mm镜头，软件运行在PIV 2.4G，图像大小为768×576像素，单次图像特征提取和标记的平均时间小于9ms，特征点的提取精度能达到亚像素，且在任意方位都能准确标记点的对应关系。因此，本发明所提出的图像特征点提取算法能够满足机器系统自主控制过程中摄像机位置姿态估计的实时性要求。

按照上面叙述的步骤，利用图2所示的双圆目标对摄像机的内部参数进行了标定。获得的标定参数为：

摄像机内部参数： $A=\left(\begin{array}{ccc}1521.204& 0& 400.987\\ 0& 1515.462& 284.554\\ 0& 0& 1\end{array}\right)\mathrm{pixel}$

用标定过的摄像机，拍摄任意一位置的双圆目标图像，如图5(a)所示。图像大小为768×576像素。按照本发明的测量方法获得的摄像机三维位置姿态参数为：

位置为：(-2415.3，-929.5，-1167.2)mm

姿态按ZYX顺序为：(39.2°，-6.2°，-27.3°)

采用以下仿真试验验证本发明所提出的算法，主要步骤为：

1)由给定的着陆靶标图案上双圆的基本参数计算21个特征点的世界坐标。假定圆的半径为200mm，圆心距为600mm。

2)给定摄像机的基本参数：焦距为2900像素(pixels)，约为25mm，主点坐标取图像中心。图像大小为768×576像素。

3)给定摄像机在世界坐标系中的位置和姿态参数。位置为(200，-200，-1000)cm，姿态按ZYX顺序，为(10°，-10°，-10°)。

4)分别在2个圆周上取200个测试点，由摄像机投影模型和给定参数，产生无噪声的测试点的图像坐标。所合成的仿真图像如图6所示。

5)随机取400个测试点的一种排列顺序，在其图像坐标中加入0均值，标准偏差为σ(σ＝0～1.5)的高斯白噪声，并分别由带噪声的测试点分别拟合2个椭圆。

6)由第5)步拟合的带噪声的椭圆，求得21个特征点的图像坐标，由公式(2)～(5)估计摄像机的位置和姿态。对每一个σ的高斯白噪声，分别取1000个不同排列顺序进行试验，然后计算1000个估计结果的RMS误差，结果如图7所示，横坐标表示所加入的高斯白噪声的标准偏差σ，其取值范围为0～1.5pixels，左图的纵坐标表示摄像机姿态的RMS误差，右图的纵坐标表示摄像机位置的RMS误差。从图中可以看出，当圆边缘轮廓的图像坐标受到1.5pixels的噪声影响时，摄像机的位置姿RMS误差分别小于6cm和0.7°。

Claims (1)

1、一种摄像机空间三维位置姿态测量方法，其特征在于，测量过程分标定和测量两个阶段，具体步骤是：

1.1、标定阶段：

1.1.1、设定一个合作目标，它为一个二维平面，在合作目标平面上有2个圆，圆的半径为10～10000mm，其精度为0.01mm～1mm，圆与背景的灰度相反，如背景为黑色，则圆为白色，或反之，2个圆心之间的距离为其半径的3倍以上，在其中一个圆的中心设定1个任意形状的小块区域，该区域的灰度与背景相同，面积为大圆的1/5～1/3；

1.1.2、将摄像机与合作目标相距一定距离固定好，打开CCD摄像机电源；

1.1.3、在摄像机的视场范围内，自由、非平行移动合作目标至少5个位置，每移动一个位置，拍摄一幅图像，称为摄像机标定图像，并保存到计算机中，要求合作目标的2个圆包含在图像内；

1.1.4、合作目标上的2个圆的透视投影图像为2个椭圆，以密集度即面积和周长平方的比值为描述特征，自动分割出标定图像中的2个椭圆，提取每一个位置的摄像机标定图像中2个椭圆的所有8个公切点，求出不同公切点的连线交点，公切点和连线交点共21点，称为目标特征点，将目标特征点的图像坐标及其对应世界坐标，存储到计算机中；

1.1.5、利用所有位置的目标特征点图像坐标及其对应的世界坐标用来标定摄像机内部参数，包括摄像机有效焦距和主点坐标，保存到系统参数文件中，以备测量阶段调用；

1.2、测量阶段：

1.2.1、设定工作模式，包括2种工作模式：摄像机移动模式和合作目标移动模式，摄像机移动模式指合作目标固定，摄像机移动；合作目标移动模式指合作目标移动，摄像机固定，在2种模式下，要求摄像机能观测到合作目标上的2个大圆，以下假定测量阶段工作在摄像机移动模式，合作目标移动模式类同；

1.2.2、移动摄像机到空间一个位置，拍摄合作目标的图像，称为测量图像；

1.2.3、在测量图像中，提取合作目标上的21个目标特征点的图像坐标并与其世界坐标对应；

1.2.4、根据所建立的摄像机的运动模型和透视投影模型，利用摄像机内部参数、目标特征点的图像坐标及对应世界坐标，由所建模型计算摄像机的三维空间位置姿态，显示测量结果；

1.2.5、重复1.2.2至1.2.4的步骤，连续测量摄像机在2个不同位置的三维位置姿态，由摄像机在不同时刻的位置和姿态分别计算得到摄像机的线位移和角位移参数，并由2个位置的运动时间，由摄像机的运动模型，计算出移动物体的线速度和角速度。

Images