Pose estimation of flying target based on bi-modal information fusion

随着信息化、智能化技术的不断发展，现代战争对武器装备打击的精准度要求越来越高。导弹飞行试验中实时解算导弹位姿、准确预报导弹落点有利于判断导弹是否击中目标，并且可以及时发现导弹故障实施提前炸毁^[1]。与传统的建立动力学、运动学数学模型推测导弹落点^[2]不同，如今通过对飞行过程中导弹位姿信息进行捕捉，并根据抛物线近似、非线性滤波、深度学习等方法预测导弹落点^[3-4]，符合信息化、智能化的要求。因此，实现对复杂背景下的飞行目标实现快速位姿估计，成为了目前亟需解决和攻克的问题。

国内外学者针对复杂背景下飞行目标检测、位姿估计问题提出大量研究方法。例如，乐英等^[5]等提出了基于背景差分和帧间差分法对目标进行检测，增强了背景差分法的检测能力，使提取的运动目标相对完整且噪声少。Barnic等^[6]提出ViBe (Visual Background Extractor) 算法对运动目标进行检测，利用图像初始帧来构建背景模型，计算量小且实时性高，但是当初始帧包含运动目标时，容易将运动目标初始化到背景模型中产生伪前景。刘家军等^[7]针对ViBe算法检测运动目标时产生伪前景的问题，在Vibe算法的基础上使用熵值法判断颜色和空间位置在相似程度函数中的权重，采用迭代法确定自适应阈值，以增强前景分割精度，但ViBe算法容易受光照影响，在面临光照变化、动态干扰等情况时检测性能较差。胡涛^[8]等提出RGBD相机与ViBe算法结合，将深度信息作为背景建模的样本，提高了ViBe算法的稳定性，但该算法无法直接应用于彩色相机分辨率和雷达分辨率不同的情况。尹芳^[9]采用ICP算法计算相邻关键帧之间的相对位姿信息，但对于飞行速度快的目标，ICP配准算法无法处理初始位置相差较大的点云并易陷入局部最优。田光宝等^[10]采用3个不共线的特征点对目标进行位姿求解，但在特征点提取时容易产生较大误差影响位姿估计精度；刘耀华等^[11]利用聚类算法和随机森林算法对点云进行分割，然后对分割后的实例采用FLANN算法进行特征匹配得到实例分割后特征点所对应的匹配点。最后通过RANSAC算法和ICP算法求解目标的精确位姿，但该算法不适用于背景复杂且目标物小的场景。Kechagias ^[12]提出了激光雷达和相机融合的方法估计目标位姿，利用点云与RNN网络结合进行目标相对姿态估计。但飞行目标视觉特征不明显，在训练过程中卷积神经网络容易被忽略，且目标在飞行过程中存在尺度变化幅度大的情况，也给训练带来了很大困难。尽管上述方法可以有效对目标进行检测和位姿估计，但在导弹落点精准预测和制导控制上存在无法在复杂背景下对飞行目标快速、准确提取以及位姿估计的问题。

三维测量中线阵雷达主要依赖线列推扫式，其优势在于灵活的视场配置和较高的点云密度，但受限于扫描速率，在对动态目标进行观测时，其测量结果与实际轮廓存在较大差异。而基于高灵敏度阵列的面阵激光雷达，可实现单脉冲瞬时成像，有效解决了运动扫描导致的时间同步问题和条带错位现象^[13]。

因此，在保证实时性的前提下，文中提出基于面阵式激光雷达与图像信息融合的双模态飞行目标位姿估计方法。首先，采用ViBe算法与深度信息结合，实现图像目标快速提取并且可以较好的抑制噪点；其次，通过将整体点云和图像目标融合，实现从整体点云中提取出目标点云；最后，采用PnP (Pespective-n-Point) 算法获取点云间初始旋转平移矩阵并采用基于IK-D Tree (Incremental K-dimensional Tree) 的ICP (Iterative Close Point) 算法解算精确位姿。文中方法与现有方法相比计算速度快、提取目标精度高，能够很好应对背景复杂的场景特殊性。

