CN106570905B - 一种非合作目标点云初始姿态验证方法 - Google Patents

Abstract

本发明提供了一种非合作目标点云初始姿态验证方法。三维激光扫描技术能够快速获取物体表面每个采样点的空间位置坐标，得到一个表示实体的点集合，称之为“点云”。点云中每个点的角度信息通过摆镜获得，距离通过激光飞行时间测量得到。通过距离和角度信息可以计算出每个点的三维坐标信息。目标的三维模型点云及零姿态已经存储好，获取到扫描点云后，零姿态的模型点云和扫描点云进行配准，从而求取扫描点云的初始姿态。本发明首先判断扫描点云是否完全覆盖目标，而后根据是否完全覆盖情况选择不同的策略进行配准比较，最后进一步采用菱形选点进行配准。本发明的方法运算速度快、复杂度低并且占用存储器空间小，初始姿态计算精度高。

Description

一种非合作目标点云初始姿态验证方法

技术领域

本发明涉及一种点云初始姿态验证方法，属于图像处理领域。

背景技术

三维激光扫描仪可以在空间中进行激光扫描操作，获取物体表面每个采样点的空间位置坐标，得到一个表示实体的点集合，称之为“点云”。通过点云，可以知道目标表面三维数据。

点云中每个点的距离通过激光飞行时间测距技术，即通过向目标射出一束很细的激光，由光电元件接收目标反射的激光束所需要的时间，计算出从观测者到目标的距离；扫描仪内部有摆镜，可以控制激光的出射方向，每个点的角度信息通过摆镜获得。通过距离和角度信息可以计算出每个点的三维位置坐标信息。

目标的三维模型点云(本发明中统称模型点云)通过目标设计尺寸计算出来，保存好。激光扫描目标得到所有点(本发明中统称扫描点云)后，将扫描点云和模型点云进行配准，求取扫描点云的初始姿态。

现阶段已有相关文献对点云姿态求解进行了分析。

孙军华等2013年在光学精密工程中发表的《基于分层块状全局搜索的三维点云自动配准》一文，提出了一种分层块状全局搜索到临近点局部搜索的改进迭代最近点(ICP)算法，用于进一步提高ICP算法的配准速度并消除点云缺失对点云配准的影响。该配准方法在粗略配准之后，以点云块为分层单元对模型点集进行选取，并对选取的少量模型点进行全局搜索获取其对应最近点；然后，以这些模型点对应的最近点作为搜索中心，在场景点集中进行局部搜索，获取这些模型点的大量临近点的对应最近点；最后，剔除错误对应最近点对，并求取坐标变换。陶海跻等2013年在中国激光《一种基于法向量的点云自动配准方法》中，提出了一种基于点云法向量信息的自动配准算法。根据点云局部法向量的变化提取特征点，通过比较特征点的直方图特征向量获得初始匹配点对；使用随机抽样一致性(RANSAC)算法，根据刚性距离约束条件得到精确匹配点对；利用四元素法计算得到初始配准参数。任同群等2013年在光学精密工程的《自由曲面测量的三维散乱点云无约束配准》中，提出了用于大尺寸自由曲面形貌测量中多视三维散乱点云自动配准的算法。首先，对点云微小局部区域进行标准最小二乘曲面拟合，根据拟合残差提取给定点云的全部非平坦区；借助图论中“邻接”与“可达”的概念以及非平坦区的空间分布统计特征，进行相邻点云非平坦区的区域聚类计算以及区域匹配，进而自动获得配准位姿初值。然后，计算源点云在目标点云中最靠近点的k邻近，并向k邻近点的局部移动最小二乘拟合曲面做正交投影，以提取对应点。最后，采用广义二分优化搜索进行位姿变换的优化求解。

现有方法多在实验室中，基于计算机对模拟点云数据进行处理。实际工程应用中多基于嵌入式处理器对激光实际扫描的点云进行处理，由于嵌入式处理器运算能力低，存储空间小，现有方法暴露出了运行耗时长、计算结果精度低的缺点，有时还会出现很大的姿态计算误差，难以满足实际工程应用需求。

