CN108876862B

CN108876862B - 一种非合作目标点云位置姿态计算方法

Info

Publication number: CN108876862B
Application number: CN201810769921.2A
Authority: CN
Inventors: 王立; 刘达; 顾营迎; 李涛; 张春明; 吴云
Original assignee: Beijing Institute of Control Engineering
Current assignee: Beijing Institute of Control Engineering
Priority date: 2018-07-13
Filing date: 2018-07-13
Publication date: 2021-12-07
Anticipated expiration: 2038-07-13
Also published as: CN108876862A

Abstract

一种非合作目标点云位置姿态计算方法，本方法从不同初始参数，同步寻找最优匹配路径，经过最终比较，最后得到了整体最优的匹配结果，整体位置姿态计算结果精度高；本发明针对非合作目标点云信息不完整情况，采用了模型平移来进行不同位置匹配，即使扫描的局部点云，该方法也能得到最优的配置效果，保证整体位置姿态计算结果准确，同时鲁棒性强。

Description

一种非合作目标点云位置姿态计算方法

技术领域

本发明涉及一种非合作目标点云位置姿态计算方法。

背景技术

三维激光扫描技术能够快速获取物体表面每个采样点的空间位置坐标，得到一个表示实体的点集合，称之为“点云”。点云中每个点的角度信息通过摆镜获得，距离通过激光飞行时间测量得到。通过距离和角度信息可以计算出每个点的三维坐标信息。目标的三维模型点云已经在存储好，获取到扫描点云后，模型点云和扫描点云进行配准，从而求取扫描点云的初始姿态。

罗天男等2016年在学位论文《基于模型的全局点云配准方法研究》中基于等效距离场的点云配准算法，采用隐式B样条曲面拟合建立距离场，并利用Levenberg-Marquardt算法优化求解配准参数，以满足配准过程的精度和速度要求。其次按照分支限界策略搜索具有非凸性的配准误差函数的全局最优解的方法，以此克服局部配准算法可能陷入局部最优位置的缺点。然后结合基于等效距离场的局部配准算法加速收敛，最终形成基于模型的全局点云配准算法，保证全局收敛性和鲁棒性要求。

郭裕兰等2012年在光学精密工程的《激光雷达目标三维姿态估计》中，提出了投影点云密度熵(PDE)特征，研究了PDE特征与目标姿态的内在关系，给出了对目标点云迭代地进行旋转投影并寻找PDE最小值以实现三维姿态估计的方法。

柳建锋2014年在毕业论文《基于对偶四元数的LIDAR点云拼接研究》中，根据直线间相交、平行和异面的拓扑关系，分别对待拼接测站和基准测站LIDAR点云的直线进行聚簇，构建直线簇；然后，分别将同名直线用Plücker坐标表示，通过待拼接测站LIDAR点云的直线簇在空间中的螺旋缩放运动，使其与基准测站LIDAR点云的直线簇比例尺一致，且同名Plücker直线重合，构建基于直线簇的共线条件方程，实现了比例因子和相对位姿一体化解算。

王勇2015年在小型微型计算机系统中的《采用点云重心距离进行边界检测的点云数据配准》中，提出用点云重心距离进行边界检测的点云数据配准新算法。该算法首先运用主成分分析法进行点云数据粗配准,获得良好点云姿态；其次,采用点云重心距离特征进行边界检测并提取待配准点云边界；在此基础上,利用K-D树在两点云边界中查找对应最近点对,并通过单位四元数法进行坐标转换,求得平移矩阵及旋转矩阵,从而点云数据配准。

空间非合作目标点云姿态计算中，受扫描范围限制，部分非合作目标的点云可能获取不到，因此上述现有技术中的匹配算法很容易落入局部最优，但无法得到精确解。

发明内容

本发明要解决的技术问题是：克服现有技术的不足，提供了一种非合作目标点云位置姿态计算方法，从不同初始参数，同步寻找最优匹配路径，经过最终比较，最后得到了整体最优的匹配结果，整体位置姿态计算结果精度高，计算速度快。

