CN115166716B - 一种基于多元数据融合的地面无人平台跟踪方法 - Google Patents

Abstract

本发明属于地面无人平台环境感知技术领域，具体涉及一种基于多元数据融合的地面无人平台跟踪方法。构建目标运动学模型，以期望跟踪目标的位置、速度、角度和角速度作为状态向量，根据运动特征和过程噪声，构建非线性状态模型；选择激光雷达和毫米波雷达作为物理传感器，根据地面无人平台的定位定向数据对激光雷达的数据进行畸变校正，利用体素滤波、直通滤波、最小分割、欧式聚类等实现感兴趣区域的提取；基于非线性的状态方程，选择无迹卡尔曼滤波对激光雷达和毫米波雷达进行数据融合；选择目标的位置作为地面无人平台的任务路径点，通过局部路径规划、速度剖面生成、纯跟踪控制等实现地面无人平台的横向行驶和纵向行驶。

Description

一种基于多元数据融合的地面无人平台跟踪方法

技术领域

本发明属于地面无人平台环境感知技术领域，具体涉及一种基于多元数据融合的地面无人平台跟踪方法。

背景技术

地面无人平台首次执行陌生任务时，经常需要跟踪引导人员或车辆进行机动行驶。核心是地面无人平台能够实时感知目标的空间位置。随着深度学习的兴起，二维目标检测技术正蓬勃发展，在监控领域得到了广泛应用，然而不能提供深度特征，无法直接用于地面无人平台的自主导航系统。基于深度学习的激光雷达检测技术虽然能够提供精确的三维目标检测轮廓，然而却受到显卡加速性能和激光雷达线束的制约。

发明内容

针对现有技术的缺陷，本发明目的在于提供一种基于多元数据融合的地面无人平台跟踪方法，该方法针对人员或车辆跟踪问题，采用激光雷达和毫米波雷达融合技术，对待跟踪的目标进行实时检测，以目标的位置作为自主导航的任务点，通过局部路径规划器、纯跟踪控制进行自主机动行驶。

本发明的技术解决方案是：

一种基于多元数据融合的地面无人平台跟踪方法，该方法的步骤包括：

第一步，采集地面无人平台的运动信息，运动信息包括地面无人平台的线速度和地面无人平台的角速度，并将采集到的线速度和角速度从全局坐标系转化到激光雷达坐标系；

第二步，实时获取待跟踪目标的激光雷达点云数据和毫米波雷达数据；

第三步，使用第一步采集到的地面无人平台的运动信息对第二步的每帧点云数据进行畸变补偿，得到补偿后的点云数据；

第四步，采用体素滤波方法对第三步得到的补偿后的点云数据进行降采样处理，在保留点云的几何特征情况下尽可能降低点云的数据量，并对噪声点进行剔除，具体方法为：首先创建一个尺寸为0.02m*0.02m*0.02m的三维体素网络，对补偿后的点云数据进行网格划分，利用每个网格内所有点云的重心近似代替网格内其他的点，每个网格的点云重心计算公式表述为：

式中，h为每个网格内点云的数量；(x_i，y_i，z_i)为每个网格内第i个点云的坐标；(x_center，y_center，z_center)为每个网格内点云的重心坐标；

第五步，对待跟踪目标的位置进行初始化，然后根据待跟踪目标的中心点坐标和半径，采用最小分割算法对输入点云的前景(属于目标的点云)和背景(不属于目标的点云)进行分割，进行分割的方法为：

(1)在每帧点云数据中构建点与点之间的拓扑关系图，每个顶点通过边与源点和汇聚点相连，并且也与自身邻域范围内的顶点相连，如图1；

(2)根据点与点之间的欧式距离构造权值为：

式中，dist为点间距离，距离越远，边被切割的可能性就越大；σ为权函数，取0.2m。

(3)设置前景和背景惩罚函数，用于缩小目标范围。前景为顶点与源点相连边的权值，设置为常量0.5。背景为顶点与汇聚点相连边的权值backgroundPenalty由公式定义为：

