CN103994755B - 一种基于模型的空间非合作目标位姿测量方法 - Google Patents

Abstract

本发明公开了一种基于模型的空间非合作目标相对位姿测量方法，其特征在于：包括以下主要步骤：根据上一时刻相对位姿获得目标模型的深度边缘与纹理边缘，得到其可见边缘模型；根据KLT算法提取当前场景图像的Harris特征；将目标可见边缘模型投影到图像场景中，沿边缘模型法向搜索图像边缘并计算对应特征点对的残差；通过深度缓存估计上一帧Harris特征的3D坐标并将其以当前估计位姿投影到当前图像，计算对应特征点对残差；根据M估计子鲁棒方法和两种方法的平均残差分配特征点的权重；根据视觉伺服方法迭代计算相对位姿。根据本发明的方法，可以实现对空间非合作目标相对位姿的测量，而且具有计算量低的优点。

Description

一种基于模型的空间非合作目标位姿测量方法

技术领域

本发明涉及航空航天技术领域，尤其是涉及一种基于模型的空间非合作目标位姿测量方法。

背景技术

随着航空航天技术的发展，空间在轨服务成为未来空间探索任务的一个重要趋势，世界各航天大国纷纷开展空间在轨交会对接的演示验证试验，典型的试验如日本的ETS-VII和美国的“轨道快车”。对空间目标尤其是空间非合作目标的交会首先要实现对空间目标的相对位置和姿态(以下简称相对位姿)的精确测量。

目前在轨的空间目标大多没有考虑被在轨服务，通常具有以下特征：一、目标航天器上没有安装被抓捕装置及用于辅助测量的合作标靶等；二、目标运动状态不确定，可能处于三轴姿态稳定、自旋稳定甚至自由翻滚；三、目标与服务航天器之间无法进行通讯，不能提供自身运动参数等信息。具有以上特征的空间目标都可以称为空间非合作目标。而航天领域已有的测量技术大多针对合作目标开展，针对空间非合作目标测量的技术还不成熟。

已经出现的一些针对空间非合作目标的测量方法或采用双目立体视觉匹配，或假设目标部分尺寸已知通过提取直线交叉点作为特征求解N点透视问题(PnP)。这些方法或受限于立体视觉的基线限制，或受限于特征提取的复杂性，对空间非合作目标的测量精度和实时性都不高，无法应用于空间非合作目标的在轨服务任务。因此，研究一种适于空间非合作目标测量的方法就显得非常必要。

发明内容

本发明所要解决的技术问题是克服现有技术的不足，提供一种基于模型的空间非合作目标位姿测量方法。

本发明的技术问题通过下述技术手段予以解决：

一种基于模型的空间非合作目标相对位姿测量方法，用于在已知空间非合作目标的结构模型的情况下，通过一个单目相机实现对所述空间非合作目标的实时位姿的测量，包括以下步骤：

步骤1、基于所述结构模型，计算得到空间非合作目标上一时刻相对位姿下的可见边缘模型；将所述可见边缘模型投影到当前时刻的场景图像中，沿所述可见边缘模型各边缘的法向搜索所述场景图像的对应边缘以确定预定数量的对应特征点对，计算各对应特征点对的第一残差R_e及第一残差R_e相对当前估计位姿的第一Jacobi矩阵J_e，并计算所有对应特征点对的第一残差平均值

步骤2、通过所述结构模型在上一时刻相对位姿下的深度缓存和所述空间非合作目标上一时刻场景图像的Harris特征点的位置，计算所述Harris特征点在当前时刻场景图像中的位置以及所述Harris特征点在上一时刻相对位姿下的3D坐标，将所述3D坐标以当前估计位姿投影到当前时刻的场景图像中并计算其与所述Harris特征点在当前时刻场景图像中的位置之间的第二残差R_p及第二残差R_p相对当前估计位姿的第二Jacobi矩阵J_p，并计算所有对应特征点对的第二残差平均值

步骤3、根据M估计子鲁棒方法确定所述步骤1中各第一残差R_e的权重ω_e、及所述步骤2中各第二残差R_p的权重ω_p；以第一残差平均值和第二残差平均值确定所述步骤1与所述步骤2之间的权重分配，其中，第一残差R_e与第一Jacobi矩阵J_e的权重第二残差R_p与第二Jacobi矩阵J_p的权重Factor_KLT＝1-Factor_MBT；

步骤4、根据视觉伺服方法迭代计算空间非合作目标当前时刻的相对位姿 $M_o^{k+1}{}_c=M_o^k{}_c{e}^v,$ 其中：

