CN110675418B - 一种基于ds证据理论的目标轨迹优化方法 - Google Patents

Abstract

本发明提供一种基于DS证据理论的目标轨迹优化方法，包括数据采集装置获取多目标信息，输出目标坐标位置、态姿、轨迹及高低分辨率图像集合；构建生成对抗网络重建仿真目标，输出仿真目标坐标位置、态姿、轨迹及真实目标的仿真高低分辨率图像集合；检测关键帧计算特征重复率，基于二维高低分辨率图像集合及三维目标RGB点云，构建字典判断回环，实现轨迹优化；建立样本矩阵表示输出轨迹，包括真实目标轨迹、生成对抗网络输出仿真目标轨迹、回环检测后的优化轨迹；利用DS证据理论基于空间特征、图像特征和轨迹信息对轨迹进行正确性判断，输出最优轨迹。本发明输出最优轨迹，为无人控制、目标识别和探测、区域检测、路径规划等提供精准信息。

Description

一种基于DS证据理论的目标轨迹优化方法

技术领域

本发明涉及人工智能及目标探测与识别、多传感器测量和环境感知等领域，尤其是涉及了一种基于DS证据理论的目标轨迹优化方法。

背景技术

对于人工智能及目标探测与识别、多传感器测量和环境感知技术的日臻成熟，对于目标探测与识别，环境感知是必要前提，而目标探测是环境感知的重要组成部分，常见的目标探测传感器如TOF测距图像传感器，通过发射波长各异的光波或声波，然后采集发射波的回波信号获取目标的距离、角度、反射强度、速度等信息，生成目标多维度图像，可以获取目标的距离数据，且不容易受到光照、烟雾、环境的影响，但无法感知目标的状态；相机获取更为丰富的色彩信息，识别长宽高和颜色等物体的基本状态属性，且具有更高的隐蔽性，但容易受到光照、烟雾、环境影响。

由上可知，不同类型的传感器具有不同的优点和弱点。传统的目标探测中采用大量的独立信息采集传感器，相互之间独立工作，各个传感系统采用多个处理器进行计算汇总，功耗大、效率低，采集的信息复杂而散乱甚至相冲突，降低了信息采集的可靠性。

轨迹优化是目标探测与识别，无人机、无人船导航和机器人技术中的重要问题，有广泛的应用场景：GPS导航、无人驾驶、无人飞行器飞行、计算机动画、路由问题、空间非合作目标和生活以及工业领域的很多方面。因此对轨迹规划问题的研究近年来成为一个研究热点。随着GPS、北斗等定位技术的快速发展，定位功能被广泛应用于目标探测与识别中，GPS、北斗等定位技术在应用时，由于电流层、比流层等环境因素的影响，生成的运动轨迹具有一定的误差。运动轨迹是根据一系列定位得到的位置点信息生成的。目前针对运动轨迹的优化方法主要有两种，一种是根据之前生成的运动轨迹，预测下一个位置点信息，然后与实际生成的位置点信息进行比对，从而纠正实际生成的位置点信息；另一种方法是滤波方法，如中值滤波、多点滤波等。由上可知，现有技术中对运动轨迹的优化主要采用预测法和滤波法，优化后的运动轨迹不够准确，与实际轨迹差距较大，虽然能寻找到最优航迹，但耗时长，不能满足实时规划的要求。

本发明通过激光雷达和双目立体视觉相机、RGB-D相机联合获取目标的信息，输出目标坐标位置、态姿、轨迹及高低分辨率图像集合；构建生成对抗网络重建生产仿真目标，包括第一生成器和第二生成器，三个局部鉴别器和一个全局鉴别器，输出仿真目标坐标位置、态姿、轨迹及仿真高低分辨率图像集合；回环检测修正目标轨迹，在二维层面上通过对同一目标的高低分辨率图像按照相似性计算划分构成分辨率字典，三维判断目标点云RGB划分构建字典以及位移和旋转度是否满足阈值，判断回环；基于回环帧对目标轨迹进行优化，输出优化轨迹；建立样本矩阵用于表示输出轨迹，包括由数据采集设备获取的真实目标轨迹、由生成对抗网络输出获取的仿真目标轨迹、经过回环检测后的优化轨迹；利用DS证据理论基于空间特征、图像特征和目标轨迹信息对轨迹进行正确性判断，输出最优轨迹。本发明输出最优轨迹，为无人控制、目标识别和探测、可行区域检测、路径规划等提供精准的信息。

发明内容

为解决现有技术对目标运动轨迹的优化不够准确，与实际轨迹差距较大等问题，本发明的目的在于提供一种基于DS证据理论的目标轨迹优化方法，以实现目标在运动过程中的最优轨迹的输出，在实时更新轨迹的同时寻找到最优轨迹；能进一步提升在线轨迹规划的速度，更有助于目标对环境改变做出及时响应。

本发明通过激光雷达和双目立体视觉相机、RGB-D相机联合获取目标的信息，输出目标坐标位置、态姿、轨迹及高低分辨率图像集合；构建生成对抗网络重建生产仿真目标，包括第一生成器和第二生成器，三个局部鉴别器和一个全局鉴别器，输出仿真目标坐标位置、态姿、轨迹及仿真高低分辨率图像集合；回环检测修正目标轨迹，在二维层面上通过对同一目标的高低分辨率图像按照相似性计算划分构成分辨率字典，三维判断目标点云RGB划分构建字典以及位移和旋转度是否满足阈值，判断回环；基于回环帧对目标轨迹进行优化，输出优化轨迹；建立轨迹样本空间矩阵，包括由数据采集设备获取的真实目标轨迹、由生成对抗网络输出获取的仿真目标轨迹、经过回环检测后的优化轨迹；利用DS证据理论基于空间特征、图像特征和目标轨迹信息对轨迹进行正确性判断，输出最优轨迹。本发明输出最优轨迹，为无人控制、目标识别和探测、可行区域检测、路径规划等提供精准的信息。

实现本发明目的的技术方案为：

(一)利用数据采集装置获取多目标信息；所述目标，为真实目标，包括目标的位置坐标、态姿、真实轨迹以及通过图像处理算法输出高低分辨率图像集合；

(二)构建生成对抗网络重建目标；所述目标为仿真目标，包括仿真目标的位置坐标、态姿、轨迹以及真实目标的仿真高低分辨率图像集合；仿真低分辨率图像集合与低分辨率图像集合产生的差值将作为弥补生成对抗网络的损失值；

(三)检测目标关键帧，判断回环帧，基于回环帧对目标轨迹进行优化，获取优化轨迹；