文中所提方法的总体框架包含两个部分：图像目标提取部分和点云信息处理部分，其中点云信息处理包含特征叠加和目标点云提取，其流程如图1所示。

Figure 1.  Flow chart of bimodal fusion algorithm

研究中所用彩色相机的出厂基本参数不能满足高精度估计要求，且彩色相机在成像过程中会因镜头制造误差和组装误差等原因导致成像效果不理想。因此，采用张正友相机标定法对彩色相机内参进行重新标定。标定完成后采用PnP算法求解激光雷达至彩色相机的映射矩阵。具体方法为：用彩色相机和激光雷达对同一特征目标进行拍摄，采用手动标记的方式标记出特征点在彩色相机和点云中的位置。假设标记的特征点在激光雷达坐标系下的坐标为(X_L,Y_L,Z_L)，在图像像素坐标系下的坐标为(u, v)可建立如下关系：

式中：Q表示彩色相机内参；R_L−C表示激光雷达坐标系到彩色相机坐标系的旋转矩阵；T_L−C表示平移矢量；m₁₁~m₃₄表示待求矩阵的12个元素。

通过提取多组激光雷达与图像对应特征点，建立如下约束方程组，即可求解出矩阵的12个元素。由公式(1)展开可得到：

求解公式(2)至少需要6组特征点，为减小误差采集多组特征点进行解算。应用最小二乘法求得映射矩阵初始解，并用Levenberg-Marquarat算法对其结果进行优化得到最终结果。

求解出彩色相机内参和激光雷达至彩色相机的映射矩阵后，通过转换矩阵实现点云坐标到图像像素坐标的投影。为了实现点云坐标与图像像素坐标的相互转换，将各变换矩阵扩展为4×4的方阵。若将点云坐标表示为P(X_L,Y_L,Z_L)，映射到图像像素坐标系的二维映射点云表示为Y(u ,v)，则点云坐标到图像像素坐标的映射计算方式如下:

式中：M₀为扩充后的外参矩阵。

将点云数据转换至图像像素坐标系，即可完成点云数据与图像数据的特征叠加。点云与图像融合效果如图2所示。

Figure 2.  Point cloud and image fusion

针对在传统ViBe算法中存在的因光照强度改变而影响算法精度的问题，利用激光雷达不受光照影响的优势，采用ViBe算法与深度信息融合，将深度信息加入背景模型样本中作为运动目标检测的权值。

文中采用将点云信息通过坐标转换投影至图像上的方式赋予每个像素点深度值。但受到当前传感器芯片技术水平的限制，直接采集到的三维点云数据通常分辨率较低，远不及可见光图像^[14]，无法做到给每个像素赋予深度信息。因此需要对点云进行稠密化。

由面阵式激光雷达扫描成像原理可知，点云在空间的分布情况为呈锥形方向均匀展开。因此通过视场角对相同物距的某一平面激光雷达可视区域的4个角点计算，即可得出雷达在此平面的最大投影面积^[15]。假设雷达到测量面的距离为L，彩色相机分辨率为：a_C×b_C，雷达分辨率为a_L×b_L，雷达水平视场角为H，垂直视场角为G，4个角点的分别为P₁、P₂、P₃、P₄，4个角点的坐标为：${{{P}}_{{1}}}{{( - L}} \cdot {\tan} \left(\dfrac{{{H}}}{{{2}}}\right),{L} \cdot {\tan}\left(\dfrac{{{G}}}{{{2}}}\right),{L)}$，${{{P}}_{{2}}}{{(L}} \cdot {{\tan}}\left( \dfrac{{{H}}}{{{2}}} \right),{{L}} \cdot {{\tan}}\left( \dfrac{{{G}}}{{{2}}} \right),{{L)}}$，${{{P}}_{{3}}}{{( - L}} \cdot {{\tan}}\left( \dfrac{{{H}}}{{{2}}} \right), {{- L}} \cdot {{\tan}}\left( \dfrac{{{G}}}{{{2}}} \right), {{L)}}$，$ P\left(L-\tan \left(\dfrac{H}{2}\right),-L-\tan \left(\dfrac{G}{2}\right), L\right) $。假设P₁、P₂、P₃、P₄对应像素坐标分别为：Y₁(u₁, v₁)、Y₂(u₂, v₂)、Y₃(u₃, v₃)、Y₄(u₄,v₄)，则点云转化至图像坐标系后，投影最大面积为$ 4\cdot {u}_{2}\cdot {v}_{2} $，深度信息在彩色图像上的占比率V为$ \dfrac{4\cdot {u}_{2}\cdot {v}_{2}}{{a}_{L}\cdot {b}_{L}} $。