表示当前时刻第k次迭代估计得到的相对位姿；e代表自然指数；

相机的速度v＝-λ(J^TJ)+J^TR，0＜λ＜1为预设的残差衰减指数，(J^TJ)⁺为J^TJ的广义逆矩阵， $R=\left[\begin{array}{c}{R}_e.*{\mathrm{\ω}}_e.*{\mathrm{Factor}}_{MBT}\\ R_p.*{\mathrm{\ω}}_p.*{\mathrm{Factor}}_{KLT}\end{array}\right],J=\left[\begin{array}{c}{J}_e.*{\mathrm{\ω}}_e.*{\mathrm{Factor}}_{MBT}\\ J_p.*{\mathrm{\ω}}_p.*{\mathrm{Factor}}_{KLT}\end{array}\right].$

本发明与现有技术相比具有如下优点：(1)采用基于结构模型的空间非合作目标测量方法，仅需要目标的结构模型，而不需要目标安装反射器或用于辅助测量的合作标靶等装置，这对需要进行在轨服务的在轨目标而言一般都可以得到；(2)可以对目标的完整结构模型进行处理，根据目标距离的远近自动确定合适的目标边缘模型，既避免因利用完整结构模型带来的计算复杂度，也无需事先对结构模型进行繁琐的人工简化，只要在相机测量范围内即可较好自动确定目标可见边缘模型；(3)仅依靠一个单目视觉即可完成对目标相对位姿的实时精确测量，成本低，实时性好，同时避免多传感器带来的系统可靠性降低。

优选地：

所述步骤1中基于所述结构模型，计算得到空间非合作目标上一时刻相对位姿下的可见边缘模型包括以下步骤：

对所述结构模型在上一时刻相对位姿下深度缓存进行二阶差分操作提取空间非合作目标可见部分的深度边缘，通过3D渲染引擎对所述结构模型进行渲染，并对上一时刻相对位姿下的渲染结果进行简单的Canny边缘检测得到空间非合作目标的纹理边缘；将所述深度边缘和所述纹理边缘进行结合获得所述可见边缘模型。

优选地：

所述步骤1中沿所述可见边缘模型法向搜索所述场景图像的边缘，包括以下步骤：

对投影到当前时刻的场景图像的所述可见边缘模型沿边缘按预定间隔进行采样以确定所述可见边缘模型上的特征点集，对所述特征点集中的每一个特征点，沿着所述特征点所在边缘的法向方向在预定范围内搜索，对搜索范围内的每一个像素点进行一次掩膜卷积计算，掩膜方向和所述特征点所在边缘的方向平行，当某一所述像素点与掩膜的卷积结果超过预先设定的阈值时即认为该像素点为当前时刻的场景图像中与所述特征点对应的边缘特征点，如此确定所述可见边缘模型上各特征点在当前时刻的场景图像中对应的边缘特征点以形成对应特征点对，即确定所述场景图像与可见边缘模型在当前时刻所对应的目标图像边缘。

优选地：

所述步骤2包括以下步骤：

步骤210、利用KLT算法提取上一时刻场景图像的Harris特征点在当前时刻场景图像中的位置

步骤220、通过上一时刻场景图像中Harris特征点和所述结构模型在上一时刻相对位姿下的深度缓存，估计所述Harris特征点在上一时刻的3D坐标

步骤230、将所述3D坐标以当前估计的位姿投影到当前图像中，得到

步骤240、计算与之间的残差为所述第二残差R_p并计算第二残差R_p相对当前估计位姿的Jacobi矩阵J_p。

本优选方案一方面通过KLT算法确定特征点在当前图像中的位置，另一方面通过图像中特征点在目标模型深度缓存中的位置，计算图像中各特征点在模型中3D坐标，再通过3D渲染引擎将这些特征点的3D坐标以当前估计位姿投影出来，从而确定图像中特征点对的匹配，避免了复杂耗时的特征点对的检测与匹配过程，提高跟踪测量的效率。

优选地：所述结构模型为CAD模型。

附图说明

图1是本发明具体实施例的基于模型的空间非合作目标相对位姿测量方法流程图；

图2是本具体实施例的基于模型与KLT方法的图像特征确定原理示意图。

具体实施方式

下面结合优选的实施方式对本发明作进一步说明。