(四)建立样本矩阵用于表示目标的轨迹，包括：由数据采集设备获取的真实目标轨迹、由生成对抗网络输出获取的仿真目标轨迹、经过回环检测后的优化轨迹；

(五)利用DS证据理论对轨迹进行正确性判断，输出最优轨迹。

其中，所述(一)利用数据采集装置获取多目标信息，具体包括如下步骤：

步骤1，对目标进行检测，获取包含深度信息的深度图像、RGB彩色信息的彩色图像、RGB-D数据、三维RGB型点云数据；

通过激光雷达传感器获取目标位置、目标三维点云及点云坐标；通过双目立体视觉相机获取目标三维轮廓；通过RGB-D相机获取目标当前彩色信息和深度信息；

步骤2，对激光雷达传感器获取的点云数据进行预处理，基于双边滤波对点云进行降噪处理，同时对目标进行提取，将待配准目标从全局场景中分离；

步骤3，获取完整目标点云轮廓：通过基于点云的深度信息，利用NARF关键点提取算法对点云进行边缘检测，获得表面稳定而邻域变化大的边缘点，得到点云轮廓；将所述点云轮廓与所述由双目立体视觉相机获取的三维点云轮廓进行配准，生成完整的目标轮廓；

步骤4，关键点提取，由所述RGB-D相机获取的彩色信息，基于彩色信息对点云进行描述，通过点云RGB描述算子检测RGB关键点，对所述点云数据建立kd-tree以检测目标关键点，首先将目标的所有候选点按照方差进行排序，然后取方差最大的前10％的点构成目标候选关键点集合，对目标候选关键点集合中的点，在以阈值R为半径的区域进行搜索，保留区域中方差最大的点为关键点，直至提取出目标所有关键点；

步骤5，对目标点云进行粗配准和精配准，直至获取完整的目标信息；用标签对目标进行标记，添加时间戳，以获取时间同步的被标记的多个目标的数据序列。

进一步的，所述数据采集装置，使用双目立体视觉相机、激光雷达传感器和RGB-D相机，在不同的距离阶段使用不同设备获取目标；

当目标距离较远，利用双目立体视觉相机和RGB-D相机捕获目标，通过图像处理算法获得目标的方位角信息，接着根据方位角信息引导激光雷达传感器对目标进行定位扫描测距，获得目标的相对距离信息；由于距离较远，测距信息以激光测量信息为准，该阶段得到目标相对距离和方位信息，能够为下一阶段的位姿获取和目标跟踪做准备；

对于近距离阶段，利用激光雷达传感器、双目立体视觉相机和RGB-D相机实现目标的相对位姿测量，为导航实现必要的相对位姿；当光照条件不能满足视觉测量要求，利用激光雷达传感器对目标进行位姿测量；当光照条件满足视觉要求时，利用视觉和激光融合的方式进行位姿测量。

进一步的，所述点云的深度信息，对于同一目标，计算前后帧的相对位移和角度，以角度垂直方向作为点云的投影方向，获取点云深度信息，与所述RGB-D相机获取的点云深度信息进行加权处理，输出当前目标的点云深度信息；

所述获取目标，还包括对于同一目标的RGB图像进行处理，得到包含目标的高分辨率图像数据集和对应的包含目标低分辨率图像数据集。

其中，所述(二)构建生成对抗网络重建目标，具体包括：由于场景中可能出现多个目标的情况，而多个目标之间可能会发生遮挡，造成识别不清晰，因此利用生成对抗网络，将包含目标的连续多帧图像输入到生成对抗网络，获取仿真目标；所述仿真目标，还包括目标的坐标位置、态姿、轨迹以及包含目标的高低分辨率图像；

生成对抗网络中包含生成器和鉴别器，交替优化，使目标函数达到最优，在鉴别器网络、生成器网络中均使用Adam作为优化器，选择循环次数20000，批处理大小为32。

进一步的，所述生成器，包括第一生成器网络和第二生成器网络；

所述第一生成器网络，将包含由所述数据采集装置获取的真实目标的外部三维轮廓、内部三维点云、深度信息和彩色信息作为输入，经过3层卷积层、4层扩张卷积层、3层反卷积层以及最后卷积层，输出仿真目标；

所述第二生成器网络，将包含由所述数据采集装置获取的真实目标及所述真实目标彩色信息的高分辨图像和低分辨率图像作为输入，经过3层卷积层、6层残差层、3层反卷积层和最后卷积层，输出包含所述真实目标的仿真高分辨率图像集合和仿真低分辨率图像集合；

计算生成器损失：

其中，α、β和γ表示平衡因子，用于控制及平衡各项损失函数，

表示感知损失，LKJ表示三维点云坐标在卷积神经网络中的映射值，I′_i表示第i幅图像重建值，

表示目标重建时在训练过程中权值；F(·)表示学习映射函数；

表示分辨率损失，仿真低分辨率图像集合与低分辨率图像集合产生的差值将作为弥补生成对抗网络的损失值；

表示轮廓边缘损失，y_i和f_i表示第i幅低分辨率图像和第i幅高分辨率图像；Le_i和He_i分别表示第i幅低分辨率图像对应的边缘和第i幅高分辨率图像对应的边缘。

进一步的，所述鉴别器，包括局部鉴别器和全局鉴别器，

所述全局鉴别器，用于对包含所述真实目标的仿真高分辨率图像、低分辨率图像、包含所述仿真目标及所述仿真目标的轨迹的整体质量进行全局判断；

所述局部鉴别器，包括3个鉴别器网络，即第一鉴别器网络、第二鉴别器网络、第三鉴别器网络；

所述第一鉴别器网络，用于鉴别由所述数据采集装置获取的真实目标以及由所述第一生成器网络输出的仿真目标；所述仿真目标，包括目标的坐标位置、姿态形状；

所述第二鉴别器网络，用于鉴别包含所述真实目标的高分辨率图像集及其对应的低分辨率图像集，与由所述第二生成器输出的包含所述真实目标的仿真高分辨率图像集合及对应的低分辨率图像集合；

所述第三鉴别器网络，用于鉴别包含所述真实目标的高分辨率图像、对应的低分辨率图像以及真实目标融合后的目标数据，与包含目标的第二高分辨率图像、对应的第二低分辨率图像以及仿真目标融合后的仿真目标数据；

计算鉴别器损失：

L_D＝λ₁L₁+λ₂L₂+λ₃L₃+λ₄L₄         (2)

其中，λ₁、λ₂、λ₃和λ₄分别表示为与局部鉴别器损失L₁、L₂和L₃以及全局鉴别器损失L₄对应的权重，用于平衡不同损失对损失函数的影响；