式中，radius为物体水平半径，取0.5m；(x,y)为点云坐标；(CenterX,CenterY)为目标中心坐标；distanceToCenter为点到目标中心的距离。

(4)基于点间的权值，对点云中的前景和背景进行分割，通过欧式聚类对前景点云进行聚类，获得目标的中心位置

第六步，构建待跟踪目标的运动学模型，具体步骤为：

根据待跟踪目标的位置、速度、角度和角速度构建5维的状态量；

式中，(p_x，p_y)为目标在x、y方向上的位置；v为目标的速度；ψ为目标的朝向；为目标的角速度。

对状态量进行求导和积分，得到状态方程为：

式中，Δt为时间差；x_k、x_k+1分别为k、k+1时刻的状态向量。

根据公式可知，若ψ_k＝0时，将使得结果趋于无穷大，即无法解决目标沿着直线行驶问题，因此对此种情况单独考虑：

确定完状态方程的固定项后，还需要考虑过程噪声项：

采用二维噪声向量对过程模型进行描述，不确定性v_k由两个独立的标量过程噪声构成，第一个过程噪声是纵向加速度噪声v_a，k，可以被看作是随机分布的白噪声，均值为0，方差为取0.01。

第二个过程噪声是角加速度噪声可以被看作是随机分布的白噪声，均值为0，方差为取0.01。

由于纵向加速度和角加速度均会对位置p_x、p_y产生影响。通常情况下，目标的角速度变化不大，忽略角加速度，噪声对位置的影响可表征为：

噪声对速度、角度、角速度的影响可表征为：

待跟踪目标的运动学模型为：

第七步，由于待跟踪目标的运动学模型是非线性的，采用无迹卡尔曼滤波对待跟踪目标位置进行预测，具体方法为：首先对当前待跟踪目标位置的分布进行采样，保障采样点与分布均值的距离是标准差的整倍数，一般情况下，n维高斯分布选择2n+1个采样点，第一点为高斯分布的均值，其他点关于均值对称；

式中，μ为均值；λ为超参数，取λ＝3-n＝-2；λ越大，采样点距离高斯分布的均值越远，λ越小，采样点距离高斯分布的均值越近。本发明构建的目标运动学模型中，噪声也能对模型产生分线性影响，因此状态方程的协方差矩阵P可表征为：

式中，P′为目标运动学模型的5x5协方差矩阵，元素取0.01的单位矩阵；Q为噪声的协方差矩阵。

然后根据待跟踪目标运动学模型，利用非线性函数g(·)(即状态方程)对每一个采样点进行预测：

χ_k+1＝g(χ_k,μ_k) (17)

式中，χ_k+1为k+1时刻预测后的采样点；g(·)为状态方程。

计算每个采样点的权重，权重公式可以表征为：

根据每个采样点的权重计算新分布的均值x_k+1∣k和方差P_k+1∣k：

通过公式(19)获得目标位置预测值。

第八步，由于激光雷达从预测状态空间到测量空间为线性变换，可通过线性函数H_l将预测的状态空间转化为测量的状态空间：

式中，Z_k+1∣k为转换后的测量空间；x_k+1∣k为预测后的状态量；R为测量噪声。

由于毫米波雷达预测状态空间到测量空间为非线性变换，可通过非线性函数H_r将预测空间转换到测量空间：

式中，r为目标与毫米波雷达的距离，θ为目标和毫米波雷达之间的夹角，r’为目标与毫米波雷达距离的变化率。

预测无迹变换后的先验估计均值z_k+1∣k、先验估计协方差矩阵S_k+1∣k表述为：

状态空间和测量空间的交叉关联矩阵表述为：

计算卡尔曼增益：

分别根据激光雷达最小分割、欧式聚类的测量结果、毫米波输出的结果z_k+1(即目标中心点坐标)，计算后验状态量x_k+1∣k+1和后验估计协方差矩阵P_k+1∣k+1，具体方法为：