本发明提出的基于模型的空间非合作目标相对位姿测量方法，包括基于模型提取可见边缘特征(简称MBT)和基于模型提取图像特征点对(简称KLT)两个并行框架，然后再对所有特征点对统一分配权重以计算相对位姿。整个方法流程如图1所示，各部分步骤如下：

所述的基于模型提取可见边缘特征框架，具体包含以下步骤：

S101、基于空间非合作目标(下文称目标)的结构模型(下文简称目标模型)，利用3D渲染引擎获取目标上一时刻相对位姿下的可见边缘模型，该可见边缘模型包括深度边缘和纹理边缘。深度边缘通过对目标模型上一时刻估计位姿下的深度缓存进行二阶差分操作可提取目标可见部分的显著边缘，纹理边缘通过对目标模型渲染结果进行简单的Canny边缘检测得到。本实施例的结构模型为CAD模型。

S102、将S101得到的两个边缘图结合，就可以得到简化了的目标可见部分的显著边缘与纹理边缘(即：可见边缘模型)。该步骤避免了复杂的人工简化模型操作，也避免了利用完整模型进行非合作目标测量的计算复杂性。该步骤得到的可见边缘模型同时适合目标模型纹理丰富与纹理匮乏的情形。

S103、将S102得到的可见边缘模型以上一时刻的相对位姿投影到当前时刻的场景图像中。该步骤通过3D渲染引擎实现。

S104、在当前场景图像中，对S103投影的可见边缘模型以合适的间距进行点采样，从而将整个连续的可见边缘模型离散化为一堆控制点的集合。对采样后的控制点沿着所在边缘直线的法向方向在预定范围内进行搜索，对法向方向的每个像素点通过一个掩膜来判定该点是否超过一定的亮度梯度阈值，从而确定该点是否为场景图像中的边缘点，每一个控制点与对应的边缘点构成一对对应特征点对中的两个特征点。该步骤避免了整个模型边缘与图像边缘的完全匹配，大大提高位姿计算的效率。

S105、根据S104的边缘模型上的控制点与法向搜索到的对应的图像中的边缘点，计算控制点到对应图像边缘点的距离R_e(即对应特征点对的残差R_e)，并计算该距离(即残差R_e)相对当前估计相对位姿的Jacobi矩阵J_e。

所述的基于模型提取图像特征点对的框架，具体包含以下步骤：

S201、从相机(也可以对采集的视频流进行类似处理)采集当前场景图像。

S202、利用KLT算法提取上一时刻场景图像的Harris特征在当前时刻场景图像中的位置优选地，当KLT算法得到的当前有效特征点数目少于一定比例时，重新搜索Harris特征，避免每一步都检测点特征引起的费时操作。

S203、通过上一时刻场景图像中Harris特征点的位置和目标模型在上一时刻相对位姿下的深度缓存，估计Harris特征点在上一时刻的3D坐标

S204、将特征点的3D坐标以当前估计的位姿投影到当前场景图像中，得到

S205、计算各特征点对与之间的残差R_p及残差R_p相对当前位姿的Jacobi矩阵J_p。

在分别得到了基于模型和基于图像的特征点对的残差R_e和R_p后，将两者结合起来，具体步骤包含：

S301、分别根据R_e和R_p对基于模型和基于图像得到的特征点对分配权重。本发明提出的权重包含特征点对在各自框架中的子权重ω与整体上两个框架的权重Factor。这样提高了整个方法对于光照、视角、目标运动等各种噪声的鲁棒性。

S302、对各自的Jacobi矩阵J_e和J_p按照S301中各特征点对的权重分别进行处理，然后用视觉伺服方法计算相对位姿。

S303、对S103～S302和S203～S302步骤进行并行迭代，不断更新估计的相对位姿，直到迭代条件终止。

所述深度边缘通过对目标模型在上一时刻估计相对位姿的深度缓存进行二阶差分操作得到，目标的纹理边缘通过Canny算法检测。对一个在给定位姿下的目标CAD模型而言，目标模型深度缓存不连续处为深度发生变化的地方，可以视为在给定相机视线方向下的边缘。这种深度边缘可以通过对目标模型在给定相对位姿的深度缓存进行二阶差分操作得到，如简单的2维Laplacian滤波器。为保证得到的深度边缘只有1个像素，还要对上述得到的深度边缘进行非极大值抑制。目标的纹理边缘通过对目标模型渲染结果进行简单的Canny算法得到。Canny算法通过高斯滤波平滑图像，然后对图像应用一阶偏导有限差分并用非极大值抑制来计算梯度的幅值和方向，通过使用双阈值来检测和连接边缘。深度边缘使得本发明可以跟踪目标完整CAD模型中可见部分的显著边缘，而不是跟踪整个完整CAD模型，并且深度边缘可以根据与目标的距离得到不同距离下的合适的显著边缘，使得本发明在相机工作范围内不同距离下都可以测量。而纹理边缘可用于对纹理丰富的目标进行辅助测量。