在鉴别器网络的每个卷积层后增加dropout，dropout层将dropout率设为梯度更新的学习率，当生成器输出值损失大于预设阈值时，为保证模型在稳定的情况下保留原始信息的传递，丰富图像细节，设置梯度更新的学习率，公式如下：

其中，f(x)表示dropout的比例，n表示循环次数，b表示调节参数，当n＝0时b＝0，其余情况自行设定。

其中，所述(三)检测目标关键帧，判断回环帧，基于回环帧对目标轨迹进行优化，获取优化轨迹，具体包括：由于目标可能在相同帧时刻出现多个目标，如第t-1帧出现n个目标，而第t帧时刻出现n-1个目标，因此为获取同一目标的运动轨迹，首先检测出包含同一个目标的所有帧时刻，然后检测出关键帧；而对于仅出现单个目标，目标发生旋转、静止；或出现多个目标，多目标之间相对静止、旋转的情况，仅根据相邻帧数据的误差信息无法消除目标在运动过程中的出现的累计误差，因此利用回环检测定义额外的间隔更长的约束关系，所以检测回环帧，步骤具体如下：

步骤1，以当前帧时刻t为基础，在第1至t时刻获取目标的完整信息数据，包括目标三维点云及轮廓特征、彩色特征以及目标态姿、位置，对于同一目标保留其所有帧，组成关键帧参考集合；

步骤2，将第一帧预设关键帧，计算第2至t-1帧的包含目标的周围环境特征信息的特征重复率，若特征重复率不小于特征重复率预设值，保留符合要求的帧图像，构成候选关键帧集合，否则不作为候选关键帧集合；

从关键帧参考集合中选择任意一帧与当前帧进行比较，特征重复率阈值为当特征重复率大于25％且跟踪到20个点以上时，或当目标短时间旋转角度超过15°时，或当目标静止时每5秒产生一个关键帧，将生成一个候选关键帧；

同一目标的位移和旋转角度满足预设值，则将当前帧作为关键帧，存入候选关键帧集合，运动预设值为0.2-0.5之间，若满足运动预设值，则当前帧为新的关键帧：

其中，E表示运动值，ω₁、ω₂、ω₃表示目标运动过程距离、方位角和俯仰角的平衡权重，表示测量过程中对各测量值的信任程度；d表示距离变化量，θ表示方位角变化量，

表示俯仰角变化量，与平移和旋转角有关；

步骤3，从候选关键帧中选择帧，基于二维高分辨率图像、低分辨率图像以及三维RGB点云，通过kd-tree表达字典，构建点云区域块、高分辨率字典块、低分辨率字典块，计算相似性，构成关键帧集合：

基于二维，包含所述真实目标的高分辨率图像集合和对应的低分辨率图像集合，以及将包含所述真实目标的仿真高分辨率图像集合和对应的低分辨率图像集合，构成分辨率图像集合P，按照像素之间的颜色相似性进行组成图像块，将高分辨率图像块和低分辨率图像块分别进行训练，获取高分辨率字典和低分辨率字典，通过kd-tree表达字典，作为获取关键帧的重要判断因素之一，加强回环帧的检测；

高分辨率图像块组成高分辨率字典D_h，低分辨率图像块组成低分辨率字典D_l，由于由于高分辨率图像块在高分辨率字典D_h下，和低分辨率图像块在低分辨率字典D_l下存在相同的稀疏向量，通过联合训练，优化高分辨率字典和低分辨率字典：

其中，ε表示为了平衡稀疏度和图像块的精度，ε＝0.1，x表示高分辨率图像块，y表示低分辨率图像块，N和M分别表示在高分辨率和低分辨率图像矢量下的维度，P表示图像块的个数，p表示第p个图像块；

基于三维，对所述目标的XYZ-RGB点云数据建立kd-tree，XYZRGB点云数据由RGB-D数据经过转换后获得，以彩色特征计算像素之间是否相似，根据彩色相似性组成点云区域块，

其中，S表示(i,j,k)位置点云中心像素点与近邻位置(r,o,c)点云的相似性，I_M表示目标；B(i,j,k)、G(i,j,k)、R(i,j,k)表示提取以(i,j,k)为中心的目标三维点云块的彩色特征，Z是归一化因子；

步骤4，以第一个关键帧的坐标作为初始坐标，计算随后新增的关键帧相对于初始关键帧的位姿变换矩阵；当前帧为关键帧时，将当前帧与之前关键帧集合中的关键帧进行匹配，若当前关键帧已存在于关键帧集合中，则当前关键帧为回环帧，计算此时的位姿矩阵；

步骤5，根据回环帧对相应的位姿变换矩阵进行优化更新，调整当前关键帧，得到更新位姿变换矩阵；所述根据所述位姿变换矩阵确定每个目标对应的三维坐标，通过连续多个回环帧更新目标位姿，完成目标的轨迹优化。

其中，所述(四)建立轨迹样本空间矩阵，包括：由数据采集设备获取的真实目标轨迹、由生成对抗网络输出获取的仿真目标轨迹、经过回环检测后的优化轨迹；

其中，所述(五)利用DS证据理论对迹进行正确性判断，输出最优轨迹，具体包括：

(1)对目标实时数据进行记录；

(2)建立轨迹样本空间矩阵，D＝{C，G，Q}，C表示由数据采集设备获取的真实目标轨迹、G表示由生成对抗网络输出获取的仿真目标轨迹、Q表示经过回环检测后的优化轨迹；

(3)基于样本空间矩阵，对实时目标数据进行分类，按照聚类将特征分为空间特征信息、图像特征信息，目标轨迹信息；输出转换成概率得到证据m1、m2、m3，表示为m_i，基本概率赋值表示为m_i(A_i)；

所述空间特征信息，包括：形状、位置、点云等；所述目标轨迹信息，包括径距、高度、速度、加速度、航向角等；所述图像特征信息，包括：形状、纹理、颜色、波段等；

(4)运用D-S证据理论对m1、m2、m3进行合成，将目标轨迹情况作为基本命题A，将目标空间信息、图像信息以及目标轨迹信息作为基本证据；

进一步的，运用D-S证据理论对m1、m2、m3进行合成，其步骤如下：

首先计算基本概率：

其中，(1-∑Bel)表示可分配基本概率赋值；α表示置信度函数Bel与似然函数Pl对基本概率赋值分配的影响程度：

Δm_xyz(n)表示多个证据之间关于第n个特征指标的差，

表示三级最小差，