本发明目的通过以下技术方案予以实现：

一种非合作目标点云位置姿态计算方法，包括如下步骤：

步骤一、扫描非合作目标获取扫描点云，在相机坐标系下，扫描点云中所有扫描点在x轴、y轴、z轴方向的位置均值分别为x₀、y₀、z₀；扫描点云的俯仰角α、偏航角β、滚动角γ分别设为0°、0°、0°；目标外形点云在相机坐标系下的位置坐标为0、0、0，姿态为0°、0°、0°；

步骤二、将目标外形点云的位置坐标在相机坐标系下沿x轴平移x₀、沿y轴平移y₀、沿z轴平移z₀；然后将目标外形点云的俯仰角沿x轴转动-α角度，偏航角沿y轴转-β角度，滚动角沿z轴转动-γ角度；

步骤三、在步骤二的基础上将目标外形点云的位置坐标沿x轴、y轴分别进行平移，然后与步骤一中的扫描点云进行点云配准，计算并统计每次平移后的匹配误差；所述平移后匹配误差中最小值对应的目标外形点云位置坐标x_s、y_s、z_s即为扫描点云的位置坐标；

步骤四、在步骤二的基础上将目标外形点云的俯仰角沿x轴、偏航角沿y轴、滚动角沿z轴分别进行旋转，然后与步骤一中的扫描点云进行点云配准，计算并统计每次旋转后的匹配误差；所述旋转后匹配误差中最小值对应的目标外形点云姿态坐标α_s、β_s、γ_s即为扫描点云的姿态；

步骤五、将步骤三中目标外形点云位置坐标x_s、y_s、z_s分别赋值给x₀、y₀、z₀，将步骤四中目标外形点云的姿态α_s、β_s、γ_s分别赋值给α、β、γ，扫描非合作目标获取下一组扫描点云，然后转入步骤二。

上述非合作目标点云位置姿态计算方法，所述步骤三中目标外形点云在x轴、y轴的位置坐标分别进行平移的方法为：

将目标外形点云的位置坐标沿x轴或y轴分别平移2s次，每次平移量分别为-s*m、(-s+1)*m、...、0、...、(s-1)*m、s*m；m为平移步长，s为正整数；

当计算第一组扫描点云的位置时，目标外形点云的平移次数s和平移步长m为预设值；当计算后续扫描点云的位置时，x轴或y轴的平移次数s和平移步长m根据上一组扫描点云与目标外形点云在该轴平移配准时的匹配误差确定。

上述非合作目标点云位置姿态计算方法，所述步骤四中目标外形点云的俯仰角沿x轴、偏航角沿y轴、滚动角沿z轴分别进行旋转的方法为：

将目标外形点云的俯仰角或偏航角或滚动角分别旋转2t次，每次旋转量分别为-t*n、(-t+1)*n、...、0、...、(t-1)*n、t*n；n为旋转步长，t为正整数；

当计算第一组扫描点云的姿态时，目标外形点云的旋转次数t和旋转步长n为预设值；当计算后续扫描点云的姿态时，三轴的每轴旋转次数t和旋转步长n根据上一组扫描点云与目标外形点云在该轴旋转配准时的匹配误差确定。

上述非合作目标点云位置姿态计算方法，所述x轴或y轴的平移次数s和平移步长m根据上一组扫描点云与目标外形点云在该轴平移配准中时的匹配误差确定，所述确定方法为：

当目标外形点云的位置坐标沿x轴或y轴进行任一方向平移时，将上一组扫描点云与目标外形点云进行平移配准；在沿该轴平移配准中，所有平移匹配误差的最大值为dmx，平移匹配误差的最小值为dmi，平移匹配误差的最大值dmx与平移匹配误差的最小值dmi的差值为第一差值d1；