所述的对目标模型深度缓存的处理以及对目标模型在特定位姿下的投影都是通过3D渲染引擎完成。有许多3D渲染引擎如OGRE3D、OpenSceneGraph可以完成这个工作，其实质是基于OpenGL的图像渲染，但直接采用3D渲染引擎是因为其并行加速的优点，可以使得这部分工作能快速完成。

所述的权重分配包括两个部分：一部分是对基于图像点特征的KLT框架和基于模型边缘的MBT框架权重分配，FactorKLT和FactorMBT，这部分权重以两者各自的平均残差进行分配；另一部分是对基于KLT和基于MBT的两个框架中所用的特征点对的权重分配，这部分的权重通过一个鲁棒估计子M-estimator(又称M估计子鲁棒方法)来分配。

所述的基于模型确定图像中目标的边缘是通过对模型可见边缘上采样控制点的法向搜索确定，而不是直接对整个图像进行边缘检测然后匹配。该部分首先对投影到图像的目标可见边缘模型沿边缘按一定间隔进行采样，确定目标模型可见边缘上的控制点集，对该控制点集中的每一个点，沿着该点所在边缘的法向方向在一定范围内搜索，对前述法向方向上的每一个像素点进行一次掩膜卷积计算，该掩膜方向和该控制点所在边缘的方向平行。当法向上某一像素点与掩膜的卷积结果超过预先设定的阈值时即认为该像素点为图像中的边缘点。如此确定模型边缘上各控制点在图像中对应的边缘点。

为进一步对本发明进行更详细的说明，下文对本发明的三个主要部分更加优选和具体的算法进行举例说明：

一、基于目标模型提取边缘特征

基于目标模型的空间非合作目标测量方法其思想是通过将目标模型投影到场景图像中，当目标模型的边缘和场景图像中目标的图像边缘重合时，此时目标模型相对于相机的相对位姿即为场景中空间非合作目标相对追踪航天器的相对位姿。为了避免直接用目标模型边缘与场景图像中目标图像边缘进行匹配带来的复杂计算，本实施例将场景图像中目标图像边缘的确定与匹配问题转化为沿目标模型边缘法向的一维搜索问题，而对位姿的估计则可以转化为目标模型边缘上控制点到目标图像边缘距离的最小化问题，即对空间非合作目标相对位姿的估计可以表示为：

$\hat{r}=\arg \underset{r}{min}\mathrm{\Δ}\left(r\right)$

其中Δ(r)＝Σ_i(e_i(r))²为相对位姿为r时目标模型边缘上采样控制点的投影与目标图像边缘对应特征点间的误差之和，当误差最小时即认为目标模型的边缘与目标图像边缘重合，即得到场景图像中目标的相对位姿估计。目标模型边缘上的采样控制点通过对目标可见边缘模型上的边缘按一定间距采样得到。

在本实施例的基于模型边缘的框架中，误差定义为目标模型边缘上的采样控制点与场景图像中目标对应边缘上特征点的垂直距离，因此最小化目标函数可以化为：

$\mathrm{\Δ}=\underset{i}{\Σ}\underset{j}{\Σ}{\left(d_{\⊥}\right(l_i\left(r,x_i^{\′}\right)\left)\right)}^2$

这里的d_⊥(l_i(r，x′_i))为目标可见边缘模型中的直线l_i在相对位姿为r时与场景图像中目标对应边缘点x′_i的垂直距离。

基于模型的测量首先要获得目标的边缘(线框)模型，传统方法是通过人工对目标的CAD模型进行重新设计，而且为了保证测量过程中的实时性，只保留目标模型的主要部分。然而，人工重新设计模型非常复杂费时，而且得到的边缘模型因为复杂度的原因只能适合相机在一定距离范围内工作。本实施例则采用一种新的手段实现对目标边缘模型的提取。

