[发明专利]一种三维点云运动补偿方法

权利要求书

1.一种三维点云运动补偿方法，其特征在于，包括如下步骤：

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

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

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

步骤四、在点(x_m(i),y_m(i),z_m(i))与点(X_i,Y_i,Z_i)的连线上任意选取一点A_i，在点(x_m(i-1),y_m(i-1),z_m(i-1))与点(X_i-1,Y_i-1,Z_i-1)的连线上任意选取一点A_i-1，计算点A_i和点A_i-1的距离值，将距离值最小的两点连线A_iA_i-1的中点的坐标作为疑似目标旋转中心坐标；

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

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

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

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

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

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

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

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

2.根据权利要求1所述的一种三维点云运动补偿方法，其特征在于，所述步骤一中，如果步骤一中i为1，则i更新为2，继续步骤一扫描下一组点云并存储第2组扫描点云的三维坐标，否则转入步骤二。

3.根据权利要求1或2所述的一种三维点云运动补偿方法，其特征在于，所述步骤七中计算第i组扫描点云中每一个点的补偿后位置的公式为：

p_j′＝p_j+rotate(c_i+Δt_j×v,Δt_j×ω)

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