如果第一差值d1位于预设第一阈值k1与预设第二阈值k2的区间内，则s的值不变，其中k1<k2；如果第一差值d1小于预设第一阈值k1，则s在原有数值的基础上减少s*(k1-d1)/(k2-k1)；如果第一差值d1大于预设第二阈值k2，则s在原有数值的基础上增加s*(d1-k2)/(k2-k1)，s的最小值为0，s的最大值不超过第一预设值z1；

如果平移匹配误差的最小值dmi位于预设第三阈值g1与预设第四阈值g2的区间内，则平移步长m不变，其中g1<g2；如果平移匹配误差的最小值dmi小于预设第三阈值g1，则m值减少0.1～0.5倍；如果平移匹配误差的最小值dmi大于预设第四阈值g2，则m值增加0.1～0.5倍，m的最小值大于0，m的最大值不超过第二预设值z2。

上述非合作目标点云位置姿态计算方法，所述三轴中每轴的旋转次数t和旋转步长n根据上一组扫描点云与目标外形点云在该轴旋转配准时的匹配误差确定，所述确定方法为：

当目标外形点云的俯仰角沿x轴或偏航角沿y轴或滚动角沿z轴分别进行任一角度旋转时，将上一组扫描点云与目标外形点云进行旋转配准；在沿该轴旋转配准过程中，所有旋转匹配误差的最大值为djx，旋转匹配误差的最小值为dj i，旋转匹配误差的最大值djx与旋转匹配误差的最小值dj i的差值为第二差值d2；

如果第二差值d2位于预设第五阈值j1与预设第六阈值j2的区间内，则t的值不变，其中j1<j2；如果第二差值d2小于预设第五阈值j1，则t在原有数值的基础上减少t*(j1-d2)/(j2-j1)；如果第二差值d2大于预设第六阈值j2，则t在原有数值的基础上增加t*(d2-j2)/(j2-j1)，t的最小值为0，t的最大值不超过第三预设值z3；

如果旋转匹配误差的最小值dj i位于预设第七阈值h1与预设第八阈值h2的区间内，则旋转步长n不变，其中h1<h2；如果旋转匹配误差的最小值dj i小于预设第七阈值h1，则n值减少0.1～0.5倍；如果旋转匹配误差的最小值dj i大于预设第八阈值h2，则n值增加0.1～0.5倍，n的最小值大于0，n的最大值不超过第四预设值z4。

本发明相比于现有技术具有如下有益效果：

(1)本发明匹配过程中有不同位置及姿态的初始参数，避免了陷入局部最优，姿态精度高；现有技术中的匹配都是只有一个初始位置及姿态参数，如果初始参数不恰当，很容易进入局部最优，无法得到整体最优结果；本方法从不同初始参数，同步寻找最优匹配路径，经过最终比较，最后得到了整体最优的匹配结果，整体位置姿态计算结果精度高；

(2)本发明根据模型点云位置偏移引起的匹配误差对比及不同姿态引起的匹配误差，统计了模型点云位置及姿态引起的匹配误差最大值与最小值的差值，根据差值情况，动态决定下幅点云配准过程中的参数；如果匹配误差最大值与最小值的差值小于一定阈值，则下次姿态计算，减少参数，如果大于一定阈值，则下次姿态计算，增加参数，否则不动；伸缩有度，计算灵活；

(3)本发明针对非合作目标点云信息不完整情况，本方法采用了模型平移来进行不同位置匹配；即使扫描的局部点云，该方法也能得到最优的配置效果；保证整体位置姿态计算结果准确；

(4)本发明由各种位置姿态初值进行匹配寻优操作，由于初始位置姿态多，提高了最终位置姿态的计算精度，避免了落入局部最优点的情况；尤其针对扫描的非合作目标点云信息不完整的情况，该方法鲁棒性强，能够得到最终的正确匹配结果；

