阅读说明：本技术 一种合作目标位置与姿态的估计方法 (A kind of estimation method of cooperative target position and posture ) 是由 牟金震 方亚毜 刘宗明 李桂存 温凯瑞 于 2019-07-19 设计创作，主要内容包括：本发明公开了一种合作目标位置与姿态的估计方法,包含以下步骤：从激光雷达采集的强度图和深度图中提取合作目标标示区域的关键特征点；利用确定性退火实现合作目标标示区域的数学模型关键特征点与观测模型关键特征点的匹配；利用关键特征点匹配信息,基于奇异值分解算法解算位置与姿态解算公式中的旋转矩阵与平移向量,获得合作目标的相对位置与姿态信息。本发明采用激光雷达的测量方式,可以有效降低环境光对估计结果的影响,激光雷达采集的图像提供了每个像素的强度和相对距离信息,增强了测量的鲁棒性和实时性,同时,最终解算合作目标位置与姿态信息过程中,计算量少。(The invention discloses the estimation methods of a kind of cooperative target position and posture comprising the steps of: the key feature points in cooperative target mark region are extracted from the intensity map and depth map that laser radar acquires；The matching of the mathematical model key feature points and observation model key feature points in cooperative target mark region is realized using definitive operation；Using key feature points match information, the spin matrix and translation vector in position and attitude algorithm formula are resolved based on singular value decomposition algorithm, obtain relative position and the posture information of cooperative target.The present invention uses the measurement method of laser radar, influence of the environment light to estimated result can be effectively reduced, the image of laser radar acquisition provides the intensity and relative distance information of each pixel, enhance the robustness and real-time of measurement, simultaneously, with during posture information, calculation amount is few for the final cooperative target position that resolves.)

一种合作目标位置与姿态的估计方法

技术领域

本发明涉及空间飞行器合作目标相对位置与姿态确定技术领域，特别涉及一种基于激光雷达的合作目标位置与姿态的估计方法。

背景技术

随着人类空间活动的增多，空间操作任务数量逐年的增长，空间操作任务通常涉及到两个空间飞行器，分别称为目标飞行器与追踪飞行器。空间飞行器在轨服务等操控任务开展时，需要精确测量合作目标位置与姿态信息，合作目标是指能提供有效合作信息的空间飞行器，如事先安装人工设计的靶标。为顺利执行空间操作任务，必须精确获取目标航天器相对于追踪飞行器的空间位置和姿态和运动参数(速度和角速度)信息。因此，自主精确地获取空间合作目标航天器的相对位置与姿态信息是实现在轨服务的前提条件。

空间合作目标距离测量存在尺度变化大、空间环境变化剧烈等特点，尤其是光照变化对测量的影响非常明显，给空间合作目标距离测量带来了很大的挑战。传统测量方式采用可见光相机求得目标相对位置与姿态估计精度受限于空间环境的噪声干扰影响会导致测量误差放大，鲁棒性差。

发明内容

本发明目的在于提供一种合作目标位置与姿态的估计方法，基于雷达测量方法可以有效降低环境光对估计结果的影响，同时激光雷达采集的图像提供了每个像素的强度和相对位置信息，可以增强测量的鲁棒性和实时性。

为了达到上述目的，本发明通过以下技术方案实现：一种合作目标位置与姿态的估计方法，其特点是，包含以下步骤：

S1、从激光雷达采集的强度图和深度图中提取合作目标标示区域的关键特征点；

S2、利用确定性退火实现合作目标标示区域的数学模型关键特征点与观测模型关键特征点的匹配；

S3、利用关键特征点匹配信息，基于奇异值分解算法解算位置与姿态解算公式中的旋转矩阵与平移向量，获得合作目标的相对位置与姿态信息。

所述的合作目标标识区域是具有一定面积且反光率高的图形域，其安置在合作目标的不同位置上且不共面，数量为六个以上。

所述的步骤S1中从激光雷达采集的强度图和深度图中提取合作目标标示区域的关键特征点，包含以下步骤：

S1.1、阈值筛选强度图中的有效像素并获得像素活跃集：首先应用一个阈值去除激光雷达采集的强度图中的无效像素，保留通过阈值筛选的所有像素，再将有效像素按亮度低到高排序，删除亮度最高的像素点以及亮度最低的像素点，直到像素数量减少到允许的最大值，将最终筛选的像素当作活跃集；

S1.2、对活跃集执行二维卷积确定标识区域位置并将其对应的像素集作为观测模型集：对活跃像素集执行二维卷积，通过二维卷积后数据的正负值确定每一个标示区域在图像中的位置，将标识区域的像素集作为观测模型集；

