一种基于pnp透视模型的合作目标位姿精度测量方法
【技术领域】
[0001] 本发明属于光电测量领域，涉及一种基于PNP透视模型的合作目标位姿精度测量 方法，尤其适用于空间合作目标三维位置姿态测量精度的量化估计方法。
【背景技术】
[0002] 空间合作目标通常是安装有人工视觉标记等特征标识的观测目标，其中，人工视 觉标记主要是指已知外形、尺寸、颜色、数量以及空间布局的视觉标记点集合。利用单目视 觉相机包含人工视觉标记的目标图像信息，根据各标记点在目标坐标系下的三维空间坐标 值及其在像平面中的特征点中心二维坐标值之间的一一对应关系约束，计算出的旋转矩阵 和平移矢量，即可准确反映合作目标相对于相机之间的空间位置与姿态，上述测量过程应 归结为PNP透视模型问题（Perspective-N-Point Problem)。因此，视觉标记不仅是单目视 觉相机的观测对象，还是空间合作目标位姿测量的基准与解算依据。
[0003] 在PNP透视模型中，N表示标记点的数目，该问题的求解条件要求标记点个数N不 小于3。当N>5时，可以利用直接线性变换法（Direct Linear Transformation)求解最小 二乘解作为初值，再代入非线性优化算法，解算出位姿参数的最优解；当N < 5时，PNP问题 转化为求解多元高次非线性方程组，且解不唯一。
[0004] 基于PNP透视模型的目标位姿测量精度的影响因素主要包括相机内参标定误差、 标记点三维空间坐标值获取误差以及图像标记点中心二维坐标定位误差等参数。目前，围 绕上述模型的目标位姿测量精度的合理量化预估的研宄，主要集中于定性、定量分析单一 误差因素对位姿测量精度的影响。例如：Larry Davis在《Predicting Accuracy in Pose Estimation for Marker-based Tracking》文献中，利用公式推导与仿真试验相结合，归纳 出观测目标自身的设计尺寸越小，其三维空间坐标值获取误差越大，则位姿测量误差将越 大。程凌、周前祥在《基于视觉方法的交会对接相对位姿测量》文献中，通过建立高精度的 测量控制场以提高标记点三维空间坐标的精度定位，以进一步改善目标位姿的测量精度。 朱楓等在《合作目标姿态对视觉位姿测量精度的影响分析》文献中，针对P3P透视模型问 题，量化分析目标的初始位姿对位姿测量精度的影响，仿真实验结果表明，当3个标记点构 成的三角形所在平面与相机像平面垂直时位姿精度最高，而当这两个平面平行时位姿精度 最差的结论。周静等在《摄像机标定参数误差对位姿测量精度的影响》文献中，从标定方法 选择的角度，量化分析相机标定参数误差对P3P位姿精度的影响关系，公式推导与试验结 果表明，测量距离方向位置精度主要取决于焦比误差和光轴方向平移量误差的影响，二者 分别在远距离段和近距离段起主要作用；姿态角精度则主要受限于主点坐标误差和外姿态 角误差。郝颖明在《基于点特征的位姿测量精度与鲁棒性研宄》文献中，基于简化条件下的 理论分析与一般条件下的误差统计分析，针对P3P透视模型，量化分析了 6个位姿分量误差 (即三个旋转角度和三个平移分量）与相机标定误差、目标模型误差、图像坐标检测误差之 间的关系表达式，并通过直接仿真试验得出以下结论：（1)图像坐标检测误差和相机标定 误差对位姿误差的影响远大于目标模型误差对位姿结果的影响，故后者可以忽略不计；（2) 当其他因素均固定不变时，测量距离与位姿误差呈正比例关系；（3)在远距离处，图像坐标 检测误差对位姿误差的影响占主导地位；(4)在近距离处，相机内参标定误差对位姿测量 误差的影响占主导地位。
[0005] 现有研宄成果往往都是针对P3P透视模型开展位姿精度量化分析，仅专注于单一 误差因素对位姿测量精度的影响权重。对于N>3的PNP透视模型的目标位姿测量精度量化 估计等方面的研宄也相对较少，尚未检索到相关文献。
[0006] 此外，上述文献均未涉及如何将相机内参标定误差、标记点三维空间坐标值获取 误差以及图像标记点中心二维坐标定位误差等多种误差因素有效融合在一起，并建立其与 位姿精度的函数映射关系，因此，根本无法全面量化预估目标位姿精度的整体水平。
【发明内容】
[0007] 有鉴于此，本发明提供了一种基于PNP透视模型的合作目标位姿精度测量方法， 方法不受标记点个数N的约束限制，量化解析相机内参标定误差、视觉标记点三维空间坐 标值获取误差、视觉标记点中心二维坐标定位误差等一种或多种因素对位姿误差的影响关 系，实现测量系统设计初期对目标位姿测量精度水平的准确预估与客观评价。
[0008] 为了达到上述目的，本发明的技术方案为：该方法包括如下步骤：
[0009] 步骤（1)、在合作目标外表面安装N个视觉标记点，采用相机对视觉标记进行标记 图像的拍摄。
[0010] 步骤⑵、建立相机坐标系Oe-XcTA:相机的光心为原点0。，光轴为Z。轴，相机所 拍摄图像平面的水平和垂直方向分别为X。轴和Y。轴；建立关于合作目标的目标坐标系 Ow-XwYwZw^作目标的质心为原点0 w，Xw、Yw、Zw与相机坐标系中的X、Y、Z轴相平行，且正方 向均保持一致；其中第i个视觉标记点的三维空间坐标为（X wi，Ywi，Zwi)及其对应标记图像 中视觉标记点中心二维坐标为（UpV i);
[0011] 对于合作目标与相机之间的位置姿态的初始值进行预先设定。
[0012] 相机内参标定结果为：有效焦距的水平、垂直分量分别为fx，fy，主点坐标的水平、 垂直分量分别为U。，V。。
[0013] 步骤（3)、将步骤（1)中的参数fx，fy，u Q，V。以及Xwi，Ywi，Zwi确定为未确定性系统误 差，f x, fy, U。，V。，Xwi, Ywi, Zwi各自对应的误差极限已知，且分别为 e (f x)，e (fy)，e (uQ)，e (V。)，e (Xwi), e (Ywi), e (Zwi) 〇
[0014] 将4，\确定为随机误差，对应的误差极限已知，且分别为δ (Ui)，δ (Vi)。
[0015] 步骤（4)、量化解析各项参数对位姿误差的影响权重系数。
[0016] 对于第i个视觉标记点，其位姿测量计算公式包括F2i_JP F 2i，二者分别为：
【主权项】
1. 一种基于PNP透视模型的合作目标位姿精度测量方法，其特征在于， 步骤（1)、在所述合作目标外表面安装N个视觉标记点，采用相机对视觉标记进行标记 图像的拍摄； 步骤⑵、建立相机坐标系mz。:相机的光心为原点O。，光轴为Z。轴，相机所拍摄图 像平面的水平和垂直方向分别为X。轴和Y。轴；建立关于合作目标的目标坐标系O W-XWYWZW: 合作目标的质心为原点〇w，xw、Yw、Zw与相机坐标系中的X、Y、Z轴相平行，且正方向均保持 一致；其中第i个视觉标记点的三维空间坐标为（X wi，Ywi，Zwi)及其对应标记图像中视觉标 记点中心二维坐标为（UpV i); 合作目标与相机之间的位置姿态为[tx，ty，tz，α，β，γ] τ，则tx，ty，tz*别为目标坐标 系相对于相机坐标系中沿Xw、Yw、Zw轴的平移分量，α，β， γ分别为合作目标围绕目标坐标 系中Xw、Yw、Zji的旋转角度； 对于合作目标与相机之间的位置姿态的初始值进行预先设定； 相机内参标定结果为：有效焦距的水平、垂直分量分别为fx，fy，主点坐标的水平、垂直 分量分别为U。，V0; 步骤（3)、将步骤（1)中的参数fx，Guc^vtl以及Xwi，Y wi，Zwi确定为未确定性系统误差， fx，fy，uQ，Vq，Xwi，Y wi，Zwi各自对应的误差极限已知，且分别为 e (f x)，e (fy)，e (uQ)，e (V。)，e (Xwi ),e (Ywi), e (Zwi)； 将Ui, \确定为随机误差，对应的误差极限已知，且分别为δ (Ui)，δ (Vi); 步骤（4)、量化解析各项参数对位姿误差的影响权重系数 对于第i个视觉标记点，其位姿测量计算公式包括F2i_JP F 2i，二者分别为：
，其中W为6行X2N列的矩阵，自第10列 开始重复前9列内容；Y'为2N行X 2N列的矩阵，且自第10列开始重复前9列内容； 求解方程M · X' =-Y'中的X'，存在以下两种情况： ① 当2XN = 6时，利用最小二乘法求解，得到最优解为： X, = (-1) · IT1 · Y, ② 当2XN > 6时，利用最小二乘法求解，得到最优解为： X，= (-1) · (Μτ · ΜΓ1 · (MT.Y'） 步骤（5)、在所述随机误差Ui,\的影响下，合作目标与相机之间的位置姿态 [tx，ty，tz，α，β，γ]τ的随机误差分量分别为 δ (tx)，δ (ty)，δ (tz)，δ (α)，δ (β)，δ ( γ);根据如下公式进行计算：
在所述未确定性系统误差fx，fy，Utl, V(l，Xwi，Ywi，Zwi影响下，合作目标与相机之间的位置 姿态[tx，ty，tz，α，β，γ]τ的未确定性系统误差分量分别为 e(tx)，e(ty)，e(t z)，e(a)，e( β )，e (γ );根据如下公式进行计算：
步骤（6)采用目标位姿随机误差分量与目标位姿未确定性系统误差分量依据如下公 式相加获得目标位姿测量误差，完成对目标位姿测量精度的预估：
,其中η任意取值。
【专利摘要】本发明公开了一种基于PNP透视模型的合作目标位姿精度测量方法，包括如下步骤：在合作目标外表面安装N个视觉标记点，采用相机对视觉标记进行标记图像的拍摄；对合作目标与相机之间的位置姿态的初始值进行预先设定；其中第i个视觉标记点的三维空间坐标和有效焦距的水平、垂直分量，主点坐标的水平、垂直分量为未确定性系统误差，误差极限已知；第i个视觉标记点对应标记图像中视觉标记点中心二维坐标为随机误差，对应的误差极限已知；量化解析各项参数对位姿误差的影响权重系数；计算合作目标与相机之间的位置姿态的未确定性系统误差分量和随机误差分量并加权获得目标位姿测量误差，完成对目标位姿测量精度的预估。
【IPC分类】G01C11-00
【公开号】CN104729481
【申请号】CN201510107422
【发明人】谭启蒙, 李劲东, 蔡伟, 胡成威, 高升, 袁宝峰, 陈磊, 杜晓东
【申请人】北京空间飞行器总体设计部
【公开日】2015年6月24日
【申请日】2015年3月12日