表示三级最大差，S(m_i)表示证据支持度；

其中，证据支持度：

证据支持度反映该证据被其他证据的支持程度，S(m_i)值越大，表明证据之间的距离越小，该证据的支持度就越大，D(m_i)是证据之间距离；

其中，S_max(m_i)表示证据的最大支持度，S_min(m_i)表示证据的最小支持度；

计算基本概率赋值：

m_i(A_i)＝S(m_i)‘′*

P(m_i)           (11)

最后合成：

输出合成概率结果，将值最大的作为最终优化轨迹。

本发明与现有技术相比，其显著优点为：

(1)通过激光雷达传感器、双目立体视觉相机和RGB-D相机获取目标三维点云数据、点云坐标以及三维轮廓，能够有效避免传统摄像头受距离、环境光照影响带来的目标检测不确定性和不稳定性。

双目立体视觉相机可以拓展单目相机的视场角，并对测量数据进行融合；其次，双目立体视觉相机互相备份，当其中一个出现故障时可以继续应用单目相机的相关算法进行测量，从而提高系统的可靠性；最后，双目立体视觉相机可以进行空间三维信息的恢复，进而完成目标的相对位姿测量；

激光雷达传感器在远距离和近距离都能提供精确的深度信息，并且对环境的光照条件不敏感。远距离时，激光扫描可以获取目标的距离信息和方位角信息，近距离时，利用目标三维点云图像计算目标相对位姿。通过自适应地调整扫描仪的视场范围、扫描分辨率和扫描模式，还可以在更短的扫描时间里获得更多的有效数据。

(2)改进生成对抗网络结构，包括第一生成器、第二生成器、第一鉴别器、第二鉴别器、第三鉴别器和全局鉴别器，进行目标点云数据、高分辨率图像以及低分辨率图像的内在潜在关联学习，生成仿真目标及轨迹，比传统深度学习网络效率更高，且生成网络和判别网络之间存在相互竞争，能够加速网络参数调整趋向更优结果。

(3)利用回环检测修正目标轨迹，对于出现的多个目标或者仅出现单个目标，首先检测目标周围场景的特征重复率，然后在二维层面通过对同一目标的高低分辨率图像按照相似性计算划分构成分辨率字典；三维层面目标的XYZ-RGB点云数据建立kd-tree，判断是否出现回环；根据位姿变换矩阵确定每个视频帧图像目标对应的三维坐标，通过连续多个回环帧更新目标位姿，完成目标轨迹优化。

(4)利用DS证据理论，对由数据采集设备获取的真实目标轨迹、由生成对抗网络输出获取的仿真目标轨迹、对经过回环检测后的优化目标轨迹进行轨迹判断，结合不同轨迹规划方法各自的优点，输出最优轨迹，规划时长更短、规划所得轨迹更优，能够为无人控制，如目标识别和检测，可行区域检测，路径规划等提供精准的信息。

本发明基于智能设备传感器数据融合和机器学习实现目标轨迹提取以及优化，不受天气、光线等环节因素的影响；通过将设备固定在各种设备上，如无人机、无人船、卫星；通过采集的大量数据，能够有效保证训练集的完备性和全面性；同时模型能够迭代更新优化，保证准确性和稳定性都相对较高。

附图说明

图1是本发明一种基于DS证据理论的目标轨迹优化方法的实现流程图。

图2是本发明一种基于DS证据理论的目标轨迹优化方法的目标点云处理流程图。

图3是本发明一种基于DS证据理论的目标轨迹优化方法的XYZRGB点云数据转换流程图。

图4是本发明一种基于DS证据理论的目标轨迹优化方法的生成对抗网络流程图。

图5是本发明一种基于DS证据理论的目标轨迹优化方法的回环检测流程图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互结合，下面结合附图和具体实施例对本发明作进一步详细说明。

图1是本发明一种基于DS证据理论的目标轨迹优化方法的实现流程图，主要包括：首先利用数据采集装置获取多目标信息，获取真实目标点云、位置坐标、态姿、轨迹以及输出高低分辨率图像集合；然后构建生成对抗网络重建目标，输出仿真目标的坐标位置、态姿、轨迹以及包含目标的仿真高低分辨率图像集合；检测目标关键帧，判断回环帧，基于回环帧对目标轨迹进行优化，获取优化轨迹；建立轨迹样本空间矩阵，包括由数据采集设备获取的真实目标轨迹、由生成对抗网络输出获取的仿真目标轨迹、经过回环检测后的优化轨迹；利用DS证据理论基于空间特征、图像特征和目标轨迹信息对轨迹进行正确性判断，输出最优轨迹。

其中，数据采集装置，包括使用双目立体视觉相机、激光雷达传感器和RGB-D相机，在不同距离阶段使用不同设备获取目标；

当目标距离较远，利用双目立体视觉相机和RGB-D相机捕获目标，通过图像处理算法获得目标的方位角信息，接着根据方位角信息引导激光雷达传感器对目标进行定位扫描测距，获得目标的相对距离信息；由于距离较远，测距信息以激光测量信息为准，该阶段得到目标相对距离和方位信息，能够为下一阶段的位姿获取和目标跟踪做准备；

对于近距离阶段，利用激光雷达传感器、双目立体视觉相机和RGB-D相机实现目标的相对位姿测量，为导航实现必要的相对位姿；当光照条件不能满足视觉测量要求，利用激光雷达传感器对目标进行位姿测量；当光照条件满足视觉要求时，利用视觉和激光融合的方式进行位姿测量。

其中，利用DS证据理论对迹进行正确性判断，输出最优轨迹，具体包括：

步骤1，对目标实时数据进行记录；

步骤2，建立轨迹的样本空间矩阵，D＝{C，G，Q}，C表示由数据采集设备获取的真实目标轨迹、G表示由生成对抗网络输出获取的仿真目标轨迹、Q表示经过回环检测后的优化轨迹；

步骤3，基于样本空间矩阵，对实时目标数据进行分类，按照聚类将特征分为空间特征信息、图像特征信息，目标轨迹信息；输出转换成概率得到证据m1、m2、m3，表示为m_i，基本概率赋值表示为m_i(A_i)；

所述空间特征信息，包括：形状、位置、点云等；所述目标轨迹信息，包括径距、高度、速度、加速度、航向角等；所述图像特征信息，包括：形状、纹理、颜色、波段等；

步骤4，运用D-S证据理论对m1、m2、m3进行合成，将目标轨迹情况作为基本命题A，将目标空间信息、图像信息以及目标轨迹信息作为基本证据；

首先计算基本概率：