P_k+1∣k+1＝P_k+1∣k-K_k+1∣kS_k+1∣k (27)

x_k+1∣k+1＝x_k+1∣k+K_k+1∣k(z_k+1-z_k+1∣k) (28)

通过公式(28)对目标位置进行更新。

第九步，根据待跟踪目标的位置和地面无人平台的自身定位，在线生成一簇备选路径，路径模型采用三次贝塞尔曲线，该曲线具有连续的一阶和二阶导数，确保了所规划的路径是无人平台可以执行的，对每条备选路径成本、最大可允许曲率和与全局路径的偏置距离进行加权计算，获得每条备选路径的评价值，选择最小的评价值作为最优路径；

J_i＝ω₁c_i+w₂k_imax+ω₃Δd_i (29)

式中：c_i为第i条备选路径的通过成本；k_imax为第i条备选路径的最大曲率；Δd_i第i条备选路径的偏置距离；ω₁、ω₂、ω₃分别为通过成本、最大曲率和偏置距离的权重。

第十步，根据最优路径规划轨迹上各点的期望速度，对于最优路径，设定末端点的目标速度为零，而从当前速度到零需要经历三个阶段，即分为加速行驶、匀速行驶和减速行驶三个阶段，其中加速阶段对应从当前速度vC加速到期望速度vD，减速阶段对应从期望速度vD到速度零，其它为匀速阶段，根据平台的加速度和减速度，可以计算三个阶段对应的轨迹长度，以此得到速度剖面。

第十一步，采用纯追踪算法来计算运动控制指令，包括平台行驶速度v和角速度w，其中，(x_v，y_v)为平台位置，(x_G，y_G)为全局路径上目标点位置，O为平台瞬时转向中心，R为转向半径，L为平台与目标点之间的距离，即预瞄距离，α为平台航向与目标位置之间的偏差角；

地面无人平台航向与待跟踪目标位置的偏差角为：

由圆弧和平台航向相切，得到圆弧对应的圆心角为：

∠GOS＝2α (31)

由此，得到在三角形ΔGOS中有如下关系：

因此根据最优路径输出的控制速度v，可以求得地面无人平台的角速度为：

第十二步，考虑到无人巡逻车在低速场景下运动，忽略轮胎侧偏角，转向关系可以近似为阿克曼转向几何模型，结合公式，小转角情况下，左右轮的转角为：

第十三步，根据输入的速度剖面和左右轮的转向角，底盘控制器能够输出轮毂电机的转矩，电动助力转向系统(Electric Power Steering,EPS)能够输出转向电机的转矩，以此实现地面平台的跟随行驶，包括纵向运动和横向运动。

有益效果

(1)本发明的方法通过构建目标运动学模型，以期望跟踪目标的位置、速度、角度和角速度作为状态向量，根据运动特征和过程噪声，构建非线性状态模型；选择激光雷达和毫米波雷达作为物理传感器，根据地面无人平台的定位定向数据对激光雷达的数据进行畸变校正，利用体素滤波、直通滤波、最小分割、欧式聚类等实现感兴趣区域的提取；基于非线性的状态方程，选择无迹卡尔曼滤波对激光雷达和毫米波雷达进行数据融合；选择目标的位置作为地面无人平台的任务路径点，通过局部路径规划、速度剖面生成、纯跟踪控制等实现地面无人平台的横向行驶和纵向行驶。

(2)所述的构建目标运动学模型为：选择目标的位置、速度、角度和角速度作为状态向量，根据目标运动特征、微积分技术推导目标的运动过程，即状态方程；考虑到目标角速度为0，即目标直线运动时，状态将趋于无穷大，作为特殊情况进行考虑；此外，将纵向加速度噪声和角加速度噪声看作是随机分布的白噪声，作为过程噪声项。