(5)本发明根据模型点云位置偏移引起的匹配误差对比及不同姿态引起的匹配误差，动态决定下幅点云配准过程中的参数；如果匹配误差最大值与最小值的差值小于一定阈值，则下次姿态计算，减少参数，这样在跟踪目标过程中，由于计算的收敛，位置姿态速度快。

附图说明

图1为本发明的步骤流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明的实施方式作进一步详细描述。

一种非合作目标点云位置姿态计算方法，如图1所示，包括如下步骤：

步骤101、扫描非合作目标获取扫描点云，在相机坐标系下，扫描点云中所有扫描点在x轴、y轴、z轴方向的位置均值分别为x₀、y₀、z₀；扫描点云的俯仰角α、偏航角β、滚动角γ分别设为0°、0°、0°；目标外形点云存储在处理器内存中，用于与扫描点云配准，目标外形点云在相机坐标系下的位置坐标为0、0、0，姿态为0°、0°、0°。

步骤102、将目标外形点云的位置坐标在相机坐标系下沿x轴平移x₀、沿y轴平移y₀、沿z轴平移z₀；然后将目标外形点云的俯仰角沿x轴转动-α角度，偏航角沿y轴转-β角度，滚动角沿z轴转动-γ角度。

步骤103、在步骤102的基础上将目标外形点云的位置坐标沿x轴、y轴分别进行平移，然后与步骤101中的扫描点云进行点云配准，计算并统计每次平移后的匹配误差；所述平移后匹配误差中最小值对应的目标外形点云位置坐标x_s、y_s、z_s即为扫描点云的位置坐标。

目标外形点云在x轴、y轴的位置坐标分别进行平移的方法为：

当计算第一组扫描点云的位置时，目标外形点云的平移次数s和平移步长m为预设值；当计算后续扫描点云的位置时，该轴的平移次数s和平移步长m根据上一组扫描点云与目标外形点云在该轴平移配准时的匹配误差确定，所述确定方法为：

当目标外形点云的位置坐标沿x轴或y轴进行任一方向平移时，将上一组扫描点云与目标外形点云进行平移配准；在沿该轴平移配准中，所有平移匹配误差的最大值为dmx，平移匹配误差的最小值为dmi，平移匹配误差的最大值dmx与平移匹配误差的最小值dmi的差值为第一差值d1,利用这些值进行后续沿该轴平移次数和平移步长的计算；即用上一组扫描点云与目标外形点云进行x轴平移配准时的x轴平移匹配误差的最大值、x轴平移匹配误差的最小值、x轴平移匹配误差的最大值与x轴平移匹配误差的最小值的差值，进行后续确定x轴平移次数和平移步长的计算；用上一组扫描点云与目标外形点云进行y轴平移配准时的y轴平移匹配误差的最大值、y轴平移匹配误差的最小值、y轴平移匹配误差的最大值与y轴平移匹配误差的最小值的差值，进行后续确定y轴平移次数和平移步长的计算；

沿x、y两轴平移的s、m值可能不同。

步骤104、在步骤102的基础上将目标外形点云的俯仰角沿x轴、偏航角沿y轴、滚动角沿z轴分别进行旋转，然后与步骤101中的扫描点云进行点云配准，计算并统计每次旋转后的匹配误差；所述旋转后匹配误差中最小值对应的目标外形点云姿态α_s、β_s、γ_s即为扫描点云的姿态。

目标外形点云的俯仰角沿x轴、偏航角沿y轴、滚动角沿z轴分别进行旋转的方法为：

当计算第一组扫描点云的姿态时，目标外形点云的旋转次数t和旋转步长n为预设值；当计算后续扫描点云的姿态时，三轴中每轴的旋转次数t和旋转步长n根据上一组扫描点云与目标外形点云在该轴旋转配准时的匹配误差确定，所述确定方法为：