其中，(1-∑Bel)表示可分配基本概率赋值；α表示置信度函数Bel与似然函数Pl对基本概率赋值分配的影响程度：

Δm_xyz(n)表示多个证据之间关于第n个特征指标的差，

表示三级最小差，

表示三级最大差，S(m_i)表示证据支持度；

其中，证据支持度：

证据支持度反映该证据被其他证据的支持程度，S(m_i)值越大，表明证据之间的距离越小，该证据的支持度就越大，D(m_i)是证据之间距离；

其中，S_max(m_i)表示证据的最大支持度，S_min(m_i)表示证据的最小支持度；

计算基本概率赋值：

m_i(A_i)＝S(m_i)‘′*

P(m_i)           (11)

最后合成：

输出合成概率结果，将值最大的作为最终优化轨迹。

图2是本发明一种基于DS证据理论的目标轨迹优化方法的目标点云处理流程图。其流程主要为：

步骤1，对目标进行检测，获取包含深度信息的深度图像、RGB彩色信息的彩色图像、RGB-D数据、三维RGB型点云数据；

通过激光雷达传感器获取目标位置、目标三维点云及点云坐标；通过双目立体视觉相机获取目标三维轮廓；通过RGB-D相机获取目标当前彩色信息和深度信息；

XYZ型点云反映物体表面的坐标信息，将其视为将物体表面离散化后得到的点集，XYZRGB型点云，相较XYZ型点云多出一个维度用于描述点的颜色信息，XYZRGB点云数据通过RGB-D数据进行转换，图3一种基于DS证据理论的目标轨迹优化方法的XYZRGB点云数据转换流程图所示，主要显示了通过RGB-D相机、激光雷达传感器和莪双目立体视觉相机获取深度图像和RGB图像，通过对齐得到RGB-D图片，再其转化为XYZRGB点云数据。

步骤2，对激光雷达传感器获取的点云数据进行预处理，使用双边滤波算法、邻域平均法滤波算法或者均值滤波算法进行滤波降噪；同时对目标进行提取，将待配准目标从全局场景中分离，使用深度图像分割算法、基于阈值的点云数据分割算法、采样一致性点云分割算法；

步骤3，获取完整目标点云轮廓：通过基于点云的深度信息，利用NARF关键点提取算法对点云进行边缘检测，获得表面稳定而邻域变化大的边缘点，得到点云轮廓；将所述点云轮廓与所述由双目立体视觉相机获取的三维点云轮廓进行配准，生成完整的目标轮廓；

计算点云的深度信息：对于同一目标，计算前后帧的相对位移和角度，以角度垂直方向作为点云的投影方向，获取点云深度信息，与所述RGB-D相机获取的点云深度信息进行加权处理，输出当前目标的点云深度信息；

步骤4，关键点提取，由所述RGB-D相机获取的彩色信息，基于彩色信息对点云进行描述，通过点云RGB描述算子检测RGB关键点，对所述点云数据建立kd-tree以检测目标关键点，首先将目标的所有候选点按照方差进行排序，然后取方差最大的前10％的点构成目标候选关键点集合，对目标候选关键点集合中的点，在以阈值R为半径的区域进行搜索，保留区域中方差最大的点为关键点，直至提取出目标所有关键点；

步骤5，对目标点云进行粗配准和精配准，直至获取完整的目标信息；用标签对目标进行标记，添加时间戳，以获取时间同步的被标记的多个目标的数据序列。

所述获取目标，还包括对于同一目标的RGB图像进行处理，得到包含目标的高分辨率图像数据集和对应的包含目标低分辨率图像数据集。

图4是本发明一种基于DS证据理论的目标轨迹优化方法的生成对抗网络流程图，主要包括生成器和鉴别器，生成器包括第一生成器和第二生成器，鉴别器包括局部鉴别器和全局鉴别器，局部鉴别器包括第一鉴别器、第二鉴别器和第三鉴别器；生成器和鉴别器交替优化，使目标函数达到最优，在鉴别器网络、生成器网络中均使用Adam作为优化器，选择循环次数20000，批处理大小为32。

利用生成对抗网络重建目标，由于场景中可能出现多个目标的情况，而多个目标之间可能会发生遮挡，造成识别不清晰，因此利用生成对抗网络，将包含目标的连续多帧图像输入到生成对抗网络，获取仿真目标；所述仿真目标，包括目标的坐标位置、态姿、轨迹以及包含目标的高低分辨率图像；

所述第一生成器网络，将包含由所述数据采集装置获取的真实目标的外部三维轮廓、内部三维点云、深度信息和彩色信息作为输入，经过3层卷积层、4层扩张卷积层、3层反卷积层以及最后卷积层，输出仿真目标；

进一步的，第一生成器网络，结构为3层卷积层、4层扩张卷积、3层反卷积以及最后卷积层；其中，所述3层卷积层的卷积核大小分别为7×7、5×5、3×3，步长为2，特征图数量分别为64、128、256；所述4层扩张卷积的卷积核大小分别为3×3、膨胀因子分别为2、4、8、16，步长为1，特征图数量分别为256、256、256、256；所述3层反卷积层的卷积核3×3，步长为2，特征图数量分别为128、64、32，经过3层反卷积层进行填充；所述最后卷积层的卷积核大小为3×3、卷积步长为1，特征图数量为3；在每层卷积层输出加入BN层和lReLu层，最后一层卷积的输出采用Tanh函数激活，形成仿真目标数据；

所述第二生成器网络，将包含由所述数据采集装置获取的真实目标及所述真实目标彩色信息的高分辨图像和低分辨率图像作为输入，经过3层卷积层、6层残差层、3层反卷积层和最后卷积层，输出包含所述真实目标的仿真高分辨率图像集合和仿真低分辨率图像集合；

进一步的，第二生成器网络，结构为3层卷积层、6层残差层、3层反卷积层和最后卷积层；其中，所述3层卷积层的卷积核大小分别为7×7、5×5、3×3，特征图数量分别为64、128、256；所述6层残差层中的每个残差层包括两个卷积层和残差连接，卷积核大小均为3×3，特征图数量均为256；所述3层反卷积层的卷积核大小均为3×3，特征图数量分别为256、128、64；所述最后卷积层的卷积核大小为3×3，步长为2，特征图数量为3；第二生成器网络的每个卷积层随后也包含一个BN层和lReLu激活层，最后一层为Tanh函数激活函数。

计算生成器损失：

其中，α、β和γ表示平衡因子，用于控制及平衡各项损失函数，