(3)所述根据地面无人平台的定位定向数据对激光雷达的数据进行畸变校正为：采集地面无人平台的线速度和角速度，并转化到激光雷达坐标系；利用第一个激光点的方位角对原始点云进行旋转，将原始点云转化到世界坐标系，并更新线速度和角速度；利用当前扫描的角度对后续点云进行旋转和平移；根据初始旋转矩阵，重新将去畸变后的点云旋转到激光雷达坐标系。

(4)所述最小分割为：人工初始化目标的位置，在每帧点云数据中构建点与点之间的拓扑关系图；以点与点之间的欧式距离构造权值，设定距离目标越远的权值越小；设置前景和背景惩罚函数，对输入点云的前景和背景进行分割；通过对点云的前景进行欧式聚类提取目标的位置。

(5)所述局部路径规划为：根据输出目标的位置和地面无人平台的自身定位，在线生成一簇备选路径。路径模型采用三次贝塞尔曲线，该曲线具有连续的一阶和二阶导数，确保所规划的路径是无人平台可以执行的；对每条备选路径成本、最大可允许曲率和与全局路径的偏置距离进行加权计算，获得每条备选路径的评价值，选择最小的评价值作为最优路径。

(6)所述速度剖面生成为：针对期望路径，设定地面无人平台到达目标点位置的速度为零，将地面无人平台当前速度到目标点位置分为三个阶段，即加速、匀速和减速，以此得到速度剖面。

(7)所述纯跟踪控制为：采用纯追踪算法计算地面无人平台的曲率；忽略地面无人平台低速行驶时的轮胎侧偏角，利用阿克曼转向几何模型，获得左右前轮的转向角；根据输入的速度剖面和前轮的转向角，底盘控制器能够输出轮毂电机的转矩，电动助力转向系统能够输出转向电机的转矩，以此实现地面平台的跟随行驶，包括纵向运动和横向运动。

(8)本发明涉及一种基于多元数据融合的地面无人平台跟踪算法。以期望跟踪目标的位置、速度、角度和角速度作为的状态向量，根据目标的运动特征和过程噪声构建目标运动学模型。利用地面无人平台组合导航系统输出的定位定向数据对激光雷达的原始点云进行畸变校正，然后利用体素滤波、直通滤波对点云进行结构化裁剪。根据初始化目标的位置，利用最小分割算法对目标的前景和背景进行分割，通过欧式聚类获得目标的中心位置。由于构建的目标运行学模型是非线性的，引入无迹卡尔曼滤波对激光雷达和毫米波雷达检测的结果进行预测和更新，实现两者的数据融合，以此持续有效地输出目标位置。选择目标点的位置作为地面无人平台的任务路径点，利用局部路径规划器生成最优路径，通过纯跟踪算法和阿克曼转向模型获得速度和前轮转向角，以此驱动地面无人平台机动行驶。

附图说明

图1为最小分割拓扑关系图；

图2为目标运动学模型；

图3为毫米波检测示意图；

图4为局部路径规划；

图5为速度剖面；

图6为纯跟踪模型；

图7为阿克曼转向模型。

具体实施方式

为了使本发明的目的、技术方案及优点更加便于理解，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案。进一步，为了使公众对本发明有更好的了解，在下文对本发明的细节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述也可以完全理解本发明。

实施例

采集地面无人平台的运动信息(线速度、角速度等)，为每帧点云数据补偿运动量。由于地面无人平台的跟随行驶的速度为5～10km/h，假设激光雷达一个采样周期内，地面无人平台匀速运动，通过旋转和平移进行补偿。

(1)获取运动信息，并将线速度和角速度从全局坐标系转化到激光雷达坐标系。

(2)根据第一个激光点的方位角对原始点云进行旋转，将原始点云转化到世界坐标系，并更新线速度和角速度。

(3)根据顺序扫描时间和角速度获得当前扫描的角度，对后续点云进行旋转和平移。

(4)根据初始的旋转矩阵，重新将去畸变后的点云旋转到激光雷达坐标系。