S1.3、寻找每个标识区域的连通域并将其中心点作为标识区域观测模型的关键点：在激光雷达采集的深度图中找出观测模型集对应的深度信息集，将深度信息集二值化，寻找每个标识点的连通域，根据标示区域形态，将离心率不满足的区域删除，最后将保留下来的连通域中心点作为关键点。

所述的步骤S2中利用确定性退火实现合作目标标示区域的数学模型关键特征点与观测模型关键特征点的匹配，包括以下步骤：

S2.1、用马尔科夫随机场表示从标示区域的数学模型到观测模型的映射关系；

S2.2、利用确定性退火原理迭代解算映射关系的概率，完成合作目标的标示区域中的关键特征点与观测模型中的关键特征点的匹配。

所述的步骤S2.1中定义离散集L＝{0,1,2,...,g}、离散集R＝{0,1,2,...,n}分别作为数学模型与观测模型关键特征点标签的索引，其中，g值由所设计的数学模型标示区域的个数确定，n值由所设计的观测标识区域的个数确定，0索引表示空状态；

所述的马尔科夫随机场Y＝{Y₁,Y₂,...,Y_n}表示模型集关键特征点与观测集关键特征点的对应关系。

所述的步骤S2.2中迭代解算映射关系包含以下步骤：

数学模型集某个关键特征点与观测模型集某个关键特征点的点对点的匹配概率为：

式中，N_k为第k个观测标示区域的邻域标示区域，y_ki表示第k个观测标示区域与第i个模型标示区域对应关系；表示吉布斯分布函数：

式中T表示温度，E_ik与E_jk分别表示i、k以及j、k间的势能。

定义矩阵为匹配矩阵：

式中，矩阵的行数表示观测集关键特征点个数，矩阵的列数表示模型集关键特征点的个数，矩阵的每行元素和为1，表示模型集某一个关键特征点与观测集某一个关键特征点只有一个确定的匹配关系；

采用确定性退火原理寻找标示区域的数学模型关键特征点与观测模型关键特征点的点对点的匹配概率，随着迭代次数的增加，匹配矩阵的温度迭代值为：

式中表示k个观测标示区域与第i个模型标示区域的温度迭代值，表示k个观测标示区域与第h个模型标示区域的温度迭代值；

所述的公式为：

式中，ρ_kilj的值为 d_ij分别表示为观测集与模型集中两个关键特征点的欧式距离，β＝1/T，T为温度，a＝25；

若β⁰足够小，则：

当矩阵的元素>0.9，表明标示区域的数学模型与观测模型的关键特征点完成匹配。

所述的步骤S3中利用关键特征点匹配信息，基于奇异值分解算法解算位置与姿态解算公式中的旋转矩阵与平移向量，获得合作目标的相对位置与姿态信息，包含以下步骤：

使用最小二乘法求出使位置与姿态解算通用公式整体误差最小的T、r，解算得到：

式中

式中q，p分别表示为雷达坐标系下与标示坐标下的所有标示点坐标组成的矩阵，分别表示雷达坐标系下与标示坐标下的标示点坐标的测量误差矩阵；

对所述的B进行奇异值分解有：

式中D是B的奇异值，U和V分别是B的奇异向量，

则

minJ＝-tr(T(DV^T)^TU^T)＝-tr(DU^TTV)

式中，令M＝U^TTV，则：

m_ii＝1时，J值最小，对应于det(U)det(V)＝-1和det(U)det(V)＝1这两种情况，

故而

在v

本发明与现有技术相比，具有以下优点和有益效果：

本发明采用激光雷达测量方式可以有效的降低环境光对估计结果的影响，并基于合作目标标示区域的形态学进行合作目标的观测模型关键特征点的提取，确定性退火技术完成合作目标标识区域的数学模型关键特征点与观测模型关键特征点的点对点匹配，再通过奇异值解算获取相对位置与姿态信息，计算量少，鲁棒性较好。该成果对立体视觉相对测量系统的设计与工程应用具有重要的指导意义。

附图说明

图1为本发明提供的一种合作目标位置与姿态的估计方法的流程图；

图2为图1中提取观测模型关键特征点的流程图；

图3为图1中数学模型关键特征点与观测模型关键特征点匹配的流程图；

图4为本发明中的雷达坐标系、标示点坐标系与观测点转换关系示意图。

具体实施方式

以下结合附图，通过详细说明一个较佳的具体实施例，对本发明做进一步阐述。

如图1所示本发明提供一种合作目标位置与姿态的估计方法，包含以下步骤：

S1、从激光雷达采集的强度图和深度图中提取合作目标标示区域的关键特征点；

S2、利用确定性退火实现合作目标标示区域的数学模型关键特征点与观测模型关键特征点的匹配；