表示感知损失，LKJ表示三维点云坐标在卷积神经网络中的映射值，I′_i表示第i幅图像重建值，

表示目标重建时在训练过程中权值；F(·)表示学习映射函数；

表示分辨率损失，仿真低分辨率图像集合与低分辨率图像集合产生的差值将作为弥补生成对抗网络的损失值；

表示轮廓边缘损失，y_i和f_i表示第i幅低分辨率图像和第i幅高分辨率图像；Le_i和He_i分别表示第i幅低分辨率图像对应的边缘和第i幅高分辨率图像对应的边缘。

所述全局鉴别器，用于对包含所述真实目标的仿真高分辨率图像、低分辨率图像、包含所述仿真目标及所述仿真目标的轨迹的整体质量进行全局判断；

所述第一鉴别器网络，用于鉴别由所述数据采集装置获取的真实目标以及由所述第一生成器网络输出的仿真目标；所述仿真目标，包括目标的坐标位置、姿态形状；

所述第二鉴别器网络，用于鉴别包含所述真实目标的高分辨率图像集及其对应的低分辨率图像集，与由所述第二生成器输出的包含所述真实目标的仿真高分辨率图像集合及对应的低分辨率图像集合；

所述第三鉴别器网络，用于鉴别包含所述真实目标的高分辨率图像、对应的低分辨率图像以及真实目标融合后的目标数据，与包含目标的第二高分辨率图像、对应的第二低分辨率图像以及仿真目标融合后的仿真目标数据；

全局鉴别器，采用5层卷积大小为5x5，步长为2的卷积层，输出通道数分别为64，128，256，512，512；所述局部鉴别器，采用6层卷积大小为5x5，步长为2的卷积层，输出通道数分别为64，128，256，512，512，512；通过全连接层将局部鉴别器和全局鉴别器的输出结果进行融合，作为最终的输出结果；在鉴别器的最后一层卷积层中，使用Sigmoid函数作为卷积层的激活函数，其余卷积层使用LRelu作为激活函数，卷积层后接BN操作，使用批量规范处理。

计算鉴别器损失：

L_D＝λ₁L₁+λ₂L₂+λ₃L₃+λ₄L₄          (2)

其中，λ₁、λ₂、λ₃和λ₄分别表示为与局部鉴别器损失L₁、L₂和L₃以及全局鉴别器损失L₄对应的权重，用于平衡不同损失对损失函数的影响；

在鉴别器网络的每个卷积层后增加dropout，dropout层将dropout率设为梯度更新的学习率，当生成器输出值损失大于预设阈值时，为保证模型在稳定的情况下保留原始信息的传递，丰富图像细节，设置梯度更新的学习率，公式如下：

其中，f(x)表示dropout的比例，n表示循环次数，b表示调节参数，当n＝0时b＝0，其余情况自行设定。

图5是本发明一种基于DS证据理论的目标轨迹优化方法的回环检测流程图，主要显示了通过回环检测实现轨迹的优化，由于目标的不唯一性，相同帧时刻可能出现多个目标，如第t-1帧出现n个目标，而第t帧时刻出现n-1个目标，因此为获取同一目标的运动轨迹，首先检测出包含同一个目标的所有帧时刻，然后检测关键帧；而对于仅出现单个目标，目标发生旋转、静止；或出现多个目标，多目标之间相对静止、旋转的情况，仅根据相邻帧数据的误差信息无法消除目标在运动过程中的出现的累计误差，因此利用回环检测定义额外的间隔更长的约束关系，所以检测回环帧，步骤具体如下：

步骤1，以当前帧时刻t为基础，在第1至t时刻获取目标的完整信息数据，包括目标三维点云及轮廓特征、彩色特征以及目标态姿、位置，对于同一目标保留其所有帧，组成关键帧参考集合；

步骤2，将第一帧预设关键帧，计算第2至t-1帧的包含目标的周围环境特征信息的特征重复率，若特征重复率不小于特征重复率预设值，保留符合要求的帧图像，构成候选关键帧集合，否则不作为候选关键帧集合；

从关键帧参考集合中选择任意一帧与当前帧进行比较，特征重复率阈值为当特征重复率大于25％且跟踪到20个点以上时，或当目标短时间旋转角度超过15°时，或当目标静止时每5秒产生一个关键帧，将生成一个候选关键帧；

同一目标的位移和旋转角度满足预设值，则将当前帧作为关键帧，存入候选关键帧集合，运动预设值为0.2-0.5之间，若满足运动预设值，则当前帧为新的关键帧：

其中，E表示运动值，ω₁、ω₂、ω₃表示目标运动过程距离、方位角和俯仰角的平衡权重，表示测量过程中对各测量值的信任程度；d表示距离变化量，θ表示方位角变化量，

表示俯仰角变化量，与平移和旋转角有关；

步骤3，从候选关键帧中选择帧，基于二维高分辨率图像、低分辨率图像以及三维RGB点云，通过kd-tree表达字典，构建点云区域块、高分辨率字典块、低分辨率字典块，计算相似性，构成关键帧集合：

基于二维，将包含所述真实目标的高分辨率图像集合和对应的低分辨率图像集合，以及将包含所述真实目标的仿真高分辨率图像集合和对应的低分辨率图像集合，构成分辨率图像集合P，按照像素之间的颜色相似性进行组成图像块，将高分辨率图像块和低分辨率图像块分别进行训练，获取高分辨率字典和低分辨率字典，通过kd-tree表达字典，作为获取关键帧的重要判断因素之一；

高分辨率图像块组成高分辨率字典D_h，低分辨率图像块组成低分辨率字典D_l，由于由于高分辨率图像块在高分辨率字典D_h下，和低分辨率图像块在低分辨率字典D_l下存在相同的稀疏向量，通过联合训练，优化高分辨率字典和低分辨率字典：

其中，ε表示为了平衡稀疏度和图像块的精度，ε＝0.1，x表示高分辨率图像块，y表示低分辨率图像块，N和M分别表示在高分辨率和低分辨率图像矢量下的维度，P表示图像块的个数，p表示第p个图像块；

基于三维，对所述目标的XYZ-RGB点云数据建立kd-tree，XYZRGB点云数据由RGB-D数据经过转换后获得，以彩色特征计算像素之间是否相似，根据彩色相似性组成点云区域块，

其中，S表示(i,j,k)位置点云中心像素点与近邻位置(r,o,c)点云的相似性，I_M表示目标；B(i,j,k)、G(i,j,k)、R(i,j,k)表示提取以(i,j,k)为中心的目标三维点云块的彩色特征，Z是归一化因子；