采用体素滤波方法对原始点云进行降采样处理，在保留点云的几何特征情况下尽可能降低点云的数据量，并对噪声点进行剔除。首先创建一个尺寸为0.02×0.02×0.02m的三维体素网络，对输入点云进行网格划分，利用每个网格内所有点云的重心近似代替网格内其他的点，重心计算公式可表述为。

式中，h为每个网格内点云的数量；(x_i，y_i，z_i)为每个网格内第i个点云的坐标；(x_center，y_center，z_center)为每个网格内点云的重心坐标。

初始化目标点的位置，根据目标的中心点坐标和半径，采用最小分割算法对输入点云的前景(属于目标的点云)和背景(不属于目标的点云)进行分割。

(1)在每帧点云数据中构建点与点之间的拓扑关系图。每个顶点通过边与源点和汇聚点相连，并且也与自身邻域范围内的顶点相连，如图1所示。

(2)根据点与点之间的欧式距离构造权值，因为属于同一物体上的点不会相邻太远。

式中，dist为点间距离，距离越远，边被切割的可能性就越大；σ为权函数，取0.2m。

(3)设置前景和背景惩罚函数。用于缩小目标范围。前景为顶点与源点相连边的权值，设置为常量0.5。背景为顶点与汇聚点相连边的权值backgroundPenalty由公式定义为：

式中，radius为物体水平半径，取0.5m；(x,y)为点云坐标；(CenterX,CenterY)为目标中心坐标；distanceToCenter为点到目标中心的距离。

(4)基于点间的权值，对点云中的前景和背景进行分割，通过欧式聚类对前景点云进行聚类，获得目标的中心位置。

构建目标运动学模型，由于人员运动具有随机性，简单的恒定速度模型无法预测人员或车辆的姿态。根据目标的位置、速度、角度和角速度构建5维的状态量，如图2所示。

式中，(p_x，p_y)为目标在x、y方向上的位置；v为目标的速度；ψ为目标的朝向；为目标的角速度。

对状态量进行求导和积分：

式中，Δt为时间差；x_k、x_k+1分别为k、k+1时刻的状态向量。

根据公式可知，若时，将使得结果趋于无穷大，即无法解决目标沿着直线行驶问题。因此对此种情况单独考虑：

确定完状态方程的固定项后，还需要考虑过程噪声项：

采用二维噪声向量对过程模型进行描述，不确定性v_k由两个独立的标量过程噪声构成。第一个过程噪声是纵向加速度噪声v_a，k，可以被看作是随机分布的白噪声，均值为0，方差为取0.01。

第二个过程噪声是角加速度噪声可以被看作是随机分布的白噪声，均值为0，方差为取0.01：

由于纵向加速度和角加速度均会对位置p_x、p_y产生影响。通常情况下，目标的角速度变化不大，忽略角加速度，噪声对位置的影响可表征为。

噪声对速度、角度、角速度的影响可表征为：

待跟踪目标的动力学模型可表征为：

由于目标的运动学模型是非线性的，采用无迹卡尔曼滤波对目标位置进行预测。首先对当前分布进行采样，保障采样点与分布均值的距离是标准差的整倍数。一般情况下，n维高斯分布选择2n+1个，第一点为高斯分布的均值，其他点关于均值对称。

式中，μ为均值；λ为超参数，取λ＝3-n＝-2；λ越大，采样点距离高斯分布的均值越远，λ越小，采样点距离高斯分布的均值越近。本发明构建的目标运动学模型中，噪声也能对模型产生分线性影响，因此状态方程的协方差矩阵P可表征为：

式中，P′为目标运动学模型的5x5协方差矩阵，元素取0.01的单位矩阵；Q为噪声的协方差矩阵。

然后根据目标运动学模型，利用非线性函数g(·)(即状态方程)对每一个采样点进行预测：

χ_k+1＝g(χ_k,μ_k) (17)

式中，χ_k+1为k+1时刻预测后的采样点；g(·)为状态方程。

计算每个采样点的权重，权重w公式可以表征为：

根据每个采样点的权重计算新分布的均值x_k+1∣k和方差P_k+1∣k：