在实际应用中由于遮挡物(地面、天花板)等原因会导致映射至图像坐标系后，某一部分区域深度信息在彩色图像上的占比率会大于V，图像中一个像素点周围有$ {\left(2 r+1\right)}^{2}-1 $个像素(其中r为最远处像素距中心像素的水平距离)，因此将不小于V的最小$\, {\left(2 r+1\right)}^{2}-1 $作为深度信息填充的阈值，填充效果如图3所示。

Figure 3.  Point cloud densification

填充方法如下：

(1)建立零矩阵M(a_C·b_C), 将点云投影至彩色相机坐标系后对坐标信息进行向下取整，根据坐标将深度值填充至M中。

(2)寻找出M中的每个零点坐标，在周围距离为r的像素内寻找非零点，将所有非零点的均值作为该点的深度值。

在稠密深度信息构建完成后，使用三张背景图作为背景模型样本集合。在三张背景图的每一像素的8个相关邻域中随机选取5个像素值组成该坐标的像素背景模型；然后，对三张背景图对应的点云信息进行1.3.1节中所说的融合、稠密化处理，并选取对应坐标的深度值作为该坐标的深度背景模型。

背景建模完成后，需要根据阈值区分前景和背景。文中采用参考文献[6]所提算法获取像素确定自适应阈值，采用最大类间方差法获取深度信息确定自适应阈值。

首先使用背景深度矩阵减去含运动目标的深度矩阵并向下取整，记为深度差值矩阵U，并查找矩阵U中最大的深度差值，记为L。然后计算矩阵U中深度值为i的点在矩阵中的分布概率P_i，其中：$ {P}_{\text{i}}= \dfrac{U中深度差值为{i}的点数}{U总点数} $，( i=1，2，3·····L)。则最大类间方差构建方法为：

式中：P_i为矩阵G中每个深度值在矩阵中的分布概率，(i=1，2，3·····L)；k为所求自适应阈值；m为灰度级k的累加均值；mG为图像全局均值。

将公式(5)~(7)代入公式(8)中，求解出令公式(8)最大化的差值k即为最大类间方差法的阈值。

在实际应用中，由于扫描设备自身的限制或待测地形（物）的特点，获取的点云数据中往往会有大量的空洞^[16]。为避免空洞点对整体算法的影响，当某一像素点对应的深度背景模型深度值为0时仅使用像素值作为运动目标检测的权值。在图像目标提取完成时采用中值滤波去除图像中的离群噪点，提取效果如图4所示。

Figure 4.  Moving object extraction

由于目标表面像素值与背景像素值接近、目标表面由于镜面反射成像后颜色信息发生改变等原因，导致提取出的目标有缺陷、不完整。因此使用图像形态学中的腐蚀和膨胀操作对目标缺陷部分进行填充。先采用膨胀操作对二值化物体进行扩充，再采用腐蚀操作对图像大小进行还原即可得到完整的运动目标，效果如图5所示。

Figure 5.  Image morphological processing