首先通过深度缓存算法得到目标模型的深度缓存：通过3D渲染引擎读取给定相对位姿下空间非合作目标的完整CAD模型后，对深度缓存器和颜色缓存器进行初始化，把深度缓存器中所有单元置成一个最大可能的深度值，把颜色缓存器中各单元置成背景颜色。然后将目标完整CAD模型以给定相对位姿投影到图像平面上去。对于每个投影点(像素)，把目标模型在该点处的深度与深度缓存器中相应位置上的深度值进行比较，如果前者小于后者，那么就把目标模型的颜色写到颜色缓存器中去，同时用目标模型的深度去更新深度缓存器中相应像素的深度，否则不做任何操作。这样可以得到给定相对位姿下目标模型的深度缓存以及目标可见部分的渲染结果。

3D渲染引擎中的深度缓存通常是归一化为(0，1)范围的，需要将目标模型的深度缓存转化为实际深度：

$Z(i,j)=-\frac{Z_{near}{Z}_{far}}{z_{buf}(i,j)(Z_{far}-Z_{near})-Z_{far}}$

其中Z_near，Z_far为3D渲染引擎中近裁面和远裁面距离，z_buf(i，j)为深度缓存，Z(i，j)为深度即像素点在3d世界中距离摄象机的Z坐标。

目标模型深度缓存不连续的地方可以认为是深度(距离)上发生变化的地方，这在视觉上会形成一个明显的边缘，因此通过对目标模型的深度缓存进行一次二阶差分操作，如2D-Laplacian滤波器，

$\left[\begin{array}{ccc}0& 1& 0\\ 1& -4& 1\\ 0& 1& 0\end{array}\right]$

即可以得到目标的深度边缘。为保证得到的深度边缘只有1个像素，还要对上述得到的深度边缘进行非极大值抑制。这个深度边缘随着相机与目标的距离变化而变化，因此该步骤得到的深度边缘模型可以适应不同距离下的工作。

除此以外，目标模型通常还包含一定的纹理。对纹理丰富的目标，纹理可能会影响图像中边缘的检测，将纹理边缘也考虑进来则可以较好地解决这个问题。纹理边缘通过对目标模型的渲染结果进行简单的Canny边缘检测即可得到。Canny算法通过高斯滤波平滑图像，然后对图像应用一阶偏导有限差分并用非极大值抑制来计算梯度的幅值和方向，通过使用双阈值来检测和连接边缘。

将目标深度边缘和纹理边缘结合，即得到了目标的可见边缘模型。需要注意的是，由于3D渲染引擎的深度测试自动裁剪了场景中目标的不可见部分，因此所得到的可见边缘模型仅包含目标可见部分的显著边缘，这可以大大减少后续边缘检测与位姿计算的工作量。

得到目标的可见边缘模型并将其以估计位姿投影到场景图像中后，就可以对图像边缘进行确定与匹配，并计算可见边缘模型中边缘与场景图像中目标对应边缘点的距离d_⊥(l_i(r，x′_i))了。如前所述，通过将目标可见边缘模型中的边缘按一定间隔采样，将边缘搜索与匹配的问题转化为沿目标可见边缘模型边缘采样控制点法向的一维搜索问题。对可见边缘模型的边缘上某一控制点，沿其法向一定范围内的每一个像素点，都通过一个预设的掩膜卷积操作来判断该点是否为图像上可能的亮度梯度极值点，掩膜的方向和模型边缘的方向平行，示例的0°和45°的掩膜如下：

$\begin{array}{cc}\left[\begin{array}{ccc}100& 100& 100\\ 0& 0& 0\\ 100& 100& 100\end{array}\right]& \left[\begin{array}{ccc}0& -100& -100\\ 100& 0& -100\\ 100& 100& 0\end{array}\right]\end{array}$

确定目标可见边缘模型上的边缘控制点与图像中对应的目标边缘点后，对应点之间的距离可以通过极坐标系下的计算得到。在极坐标系下，相对位姿为r的可见边缘模型中直线l_i投影得到的二维直线l_i(r)可以表示为：

$xcos\mathrm{\θ}+ysin\mathrm{\θ}=\mathrm{\ρ},\∀(x,y)\∈l_i\left(r\right)$

其中ρ为直线l_i(r)与极坐标原点之间的距离，θ为图像与直线之间的夹角。

直线l_i(r)＝pr(L_i，r)上的控制点x_i＝(x_i，y_i)所对应的场景图像中的目标边缘点为x′_i＝(x′_i，y′_i)，则过点(x′_i，y′_i)与直线l_i(r)平行的直线可以表示为

x′_icosθ+y′_isinθ＝ρ_d

直线l_i(r)到对应边缘上的点x′_i＝(x′_i，y′_i)的垂直距离(残差)可以计算为