通过公式(19)获得目标位置预测值。

由于激光雷达从预测状态空间到测量空间为线性变换，可通过线性函数H_l将预测的状态空间转化为测量的状态空间：

式中，Z_k+1∣k为转换后的测量空间；x_k+1∣k为预测后的状态量；R为测量噪声。

由于毫米波雷达预测状态空间到测量空间为非线性变换，可通过非线性函数H_r将预测空间转换到测量空间：

式中，r为目标与毫米波雷达的距离，θ为目标和毫米波雷达之间的夹角，r’为目标与毫米波雷达距离的变化率。毫米波检测示意图如图3所示；

预测无迹变换后的先验估计均值z_k+1∣k、先验估计协方差矩阵S_k+1∣k表述为：

状态空间和测量空间的交叉关联矩阵表述为：

计算卡尔曼增益：

分别根据激光雷达最小分割、欧式聚类的测量结果、毫米波输出的结果z_k+1(即目标中心点坐标)，计算后验状态量x_k+1∣k+1和后验估计协方差矩阵P_k+1∣k+1，具体方法为：

P_k+1∣k+1＝P_k+1∣k-K_k+1∣kS_k+1∣k (27)

x_k+1∣k+1＝x_k+1∣k+K_k+1∣k(z_k+1-z_k+1∣k) (28)

通过公式(28)对目标位置进行更新。

根据上述输出目标的位置和地面无人平台的自身定位，在线生成一簇备选路径，如图4所示。路径模型采用三次贝塞尔曲线，该曲线具有连续的一阶和二阶导数，确保了所规划的路径是无人平台可以执行的。对每条备选路径成本、最大可允许曲率和与全局路径的偏置距离进行加权计算，获得每条备选路径的评价值，选择最小的评价值作为最优路径。

J_i＝ω₁c_i+w₂k_imax+ω₃Δd_i (29)

式中：c_i为第i条备选路径的通过成本；k_imax为第i条备选路径的最大曲率；Δd_i第i条备选路径的偏置距离；ω₁、ω₂、ω₃分别为通过成本、最大曲率和偏置距离的权重。

根据最优局部路径规划轨迹上各点的期望速度。对于给定的路径，设定末端点的目标速度为零，而从当前速度到零需要经历三个阶段，即分为加速行驶、匀速行驶和减速行驶三个阶段，如图5所示，其中加速阶段对应从当前速度vC加速到期望速度vD，减速阶段对应从期望速度vD到速度零，其它为匀速阶段。根据平台的加速度和减速度，可以计算三个阶段对应的轨迹长度，以此得到速度剖面。

采用纯追踪算法来计算运动控制指令，包括平台行驶速度v和角速度w，如图6所示。其中，(x_v，y_v)为平台位置，(x_G，y_G)为全局路径上目标点位置，O为平台瞬时转向中心，R为转向半径，L为平台与目标点之间的距离，即预瞄距离，α为平台航向与目标位置之间的偏差角。

平台航向与目标位置的偏差角为：

由圆弧和平台航向相切，得到圆弧对应的圆心角为：

∠GOS＝2α (31)

由此，得到在三角形ΔGOS中有如下关系：

因此根据局部路径规划器输出的控制速度v，可以求得平台的角速度为:

考虑到无人巡逻车在低速场景下运动，忽略轮胎侧偏角，转向关系可以近似为阿克曼转向几何模型，如图7所示。结合公式，小转角情况下，左右轮的转角为：

根据输入的速度剖面和前轮的转向角，底盘控制器能够输出轮毂电机的转矩，电动助力转向系统(Electric Power Steering,EPS)能够输出转向电机的转矩，以此实现地面平台的跟随行驶，包括纵向运动和横向运动。

Claims (3)

1.一种基于多元数据融合的地面无人平台跟踪方法，其特征在于该方法的步骤包括：

第一步，采集地面无人平台的运动信息，并将采集到的运动信息从全局坐标系转化到激光雷达坐标系；