发明内容

本发明所要解决的技术问题是：克服现有技术的不足，提供一种非合作目标点云初始姿态验证方法，该方法复杂度低、运算速度快、占用存储器空间小，并且初始姿态计算精度高。

本发明所采用的技术方案是：一种非合作目标点云初始姿态验证方法，包括步骤如下：

(1)以激光发射点为坐标原点O，建立坐标系，以水平方向为x轴，以竖直向上方向为y轴，z轴与x轴、y轴满足右手定则；

(2)激光扫描目标获得扫描点云，点云中每个点的x坐标为该点相对激光发射点的距离在x轴上的投影，y坐标为该点相对激光发射点的距离在y轴上的投影，z坐标为该点相对激光发射点的距离在z轴上的投影；求取扫描点云中各点的平均点O_s；去除离中心点O_s超过T的点，在剩余点中，求取相互距离最远的两个点A、B之间的距离；如果点A、点B之间的距离小于L，则转入步骤(3)；如果点A、点B之间的距离大于等于L，则转入步骤(10)；其中，T为设定的距离阈值，L为目标上距离最远的两点之间的距离值；

(3)扫描点云在xy轴平面投影，在xy轴投影平面内，以各点在x轴上的最小x坐标值位置为起点、最大x坐标值位置为终点，按同等间隔P_x划出m条垂直于x轴的直线；扫描点云中以各点在y轴上的最小y坐标值位置为起点、最大y坐标值位置为终点，按同等间隔P_y划出n条垂直于y轴的直线，两组直线形成m×n个交叉点；其中，m、n为正整数；

(4)扫描点云的各交叉点的z坐标取值均为步骤(2)中平均点O_s的z坐标值；根据目标设计外形获取模型点云，求取模型点云中心点，定义模型点云的零姿态，模型点云投射在步骤(1)中所建的坐标系中；将扫描点云在x、y、z轴方向平移，使得某一个交叉点与模型点云中心点对准，在模型点云零姿态下，将扫描点云与模型点云进行点云配准，计算配准误差；

(5)重复步骤(4)直至全部扫描点云交叉点均与模型点云中心点对准，计算每次对准后的配准误差，取配准误差中最小值对应的交叉点W；

(6)以步骤(5)中配准误差最小值对应的交叉点W为中心，沿x轴正负方向分别移动P_x/2距离获得点C₁、点C₂，沿y轴正负方向分别移动P_y/2距离获得点C₃、点C₄；点W、点C₁、点C₂、点C₃、点C₄的z坐标取值均为步骤(2)中平均点O_s的z坐标值；

(7)将扫描点云在坐标系内平移，使得点W、点C₁、点C₂、点C₃、点C₄其中某一点与模型点云中心点对准，根据设定的调整角度调整扫描点云的俯仰角、偏航角或滚动角，每次调整俯仰角、偏航角、滚动角后的扫描点云均与零姿态下的模型点云进行点云配准并计算配准误差，记录最小配准误差及对应的姿态；

(8)重复步骤(7)直至获得点W、点C₁、点C₂、点C₃、点C₄各自的最小配准误差及扫描点云对应的姿态；对5个点的最小配准误差进行再次比对，求取最小的配准误差；

(9)将步骤(8)中求取的最小配准误差对应的扫描点云的俯仰角、偏航角、滚动角的角度值作为目标的最终初始姿态，方法结束；

(10)求取点A、点B的连线与x轴夹角α，将扫描点云以平均点O_s为中心，绕z轴方向旋转-α角；

(11)旋转后的扫描点云在xy轴平面投影，在xy轴投影平面内，以旋转后扫描点云中各点在x轴上的最小x坐标值位置为起点、最大x坐标值位置为终点，按同等间隔P_x划出3条垂直于x轴的直线；以扫描点云中各点在y轴上的最小y坐标值位置为起点、最大y坐标值位置为终点，按同等间隔P_y划出n条垂直于y轴的直线，两组直线形成3n个交叉点；进入步骤(4)。

