一种三维点云运动补偿方法与流程

本发明涉及一种三维点云运动补偿方法。

背景技术

激光点云已经广泛应用于目标的位置姿态计算。在激光器对非合作目标扫描过程中，由于点云采集之间有时间间隔，如果非合作目标与激光器相对运动速度或者旋转角速度过快，会造成扫描点云与目标实际外形点的位置偏移。如果利用这种情况下的点云进行位置姿态计算，会产生误差。因此，针对目标运动过快或者自身姿态旋转过快情况，需要对扫描点云进行点云运动补偿处理，消除点云位置误差及相应计算出来的位置姿态误差。

王建军等2011在仪器仪表学报的《姿态角扰动对机载激光雷达点云数据的影响》中，实际飞行过程中由于受到各种内外界因素的干扰，如阵风、湍流、发动机振动及控制系统的性能缺陷等，机载平台无法保持理想的匀速直线运动状态，产生姿态角扰动，造成点云的扫描区域和密度分布发生变化。点云扫描区域的变化，对于狭长地带的测量，如公路、海岸线和电力线等，会导致目标区域漏扫；点云密度的变化会影响重构的精度。

为了进一步提高摄像机追踪的鲁棒性和精度，减小摄像机姿态估计的累积误差。浙江大学李阳2015年在毕业论文《基于深度摄像机的三维场景表面重建关键技术研究》中提出了一种基于摄像机姿态运动补偿。李阳通过计算当前帧对应的摄像机速度、加速度以及角速度等运动参数来建立对应的摄像机运动模型，根据运动模型和运动参数预测当前帧所对应的概率最大的摄像机姿态并以此作为icp算法迭代的初始值，从而避免因两帧之间较大距离的运动而造成的摄像机追踪失败，提高深度摄像机在快速运动过程中的追踪鲁棒性。

梁新合等2011年在西安交通大学学报中的《散乱点云的补偿滤波》中，提出了一种散乱点云补偿滤波的方法，采用双边滤波器计算获得滤波移动量，进行平滑处理后得到了形状补偿量，将双边滤波后的点向原始位置方向补偿，从而减少了滤波中的变形，获得了更多具有细节特征的点云模型。

技术实现要素：

本发明所要解决的技术问题是：克服现有技术的不足，提供了一种三维点云运动补偿方法，对激光扫描目标的点云中的每个点，进行高精度运动补偿，消除扫描时延引发的点云中各点的位置偏差，从而获得了精确的目标点云，用于激光点云对目标扫描后的姿态计算，能够显著提高姿态计算精度。

本发明的技术解决方案是：一种三维点云运动补偿方法，包括如下步骤：

步骤一、利用激光扫描目标，相机接收激光反射回光，存储扫描获得的第i组目标点云中各点在相机坐标系中的三维坐标值；i为正整数；

如果步骤一中i为1，则i更新为2，继续步骤一扫描下一组点云并存储第2组扫描点云的三维坐标，否则转入步骤二；

步骤二、求取第i组点云在相机坐标系下所有点的坐标值的平均值xm(i),ym(i),zm(i)；求取第i-1组点云在相机坐标系下所有点的坐标值的平均值xm(i-1),ym(i-1),zm(i-1)；

步骤三、将第i组点云和第i-1组点云分别与预先存储的目标外形点云进行匹配，获得两组扫描点云在目标外形点云中的对应位置姿态，并计算出第i次扫描时目标形状中心的位置点(xi,yi,zi)和第i-1次扫描时目标形状中心的位置点(xi-1,yi-1,zi-1)；

步骤四、在点(xm(i),ym(i),zm(i))与点(xi,yi,zi)的连线上任意选取一点ai，在点(xm(i-1),ym(i-1),zm(i-1))与点(xi-1,yi-1,zi-1)的连线上任意选取一点ai-1，计算点ai和点ai-1的距离值，将距离值最小的两点连线aiai-1的中点的坐标作为疑似目标旋转中心坐标；

步骤五、以步骤四中计算的疑似目标旋转中心为原点，在x，y，z轴分别以设定的步长在设定的范围内遍历获得遍历位置点，分别以这些遍历位置点为第i组点云旋转中心ci；

步骤六、选取步骤五中的一个点云旋转中心ci，根据该点云旋转中心ci位置和第i-1组点云旋转中心ci-1位置及第i次扫描和第i-1次扫描的时间间隔，求取出目标旋转中心位移及位移速度v；