对1.3节中求解到的目标图像使用“sobel”算子提取目标边缘轮廓的像素坐标，并将点云坐标投影至图像像素坐标。通过比对目标区域和投影点云坐标去判断出目标投影点云的区域。

在点云投影至图像像素坐标系后，点云的相对位置并未改变，因此通过观察目标投影点云在整体投影点云的位置即可得到目标三维点云在原始点云中的位置，效果如图6(a)所示。在框选目标投影点云范围选择点云时在边缘部分容易提取到背景的点云信息，因此最后对提取出的点云进行去噪处理即可得到目标的三维点云，效果如图6(b)所示。

Figure 6.  Target point cloud extraction

使用ICP算法对飞行目标进行位姿估计时，存在以下两个问题：

(1) 由于提取到的点云目标与点云模型之间距离相差大。传统ICP算法在两个待配点云欧式距离较远时，会造成匹配错误，影响最终匹配效果；

(2) 传统ICP算法进行最近点查找时需计算欧氏距离最短的点。查找速度慢，配准效率低。

针对该问题，采用基于图像和点云特征点关系的PnP算法获取点云间初始旋转平移矩阵，完成点云目标与点云模型粗配准。采用IK-D Tree优化加速临近点搜索。

对1.3节中得到的运动区域采用LSD直线检测算法检测直线段，然后采用霍夫变换对直线进行提取。在霍夫变换时存在一条直线段被提取成多条直线段的情况。因此，根据直线段到坐标原点的距离和直线段与坐标原点的夹角将直线段划分为4类，将划分在同一组的直线采用最小二乘法进行直线拟合，从而得到所需直线，特征直线提取如图7所示。最后，通过计算直线间的交点得到4个目标特征点的图像像素坐标。

Figure 7.  Feature line extraction

根据目标真实尺寸采用相关软件进行目标三维点云模型创建。以模型几何中心作为坐标原点建立物体坐标系。采用手动标点的方式得到点云模型的4个特征点。待测目标物体模型及其坐标系定义如图8所示。已知4个特征点在图像像素坐标系和在物体坐标系下的坐标，利用PnP算法计算出目标在图像像素坐标系下的位姿信息，粗配准效果如图9所示。

Figure 8.  Point cloud model coordinate system

Figure 9.  Pose solution results

假设求得的旋转矩阵和平移矩阵分别为：R_cp(3×3)和T_cp(1×3)，目标原始点云在物体坐标系下的坐标为P(X_M,Y_M,Z_M)，目标在图像像素坐标系下的点云为P_Ft(X_F,Y_F,Z_F)。对各变换矩阵全部扩展成4×4的方阵，则计算公式为：

最后，利用公式(4)将点云P_Ft从图像像素坐标系转换至激光雷达坐标系，即完成点云粗配准。

采用PnP算法获得粗略六维位姿后，模型点云与目标点云已大致重合，但精度较低。为提高配准精度，采用基于IK-D Tree^[17]的ICP算法实现点云精配准。IK-D Tree加速对近邻点搜索构建思路如下：

(1) 按照模型点云中点集的最大协方差构建传统K-D Tree，并用K近邻搜索(k Nearest Neighbor, kNN)查询点距离最近点；

(2)对未匹配的子树采用Lazy LabelInit进行删除，并使用pullup函数将未匹配的子树信息储存到节点T_i(i=1,2,3···n)。其中包括treesize (子树的尺寸)；invalidnum (子树上被标记为“deleted”的节点数字)，以及range (以当前节点为根节点的子树中，所有节点在三维空间中的最小包络盒)，完成IK-D Tree构建。

(3)检测IK-D Tree的平衡性。平衡准则由两个子准则组成：α-平衡准则和α-删除标准。假设IK-D tree的一个子树的根在T_n。当子树满足以下条件：

式中：α∈(0.5,1)；S(T_n)是节点T_n的treesize属性；I(T_n)表示子树上无效节点数量。

α_bal 和 α_del定义为：