所述步骤(7)中扫描点云的俯仰角、偏航角或滚动角根据设定的调整角度α、β、γ进行调整，其中，俯仰角的调整角度分别为0°或±α，偏航角的调整角度分别为0°或±β，滚动角的调整角度分别为0°或±γ。

本发明与现有技术相比的优点在于：

(1)本发明针对非合作目标点云信息不完整的情况，采用先密集网格对扫描点云在xy轴平面的投影进行分割，求取交叉点，而后获取交叉点的xyz轴坐标，平移扫描点云，使每个交叉点与模型点云中心点对准后进行配准，求取配准误差最小时对应的交叉点W，而后在W周围进行菱形选点，平移扫描点云使这些点与模型点云中心点对准后调整扫描点云姿态再进行配准，求取最小配准误差对应的姿态，整个过程提高了初始姿态计算精度，避免了配准中落入局部最优点的情况,同时计算速度非常快。

(2)本发明针对非合作目标点云信息完整的情况，根据非合作目标最长两个点连线与x轴夹角，对非合作目标点云进行了姿态变换，而后再进行姿态计算。加快了配准正确性的计算速度。

(3)本发明针对非合作目标点云信息完整的情况，采用稀疏网格对扫描点云在xy轴平面的投影进行分割，求取交叉点，进而配准，计算速度快，姿态计算精度高，计算时间大大缩短，内存占用量少。

附图说明

图1为流程示意图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细的说明：

如图1所示，一种非合作目标点云初始姿态验证方法，包括如下步骤：

(1)以激光发射点为坐标原点O，建立坐标系，以水平方向为x轴，以竖直向上方向为y轴，z轴与x轴、y轴满足右手定则；

(2)激光扫描目标获得扫描点云，点云中每个点的x坐标为该点相对激光发射点的距离在x轴上的投影，y坐标为该点相对激光发射点的距离在y轴上的投影，z坐标为该点相对激光发射点的距离在z轴上的投影；求取扫描点云中各点的平均点O_s；去除离中心点O_s超过T的点，在剩余点中，求取相互距离最远的两个点A、B之间的距离；如果点A、点B之间的距离小于L，则转入步骤(3)；如果点A、点B之间的距离大于等于L，则转入步骤(10)；其中，T为设定的距离阈值，L为目标上距离最远的两点之间的距离值；

(3)扫描点云在xy轴平面投影，在投影平面内，以各点在x轴上的最小x坐标值位置为起点、最大x坐标值位置为终点，按同等间隔P_x划出m条垂直于x轴的直线；扫描点云中以各点在y轴上的最小y坐标值位置为起点、最大y坐标值位置为终点，按同等间隔P_y划出n条垂直于y轴的直线，两组直线形成m×n个交叉点；其中，m、n为正整数；

(4)扫描点云的各交叉点的x、y坐标可以计算出，z坐标取值均为步骤(2)中平均点O_s的z坐标值；根据目标设计外形获取模型点云，求取模型点云中心点，定义模型点云的零姿态，模型点云投射在(1)中所建的坐标系中；将扫描点云在x、y、z轴方向平移，使一个交叉点与模型点云中心点对准，在模型点云零姿态下，将扫描点云与模型点云进行点云配准，计算配准误差；

(5)重复步骤(4)直至全部扫描点云交叉点均与模型点云中心点对应，计算每次对应后的配准误差，取配准误差中最小值对应的交叉点W；

(6)以步骤(5)中配准误差最小值对应的交叉点W为中心，沿x轴正负方向分别移动P_x/2距离获得点C₁、点C₂，沿y轴正负方向分别移动P_y/2距离获得点C₃、点C₄；点W、点C₁、点C₂、点C₃、点C₄的x、y坐标可以计算出，z坐标取值均为步骤(2)中平均点O_s的z坐标值；

(7)将扫描点云在坐标系内分别沿x、y、z轴方向平移，使得点W、点C₁、点C₂、点C₃、点C₄其中一点与模型点云中心点对准，根据设定的调整角度调整扫描点云的俯仰角、偏航角或滚动角，每次调整俯仰角、偏航角、滚动角后的扫描点云均与零姿态下的模型点云进行点云配准并计算配准误差，记录最小配准误差及对应的姿态；