根据步骤三中第i组点云和第i-1组点云匹配结果，获得两次扫描目标点云的相对角度偏差，根据角度偏差计算出目标旋转角速度ω；

步骤七、对第i组扫描点云每一个点进行补偿，计算出第i组扫描点云中每一个点的实际位置；

步骤八、计算第i组扫描点云以步骤六中选取的旋转中心ci按步骤七进行运动补偿后，补偿后扫描点云与目标外形点云匹配误差；转入步骤六，选取下一个旋转中心ci，直到步骤五中所有旋转中心ci计算完毕，进入步骤九；

步骤九、选取步骤八中匹配误差最小的一组补偿后扫描点云，作为第i组最终补偿后的点云，将第i组最终补偿后的点云的旋转中心作为第i组点云最终旋转中心；

步骤十、将步骤九中第i组最终补偿后点云作为第i组点云、第i组最终点云旋转中心作为第i组点云旋转中心，i更新为i+1，返回步骤一，对新扫描的点云进行处理。

步骤十一、利用获得的最终补偿点云计算目标姿态。

所述步骤七中计算第i组扫描点云中每一个点的实际位置的公式为：

pj′＝pj+rotate(ci+δtj×v,δtj×ω)

其中，pj′为第i组扫描点云中第j个点补偿后位置，pj为第i组扫描点云中第j个点初始位置；rotate(cj+δt×v,δt×ω)为第j个点以ci+δtj×v为旋转中心旋转δtj×ω角度后的相对位移；δtj为第j个点与第一个点的时间差。

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

(1)本发明的方法中两次扫描点云中心点分别与两次扫描时目标形状中心点连线。而后根据两根线距离最近点均值取疑似目标旋转中心，该方法能够保证高精度获得点云旋转中心，这是后期运动补偿的基础，将极大提高运动补偿精度；

(2)本发明的方法以每个候选旋转中心为基础，分别计算点云中每个点的相对位置误差，对点云中每一个点进行补偿。遍历其它候选旋转中心，依靠最小匹配精度来确定最终的旋转中心及运动补偿值，点云的补偿精度高，使得补偿后的点云在进行姿态计算时，精度大大提高。

附图说明

图1为本发明方法的流程图。

具体实施方式

如图1所示，为本发明方法的流程图，一种三维点云运动补偿方法，包括如下步骤：

步骤一、利用激光扫描目标，相机接收激光反射回光，存储扫描获得的第i组目标点云中各点在相机坐标系中的三维坐标值；

如果步骤一中i为1，则i更新为2，继续步骤一扫描下一组点云并存储第2组扫描点云的三维坐标，否则转入步骤二；

步骤二、求取第i组点云在相机坐标系下所有点的坐标值的平均值xm(i),ym(i),zm(i)；求取第i-1组点云在相机坐标系下所有点的坐标值的平均值xm(i-1),ym(i-1),zm(i-1)；

步骤三、将第i组点云和第i-1组点云分别与预先存储的目标外形点云进行匹配，获得两组扫描点云在目标外形点云中的对应位置姿态，并计算出第i次扫描时目标形状中心的位置点(xi,yi,zi)和第i-1次扫描时目标形状中心的位置点(xi-1,yi-1,zi-1)；

步骤四、在点(xm(i),ym(i),zm(i))与点(xi,yi,zi)的连线上任意选取一点ai，在点(xm(i-1),ym(i-1),zm(i-1))与点(xi-1,yi-1,zi-1)的连线上任意选取一点ai-1，计算点ai和点ai-1的距离值，将距离值最小的两点连线aiai-1的中点的坐标作为疑似目标旋转中心坐标；

步骤五、以步骤四中计算的疑似目标旋转中心为原点，在x，y，z轴分别以设定的步长在设定的范围内遍历获得遍历位置点，分别以这些遍历位置点为第i组点云旋转中心ci；

步骤六、选取步骤五中的一个点云旋转中心ci，根据该点云旋转中心ci位置和第i-1组点云旋转中心ci-1位置及第i次扫描和第i-1次扫描的时间间隔，求取出目标旋转中心位移及位移速度v；

根据步骤三中第i组点云和第i-1组点云匹配结果，获得两次扫描目标点云的相对角度偏差，根据角度偏差计算出目标旋转角速度ω；

