基于模型和点云全局匹配的空间非合作目标位姿估计方法
【专利摘要】本发明公开了一种基于模型和点云全局匹配的空间非合作目标位姿估计方法，该方法包括：利用深度相机获取目标场景点云，经过滤波后作为待配准数据点云，对目标模型点云进行三维距离变换；对初始待配准数据点云和目标模型点云进行去模糊主方向变换，确定平移域，利用全局ICP算法在平移域和旋转域中进行搜索配准，获取模型坐标系到相机坐标系的初始变换矩阵，即目标的初始位姿；将前一帧的位姿变换矩阵作用于当前帧数据点云，利用ICP算法与模型进行配准，获取当前帧的位姿；从位姿变换矩阵中解算出旋转角和平移量。本发明具有良好的抗噪声性能和实时输出目标位姿的能力，不需要计算数据点云的法线、曲率等几何特征，配准速度快，精度高。
【专利说明】
基于模型和点云全局匹配的空间非合作目标位姿估计方法
技术领域
[0001] 本发明涉及空间非合作目标位姿获取技术，特别是一种基于模型和点云全局匹配 的空间非合作目标位姿估计方法。
【背景技术】
[0002] 空间目标相对位置和姿态参数的测量是航天器间实现相对导航的重要技术之一， 通常采用的测量传感器包括单目视觉、双目视觉、雷达、GPS、惯性导航等，基于视觉的位姿 测量技术具有构造简单、性能稳定、抗干扰性好等优点，一直是研究的热点问题。在近距离 阶段，基于光学成像和图像处理的方法优于其他方法。
[0003] 空间非合作目标泛指不能够提供有效合作信息的空间航天器，它们的结构、尺寸 以及运动信息完全未知或部分未知，且不能与追踪航天器进行通信。对于大部分空间任务 涉及到的目标来说，它们都是非合作的目标，如需要移至遗弃轨道或进行抓捕的废弃卫星 或轨道碎片，等待维修或更换故障部件的在轨航天器等。
[0004] 空间非合作目标视觉位姿测量技术有着广泛的应用前景，如故障卫星的清理和维 修、空间碎片清理、及空间攻防中对敌方卫星进行监测和跟踪等，同时可应用于工业领域 中，如机器视觉工件自动抓取等，具有重要的经济价值和社会效益。
[0005] 现有的空间非合作目标位姿测量方法主要采用双目视觉的方法，例如徐文福等人 在《非合作航天器的相对位姿测量》中提出了基于立体视觉的位姿测量方法，利用Canny边 沿检测器及图像Hough变换，提取待识别对象的直线特征，并计算直线间的交点；对所提取 的左、右相机图像的点特征进行3D重构，得到各点在世界坐标系中的坐标，并据此建立目标 坐标系，求出其相对于世界坐标系的位置和姿态。由于该方法需要识别非合作目标的几何 特征，而视场内目标的成像有可能不存在有效的几何特征，因此不能保证建立出合法有效 的目标坐标系，方法可靠性差，并且该方法计算量大，不具备实时性。
【发明内容】
[0006] 本发明的目的在于提供一种基于模型和点云全局匹配的空间非合作目标位姿估 计方法。
[0007] 实现本发明目的的技术解决方案为:一种基于模型和点云全局匹配的空间非合作 目标位姿估计方法，包括下列步骤：
[0008] 步骤1，利用深度相机获取目标场景点云，经过滤波后作为待配准数据点云，对目 标模型点云进行三维距离变换；
[0009] 步骤2,对初始待配准数据点云和目标模型点云进行去模糊主方向变换，确定平移 域，利用全局ICP算法在平移域和[-^]3旋转域中进行搜索配准，获取模型坐标系到相机 坐标系的初始变换矩阵，即目标的初始位姿；
[0010] 步骤3,将前一帧的位姿变换矩阵作用于当前帧数据点云，利用ICP算法与模型进 行配准，获取当前帧的位姿；
[0011] 步骤4，从位姿变换矩阵中解算出旋转角和平移量。
[0012] 本发明与现有技术相比，其显著效果为：（1)本发明采用深度相机，而不是传统的 可见光相机，更能适应光照条件苛刻的空间环境，同时深度相机可直接获取目标的三维信 息，无扫描，帧频高；（2)本发明采用全局ICP算法可以准确可靠的获取目标的初始位姿信 息，适应噪声大、深度相机分辨率低的情况；（3)本发明利用基本ICP算法进行连续位姿估 计，将前一帧的位姿结果作为下一帧ICP算法获取位姿时的初配准结果，可以快速、连续的 获取目标的位姿信息，可靠性高，速度快。
【附图说明】
[0013] 图1是本发明基于模型和点云全局匹配的空间非合作目标位姿估计方法的流程 图。
[0014] 图2是本发明基于模型和点云全局匹配的初始位姿估计的流程图。
[0015]图3是本发明基于模型和点云全局匹配的连续位姿估计的流程图。
[0016]图4是本发明实施例中仿真数据点云和模型点云及其配准示意图，灰度较亮的为 模型，灰度较暗的为数据点云。
[0017] 图5是本发明实施例中实拍数据点云和模型点云及其配准示意图，灰度较亮的为 模型，灰度较暗的为数据点云。
[0018] 图6是本发明实施例中仿真点云位姿测量误差曲线图，图6(a)为平移量误差曲线 图，图6(b)为旋转量误差曲线图。
[0019] 图7是本发明实施例中实拍点云静态位姿测量误差曲线图，图7(a)为平移量误差 曲线图，图7(b)为旋转量误差曲线图。
[0020] 图8是本发明实施例中实拍点云动态位姿测量误差曲线图，图8(a)为平移量误差 曲线图，图8(b)为旋转量误差曲线图。
[0021]
【具体实施方式】
[0022]如图1、图2所示，本发明基于模型和点云全局匹配的空间非合作目标位姿估计方 法，包括以下步骤：
[0023]步骤1，基于深度相机的数据点云获取、滤波及模型预处理:利用深度相机获取目 标场景点云，经过滤波后作为待配准数据点云，对目标模型点云进行三维距离变换;其中滤 波采用幅值滤波和统计滤波相结合的方式：
[0024] 首先，从目标场景点云中去除幅值小于设定阈值的点；
[0025] 其次，采用统计滤波的方法进一步去除噪声，具体包括：
[0026] 对数据点云中的每个点^，计算它到它的k个近邻点的平均距离瓦;计算全局平均 距离5以及标准差Φ:
[0027]
[0028]
[0029] 其中η为数据点云的点数，1彡i彡η;
[0030] 平均距离;在标准范围d之外的点，判定为噪声点，从数据点云中去除；其中标准 范围d为：
[0031 ] d - d-l· λφ
[0032] 式中λ为标准差系数，
[0033] 步骤2,基于全局ICP配准的初始位姿估计:对初始待配准数据点云和目标模型点 云进行去模糊主方向变换，确定平移域，利用全局ICP算法在平移域和[-π,π] 3旋转域中进 行搜索配准，获取模型坐标系到相机坐标系的初始变换矩阵，即目标的初始位姿;如图2所 示，步骤2具体包括以下步骤：
[0034]步骤2-1、对初始待配准数据点云和目标模型点云进行去模糊主方向变换，确定平 移域，包括以下步骤：
[0035]步骤2-1-1、首先对目标模型点云和初始待配准数据点云利用点云的一阶矩和二 阶矩进行主方向变换：
[0036] 点云的一阶矩1_和二阶矩cov分别为：
[0037]
[0038] II
[0039]其中xi是点云中的一点；
[0040] 以均值元为原点，点云协方差矩阵cov的三个按特征值升序排列的特征向量(eV1， ev2，ev3)依次对应XYZ轴，重新建立点云的坐标系；
[0041] 对点云进行以下旋转平移变换：
[0042] Xi，=R-hi-R-吋
[0043] 其中T亏，R= (6￥1，6￥2,6￥3)1'七'为11变换后的对应点七'组成的点云为主方向点 云；
[0044] 步骤2-1-2、对主方向点云进行去模糊处理，使数据主方向点云的朝向与模型主方 向点云的朝向一致;具体为：
[0045]首先，将点云协方差矩阵的三个特征向量按特征值大小升序排列，依次为x+，y+和z +轴，其对应的相反方向为X' ylPzT轴，去模糊X轴定义为：
[0049] 对z轴进行同样的过程得到去模糊的z轴，y轴通过z Xx获得；[0050] 其次，计算去模糊主方向点云：
[0046]
[0047]
[0048]
[005? ]记初始数据点云Do和目标模型点云Mo的去模糊主方向变换分别为Pd和Pm:
[0052]
[0053]
[0054]其中Rd、Rm分别为点云Do、Mo协方差矩阵去模糊特征向量构成的旋转矩阵，Td、Tm分 别为点云DQ、MQ的均值构成的平移向量;经过Pd、Pm变换生成的去模糊主方向数据点云和去 模糊主方向模型点云分别记为Di、Mi;
[0055] 步骤2-1-3 分别求解平行于坐标轴的包围盒;记去模糊主方向数据点云包 围盒为D，去模糊主方向模型点云包围盒记为Μ，记D的中心点为0D，XYZ三轴方向的边长分别 为lx、ly和lz;记Μ的中心点为0μ，ΧΥΖ三轴方向的边长分别为l x'、ly'和lz'；将去模糊主方向 数据点云的原点移到其包围盒的中心，得到调整的去模糊主方向数据点云p t:
[0056] pt = Xi-〇D
[0057] 调整的去模糊主方向数据点云作为最终用来配准的数据点云，记为D2,同理调整 的去模糊主方向模型点云，记为M2 ;
[0058]以X轴方向为例，D2到M2的平移域为：
[0059] Xtrans={x:_( | l，x-lx|/2+XoffestXx<( | l'x-lxl/2+Xoffest)}
[0060] y轴和Z轴的平移域同理可得：
[0061] ytrans={y:-l'y-ly|/2+y〇ffestXy<( I l'y-ly|/2+y〇ffest)}
[0062] Ztrans={z：-( | 1 ' z~lz |/2+Zoffxst) ^ Z ^ ( | 1 ' z~lz |/2+Zoffest) }
[0063] 基于长方体平移域的平移不确定半径γ t为：
[0064]
[0065] 式中：l、w和h分别为长方体平移域的长、宽和高，t为平移域中任意一点，xo为平移 域中;Lj、；，Xoffest、y〇ffest和Zoffest为彳1￡移补偿，取Xoffest为lx的5%，y〇ffest为ly的5%，Zoffest为lz 的5 %;
[0066]步骤2-2、利用全局ICP算法对点云02和跑进行全局搜索配准，获取目标的初始位 姿；
[0067] 首先，计算搜索域CrXCt的配准误差上界I和下界§，分别为：
[0068]
[0069] /=1
[0070] 式中：Cr为角轴形式表示的立方体旋转域，Ct为立方体平移域，:1%为旋转矩阵，r〇 为C r中的一个向量，to为Ct中的平移量，ef是每一个点的L2误差，η为数据集点数，八和丫*分 别为旋转不确定半径和平移不确定半径：
[0071]
[0072]
[0073] 其中〇4卩〇*分别为平移子立方体的半边长和旋转子立方体的半边长，Xi为数据点 集里一点；
[0074] 记点云D#PM2的配准结果为Pdm，则最终配准结果Pf为：
[0075]
[0076]
[0077]则模型坐标系到相机坐标系的初始位姿为Pf1。
[0078] 步骤3,基于ICP算法的连续位姿估计:将前一帧的位姿变换矩阵作用于当前帧数 据点云，利用ICP算法与模型进行配准，获取当前帧的位姿，形成连续位姿估计;如图3所示， 步骤3具体为：
[0079] 将前一帧的位姿变换矩阵作为初配准结果，作用于当前帧数据点云：
[0080] D7 =Pk-iDk
[0081] 式中：Dk为当前数据点云，Ph为前一帧位姿结果，D'为初配准之后的数据点云； [0082]然后利用基本ICP算法与模型点云进行配准，基本ICP算法配准误差E ICP(a，D，M) 计算公式为：
[0083]
[0084]其中，D为数据点云，Μ为模型点云，a = (R，t)，η为数据点云点数，mj为di对应Μ中的 欧氏距离最近点，cUeD，R、t分别为旋转矩阵和平移向量；
[0085]对数据点云中的每一点，在目标模型点云中查找欧氏距离最近点，形成对应点集， 根据对应点集计算刚体变换矩阵，然后将刚体变换矩阵作用于当前数据点云，重复上述过 程，直到算法收敛，获得当前位姿矩阵Ptemp，从而获得连续的位姿Pk :
[0086] Pk = Ptemp
[0087] 步骤4,从位姿变换矩阵中解算出旋转角和平移量，具体为：
[0088] X，Y，Z三轴分别主动旋转的坐标变换矩阵为：
[0092] 3*3旋转矩阵R记如下形式：
[0089]
[0090]
[0091]
[0093]
[0094] 姿态角按：以)44勿4 v(的的顺序进行转换，目标坐标系作为静止坐标系，相机 相对目标坐标系的旋转矩阵为：
[0095]
[0096] 其中，-180彡Θ彡]Η()，-90 < 炉< 90 广 180彡 Φ 彡 180
[0097] 记平移量为：
[0098] T=[x，y，z]'
[0099] 则位姿变换矩阵pose mat为：
[0100]
[0101 ] 每个旋转矩阵可求出两组解，分别为錢肩，街)和(為肩為）:
[0102]
[0103]
[0104] 旋转角为鉍肩,⑴和(4,為為)中模长较小的一组。
[0105] 下面结合附图和具体实施例对本发明作进一步说明。
[0106] 实施例
[0107] 为了对本发明进行说明，充分展现出该方法具有连续、准确获取目标位姿的性能 和对相机低分辨率及噪声的适应性，完成位姿测量实验如下：
[0108] (1)实验初始条件及参数设置
[0109] 仿真实验采用虚拟相机对模型点云拍摄生成数据点云;虚拟相机的参数设置与实 际SR4000相机一致，即分辨率为144*176,焦距10mm，像素尺寸0.04mm。在10m处开始拍摄模 型点云。相机点云的位姿设置为:延Z轴平移10m，旋转量为绕Z轴从-180°开始以10°间隔增 加到180°，其余量为0。
[0110] 误差计算为:模拟产生相机点云的参数作为位姿真实值，仿真实验计算出的位姿 结果为测量值，计算误差为测量值减去真实值。
[0111] 实际实验分为静态实验和动态实验，静态实验是指目标模拟器不动，测量静止位 姿并与真实位姿比较，得出静态误差。动态实验是指目标模拟器连续运动，对目标进行连续 位姿估计并与真实位姿比较，得出动态位姿误差。实际实验采用SR4000相机对模拟器实拍 产生数据点云，静态实验时目标模拟器静止在6m处，动态实验时模拟器从6m位置处开始，以 〇.5m间隔接近模拟器。
[0112] (2)实验结果分析
[0113]图4为仿真点云示意图，灰度较亮的为目标模型点云，灰度较暗的为目标仿真数据 点云，图4右侧为配准结果，数据点云和目标点云配准重合，表明位姿估计结果良好；图5暗 灰色的为深度相机实拍的目标数据点云，灰度较亮的为目标模型点云，同样，配准结果良 好，获得了精确的位姿估计结果。图6(a)为仿真实验得出的平移量误差结果，图6(b)为仿真 实验得出的旋转角误差结果，从图中可以看出，平移量误差为毫米级，旋转量误差小于0.1 度，仿真实验精度很高。图7(a)为实际静态实验得出的旋转角结果曲线图，图7(b)为实际静 态实验得出的平移量结果曲线图，从图中可以看出，旋转角结果在真值附近有1°左右的误 差，而平移量很稳定，几乎没有误差。图8为动态测量误差图，其中图8(a)为旋转角误差，图8 (b)为平移量误差，从图中可以看出，旋转角误差和平移量误差均很小，小于0.1度和0.1cm， 动态测量精度高，满足实际应用要求。虽然相机分辨率较低，实拍点云比较稀疏，并且也存 在大量噪声，但本发明方法仍然能准确、鲁棒的估计出目标的位姿。
【主权项】
1. 一种基于模型和点云全局匹配的空间非合作目标位姿估计方法，其特征在于，包括 下列步骤： 步骤1，利用深度相机获取目标场景点云，经过滤波后作为待配准数据点云，对目标模 型点云进行Ξ维距离变换； 步骤2,对初始待配准数据点云和目标模型点云进行去模糊主方向变换，确定平移域， 利用全局ICP算法在平移域和[-31,31]3旋转域中进行捜索配准，获取模型坐标系到相机坐标 系的初始变换矩阵，即目标的初始位姿； 步骤3,将前一帖的位姿变换矩阵作用于当前帖数据点云，利用ICP算法与模型进行配 准，获取当前帖的位姿； 步骤4，从位姿变换矩阵中解算出旋转角和平移量。2. 根据权利要求1所述的基于模型和点云全局匹配的空间非合作目标位姿估计方法， 其特征在于，步骤1中滤波处理包括幅值滤波和统计滤波，具体为： 首先，从目标场景点云中去除幅值小于设定阔值的点； 其次，采用统计滤波的方法进一步去除噪声，具体包括： 对数据点云中的每个点XI，计算它到它的k个近邻点的平均距离7;计算全局平均距离 ?^及标准差Φ :其中η为数据点云的点数，1《i《η; 平均距离4在标准范围d之外的点，判定为噪声点，从数据点云中去除；其中标准范围d 为： ? = ? + λφ 式中λ为标准差系数，〇《λ《1。3. 根据权利要求1所述的基于模型和点云全局匹配的空间非合作目标位姿估计方法， 其特征在于，步骤2的具体过程为： 步骤2-1、对初始待配准数据点云和目标模型点云进行去模糊主方向变换，确定平移 域，包括W下步骤： 步骤2-1-1、首先对目标模型点云和初始待配准数据点云利用点云的一阶矩和二阶矩 进行主方向变换： 点云的一阶矩玄和二阶矩COV分别为：其中Xi是点云中的一点。 W均值玄为原点，点云协方差矩阵COV的Ξ个按特征值升序排列的特征向量(evi，ev2. ev3)依次对应MZ轴，重新建立点云的坐标系； 对点云进行W下旋转平移变换： xi'=R-ix 广 R-It 其中1'=玄，节=(6￥1，6￥2,6￥3)了，义1'为义1变换后的对应点，义1'组成的点云为主方向点云； 步骤2-1-2、对主方向点云进行去模糊处理，使数据主方向点云的朝向与模型主方向点 云的朝向一致;具体为： 首先，将点云协方差矩阵的Ξ个特征向量按特征值大小升序排列，依次为X%/和Z+轴， 其对应的相反方向为y^Pz^由，去模糊X轴定义为：对Z轴进行同样的过程得到去模糊的Z轴，y轴通过Z XX获得； 其次，计算去模糊主方向点云： 记初始数据点云Do和目标模型点云Mo的去模糊主方向变换分别为时和Pm:其中Rd、Rm分别为点云Do、M〇协方差矩阵去模糊特征向量构成的旋转矩阵，Td、Tm分别为 点云Do、Mo的均值构成的平移向量;经过PD、I?变换生成的去模糊主方向数据点云和去模糊 主方向模型点云分别记为化、Ml; 步骤2-1-3、对化、Ml分别求解平行于坐标轴的包围盒;记去模糊主方向数据点云包围盒 为D，去模糊主方向模型点云包围盒记为Μ，记D的中屯、点为〇D ,ΧΥΖΞ轴方向的边长分别为lx、 ^和13;记Μ的中屯、点为Ομ,ΧΥΖΞ轴方向的边长分别为lx'、k'和Iz ' ；将去模糊主方向数据点 云的原点移到其包围盒的中屯、，得到调整的去模糊主方向数据点云Pt: pit = X 广〇D 调整的去模糊主方向数据点云作为最终用来配准的数据点云，记为化，同理调整的去模 糊主方向模型点云，记为M2; Wx轴方向为例，D2到^fc的平移域为： Xtrans - {x:-(|lx-lx|/^Xoffest)《X《(|lx-lx|/^Xoffest)} 其中Xoffest是位移补偿，y轴和z轴的平移域同理可得； 基于长方体平移域的平移不确定半径丫 t为：式中：1、W和h分别为长方体平移域的长、宽和高，t为平移域中任意一点，X0为平移域中 屯、； 步骤2-2、利用全局ICP算法对点云化和M2进行全局捜索配准，获取目标的初始位姿； 首先，计算捜索域Cr X Ct的配准误差上界I和下界g，分别为：式中，Cr为角轴形式表示的立方体旋转域，Ct为立方体平移域，Rr。为旋转矩阵，ro为Cr中 的一个向量，to为Ct中的平移量，e洁每一个点的L2误差，η为数据集点数，和丫 t分别为旋 转不确定半径和平移不确定半径：其中日r和日t分别为平移子立方体的半边长和旋转子立方体的半边长，XI为数据点集里 -~*占 · 记点云化和M2的配准结果为时M，最终配准结果Pf为：其中E为单位矩阵； 则模型坐标系到相机坐标系的初始位姿为Pf^。4. 根据权利要求1所述的空间非合作目标位姿估计方法，其特征在于，步骤3的具体过 程为： 将前一帖的位姿变换矩阵作为初配准结果，作用于当前帖数据点云： 〇/ =Pk-iDk 式中:Dk为当前数据点云，Pk-i为前一帖位姿结果，〇/为初配准之后的数据点云； 然后利用基本ICP算法与模型点云进行配准;其中基本ICP算法配准误差 Eicp(a，D，M)计算公式为：D为数据点云，Μ为模型点云，曰=化〇，11为数据点云点数吨为(11对应1中的欧氏距离最 近点，diED，R、t分别为旋转矩阵和平移向量； 对数据点云中的每一点，在目标模型点云中查找欧氏距离最近点，形成对应点集，根据 对应点集计算刚体变换矩阵，然后将刚体变换矩阵作用于当前数据点云，重复上述过程，直 到算法收敛，获得配准结果Ptemp，从而获得连续的位姿Pk : Pk - Ptemp ο5. 根据权利要求1所述的基于模型和点云全局匹配的空间非合作目标位姿估计方法， 其特征在于，步骤4的具体过程为： 按坐标轴主动旋转的坐标变换矩阵为：姿态角按叫一>x(幼^ y(巧的顺序进行解算，目标坐标系作为静止坐标系，相机相对 目标坐标系的旋转矩阵为：其中，-180《目《180，-90《料《90，-180《Φ《180 记平移量为： T=[x,y,z]' 则位姿变换矩阵P〇se_mat为，每个旋转矩阵求出两组解，分别为(如0，6|)巧(知0;，马）··：旋转角为(4，巧為巧日换，巧，0,)中模长小的那一组。
【文档编号】G06T7/00GK105976353SQ201610232690
【公开日】2016年9月28日
【申请日】2016年4月14日
【发明人】赵高鹏, 杨滨华, 刘鲁江, 何莉君, 章婷婷, 薄煜明, 王建宇, 曹飞
【申请人】南京理工大学