当目标外形点云的俯仰角沿x轴或偏航角沿y轴或滚动角沿z轴分别进行任一角度旋转时，将上一组扫描点云与目标外形点云进行旋转配准；在沿该轴旋转配准过程中，所有旋转匹配误差的最大值为djx，旋转匹配误差的最小值为dj i，旋转匹配误差的最大值djx与旋转匹配误差的最小值dj i的差值为第二差值d2，利用这些值进行后续沿该轴旋转次数和旋转步长的计算；即用上一组扫描点云与目标外形点云进行俯仰角沿x轴旋转配准时的旋转匹配误差的最大值、旋转匹配误差的最小值、旋转匹配误差的最大值与旋转匹配误差的最小值的差值，进行后续确定俯仰角沿x轴旋转次数和旋转步长的计算；用上一组扫描点云与目标外形点云进行偏航角沿y轴旋转配准时的旋转匹配误差的最大值、旋转匹配误差的最小值、旋转匹配误差的最大值与旋转匹配误差的最小值的差值，进行后续确定偏航角沿y轴旋转次数和旋转步长的计算；用上一组扫描点云与目标外形点云进行滚动角沿z轴旋转配准时的旋转匹配误差的最大值、旋转匹配误差的最小值、旋转匹配误差的最大值与旋转匹配误差的最小值的差值，进行后续确定滚动角沿z轴旋转次数和旋转步长的计算；

沿三轴旋转的t、n值可能不同。

步骤105、将步骤103中目标外形点云位置坐标x_s、y_s、z_s分别赋值给x₀、y₀、z₀，即将扫描点云的位置坐标分别赋值给x₀、y₀、z₀；将步骤104中目标外形点云的姿态α_s、β_s、γ_s分别赋值给α、β、γ，即将扫描点云的姿态分别赋值给α、β、γ；扫描非合作目标获取下一组扫描点云，然后转入步骤102，开始计算下一组扫描点云的位置姿态。

因此本发明方法能够不断地扫描非合作目标，并按先后时刻获得非合作目标的扫描点云；除首次进行非合作目标的位置姿态计算时，需要进行α、β、γ、s、m、n、t的赋值，后续非合作目标扫描点云的位置姿态计算时则采用前一组扫描点云中x₀、y₀、z₀、α、β、γ、s、m、n、t的计算结果，能够加快非合作目标的位置姿态计算。

实施例1：

步骤(1)、扫描非合作目标获取扫描点云，在相机坐标系下，求取扫描点云中所有扫描点在x轴、y轴、z轴方向的位置均值分别x₀、y₀、z₀，同时将扫描点云的所有扫描点的滚动角α、偏航角β、俯仰角γ分别设为0°、0°、0°。目标外形点云存储在处理器内存中，用于与扫描点云配准，目标外形点云在相机坐标系下的位置坐标为0、0、0，姿态为0°、0°、0°。

步骤(2)、将目标外形点云的位置坐标在相机坐标系下沿x轴平移x₀、沿y轴平移y₀、沿z轴平移z₀，然后将目标外形点云的俯仰角沿x轴转动-α角度，偏航角沿y轴转-β角度，滚动角沿z轴转动-γ角度。

步骤(3)、在步骤(2)基础上，将目标外形点云的位置坐标沿x轴分别平移-S*M、(-S+1)*M、...、0、...、(S-1)*M、S*M，然后与步骤(1)中的扫描点云进行平移配准，计算并统计每次平移后的匹配误差。其中S为x轴左右单方向最大移动次数，S为正整数，M为x轴步长距离，单位毫米。

步骤(4)、在步骤(2)基础上，将目标外形点云的位置坐标沿y轴分别平移-T*N、(-T+1)*N、...、0、...、(T-1)*N、T*N，而后与步骤(1)中的扫描点云进行平移配准，计算并统计每次平移后的匹配误差。其中T为y轴左右单方向最大移动次数，T为正整数，N为y轴步长距离，单位毫米。