如果IK-D Tree不满足任一条件，则需对IK-D Tree进行双线程重建。

(4) 在完成IK-D Tree构建后利用节点的 “range”信息加速临近点搜索。

查找到最近点后，采用奇异值分解算法求取位姿增量，获取其相对坐标转换关系：旋转矩阵R_t与平移矩阵T_t。通过迭代的方式求解使得公式中目标函数达到最小的刚体变换，并将该变换作用于源点云，迭代进行这一过程直到满足某一设定的收敛准则。

式中：a_i与b_i分别代表两组扫描数据中各点的三维坐标。当配准结果相对于真实值三轴位姿精度小于1°时迭代终止。

通过以上配准过程获得最优变换矩阵，完成六维位姿估计。位置估计可由平移向量直接获得，姿态估计由旋转矩阵间接得到。设最优变换矩阵为：

则目标从物体坐标系到雷达坐标系的转换关系为：

式中：M₂为ICP配准得到的位姿变换矩阵；M₀为雷达坐标系到彩色相机坐标系的外参矩阵；M₁为PnP求得的位姿变换矩阵。令：

其中，旋转矩阵为：${{{{{R}}}}_4}{\text{ = }}\left[ {\begin{array}{*{20}{c}} {{r_{11}}}&{{r_{12}}}&{{r_{13}}} \\ {{r_{21}}}&{{r_{22}}}&{{r_{23}}} \\ {{r_{31}}}&{{r_{32}}}&{{r_{33}}} \end{array}} \right]$ 平移向量${T}_4=\left[\begin{array}{lll}t_1 & t_2 & t_3\end{array}\right]^{\rm{T}}$。传感器6自由度位姿估计为Ψ=(x,y,z,α,β,γ)，则位置与平移向量的关系为：

姿态与旋转矩阵的关系：

在仿真实验中，所需仿真数据为仿真点云数据和仿真图像数据。实物模型如图10(a)所示，尺寸约为500 mm×170 mm×170 mm。根据目标真实尺寸采用相关软件进行三维点云模型创建，并通过在点云模型中增加轴线特征点用于后续运算。点云实物模型和点云模型如图10(b)所示，其中蓝色点为点云模型，红色点为轴线特征点。

Figure 10.  Physical object and point cloud model

仿真点云数据通过本实验室提出的三维点云模拟系统，模拟面阵雷达扫描模式采用双切片法^[18]提取。假设模型初始位姿如图11所示，所采用的面阵激光雷达和彩色相机为实验室已有设备，参数信息如表1所示。利用1.1节中所提算法对雷达外参信息和彩色相机内参信息进行标定，仿真图像数据通过公式(4)将点云数据转换到图像像素坐标得到。

Figure 11.  Simulation data

目标运动模型：飞行轨迹满足参考文献[19]。以目标几何中心建立物体坐标系，绕Y轴旋转角度β为0.64(°)/帧，绕Z轴旋转角度γ为0.51(°)/帧，飞行目标做X轴初速度为20 m/s，Y轴方向自由落体的平抛运动。

位姿估计指标要求：位置误差≤ 0.01 m；旋转角度误差≤ 5°；解算时间≤ 2 s/幅。

采用实验室自研软件建立仿真环境，按照雷达成像机理模拟激光雷达扫描成像，目标与激光雷达初始物距为30 m。根据可见点判别方法完成对飞行目标进行多视角可测部位点云数据获取。其中部分仿真结果(第20帧、第30帧)如图11所示。由面阵激光雷达成像原理^[20]可知，在仿真实验的运动模型下飞行目标在运动过程中可看作静止状态。实验结果表示扫描成像数据输出率可达1 s/帧。(文中算法结果CPU AMD Ryzen 5，在内存为16 GB的PC机上运行所得，操作系统为 Windows 10)。

为检测算法的稳定性，进行50次独立测试对位姿估计结果进行评估。其中部分结果(第20帧、第30帧)如图12和图13所示。