(8)重复步骤(7)直至获得点W、点C₁、点C₂、点C₃、点C₄各自的最小配准误差及扫描点云对应的姿态；对5个点的最小配准误差进行再次比对，求取最小的配准误差；

(9)将步骤(8)中求取的最小配准误差对应的扫描点云的俯仰角、偏航角、滚动角的角度值作为目标的最终初始姿态，方法结束；

(10)求取点A、点B的连线与x轴夹角α，将扫描点云以平均点O_s为中心，绕z轴方向旋转-α角；

(11)旋转后的扫描点云在xy轴平面投影，在投影平面内，以旋转后扫描点云中各点在x轴上的最小x坐标值位置为起点、最大x坐标值位置为终点，按同等间隔P_x划出3条垂直于x轴的直线；以扫描点云中各点在y轴上的最小y坐标值位置为起点、最大y坐标值位置为终点，按同等间隔P_y划出n条垂直于y轴的直线，两组直线形成3n个交叉点；进入步骤(4)。

实施例1:

(1)以激光发射点为坐标原点O，建立坐标系，以水平方向为x轴，以竖直向上方向为y轴，z轴与x轴、y轴满足右手定则，其中，点云中每个点的x坐标为该点相对激光发射点的距离在x轴上的投影，y坐标为该点相对激光发射点的距离在y轴上的投影，z坐标为该点相对激光发射点的距离在z轴上的投影；

(2)通过卫星模型获取模型点云，模型点云中心点O_m，离O_m最远的点距离7米，模型点云中最远的两个点距离12米；扫描点云中心点O_s，未发现离中心点O_s超过7米的点，说明未扫描到其他空间飞行器。由于扫描角度原因，部分帆板未扫描到，因此在剩余点中，相互距离最远的两个点A、B之间的距离为9米，点A、点B之间的距离小于12米，则转入步骤(3)；

(3)扫描点云在xy轴平面投影，每个点的xy坐标值不变。在投影平面内，以各点在x轴上的最小x坐标值位置-2米为起点、最大x坐标值位置6米为终点，按同等间隔2米划出5条垂直于x轴的直线；扫描点云中以各点在y轴上的最小y坐标值位置-4米为起点、最大y坐标值位置+4米为终点，按同等间隔2米划出5条垂直于y轴的直线，两组直线形成5×5＝25个交叉点；

(4)扫描点云的各交叉点xy坐标可以计算出，各交叉点z坐标取值为步骤(2)中平均点O_s的z坐标值30米；根据目标设计外形获取模型点云，求取模型点云中心点，定义模型点云的零姿态为喷管正对z轴负方向，模型点云投射在(1)中所建的坐标系中；将扫描点云在x、y、z轴方向平移，使得一个交叉点与模型点云中心点对准，在模型点云零姿态下，将扫描点云与模型点云进行点云配准，即扫描点云中每个点寻找与模型点云中最近点，而后按照最近点矢量方向移动扫描点云，直至最近点收敛到一定程度，计算配准误差；

(5)重复步骤(4)直至全部扫描点云交叉点均与模型点云中心点对应，计算出25个配准误差。取25个配准误差中最小值对应的交叉点W；

(6)以步骤(5)中配准误差最小值对应的交叉点W为中心，沿x轴正负方向分别移动1米距离获得点C₁、点C₂，沿y轴正负方向分别移动1米距离获得点C₃、点C₄；点W、点C₁、点C₂、点C₃、点C₄的xy坐标可以计算出，z坐标取值为步骤(2)中平均点O的z值30米；

(7)扫描点云中点W、点C₁、点C₂、点C₃、点C₄共5个点，在x、y、z轴方向平移扫描点云，使得其中一点，与模型点云中心点对应，扫描点云俯仰角、偏航角及滚动角除原始的0度、0度、0度三种组合外，分别旋转±20°、±20°、±45°，这几种角度组合成不同姿态下的扫描点云，分别和模型点云进行配准。遍历完各种姿态的扫描点云,分别与模型点云完成配准后，求取最小配准误差及对应的姿态。