第二步，实时获取待跟踪目标的激光雷达点云数据和毫米波雷达数据；

第三步，使用第一步采集到的地面无人平台的运动信息对第二步的激光雷达点云数据进行畸变补偿，得到补偿后的点云数据；

第四步，对第三步得到的补偿后的点云数据进行降采样处理，并对噪声点进行剔除；

第五步，对待跟踪目标的位置进行初始化，然后根据待跟踪目标的中心点坐标和半径，利用最小分割算法对输入点云的前景和背景进行分割，通过欧式聚类提取目标中心位置；

第六步，构建待跟踪目标的运动学模型；

第七步，通过无迹卡尔曼滤波算法对待跟踪目标位置进行预测；

第八步，根据激光雷达和毫米波雷达数据融合结果，计算后验状态量和后验估计协方差矩阵，对目标的位置进行更新；

该方法的步骤还包括根据待跟踪目标的位置和地面无人平台的自身定位，在线生成一簇备选路径，对每条备选路径成本、最大可允许曲率和与全局路径的偏置距离进行加权计算，获得每条备选路径的评价值，选择最小的评价值作为最优路径，最后通过速度剖面生成算法、纯跟踪算法完成地面无人平台跟踪任务；

所述的第六步中，构建待跟踪目标的运动学模型的具体步骤为：

根据待跟踪目标的位置、速度、角度和角速度构建5维的状态向量x；

式中，(p_x，p_y)为目标在x、y方向上的位置；v为目标的速度；ψ为目标的朝向；为目标的角速度；

对状态量进行求导和积分，得到状态方程为：

式中，Δt为时间差；x_k、x_k+1分别为k、k+1时刻的状态向量；

若目标角速度时，有：

噪声项v_k为：

采用二维噪声向量对过程模型进行描述，不确定性v_k由两个独立的标量过程噪声构成，第一个过程噪声是纵向加速度噪声v_a，k，被看作是随机分布的白噪声，均值为0，方差为取0.01；

第二个过程噪声是角加速度噪声被看作是随机分布的白噪声，均值为0，方差为取0.01；

噪声对位置的影响表征为：

噪声对速度、角度、角速度的影响表征为：

待跟踪目标的运动学模型为：

2.根据权利要求1所述的一种基于多元数据融合的地面无人平台跟踪方法，其特征在于：

所述的第一步中，采集地面无人平台的运动信息包括地面无人平台的线速度和地面无人平台的角速度；

所述的第四步中，对噪声点进行剔除的方法为：首先创建一个尺寸为0.02m*0.02m*0.02m的三维体素网络，对补偿后的点云数据进行网格划分，利用每个网格内所有点云的重心近似代替网格内其他的点，每个网格的点云重心计算公式表述为：

式中，h为每个网格内点云的数量；(x_i，y_i，z_i)为每个网格第i个点云的坐标；(x_center，y_center，z_center)为每个网格内点云的重心坐标。

3.根据权利要求2所述的一种基于多元数据融合的地面无人平台跟踪方法，其特征在于：

所述的第五步中，采用最小分割算法对输入点云的前景和背景进行分割，进行分割的方法为：

(1)在每帧点云数据中构建点与点之间的拓扑关系图，每个顶点通过边与源点和汇聚点相连，并且也与自身邻域范围内的顶点相连；

(2)根据点与点之间的欧式距离构造权值smoothCost为：

式中，dist为点间距离，距离越远，边被切割的可能性就越大；σ为权函数，取0.2m；

(3)设置前景和背景惩罚函数，用于缩小目标范围；前景为顶点与源点相连边的权值，设置为常量0.5，背景为顶点与汇聚点相连边的权值backgroundPenalty由公式定义为：

式中，radius为物体水平半径，取0.5m；(x,y)为点云坐标；(CenterX,CenterY)为目标中心坐标；distanceToCenter为点到目标中心的距离；

(4)基于点间的权值，对点云中的前景和背景进行分割，通过欧式聚类对前景点云进行聚类，获得目标的中心位置。