步骤4，以第一个关键帧的坐标作为初始坐标，计算随后新增的关键帧相对于初始关键帧的位姿变换矩阵；当前帧为关键帧时，将当前帧与之前关键帧集合中的关键帧进行匹配，若当前关键帧已存在于关键帧集合中，则当前关键帧为回环帧，计算此时的位姿矩阵；

步骤5，根据回环帧对相应的位姿变换矩阵进行优化更新，调整当前关键帧，得到更新位姿变换矩阵；所述根据所述位姿变换矩阵确定每个目标对应的三维坐标，通过连续多个回环帧更新目标位姿，完成目标的轨迹优化。

对于本领域技术人员，本发明不限制于上述实施例的细节，在不背离本发明的精神和范围的情况下，能够以其他具体形式实现本发明。此外，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围，这些改进和变型也应视为本发明的保护范围。因此，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

Claims (8)

1.一种基于DS证据理论的目标轨迹优化方法，其特征在于，主要包括：

(一)利用数据采集装置获取多目标信息；所述目标，为真实目标，包括目标的位置坐标、态姿、真实轨迹以及通过图像处理算法输出高低分辨率图像集合；

(二)构建生成对抗网络重建目标；所述目标为仿真目标，包括仿真目标的位置坐标、态姿、轨迹以及真实目标的仿真高低分辨率图像集合；仿真低分辨率图像集合与低分辨率图像集合产生的差值将作为弥补生成对抗网络的损失值；

(三)检测目标关键帧，判断回环帧，基于回环帧对目标轨迹进行优化，获取优化轨迹；

(四)建立样本矩阵用于表示目标的轨迹，包括：由数据采集设备获取的真实目标轨迹、由生成对抗网络输出获取的仿真目标轨迹、经过回环检测后的优化轨迹；

(五)利用DS证据理论对轨迹进行正确性判断，输出最优轨迹；

所述(五)利用DS证据理论对迹进行正确性判断，输出最优轨迹，具体包括：

(1)对目标实时数据进行记录；

(2)建立轨迹的样本空间矩阵，D＝{C，G，Q}，C表示由数据采集设备获取的真实目标轨迹、G表示由生成对抗网络输出获取的仿真目标轨迹、Q表示经过回环检测后的优化轨迹；

(3)基于样本空间矩阵，对实时目标数据进行分类，按照聚类将特征分为空间特征信息、图像特征信息，目标轨迹信息；输出转换成概率得到证据m1、m2、m3，表示为m_i；

所述空间特征信息，包括：形状、位置、点云；所述目标轨迹信息，包括径距、高度、速度、加速度、航向角；所述图像特征信息，包括：形状、纹理、颜色、波段；

(4)运用D-S证据理论对m1、m2、m3进行合成，将目标优化轨迹情况作为基本命题A，将目标空间信息、图像信息以及目标轨迹信息作为基本证据。

2.基于权利要求1所述的基于DS证据理论的目标轨迹优化方法，其特征在于，所述(一)利用数据采集装置获取多目标信息，具体包括如下步骤：

步骤1，对目标进行检测，获取包含深度信息的深度图像、RGB彩色信息的彩色图像、RGB-D数据、三维RGB型点云数据；

通过激光雷达传感器获取目标位置、目标三维点云及点云坐标；通过双目立体视觉相机获取目标三维轮廓；通过RGB-D相机获取目标当前彩色信息和深度信息；

步骤2，对激光雷达传感器获取的点云数据进行预处理，基于双边滤波对点云进行降噪处理，同时对目标进行提取，将待配准目标从全局场景中分离；

步骤3，获取完整目标点云轮廓：通过基于点云的深度信息，利用NARF关键点提取算法对点云进行边缘检测，获得表面稳定而邻域变化大的边缘点，得到点云轮廓；将所述点云轮廓与由双目立体视觉相机获取的三维点云轮廓进行配准，生成完整的目标轮廓；

步骤4，关键点提取，由所述RGB-D相机获取的彩色信息，基于彩色信息对点云进行描述，通过点云RGB描述算子检测RGB关键点，对所述点云数据建立kd-tree以检测目标关键点，首先将目标的所有候选点按照方差进行排序，然后取方差最大的前10％的点构成目标候选关键点集合，对目标候选关键点集合中的点，在以阈值R为半径的区域进行搜索，保留区域中方差最大的点为关键点，直至提取出目标所有关键点；

步骤5，对目标点云进行粗配准和精配准，直至获取完整的目标信息；用标签对目标进行标记，添加时间戳，以获取时间同步的被标记的多个目标的数据序列。

3.基于权利要求2所述的基于DS证据理论的目标轨迹优化方法，其特征在于，所述数据采集装置，包括使用双目立体视觉相机、激光雷达传感器和RGB-D相机，在不同距离阶段使用不同设备获取目标；

当目标距离较远，利用双目立体视觉相机和RGB-D相机捕获目标，通过图像处理算法获得目标的方位角信息，接着根据方位角信息引导激光雷达传感器对目标进行定位扫描测距，获得目标的相对距离信息；由于距离较远，测距信息以激光测量信息为准，该阶段得到目标相对距离和方位信息，能够为下一阶段的位姿获取和目标跟踪做准备；

对于近距离阶段，利用激光雷达传感器、双目立体视觉相机和RGB-D相机实现目标的相对位姿测量，为导航实现必要的相对位姿；当光照条件不能满足视觉测量要求，利用激光雷达传感器对目标进行位姿测量；当光照条件满足视觉要求时，利用视觉和激光融合的方式进行位姿测量。

4.基于权利要求2所述的基于DS证据理论的目标轨迹优化方法，其特征在于，所述点云的深度信息，对于同一目标，计算前后帧的相对位移和角度，以角度垂直方向作为点云的投影方向，获取点云深度信息，与所述RGB-D相机获取的点云深度信息进行加权处理，输出当前目标的点云深度信息；

获取目标信息，还包括对于同一目标的RGB图像进行处理，得到包含目标的高分辨率图像数据集和对应的包含目标低分辨率图像数据集。

5.基于权利要求1所述的基于DS证据理论的目标轨迹优化方法，其特征在于，所述(二)构建生成对抗网络重建目标，具体包括：由于场景中可能出现多个目标的情况，而多个目标之间可能会发生遮挡，造成识别不清晰，因此利用生成对抗网络，将包含目标的连续多帧图像输入到生成对抗网络，获取仿真目标；所述仿真目标，还包括目标的坐标位置、态姿、轨迹以及包含目标的高低分辨率图像；