d_⊥(l_i(r，x′_i))＝p-ρ_d

该距离d_⊥(l_i(r，x′_i))相对于当前估计位姿r的Jacobi矩阵为

$J_e={\left[\begin{array}{c}{\mathrm{\λ}}_{d}cos\mathrm{\θ}\\ {\mathrm{\λ}}_{d}sin\mathrm{\θ}\\ -{\mathrm{\λ}}_d\mathrm{\ρ}\\ (1+{\mathrm{\ρ}}^2)\sin \mathrm{\θ}-\mathrm{\α}\mathrm{\ρ}cos\mathrm{\θ}\\ -(1+{\mathrm{\ρ}}^2)cos\mathrm{\θ}-\mathrm{\α}\mathrm{\ρ}sin\mathrm{\θ}\\ -\mathrm{\α}\end{array}\right]}^T$

其中，

α＝x′_ijsinθ-y′_ijcosθ

λ_d＝λ_ρ+αλ_θ

λ_ρ＝(A_ρcosθ+Bρsinθ+C)/D

λ_θ＝(Asinθ-Bcosθ)/D

而AX+BY+CZ+D＝0为直线L_i所在3D平面的方程。

二、基于模型提取图像特征点

由于边缘的相似性，仅依靠基于模型边缘来确定空间非合作目标的相对位姿有时候会出现较大误差，而点特征通常具有较好的区分性，因此本发明还引入图像中的特征点来进行跟踪测量。本发明直接采用KLT(Kanade-Lucas-Tomasi)算法对图像中的特征点进行跟踪，避免传统立体视觉方法先检测图像特征再匹配的复杂过程，而且不同于一般基于KLT算法估计位姿的方法，本实施例充分利用目标完整3维CAD模型来估计特征位置。

已有的基于特征点确定位姿的方法首先对两帧图像进行特征点检测，然后通过特征描述子之间的相似性进行匹配，整个过程非常复杂，尤其当特征点数量多的时候，匹配过程非常耗时，无法满足实时测量的需求。

本实施例基于KLT算法直接确定上一帧图像中的点特征在当前帧图像中的位置KLT方法的本质是最小化图像I(x)与参考图像T(x)之间的差异W(x，p)，即

$\underset{x}{\Σ}{\[I\left(W\right(x,p+\mathrm{\Δ}p\left)\right)-T\left(x\right)\]}^2$

其中W(x，p+Δp)表示对图像I(x)在做了W(x，p)的变形后继续寻找Δp以使得上式最小化。

假设上一时刻空间非合作目标的相对位姿r已经精确估计，上一时刻场景图像中的Harris特征点集合在当前时刻场景图像的Harris特征点集合通过KLT算法确定。下面需要对当前估计相对位姿下Harris特征的位置进行估计以确定特征点对之间的关系。

对一般的具有平面的目标，根据计算机视觉理论，同一平面上的特征点在两幅图之间的关系可以通过单应矩阵H来描述。而更一般的目标，则要通过极线集合与基本矩阵得到。考虑本实施例中用到了目标的完整CAD模型而且具有目标的深度缓存，本实施例直接基于模型来计算当前估计位姿下特征点的坐标。

特征集合在第k次迭代估计的位姿下的位置可以通过将特征点的3D坐标以相对位姿投影到当前场景图像中得到。其中3D坐标可以通过特征点估计位姿下的深度缓存估计得到，整个过程示意如图2所示。

获得特征集合在当前估计位姿下的位置与在当前图像中的位置后，即可以通过特征点对的关系确定相对位姿了。其中与残差与当前估计位姿之间的Jacobi矩阵为

$J_p={\left[\begin{array}{cccccc}-1/Z_i& 0& x_i/Z_i& x_i{y}_i& -(1+x_i^2)& y_i\\ 0& -1/Z_i& y_i/Z_i& (1+y_i^2)& -x_i{y}_i& -x_i\end{array}\right]}^T---\left(10\right)$

其中残差定义为特征点坐标之差R_p＝(x_i-x′_i，y_i-y′_i)，而Z_i为对应特征点的深度信息。

三、分配权重与位姿计算

本发明所提出的对空间非合作目标的测量所用到的特征包括前述一中基于可见边缘模型的特征点和前述二基于图像特征的特征点两个框架，基于可见边缘模型的框架和基于图像Harris特征的框架的权重自然不同，而每种框架下各特征点的权重也不一样，因此本发明提出了一种权重分配方案，使得本发明更具鲁棒性。