(8)重复步骤(7)直至获得点W、点C₁、点C₂、点C₃、点C₄的最小配准误差及扫描点云对应的姿态；对5个点的最小配准误差进行再次比对，求取最小的配准误差；

(9)将步骤(8)中求取的最小配准误差对应的扫描点云的俯仰角、偏航角、滚动角的角度值作为目标的最终初始姿态，方法结束；

实施例2:

(1)以激光发射点为坐标原点O，建立坐标系，以水平方向为x轴，以竖直向上方向为y轴，z轴与x轴、y轴满足右手定则其中，点云中每个点的x坐标为该点相对激光发射点的距离在x轴上的投影，y坐标为该点相对激光发射点的距离在y轴上的投影，z坐标为该点相对激光发射点的距离在z轴上的投影；

(2)通过卫星模型获取模型点云，模型点云中心点O_m，离O_m最远的点距离7米，模型点云中最远的两个点距离12米；在扫描点云中，未发现离中心点O_s超过7米的点，说明未扫描到其他空间飞行器。在剩余点中，相互距离最远的两个点A、B之间的距离为12米，则转入步骤(10)；

(10)求取点A、点B的连线与x轴夹角为20度，将扫描点云以平均点O_s为中心，绕z轴方向旋转-20度角；

(11)旋转后的扫描点云在xy轴平面投影。在投影平面内，以各点在x轴上的最小x坐标值位置-4米为起点、最大x坐标值位置+8米为终点，按同等间隔6米划出3条垂直于x轴的直线；以扫描点云中各点在y轴上的最小y坐标值位置-4米为起点、最大y坐标值位置+4米为终点，按同等间隔2米划出5条垂直于y轴的直线，两组直线形成15个交叉点；进入步骤(4)。

(4)根据目标设计外形获取模型点云，求取模型点云中心点，定义好模型点云的零姿态(即待配准的模型点云姿态)。扫描点云的各交叉点xy坐标可以计算出，各交叉点z坐标取值为步骤(2)中平均点O的坐标z值30米。在x、y、z轴方向平移扫描点云，使得扫描点云一交叉点与目标的三维模型点云中心点对准，在模型点云零姿态下，将扫描点云与模型点云进行点云配准(即扫描点云中每个点寻找与模型点云中最近点，而后按照最近点矢量方向移动扫描点云，直至最近点收敛到一定程度)，计算配准误差；

(5)重复步骤(4)直至全部扫描点云交叉点均与模型点云中心点对应，计算出15个配准误差。取15个配准误差中最小值对应的交叉点W；

(6)以步骤(5)中配准误差最小值对应的交叉点W为中心，沿x轴正负方向分别移动3米距离获得点C₁、点C₂，沿y轴正负方向分别移动1米距离获得点C₃、点C₄；点W、点C₁、点C₂、点C₃、点C₄的xy坐标可以计算出，z坐标取值为步骤(2)中平均点O的z坐标值30米；

(7)扫描点云中点W、点C₁、点C₂、点C₃、点C₄共5个点，在x、y、z轴方向平移扫描点云，使得其中一点与模型点云中心点对应，扫描点云俯仰角、偏航角及滚动角除原始的0度、0度、0度三种组合外，分别旋转±20°、±20°、±45°，这几种角度组合成不同姿态下的扫描点云，分别和模型点云进行配准。遍历完各种姿态的扫描点云，分别与模型点云完成配准后，求取最小配准误差及对应的姿态。

(8)重复步骤(7)直至获得点W、点C₁、点C₂、点C₃、点C₄的最小配准误差及扫描点云对应的姿态；对5个点的最小配准误差进行再次比对，求取最小的配准误差；

(9)将步骤(8)中求取的最小配准误差对应的扫描点云的俯仰角、偏航角、滚动角的角度值作为目标的最终初始姿态，方法结束；

本发明未详细描述内容为本领域技术人员公知技术。

Claims (2)

1.一种非合作目标点云初始姿态验证方法，其特征在于，包括步骤如下：