步骤(5)、在步骤(2)基础上，将目标外形点云的俯仰角沿x轴分别旋转-C*B、(-C+1)*B、...、0、...、(C-1)*B、C*B，然后与步骤(1)中的扫描点云进行旋转配准，计算并统计每次旋转后的匹配误差。其中C为俯仰轴单方向最大旋转次数，C为正整数，B为俯仰轴步长角度，单位度。

步骤(6)、在步骤(2)基础上，将目标外形点云的偏航角沿y轴分别旋转-I*P、(-I+1)*P、...、0、...、(I-1)*P、I*P，然后与步骤(1)中的扫描点云进行旋转配准，计算并统计每次旋转后的匹配误差。其中I为偏航轴单方向最大旋转次数，I为正整数，P为偏航轴步长角度，单位度。

步骤(7)、在步骤(2)基础上，将目标外形点云的滚动角沿z轴分别转动-H*Q、(-H+1)*Q、...、0、...、(H-1)*Q、H*Q，然后与步骤(1)中的扫描点云进行旋转配准，计算并统计每次旋转后的匹配误差。其中H为滚动轴单方向最大移动次数，H为正整数，Q为滚动轴步长角度，单位度。

步骤(8)、统计步骤(3)和步骤(4)中目标外形点云位置坐标平移引起的平移匹配误差；统计步骤(5)中俯仰引起的旋转匹配误差；统计步骤(6)中偏航引起的旋转匹配误差；统计步骤(7)中滚动引起的旋转匹配误差；即获得目标外形点云位置坐标平移和姿态旋转后的所有匹配误差。将步骤(3)、步骤(4)中的所有平移匹配误差进行对比，平移匹配误差的最小值对应的目标外形点云的位置坐标x_s、y_s、z_s即为扫描点云的位置坐标；将步骤(5)、步骤(6)、步骤(7)中的所有旋转匹配误差进行对比，旋转匹配误差的最小值对应的目标外形点云的姿态α_s、β_s、γ_s即为扫描点云的姿态。

步骤(9)、步骤(3)中目标外形点云在x轴不同位置偏移下，如果x轴匹配误差最大值与x轴匹配误差最小值的x轴差值D1在x轴第一阈值K1与x轴第二阈值K2之间，其中K1<K2，则下一组扫描云姿态计算时x轴左右单方向最大移动次数S不变；如果x轴差值D1小于x轴第一阈值K1，则下一组扫描云姿态计算时，x轴减少平移次数，左右单方向最大移动次数在原有S次基础之上减少S*(K1-D1)/(K2-K1)次，S最低为0次，即不平移；如果x轴差值D1大于x轴第二阈值K2，则下一组扫描云姿态计算时，增加x轴平移次数，左右单方向最大移动次数在原有S次基础之上增加S*(D1-K2)/(K2-K1)次，S的最大值不超过x轴第一预设值Z1。

步骤(3)中目标外形点云在x轴不同位置偏移下，如果x轴匹配误差的最小值位于x轴第三阈值K11与x轴第四阈值K12的区间内，则下一组扫描云姿态计算时，x轴步长距离M不变，其中K11<K12；如果x轴匹配误差的最小值小于x轴第三阈值K11，则下一组扫描云姿态计算时，M值减少0.2倍；如果x轴匹配误差的最小值大于x轴第四阈值K12，则下一组扫描云姿态计算时，M值增加0.2倍，M的最小值大于0，M的最大值不超过x轴第二预设值Z2。

步骤(10)、步骤(4)中目标外形点云在y轴不同位置偏移下，如果y轴匹配误差最大值与y轴匹配误差最小值的y轴差值D2在y轴第一阈值K3与y轴第二阈值K4之间，其中K3<K4，则下一组扫描云姿态计算时，y轴上下单方向最大移动次数T不变；如果y轴差值D2小于y轴第一阈值K3，则下一组扫描云姿态计算时，减少平移次数，y轴上下单方向最大移动次数在原有T次基础之上减少T*(K3-D1)/(K4-K3)次，T最低为0次，即不平移；如果y轴差值D2大于y轴第二阈值K4，则下一组扫描云姿态计算时，增加y轴平移次数，y轴上下单方向最大移动次数在原有T次基础之上增加T*(D2-K4)/(K4-K3)次，T的最大值不超过y轴第一预设值Z3。