S3、利用关键特征点匹配信息，基于奇异值分解算法解算位置与姿态解算公式中的旋转矩阵与平移向量，获得合作目标的相对位置与姿态信息。

所述的合作目标标识区域为六个相同的具有一定面积且反光率高的圆形，其安置在合作目标的不同位置上且不共面。

所述的步骤S1中，将激光雷达采集的强度图与深度图保存。应用一个阈值去除强度图中无效像素，保留通过阈值筛选的所有像素作为有效像素，将有效像素按亮度低到高排序，删除亮度最高以及亮度最低的像素点，直到像素数量减少达允许值，将最终筛选的像素当作活跃像素集；对活跃像素集执行二维卷积，确定标识区域在强度图中的位置，将标识区域的像素集作为观测模型集；在深度图中找出观测模型集对应的深度信息集，将深度信息集二值化，寻找连通域，选取每个连通域的中心点作为观测模型集的关键特征点。

如图2所示，所述的步骤S1包含以下步骤：

S1.1、阈值筛选强度图中的有效像素并获得像素活跃集；

本发明利用激光雷达采集的强度图进行合作目标标示区域自检，主要借助安装在合作目标上标示位置反射特性将其与背景分开。这里我们选取的标示区域反射率较高，即其在强度图对应的强度数据矩阵中的值较大，标示区域背景为黑色吸光纸，即其在强度数据矩阵中的值较低。应用一个设计阈值去除无效像素，这里阈值设计步骤为：首先确定强度数据矩阵中的最大元素I_max，然后将I_max×0.38＝I_t作为设计阈值，设计阈值I_t可以最大限度的保留标示区域。保留通过阈值筛选的所有像素信息(I(u,v)≥I_t)。为了能够排除不必要的噪声可疑点，把像素从按亮度最低排序到高，从而删除亮度最高的像素点以及亮度最低的像素点，直到像素的数量减少到允许的最大值N_max，将最终筛选出的像素集作为活跃像素集。

S1.2、对活跃集执行二维卷积确定标识区域位置并将其对应的像素集作为观测模型集；

通过强度阈值筛选后的活跃像素集不仅包含标示区域的数据，还包含标示区域周围的背景数据，因此需要继续将标示区域从活跃集里面分离出来。标示区域的边缘是局部强度变化最显著的部分，对活跃像素集执行二维卷积可找到每一个标示区域的边缘位置。首先，将一个负的拉普拉斯-高斯滤波器应用于活跃像素集。然后在拉普拉斯-高斯滤波器的作用下，对活跃像素集二阶微分，二阶微分在活跃像素集中亮的一边是负值，在暗的一边正值，最终通过正负值确定每一个标示点的边缘位置。至此，可以确定标示区域在图像中的位置，将标识区域的像素集作为观测模型集。

S1.3、寻找每个标识区域的连通域并将其中心点作为标识区域观测模型的关键点。

在深度图中找出观测模型集对应的深度信息集，将深度信息集二值化，寻找每个标示区域的连通域。根据标示区域形态，将形态特征不满足的区域删除，选取每个连通域的中心点作为标示区域的观测模型的关键特征点。

所述的S2中，将标示区域的数学模型与观测模型用马尔科夫随机场表示，然后利用确定性退火寻找标识区域数学模型与观测模型的点对点匹配概率。由于标示区域数学模型已知，标示区域的数学模型的关键特征点也是已知，这样就可以直接进行点对点匹配，当数学模型中某一个关键特征点与观测模型中某一个关键特征点的匹配概率>0.9，表明标识区域数学模型与观测模型的关键特征点完成匹配。

如图3所示，所述的步骤S2包含以下步骤：

S2.1、用马尔科夫随机场表示从标示区域的数学模型到观测模型的映射关系；

定义离散集L＝{0,1,2,...,g}作为模型关键点标签的索引，其中，g值由所设计的模型标示点的个数确定，0索引表示空状态。

模型集中两个关键特征点的欧式距离：

d_ij＝d_ji＝||x_i-x_j||,i,j≠0

式中，i,j∈L，x_i、x_j分别表示第i个和第j个模型关键特征点。

定义离散集R＝{0,1,2,...,n}作为观测关键点的索引，其中，n值由所设计的观测标识点的个数确定，0索引表示空状态。

观测集中两个关键特征点的欧式距离：

式中，k,l∈R，分别表示第k个和第l个模型关键特征点。