步骤七、对第i组扫描点云每一个点进行补偿，计算出第i组扫描点云中每一个点的实际位置：

p′j＝pj+rotate(ci+δtj×v,δtj×ω)

其中，p′j为第i组扫描点云中第j个点补偿后位置，pj为第i组扫描点云中第j个点初始位置；rotate(cj+δt×v,δt×ω)为第j个点以ci+δtj×v为旋转中心旋转δtj×ω角度后的相对位移；δtj为第j个点与第一个点的时间差；

步骤八、计算第i组扫描点云以步骤六中选取的旋转中心ci按步骤七进行运动补偿后，补偿后扫描点云与目标外形点云匹配误差；转入步骤六，选取下一个旋转中心ci，直到步骤五中所有旋转中心ci计算完毕，进入步骤九；

步骤九、选取步骤八中匹配误差最小的一组补偿后扫描点云，作为第i组最终补偿后的点云，将第i组最终补偿后的点云的旋转中心作为第i组点云最终旋转中心；

步骤十、将步骤九中第i组最终补偿后点云作为第i组点云、第i组最终点云旋转中心作为第i组点云旋转中心，i更新为i+1，返回步骤一，对新扫描的点云进行处理。

实施例：

一种三维点云运动补偿方法，以下对各步骤进行详细的说明：

步骤一、利用激光扫描目标，相机接收激光反射回光，扫描获得的第1组目标点云,存储各点在相机坐标系中的三维坐标值；

发现扫描的点云为第1组，则继续步骤一扫描第2组点云并存储三维坐标；而后发现点云扫描次数i为2，大于1,转步骤二；

步骤二、求取第2组点云在相机坐标系下所有点的坐标值的平均值(10,10,100)；求取第1组点云在相机坐标系下所有点的坐标值的平均值(16,16,106)；

步骤三、将第2组点云和第1组点云分别与预先存储的目标外形点云进行匹配，获得两组扫描点云在目标外形点云中的对应位置姿态，并计算出第2次扫描时目标形状中心的位置点(12,13,104)和第1次扫描时目标形状中心的位置点(11,12,103)；

步骤四、在点(10,10,100)与点(12,13,104)的连线上任意选取一点a2，在点(16,16,106)与点(11,12,103)的连线上任意选取一点a1，计算点a2和点a1的距离值，将距离值最小的两点连线a2a1的中点的坐标作为疑似目标旋转中心坐标；

步骤五、以步骤四中计算的疑似目标旋转中心为原点，在x，y，z轴分别以设定的步长0.001米在设定的范围0.2米内遍历获得遍历位置点，分别以这些遍历位置点为第2组点云旋转中心c2；

步骤六、选取步骤五中的一个点云旋转中心c2，根据该点云旋转中心c2位置和第1组点云旋转中心c1位置及第2次扫描和第1次扫描的时间间隔，求取出目标旋转中心位移及位移速度v；

根据步骤三中第2组点云和第1组点云匹配结果，获得两次扫描目标点云的相对角度偏差，根据角度偏差计算出目标旋转角速度ω；

步骤七、对第2组扫描点云每一个点进行补偿，计算出第2组扫描点云中每一个点的实际位置：

p′j＝pj+rotate(ci+δtj×v,δtj×ω)

其中，p′j为第2组扫描点云中第j个点补偿后位置，pj为第2组扫描点云中第j个点初始位置；rotate(cj+δt×v,δt×ω)为第j个点以ci+δtj×v为旋转中心旋转δtj×ω角度后的相对位移；δtj为第j个点与第一个点的时间差；

步骤八、计算第2组扫描点云以步骤六中选取的旋转中心c2按步骤七进行运动补偿后，补偿后扫描点云与目标外形点云匹配误差；转入步骤六，选取下一个旋转中心c2，直到步骤五中所有旋转中心c2计算完毕；

步骤九、选取步骤八中匹配误差最小的一组补偿后扫描点云，作为第2组最终补偿后的点云，将其旋转中心作为第2组点云最终旋转中心；

步骤十、将步骤九中第2组最终补偿后点云作为第2组点云、第2组最终点云旋转中心作为第2组点云旋转中心，下次扫描点云次数i更新为3，返回步骤一，对新扫描的点云进行处理。

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。