对两个框架的权重分配，本实施例采用两种框架下的平均残差来分配权重，其中平均残差定义为：

$\stackrel{\‾}{R}=\frac{{\mathrm{\Σ}}_{i=1}^N\left|R\[i\]\right|}{N}---\left(11\right)$

其中R[i]为所属框架下特征点i的残差，基于边缘的框架中残差R定义为模型边缘上采样控制点到图像中对应边缘上的特征点的垂直距离R_ei＝d_⊥(l_i(r，x′_i))，而基于图像特征点的框架中残差定义为对应特征点对的坐标之差R_pi＝(x_i-x′_i，y_i-y′_i)。N为对应框架所用到的特征点总数。

基于模型边缘的残差分配的权重为

${\mathrm{Factor}}_{MBT}=\frac{\stackrel{\‾}{R_e}}{\stackrel{\‾}{R_e}+\stackrel{\‾}{R_p}}---\left(12\right)$

而基于图像特征点的残差分配的权重为

Factor_KLT＝1-Factor_MBT(13)

从直观上理解即为，两种方法中平均残差小的其估计得到的相对位姿可靠性高，相应的权重应该大。

对于每一个框架中各特征点所占的权重，则通过在各自的方法中引入M-Estimator来确定即：根据M估计子鲁棒方法确定前述一中各对特征点对的第一残差R_e的权重ω_e、及前述二中各对特征点对的第二残差R_p的权重ω_p。作为示例，以Tukey估计子为例，Tukey估计子的定义为：