Figure 12.  Display of pose calculation process in frame 20

Figure 13.  Display of pose calculation process in frame 30

解算得到的目标点云数据的轴线位置由轴线特征点可知，目标点云数据由3.1节中所提目标运动模型计算可得，然后再由轴线特征点可知目标的轴线位置。通过观察仿真目标点云数据与解算得到的目标点云数据的轴线与Y、Z轴的夹角来判断旋转角度误差。

目标位姿数据包括位置参数(P_x,P_y,P_z)和姿态参数(α,β,γ)，其中P_x、P_y、P_z为X、Y、Z轴的位置参数，α为绕X轴旋转角度，β为绕Y轴旋转角度，γ为绕Z轴旋转角度。由于目标为类圆柱形，因此α不作为此次实验结果的参考值。定义估计误差为(ΔP_x, ΔP_y, ΔP_z, Δβ, Δγ)。对三轴平移距离误差及旋转角度误差、算法过程耗时进行了统计分析，平均误差如图14所示。为证明文中算法的高精度性，采用ICP算法对仿真数据进行直接配准，误差结果如图15所示。与参考文献[9-10]所提算法进行对比，比较效果见表2。

Figure 14.  Pose solution error

Figure 15.  ICP algorithm error

由表2可知,三轴平移误差低于ICP算法和其他算法；文中算法采用PnP算法完成点云粗配准，获取点云间初始旋转平移矩阵；利用IK-D Tree加速临近点搜索，采用ICP算法完成点云精配准。因此运算速度优于ICP算法劣于PnP算法。

实验结果表明，在不同运动姿态下文中算法均可完成目标位姿估计。所提方法具有较高的估计精度，同时也证明了所提方法的有效性和准确性。

建立半实物仿真试验环境对算法有效性进行验证。由于实验室不具备对炮弹实物进行抛洒的场地条件和抛洒装置条件。遂采用约为3∶1缩放模型的铁质圆柱筒完成半物理实验。目标模型长宽高为160 mm×50 mm×50 mm，激光雷达和彩色相机参数信息与数字仿真试验一致。采用人工抛洒目标的方法模拟目标飞行过程进行了两组试验，目标距离雷达相对距离为[10 m， 11 m]范围内变化，测试不同环境(A组背景为静态建筑物和树木；B组背景为花坛、草地和静态建筑物)、不同光照条件(A组拍摄时间为上午10点，天气状况：晴；B组拍摄时间为下午4点，天气状况：多云)下的文中算法的估计效果。每组试验条件下拍摄图像数量在20张以上，部分拍摄图像如图16所示。

Figure 16.  Two scene color pictures

为检测图像目标检测算法的稳定性与有效性，对两组图片进行目标提取测试，并与背景差分法、传统ViBe算法以及参考文献[5]所提算法进行比对，对结果进行统计与分析。目标提取结果如图17所示，为定量比较改进算法与原算法的性能，采用了正确率(Precision, P)、召回率(Recall, R)、错误分类比(Percentage of Wrong Classifications, PWC)作为评价指标，定义如下：

Figure 17.  Comparison of image target extraction algorithms

式中：TP为正确检测为前景像素的个数；FP为错误检测为前景像素的个数；TN为正确检测为背景像素的个数；FN 为错误检测为背景像素的个数；P为正确判定为前景像素的个数占判定为前景像素总数的百分比；R为正确判定为前景像素的个数占真正前景像素总数的百分比。提取点数以及性能评估见表3~5。