步骤(4)中目标外形点云在y轴不同位置偏移下，如果y轴匹配误差的最小值位于y轴第三阈值K13与y轴第四阈值K14的区间内，则下一组扫描云姿态计算时，y轴步长距离N不变，其中K13<K14；如果y轴匹配误差的最小值小于y轴第三阈值K13，则下一组扫描云姿态计算时，N值减少0.2倍；如果y轴匹配误差的最小值大于y轴第四阈值K14，则下一组扫描云姿态计算时，N值增加0.2倍，N的最小值大于0，N的最大值不超过y轴第二预设值Z4。

步骤(11)、步骤(5)中目标外形点云在不同俯仰情况下，如果俯仰匹配误差最大值与俯仰匹配误差最小值的俯仰差值D3在俯仰第一阈值K5与俯仰第二阈值K6之间，其中K5<K6，则下一组扫描云姿态计算时，俯仰轴方向最大转动次数C不变；如果俯仰差值D3小于俯仰第一阈值K5，则下一组扫描云姿态计算时，减少转动次数，在原有C次基础之上减少C*(K5-D3)/(K6-K5)次，C最低为0次，即不转动；如果俯仰差值D3大于俯仰第二阈值K6，则下一时刻的姿态计算，增加转动次数，在原有C次基础之上增加C*(D3-K6)/(K6-K5)次，C的最大值不超过俯仰第一预设值Z5。

步骤(5)中目标外形点云在不同俯仰情况下，如果俯仰匹配误差的最小值位于俯仰第三阈值K15与俯仰第四阈值K16的区间内，则下一组扫描云姿态计算时，俯仰轴步长角度B不变，其中K15<K16；如果匹配误差的最小值小于俯仰第三阈值K15，则下一组扫描云姿态计算时，B值减少0.2倍；如果匹配误差的最小值大于俯仰第四阈值K16，则下一组扫描云姿态计算时，B值增加0.2倍，B的最小值大于0，B的最大值不超过俯仰第二预设值Z6。

步骤(12)、步骤(6)中目标外形点云不同偏航情况下，如果偏航匹配误差最大值与偏航匹配误差最小值的偏航差值D4在偏航第一阈值K7与偏航第二阈值K8之间，其中K7<K8，则下一组扫描云姿态计算时，偏航轴方向最大转动次数I不变；如果偏航差值D4小于偏航第一阈值K7，则下一组扫描云姿态计算时，减少转动次数，在原有I次基础之上减少I*(K7-D4)/(K8-K7)次，I最低为0次，即不转动；如果偏航差值D4大于偏航第二阈值K8，则下一组扫描云姿态计算时，增加转动次数，在原有I次基础之上增加I*(D3-K8)/(K8-K7)次，I的最大值不超过偏航第一预设值Z7。

步骤(6)中目标外形点云在不同偏航情况下，如果偏航匹配误差的最小值位于偏航第三阈值K17与偏航第四阈值K18的区间内，其中K17<K18，则下一组扫描云姿态计算时，偏航轴步长角度P不变；如果匹配误差的最小值小于偏航第三阈值K17，则下一组扫描云姿态计算时，P值减少0.2倍；如果匹配误差的最小值大于偏航第四阈值K18，则下一组扫描云姿态计算时，P值增加0.2倍，P的最小值大于0，P的最大值不超过偏航第二预设值Z8。