$\mathrm{\&rho;}(r_i,C)=\left\{\begin{array}{cc}\frac{r_i^6}{6}-\frac{C^2{r}_i^4}{2}+\frac{C^4{r}_i^2}{2},& \left|r_i\right|<C\\ \frac{1}{6}{C}^6,& others\end{array}\right.---\left(14\right)$

其中 $C=4.7\widehat{\mathrm{\&sigma;}},$ 而 $\widehat{\mathrm{\&sigma;}}=1.48{\mathrm{Med}}_i\left(\right|r_i-{\mathrm{Med}}_{j}j\left(r_j\right)\left|\right)$ 为有效数据的标准差，Med(Δ)为取中位数操作。Tukey估计子的影响函数为

$\mathrm{\&psi;}(r_i,C)=\left\{\begin{array}{cc}{r}_i{(r_i^2-C^2)}^2,& \left|r_i\right|<C\\ 0,& others\end{array}\right.---\left(15\right)$

即将残差超出阈值C的点认为是野值而完全剔除，野值对位姿计算完全不起作用。

如此，各特征点在对应方法中的权重分配定义为：

$\mathrm{\&omega;}\left(i\right)=\frac{\mathrm{\&psi;}\left(r_i\right)}{r_i}---\left(16\right)$

而对于整个方法中每一个特征点的残差R及残差相对于当前位估计姿的Jacobi矩阵J则需要乘以相应的权重

$R=\left[\begin{array}{c}{R}_e.*{\mathrm{\&omega;}}_e.*{\mathrm{Factor}}_{MBT}\\ R_p.*{\mathrm{\&omega;}}_p.*{\mathrm{Factor}}_{KLT}\end{array}\right]---\left(17\right)$ $J=\left[\begin{array}{c}{J}_e.*{\mathrm{\&omega;}}_e.*{\mathrm{Factor}}_{MBT}\\ J_p.*{\mathrm{\&omega;}}_p.*{\mathrm{Factor}}_{KLT}\end{array}\right]---\left(18\right)$

根据视觉伺服计算位姿的方法，相机的速度为

υ＝-λ(J^TJ)⁺J^TR(19)

其中0＜λ＜1为预设的残差衰减指数，(J^TJ)⁺为J^TJ的广义逆矩阵。

则相对位姿可以计算为

$M_o^{k+1}{}_c=M_o^k{}_c{e}^v---\left(20\right)$

其中表示当前时刻第k次迭代估计得到的相对位姿。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。

Claims (5)

1.一种基于模型的空间非合作目标相对位姿测量方法，用于在已知空间非合作目标的结构模型的情况下，通过一个单目相机实现对所述空间非合作目标的实时位姿的测量，其特征在于，包括以下步骤：

步骤1、基于所述结构模型，计算得到空间非合作目标上一时刻相对位姿下的可见边缘模型；将所述可见边缘模型投影到当前时刻的场景图像中，沿所述可见边缘模型各边缘的法向搜索所述场景图像的对应边缘以确定预定数量的对应特征点对，计算各对应特征点对的第一残差R_e及第一残差R_e相对当前估计位姿的第一Jacobi矩阵Je，并计算所有对应特征点对的第一残差平均值

步骤2、通过所述结构模型在上一时刻相对位姿下的深度缓存和所述空间非合作目标上一时刻场景图像的Harris特征点的位置，计算所述Harris特征点在当前时刻场景图像中的位置以及所述Harris特征点在上一时刻相对位姿下的3D坐标，将所述3D坐标以当前估计位姿投影到当前时刻的场景图像中并计算其与所述Harris特征点在当前时刻场景图像中的位置之间的第二残差R_p及第二残差R_p相对当前估计位姿的第二Jacobi矩阵J_p，并计算所有对应特征点对的第二残差平均值

步骤3、根据M估计子鲁棒方法确定所述步骤1中各第一残差R_e的权重ω_e、及所述步骤2中各第二残差R_p的权重ω_p；以第一残差平均值和第二残差平均值确定所述步骤1与所述步骤2之间的权重分配，其中，第一残差R_e与第一Jacobi矩阵J_e的权重第二残差R_p与第二Jacobi矩阵J_p的权重Factor_KLT＝1-Factor_MBT；

步骤4、根据视觉伺服方法迭代计算空间非合作目标当前时刻的相对位姿 $M_o^{k+1}{}_c=M_o^k{}_c{e}^v,$ 其中：

表示当前时刻第k次迭代估计得到的相对位姿；e代表自然指数；

相机的速度v＝-λ(J^TJ)+J^TR，0＜λ＜1为预设的残差衰减指数，(J^TJ)⁺为J^TJ的广义逆矩阵， $R=\left[\begin{array}{c}{R}_e.*w_e.*Facto{r}_{MBT}\\ R_p.*w_p.*{\mathrm{Factor}}_{KLT}\end{array}\right],$ $J=\left[\begin{array}{c}{J}_e.*w_e.*Facto{r}_{MBT}\\ J_p.*w_p.*{\mathrm{Factor}}_{KLT}\end{array}\right].$

2.根据权利要求1所述的方法，其特征在于：

所述步骤1中基于所述结构模型，计算得到空间非合作目标上一时刻相对位姿下的可见边缘模型包括以下步骤：

对所述结构模型在上一时刻相对位姿下深度缓存进行二阶差分操作提取空间非合作目标可见部分的深度边缘，通过3D渲染引擎对所述结构模型进行渲染，并对上一时刻相对位姿下的渲染结果进行简单的Canny边缘检测得到空间非合作目标的纹理边缘；将所述深度边缘和所述纹理边缘进行结合获得所述可见边缘模型。

3.根据权利要求1所述的方法，其特征在于：

所述步骤1中沿所述可见边缘模型法向搜索所述场景图像的边缘，包括以下步骤：

对投影到当前时刻的场景图像的所述可见边缘模型沿边缘按预定间隔进行采样以确定所述可见边缘模型上的特征点集，对所述特征点集中的每一个特征点，沿着所述特征点所在边缘的法向方向在预定范围内搜索，对搜索范围内的每一个像素点进行一次掩膜卷积计算，掩膜方向和所述特征点所在边缘的方向平行，当某一所述像素点与掩膜的卷积结果超过预先设定的阈值时即认为该像素点为当前时刻的场景图像中与所述特征点对应的边缘特征点，如此确定所述可见边缘模型上各特征点在当前时刻的场景图像中对应的边缘特征点以形成对应特征点对，即确定所述场景图像与可见边缘模型在当前时刻所对应的目标图像边缘。

4.根据权利要求1所述的方法，其特征在于：

所述步骤2包括以下步骤：

步骤210、利用KLT算法提取上一时刻场景图像的Harris特征点在当前时刻场景图像中的位置

步骤220、通过上一时刻场景图像中Harris特征点和所述结构模型在上一时刻相对位姿下的深度缓存，估计所述Harris特征点在上一时刻的3D坐标 ${\left\{X_i\right\}}_{i=1}^N;$

步骤230、将所述3D坐标以当前估计的位姿投影到当前图像中，得到 ${\left\{x_i\left(\hat{r}\right)\right\}}_{i=1}^N;$

步骤240、计算与之间的残差为所述第二残差R_p并计算第二残差R_p相对当前估计位姿的Jacobi矩阵J_p。

5.根据权利要求1所述的方法，其特征在于：所述结构模型为CAD模型。