马尔科夫随机场Y＝{Y₁,Y₂,...,Y_n}表示模型集关键特征点与观测集关键特征点的对应关系。假设随机变量Y_k的值由y_k,(k∈R)给出，集合y＝{y₁,y₂,…,y_n}表示为马尔科夫随机场Y的一个特定关系。因为y_k表示为模型集与第k个观测关键特征点的映射关系，所以y_k的是集合L中的一个元素。故匹配算法的目的是找出从R到L的映射Y:

y:R→L

S2.2、利用确定性退火原理迭代解算映射关系的概率，完成合作目标的标示区域中的关键特征点与观测模型中的关键特征点的匹配。

设表示模型集关键特征点与观测集关键特征点的点对点匹配概率，式中，y_ki表示第k个观测标示区域与第i个模型标示区域对应关系，N_k为第k个观测标示区域的邻域标示区域，表示吉布斯分布函数：

式中T表示温度，E_ik与E_jk分别表示i、k以及j、k间的势能。

定义矩阵为模型集关键特征点与观测集关键特征点的匹配矩阵：

式中，矩阵的行数表示观测集关键特征点个数，矩阵的列数表示模型集关键特征点的个数。矩阵的每行元素和为1，表示模型集某一个关键特征点与观测集某一个关键特征点只有一个确定的匹配关系。

定义参数β为β＝1/T，T为温度。得到：

β^(m+1)＝cβ^(m)

由确定性退火原理知，随着迭代次数的增加，温度降低。如果在任何特定的温度迭代中定义

式中表示k个观测标示区域与第i个模型标示区域的温度迭代值，ρ_kilj的值为α的值为25。则有

式中，表示k个观测标示区域与第h个模型标示区域的温度迭代值。

若β⁰足够小，则矩阵的初值已知，则：

随着迭代的进行，矩阵的所有元素值将只会趋向于0或者1这两个数值。当元素值趋向于0表明标示区域数学模型某一个关键特征点与观测模型中某一个关键特征点的匹配概率<0.9，即标示区域数学模型中的某一个关键特征点与观测模型的关键特征点不对应。当元素值趋向于1表明标示区域数学模型某一个关键特征点与观测模型中某一个关键特征点的匹配概率>0.9，即标示点数学模型某一个关键特征点与观测模型的关键特征点完成匹配。

所述的步骤S3中，标识区域的数学模型与观测模型的关键特征点完成匹配后，利用关键特征点匹配信息，基于奇异值分解算法解算平移矩阵T与平移向量r，最后获取相对位置与姿态信息。

由位置与姿态解算通用公式p_i＝Tq_i+r,i＝1,2,…,n可知，求解相对位置与姿态的关键是求解旋转矩阵T与平移向量r，式中p_i、q_i分别表示为雷达坐标系下与标示坐标下的每个标示点坐标，p_i、q_i可通过标示点的数学模型和观测模型获得，p_i、q_i的对应关系可通过算法匹配获取。如图4所示为雷达坐标系、标示点坐标系与观测点之间的关系，箭头1表示雷达坐标系平移到标示点坐标系的平移向量r，箭头2与箭头3联合表示旋转矩阵T。由图4可见，当p_i、q_i以及p_i、q_i的对应关系已知情况下可以求解转矩阵T与平移向量r。为了求出使整体误差最小的T,r，使用最小二乘法：

式中w_i为权重系数，m表示匹配个数。平移向量r可以表示为：

式中

定义

式中q，p分别表示为雷达坐标系下与标示坐标下的所有标示点坐标组成的矩阵，分别表示雷达坐标系下与标示坐标下的标示点坐标的测量误差矩阵。

由于T是正交矩阵，故T^TT＝E，而均是标量，且且p_i ^Tp_i与为常数，故最终求得：

式中对B进行奇异值分解有：

式中U和V分别是B的奇异向量，而D是B的奇异值。BB^T的特征向量组成U，特征值组成D^TD，BB^T的特征向量组成V，特征值组成D^TD。则

minJ＝-tr(T(DV^T)^TU^T)＝-tr(DU^TTV)

由于U,T,V都是正交矩阵，故M＝U^TTV也是正交矩阵，所以有：

这里需使J最小，则m_ii＝1，但是这种结果可能得到反射矩阵，当m_ii＝-1时，才能求出旋转矩阵。而det(U)det(V)＝1和det(U)det(V)＝-1恰好对应两种情况，故：

则有

本发明采用激光雷达测量方式可以有效的降低环境光对估计结果的影响，并基于合作目标标示区域的形态学进行合作目标的观测模型关键特征点的提取，确定性退火技术完成合作目标标识区域的数学模型关键特征点与观测模型关键特征点的点对点匹配，再通过奇异值解算获取相对位置与姿态信息，计算量少，鲁棒性较好。该成果对立体视觉相对测量系统的设计与工程应用具有重要的指导意义。

尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。