(1)以激光发射点为坐标原点O，建立坐标系，以水平方向为x轴，以竖直向上方向为y轴，z轴与x轴、y轴满足右手定则；

(2)激光扫描目标获得扫描点云，点云中每个点的x坐标为该点相对激光发射点的距离在x轴上的投影，y坐标为该点相对激光发射点的距离在y轴上的投影，z坐标为该点相对激光发射点的距离在z轴上的投影；求取扫描点云中各点的平均点O_s；去除离中心点O_s超过T的点，在剩余点中，求取相互距离最远的两个点A、B之间的距离；如果点A、点B之间的距离小于L，则转入步骤(3)；如果点A、点B之间的距离大于等于L，则转入步骤(10)；其中，T为设定的距离阈值，L为目标上距离最远的两点之间的距离值；

(3)扫描点云在xy轴平面投影，在xy轴投影平面内，以各点在x轴上的最小x坐标值位置为起点、最大x坐标值位置为终点，按同等间隔P_x划出m条垂直于x轴的直线；扫描点云中以各点在y轴上的最小y坐标值位置为起点、最大y坐标值位置为终点，按同等间隔P_y划出n条垂直于y轴的直线，两组直线形成m×n个交叉点；其中，m、n为正整数；

(4)扫描点云的各交叉点的z坐标取值均为步骤(2)中平均点O_s的z坐标值；根据目标设计外形获取模型点云，求取模型点云中心点，定义模型点云的零姿态，模型点云投射在步骤(1)中所建的坐标系中；将扫描点云在x、y、z轴方向平移，使得某一个交叉点与模型点云中心点对准，在模型点云零姿态下，将扫描点云与模型点云进行点云配准，计算配准误差；

(5)重复步骤(4)直至全部扫描点云交叉点均与模型点云中心点对准，计算每次对准后的配准误差，取配准误差中最小值对应的交叉点W；

(6)以步骤(5)中配准误差最小值对应的交叉点W为中心，沿x轴正负方向分别移动P_x/2距离获得点C₁、点C₂，沿y轴正负方向分别移动P_y/2距离获得点C₃、点C₄；点W、点C₁、点C₂、点C₃、点C₄的z坐标取值均为步骤(2)中平均点O_s的z坐标值；

(7)将扫描点云在坐标系内平移，使得点W、点C₁、点C₂、点C₃、点C₄其中某一点与模型点云中心点对准，根据设定的调整角度调整扫描点云的俯仰角、偏航角或滚动角，每次调整俯仰角、偏航角、滚动角后的扫描点云均与零姿态下的模型点云进行点云配准并计算配准误差，记录最小配准误差及对应的姿态；

(8)重复步骤(7)直至获得点W、点C₁、点C₂、点C₃、点C₄各自的最小配准误差及扫描点云对应的姿态；对5个点的最小配准误差进行再次比对，求取最小的配准误差；

(9)将步骤(8)中求取的最小配准误差对应的扫描点云的俯仰角、偏航角、滚动角的角度值作为目标的最终初始姿态，方法结束；

(10)求取点A、点B的连线与x轴夹角α，将扫描点云以平均点O_s为中心，绕z轴方向旋转-α角；

(11)旋转后的扫描点云在xy轴平面投影，在xy轴投影平面内，以旋转后扫描点云中各点在x轴上的最小x坐标值位置为起点、最大x坐标值位置为终点，按同等间隔P_x划出3条垂直于x轴的直线；以扫描点云中各点在y轴上的最小y坐标值位置为起点、最大y坐标值位置为终点，按同等间隔P_y划出n条垂直于y轴的直线，两组直线形成3n个交叉点；进入步骤(4)。

2.根据权利要求1中所述的一种非合作目标点云初始姿态验证方法，其特征在于：所述步骤(7)中扫描点云的俯仰角、偏航角或滚动角根据设定的调整角度c、β、γ进行调整，其中，俯仰角的调整角度分别为0°或±c，偏航角的调整角度分别为0°或±β，滚动角的调整角度分别为0°或±γ。