生成对抗网络中包含生成器和鉴别器，交替优化，使目标函数达到最优，在鉴别器网络、生成器网络中均使用Adam作为优化器，选择循环次数20000，批处理大小为32。

6.基于权利要求5所述的基于DS证据理论的目标轨迹优化方法，其特征在于，所述生成器，包括第一生成器网络和第二生成器网络，

所述第一生成器网络，将包含由所述数据采集装置获取的真实目标的外部三维轮廓、内部三维点云、深度信息和彩色信息作为输入，经过3层卷积层、4层扩张卷积层、3层反卷积层以及最后卷积层，输出仿真目标；

所述第二生成器网络，将包含由所述数据采集装置获取的真实目标及真实目标彩色信息的高分辨图像和低分辨率图像作为输入，经过3层卷积层、6层残差层、3层反卷积层和最后卷积层，输出包含所述真实目标的仿真高分辨率图像集合和仿真低分辨率图像集合；

计算生成器损失。

7.基于权利要求6所述的基于DS证据理论的目标轨迹优化方法，其特征在于，所述鉴别器，包括局部鉴别器和全局鉴别器，

所述全局鉴别器，用于对包含所述真实目标的仿真高分辨率图像、低分辨率图像、包含所述仿真目标及所述仿真目标的轨迹的整体质量进行全局判断；

所述局部鉴别器，包括3个鉴别器网络，即第一鉴别器网络、第二鉴别器网络、第三鉴别器网络；

所述第一鉴别器网络，用于鉴别由所述数据采集装置获取的真实目标以及由所述第一生成器网络输出的仿真目标；所述仿真目标，包括目标的坐标位置、姿态形状；

所述第二鉴别器网络，用于鉴别包含所述真实目标的高分辨率图像集及其对应的低分辨率图像集，与由第二生成器输出的包含所述真实目标的仿真高分辨率图像集合及对应的低分辨率图像集合；

其中，所述第三鉴别器网络，用于鉴别包含所述真实目标的高分辨率图像、对应的低分辨率图像以及真实目标融合后的目标数据，与包含目标的第二高分辨率图像、对应的第二低分辨率图像以及仿真目标融合后的仿真目标数据；

计算鉴别器损失：

L_D＝λ₁L₁+λ₂L₂+λ₃L₃+λ₄L₄                                   (2)

其中，λ₁、λ₂、λ₃和λ₄分别表示为与局部鉴别器损失L₁、L₂和L₃以及全局鉴别器损失L₄对应的权重，用于平衡不同损失对损失函数的影响；

在鉴别器网络的每个卷积层后增加dropout，dropout层将dropout率设为梯度更新的学习率，当生成器输出值损失大于预设阈值时，为保证模型在稳定的情况下保留原始信息的传递，丰富图像细节，设置梯度更新的学习率，公式如下：

其中，f(x)表示dropout的比例，n表示循环次数，b表示调节参数，当n＝0时b＝0，其余情况自行设定。

8.基于权利要求1所述的基于DS证据理论的目标轨迹优化方法，其特征在于，所述(三)检测目标关键帧，判断回环帧，基于回环帧对目标轨迹进行优化，获取优化轨迹，具体包括：目标可能在相同帧时刻出现多个目标，如第t-1帧出现n个目标，而第t帧时刻出现n-1个目标，因此为获取同一目标的运动轨迹，首先检测出包含同一个目标的所有帧时刻，然后检测出关键帧；而对于仅出现单个目标，目标发生旋转、静止；或出现多个目标，多目标之间相对静止、旋转的情况，仅根据相邻帧数据的误差信息无法消除目标在运动过程中的出现的累计误差，因此利用回环检测定义额外的间隔更长的约束关系，所以检测回环帧，步骤具体如下：

步骤1，以当前帧时刻t为基础，在第1至t时刻获取目标的完整信息数据，包括目标三维点云及轮廓特征、彩色特征以及目标态姿、位置，对于同一目标保留其所有帧，组成关键帧参考集合；

步骤2，将第一帧预设关键帧，计算第2至t-1帧的包含目标的周围环境特征信息的特征重复率，若特征重复率不小于特征重复率预设值，保留符合要求的帧图像，构成候选关键帧集合，否则不作为候选关键帧集合；

从关键帧参考集合中选择任意一帧与当前帧进行比较，特征重复率阈值为当特征重复率大于25％且跟踪到20个点以上时，或当目标短时间旋转角度超过15°时，或当目标静止时每5秒产生一个关键帧，将生成一个候选关键帧；

同一目标的位移和旋转角度满足预设值，则将当前帧作为关键帧，存入候选关键帧集合，运动预设值为0.2-0.5之间，若满足运动预设值，则当前帧为新的关键帧；

步骤3，从候选关键帧中选择帧，基于二维高分辨率图像、低分辨率图像以及三维RGB目标点云，通过kd-tree表达字典，构建点云区域块、高分辨率字典块、低分辨率字典块，计算相似性，构成关键帧集合：

基于二维，包含所述真实目标的高分辨率图像集合和对应的低分辨率图像集合，以及将包含所述真实目标的仿真高分辨率图像集合和对应的低分辨率图像集合，构成分辨率图像集合P，按照像素之间的颜色相似性进行组成图像块，将高分辨率图像块和低分辨率图像块分别进行训练，获取高分辨率字典和低分辨率字典，通过kd-tree表达字典，作为获取关键帧的重要判断因素之一，加强回环帧的检测；

高分辨率图像块组成高分辨率字典D_h，低分辨率图像块组成低分辨率字典D_l，由于高分辨率图像块在高分辨率字典D_h下，和低分辨率图像块在低分辨率字典D_l下存在相同的稀疏向量，通过联合训练，优化高分辨率字典和低分辨率字典；

基于三维，对所述目标的XYZ-RGB点云数据建立kd-tree，XYZRGB点云数据由RGB-D数据经过转换后获得，以彩色特征计算像素之间是否相似，根据彩色相似性组成点云区域块；

步骤4，以第一个关键帧的坐标作为初始坐标，计算随后新增的关键帧相对于初始关键帧的位姿变换矩阵；当前帧为关键帧时，将当前帧与之前关键帧集合中的关键帧进行匹配，若当前关键帧已存在于关键帧集合中，则当前关键帧为回环帧，计算此时的位姿矩阵；

步骤5，根据回环帧对相应的位姿变换矩阵进行优化更新，调整当前关键帧，得到更新位姿变换矩阵；所述根据所述位姿变换矩阵确定每个目标对应的三维坐标，通过连续多个回环帧更新目标位姿，完成目标的轨迹优化。

Images