步骤(13)、步骤(7)中目标外形点云不同滚动情况下，如果滚动匹配误差最大值与滚动匹配误差最小值的滚动差值D5在滚动第一阈值K9与滚动第二阈值K10之间，则下一组扫描云姿态计算时，滚动轴方向最大转动次数H不变,其中K9<K10；如果滚动差值D5小于滚动第一阈值K9，则下一组扫描云姿态计算时，减少转动次数，在原有H次基础之上减少H*(K9-D4)/(K10-K9)次，H最低为0次，即不转动；如果滚动差值D5大于滚动第二阈值K10，则下一组扫描云姿态计算时，增加转动次数，在原有H次基础之上增加H*(D5-K10)/(K10-K9)次，H的最大值不超过滚动第一预设值Z9。

步骤(7)中目标外形点云在不同滚动情况下，如果滚动匹配误差的最小值位于滚动第三阈值K19与滚动第四阈值K20的区间内，其中K19<K20，则下一组扫描云姿态计算时，滚动轴步长角度Q不变；如果匹配误差的最小值小于滚动第三阈值K19，则下一组扫描云姿态计算时，Q值减少0.2倍；如果匹配误差的最小值大于滚动第四阈值K20，则下一组扫描云姿态计算时，P值增加0.2倍，Q的最小值大于0，Q的最大值不超过滚动第二预设值Z10。

步骤(14)、将步骤(8)中目标外形点云位置坐标x_s、y_s、z_s分别赋值给x₀、y₀、z₀，目标外形点云的姿态α_s、β_s、γ_s分别赋值给α、β、γ，扫描非合作目标获取下一时刻的扫描点云，然后转入步骤(2)，继续处理新的扫描点云。

所述步骤(9)～步骤(14)中的K1～K20均为预设值。

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

Claims

1.一种非合作目标点云位置姿态计算方法，其特征在于：包括如下步骤：

步骤五、将步骤三中目标外形点云位置坐标x_s、y_s、z_s分别赋值给x₀、y₀、z₀，将步骤四中目标外形点云的姿态α_s、β_s、γ_s分别赋值给α、β、γ，扫描非合作目标获取下一组扫描点云，然后转入步骤二；

所述步骤三中目标外形点云在x轴、y轴的位置坐标分别进行平移的方法为：

当计算第一组扫描点云的位置时，目标外形点云的平移次数s和平移步长m为预设值；当计算后续扫描点云的位置时，x轴或y轴的平移次数s和平移步长m根据上一组扫描点云与目标外形点云在该轴平移配准时的匹配误差确定；

所述x轴或y轴的平移次数s和平移步长m根据上一组扫描点云与目标外形点云在该轴平移配准中时的匹配误差确定，具体确定方法为：

2.根据权利要求1所述的一种非合作目标点云位置姿态计算方法，其特征在于：所述步骤四中目标外形点云的俯仰角沿x轴、偏航角沿y轴、滚动角沿z轴分别进行旋转的方法为：

3.根据权利要求2所述的一种非合作目标点云位置姿态计算方法，其特征在于：所述三轴中每轴的旋转次数t和旋转步长n根据上一组扫描点云与目标外形点云在该轴旋转配准时的匹配误差确定，具体确定方法为：

当目标外形点云的俯仰角沿x轴或偏航角沿y轴或滚动角沿z轴分别进行任一角度旋转时，将上一组扫描点云与目标外形点云进行旋转配准；在沿该轴旋转配准过程中，所有旋转匹配误差的最大值为djx，旋转匹配误差的最小值为dji，旋转匹配误差的最大值djx与旋转匹配误差的最小值dji的差值为第二差值d2；

如果旋转匹配误差的最小值dji位于预设第七阈值h1与预设第八阈值h2的区间内，则旋转步长n不变，其中h1<h2；如果旋转匹配误差的最小值dji小于预设第七阈值h1，则n值减少0.1～0.5倍；如果旋转匹配误差的最小值dji大于预设第八阈值h2，则n值增加0.1～0.5倍，n的最小值大于0，n的最大值不超过第四预设值z4。