由表3~4可见背景差分法在场景存在动态干扰物时提取效果不佳；在背景为静态建筑物居多时，参考文献[5]均有较好的前景目标提取效果，但当背景为动态植物时，背景差分法和参考文献[5]提取效果不佳。ViBe算法在静态背景和动态背景下均有较好的提取效果。但当光线不足且有变化时会产生部分噪点，文中算法召回率相比其他算法略有降低，但在正确率以及错误分类比上都有较好的提升，与传统ViBe算法相比正确率提高了27%，错误分类比降低了36%。背景差分法和参考文献[5]算法直接将运动图像和背景相比，运算量低鲁棒性差且两种方法均为手动设置阈值，因此与文中所提算法相比耗时短；文中算法相比与传统ViBe算法相比，将深度信息加入背景模型样本中作为运动目标检测的权值且采用自适应阈值减少了人工干预，因此耗时有所提升。

采用第2章中所提算法对目标点云进行提取和位姿估计，结果如图18和表6所示。

单次激光雷达扫描无法直接提取飞行目标的完整轮廓，从而无法掌握目标准确的位姿信息。因此通过彩色图像分别取目标的4个角点和目标每条边的中点作为特征点。利用PnP算法对飞行目标位姿进行两次估计，然后取两次估计结果的平均值与文中位姿求取结果进行对比，来分析实验的有效性。两次PnP算法的位姿求取结果如表7和表8所示。求取平均值后与表6所求数据进行对比，误差结果如表9所示。

Figure 18.  Calculation of target pose in different scenes

半实物仿真试验结果显示，位姿误差小于指标要求。文中算法可以正确有效的对于目标点云进行提取和位置求取，并且能够稳定适用于实际环境之中。

激光雷达对目标进行扫描时，目标物与激光雷达的距离和采集点云数呈负相关；目标表面颜色对脉冲信号的反射强度影响较大。点云反射强度从高到低依次为白色、绿色、蓝色、红色、黑色^[21]。目标的表面粗糙度对点云数量和点云反射强度均无明显影响。采用参考文献[21]中提供的目标颜色对点云影响结果，假设白色时目标物的采集率为1，则不同颜色条件下对采集点云数量的影响如表10所示。在文中预设仿真条件下，分别在30 m、15 m和5 m处完成飞行目标点云采集并根据提取率在目标点云中取随机点的方式实现目标不同颜色下的取点模拟，统计不同颜色下采集点的数量，结果如表11所示。

由表11可知，影响点云采集数量的主要因素为激光雷达距离点云的距离。虽然目标表面颜色也对点云点数有所影响，但由于相差的点云数量过少，在ICP配准时可以忽略不计。

在文中设计算法中，采用改良ViBe算法对目标进行提取时会丢失一部分像素点，经图像形态学处理后可得到完整目标，在恢复完整目标时采用膨胀和腐蚀操作，会使得到的目标与真实目标尺寸略有差别；采用LSD直线检测算法和霍夫变换对图像特征点进行提取时会出现提取误差；采用改进的ICP算法对目标点云进行位姿估计，目标点云的数量影响配准的精度。

针对以上测量误差原因，采用控制变量法设置对照组与文中试验结果进行对比验证。分别设置两个对照组与表2中文中算法造成误差进行比较。A组实验条件为：物距30 m，手动提取图像特征点；B组实验条件为：物距5 m，手动提取图像特征点；

由文中算法与A组对比可知，采用手动提取图像特征点可以减小误差，但效果不佳；由文中算法与B组对比可知，当激光雷达与目标的距离缩短时，可以有效减少误差。

文中提出一种基于双模态信息融合飞行目标位姿估计算法，选择合适参数的基础上，可有效估计飞行目标的位姿信息。首先使用改进的ViBe算法对图像目标进行提取，较好的克服了传统ViBe算法光照变化不敏感问题，提高了搜索率降低了错误分类比；然后利用点云和图像融合完成对点云目标的快速提取；最后采用PnP算法获取点云间初始旋转平移矩阵，并采用基于IK-D Tree的ICP算法完成目标位姿估计，加快最近点搜索并提高了位姿估计精度。弥补了单源数据难以全面描述运动目标的缺陷，为飞行目标位姿估计提供了一个客观的解决方案。可应用于飞行目标落点精准预测和制导控制，具有较高军事应用价值。