CN115342817B

CN115342817B - 无人履带车辆轨迹跟踪状态监测方法、系统、设备、介质

Info

Publication number: CN115342817B
Application number: CN202211282972.5A
Authority: CN
Inventors: 王博洋; 刘海鸥; 薛明轩
Original assignee: Beijing Institute of Technology BIT
Current assignee: Beijing Institute of Technology BIT
Priority date: 2022-10-20
Filing date: 2022-10-20
Publication date: 2023-02-03
Anticipated expiration: 2042-10-20
Also published as: CN115342817A

Abstract

本发明涉及一种无人履带车辆轨迹跟踪状态监测方法、系统、设备、介质，属于状态监测技术领域。先获取无人履带车辆跟踪目标路径的期望轨迹所形成的多条历史轨迹，进一步对多条历史轨迹进行高斯过程回归，得到误差带。然后获取上一观测周期无人履带车辆跟踪目标路径的期望轨迹所形成的观测轨迹，并根据观测轨迹和误差带确定上一观测周期的正常点数量和异常点数量。最后根据正常点数量和异常点数量确定异常率的后验分布，并根据后验分布预测当前观测周期的状态，从而能够基于概率统计实现无人履带车辆的轨迹跟踪状态监测，为无人履带车辆提升越野工况下的轨迹跟踪的安全性提供了思路。

Description

无人履带车辆轨迹跟踪状态监测方法、系统、设备、介质

技术领域

本发明涉及状态监测技术领域，特别是涉及一种无人履带车辆轨迹跟踪状态监测方法、系统、设备、介质。

背景技术

随着无人技术的发展，履带车辆由于其通过性能好、安全性高、运载能力强以及能适应条件复杂的恶劣环境等众多特点，一直是地面无人平台的重要组成部分并得到了极大的发展。近些年来，无人履带车辆被广泛应用于多种场合，比如灾区救援、边境巡逻等。但无人履带车辆在进行期望轨迹跟踪控制的过程中，尤其在实际的动态越野工况下，振动条件恶劣，温度范围较大，环境复杂多变，具有较高的不确定性，加之硬件条件的不可靠性、建模的误差、环境噪声以及软件的性能和逻辑漏洞等，无人履带车辆的实际表现非常有可能超出由建模过程中的假设条件推导出的车辆运动安全边界，对车辆的行车安全造成很大的威胁。为了最大程度保证无人履带车辆的行车安全、提升系统的健壮性，对无人履带车辆的轨迹跟踪状态进行异常监测是十分必要的。

基于此，亟需一种无人履带车辆轨迹跟踪状态监测的技术。

发明内容

本发明的目的是提供一种无人履带车辆轨迹跟踪状态监测方法、系统、设备、介质，能够基于概率统计实现无人履带车辆的轨迹跟踪状态监测，为无人履带车辆提升越野工况下的轨迹跟踪的安全性提供了思路。

为实现上述目的，本发明提供了如下方案：

一种无人履带车辆轨迹跟踪状态监测方法，所述状态监测方法包括：

获取无人履带车辆跟踪目标路径的期望轨迹所形成的多条历史轨迹；所述历史轨迹包括多个采样点的位置坐标；

对所述多条历史轨迹进行高斯过程回归，得到误差带；所述误差带包括每一所述采样点的横向偏差的正常范围；

获取上一观测周期所述无人履带车辆跟踪所述目标路径的期望轨迹所形成的观测轨迹，根据所述观测轨迹和所述误差带确定所述上一观测周期的正常点数量和异常点数量；

根据所述正常点数量和所述异常点数量确定异常率的后验分布，并根据所述后验分布预测当前观测周期的状态。

一种无人履带车辆轨迹跟踪状态监测系统，所述状态监测系统包括：

历史轨迹获取模块，用于获取无人履带车辆跟踪目标路径的期望轨迹所形成的多条历史轨迹；所述历史轨迹包括多个采样点的位置坐标；

高斯过程回归模块，用于对所述多条历史轨迹进行高斯过程回归，得到误差带；所述误差带包括每一所述采样点的横向偏差的正常范围；

观测模块，用于获取上一观测周期所述无人履带车辆跟踪所述目标路径的期望轨迹所形成的观测轨迹，根据所述观测轨迹和所述误差带确定所述上一观测周期的正常点数量和异常点数量；

状态监测模块，用于根据所述正常点数量和所述异常点数量确定异常率的后验分布，并根据所述后验分布预测当前观测周期的状态。

一种无人履带车辆轨迹跟踪状态监测设备，包括：

处理器；以及

存储器，其中存储计算机可读程序指令，

其中，在所述计算机可读程序指令被所述处理器运行时执行上述的状态监测方法。

一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述状态监测方法的步骤。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明用于提供一种无人履带车辆轨迹跟踪状态监测方法、系统、设备、介质，先获取无人履带车辆跟踪目标路径的期望轨迹所形成的多条历史轨迹，进一步对多条历史轨迹进行高斯过程回归，得到误差带。然后获取上一观测周期无人履带车辆跟踪目标路径的期望轨迹所形成的观测轨迹，并根据观测轨迹和误差带确定上一观测周期的正常点数量和异常点数量。最后根据正常点数量和异常点数量确定异常率的后验分布，并根据后验分布预测当前观测周期的状态，从而能够基于概率统计实现无人履带车辆的轨迹跟踪状态监测，为无人履带车辆提升越野工况下的轨迹跟踪的安全性提供了思路。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例1所提供的状态监测方法的方法流程图；

图2为本发明实施例1所提供的状态监测方法的原理框图；

图3为本发明实施例1所提供的基于高斯过程回归的轨迹跟踪误差带分布的示意图；

图4为本发明实施例1所提供的轨迹跟踪误差带分布的局部放大图；

图5为本发明实施例1所提供的轨迹跟踪异常状态监测结果的示意图；

图6为本发明实施例1所提供的高斯过程回归原理图；

图7为本发明实施例2所提供的状态监测系统的系统框图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例1：

本实施例用于提供一种无人履带车辆轨迹跟踪状态监测方法，如图1和图2所示，所述状态监测方法包括：

S1：获取无人履带车辆跟踪目标路径的期望轨迹所形成的多条历史轨迹；所述历史轨迹包括多个采样点的位置坐标；

对于同一条目标路径，根据目标路径的期望轨迹控制无人履带车辆运动，并在无人履带车辆运动的过程中，采集无人履带车辆的位置坐标，所有采样点的位置坐标形成一条历史轨迹，多次重复，得到多条历史轨迹。本实施例可以通过在无人履带车辆上安装惯导的方式来获取位置坐标。

S2：对所述多条历史轨迹进行高斯过程回归，得到误差带；所述误差带包括每一所述采样点的横向偏差的正常范围；

具体的，S2可以包括：

（1）对于每一条历史轨迹，计算该历史轨迹和期望轨迹的差值，得到该历史轨迹中每一采样点的横向偏差；以历史轨迹作为样本数据，以历史轨迹中每一采样点的横向偏差作为历史轨迹的标签数据，构建样本数据集；

在计算历史轨迹和期望轨迹的差值时，所用的方法为：历史轨迹和期望轨迹的采样点一一对应，对于历史轨迹的每一采样点，计算该采样点与期望轨迹中的对应采样点之间的横向偏差，横向偏差即为横向坐标的差值，从而得到历史轨迹中每一采样点的横向偏差。

以每一历史轨迹作为一个样本数据，以历史轨迹中每一采样点的横向偏差作为该历史轨迹的标签数据，构建得到样本数据集。

（2）对样本数据集进行高斯过程回归，得到每一采样点的横向偏差的均值和标准差；

通过高斯过程回归对历史轨迹进行分析，得到不同采样点处横向偏差的均值与方差，通过方差可以进一步计算得到标准差。

本实施例的高斯过程回归在实际应用中主要通过调取库函数的方式来获取所需要的均值和方差，从而生成误差带。

（3）根据每一采样点的横向偏差的均值和标准差以及预设置信度确定误差带。

本实施例的预设置信度可为96%，此时误差带中第i个采样点的横向偏差的正常范围为：

；其中，

为第i个采样点的均值；

为第i个采样点的标准差。

通过提取无人履带车辆越野工况下的实际轨迹跟踪响应数据（即历史轨迹），针对相同的期望轨迹，以横向偏差作为状态输出，历史轨迹的采样点为状态输入，进行统计和分析，得到横向偏差的均值以及分布概率范围。在一个示例中，共提取35组历史轨迹，以用于高斯过程回归的训练，如图3所示，其为误差带的示意图，曲线为历史轨迹和基于高斯过程回归求解得到的每个采样点的横向偏差的均值，浅色区域为基于高斯过程回归计算的均值和标准差所确定的“误差带”，此处选取具有96%置信度的区间，“误差带”大小选择为

。如图4所示，其为路径s=49m位置处的误差带的局部放大图，其中数据圆点表示轨迹跟踪的采样点和当前位置的横向偏差的均值，浅色区域为当前位置的轨迹跟踪横向偏差满足的高斯分布，此处的“误差带”大小选择

。

S3：获取上一观测周期所述无人履带车辆跟踪所述目标路径的期望轨迹所形成的观测轨迹，根据所述观测轨迹和所述误差带确定所述上一观测周期的正常点数量和异常点数量；

本实施例的观测周期的时间长度可以人为设定，比如可以为1s。

其中，根据观测轨迹和误差带确定上一观测周期的正常点数量和异常点数量可以包括：

对于观测轨迹的每一第一采样点，计算第一采样点的位置坐标以及期望轨迹中与第一采样点相对应的采样点的位置坐标的差值，得到第一采样点的横向偏差；判断第一采样点的横向偏差是否处于误差带中与第一采样点相对应的采样点的横向偏差的正常范围内；若是，则第一采样点为正常点；否则，则第一采样点为异常点；统计观测轨迹中为正常点的第一采样点的数量，得到上一观测周期的正常点数量；统计观测轨迹中为异常点的第一采样点的数量，得到上一观测周期的异常点数量。

S4：根据所述正常点数量和所述异常点数量确定异常率的后验分布，并根据所述后验分布预测当前观测周期的状态。

本实施例基于概率统计进行轨迹跟踪异常状态监测。基于高斯过程回归预测的结果，可以通过一个带有置信概率密度的统计区域，来描述轨迹跟踪在给定参考路径位置处的期望状态。在对轨迹跟踪响应状态进行异常状态检测时，可以通过车辆是否处于期望状态的概率统计区域来判断，这种方式简单易懂，便于应用。然而，这种带有概率密度的期望状态统计区域，并不包含车辆进行轨迹跟踪响应状态的所有情况，即车辆仍然存在一定的概率的正常状态使得其离开该状态统计区域。例如，选择一个具有96%置信度的期望状态统计区域，则车辆仍在存在4%的时间可处于该区域之外。因此，基于上述简单的通过车辆状态是否离开该概率统计区域来判断车辆异常状态的方式，具有很大的偶然性。一般来说，如果车辆状态经常偏离上述基于数据统计得到的概率统计区域，车辆很有可能处于异常状态。

定义

为车辆状态偏离概率统计区域的可能性，即异常率，并且

为随机变量，服从贝塔分布，作为先验概率，即

；

其中，Beta（）为贝塔分布，贝塔分布的参数

，贝塔分布的参数

，N为先验概率的规模，也即观测轨迹的第一采样点的总个数；P为基于高斯回归模型得到的概率统计区域的置信度，也即预设置信度。

基于上述先验概率，计算

的后验概率与异常率，具体包括：

在一个观测周期t _w内，观测点处于异常状态的次数服从二项分布：

；

；

其中，n _abnormal为车辆处于异常状态的观测点的数量，也即异常点数量；B为二项分布；n _#为所有观测点的数量，也即观测轨迹的第一采样点的总个数；n _normal为车辆处于正常状态的观测点的数量，也即正常点数量。

由于Beta分布是二项分布的共轭先验，则

的后验也服从Beta分布，根据后验分布的定义可以得到：

；

后验分布用来预测在给定条件下，当前或者说还没有发生的状态的分布概率。

基于Beta分布的累计分布函数，可以很方便的得到异常率

大于期望值的可能性：

；

其中，

。

式中，q为积分的过程变量。

可以通过对

进行阈值设置，来实现对车辆轨迹跟踪异常状态的监测。而精度和召回率之间的权衡，可以通过调整模型的阈值（即预设概率）、先验数据的规模（即观测轨迹的第一采样点的总个数）来实现。当所获得的车辆在当前观测周期内的状态与行为离开通过高斯过程回归得到的置信区域内的概率大于设定的阈值（即预设概率）时，将其判断为异常状态。

基于上述理论，进行异常状态监测时所用的方法为：先采集上一观测周期正常点数量和异常点数量，并带入异常率的后验分布，即可确定准确的不含未知参数的异常率的后验分布。然后基于该后验分布的累计分布函数，计算异常率大于预设值的概率，预设值即为1与预设置信度的差值，再判断概率是否大于预设概率；若是，则当前观测周期的状态为异常状态；否则，则当前观测周期的状态为正常状态，以实现状态监测。

对于同一条路径进行多次行驶轨迹的数据提取，通过高斯过程回归对历史轨迹进行分析，得到关于轨迹不同采样点处横向偏差的均值与方差，使用96%置信区域作为安全范围，得到误差带。因为考虑到偶尔离开安全范围的行为也是应该被允许的，因此通过概率论原理，判断当前行驶状态是否属于正常状态。定义了异常率

服从贝塔分布，通过先验概率以及观测周期内的异常点数量与正常点数量，得到后验分布。对于

的后验分布，求取其大于1-P的概率，对于这个概率大于预设概率（例如90%）时判断为异常状态。相比于直接通过误差带进行判断，该方法通过判断处于误差带内的概率进行异常检测，具有更高的召回率。

为了进一步说明本实施例的状态监测方法的优越性，对状态监测方法进行测试，测试结果如下：

本实施例共提取相同参考轨迹、不同控制参数生成的实际轨迹跟踪观测数据共计50组，其中35组用于模型的训练，其余15组数据用于轨迹跟踪异常状态监测的方法验证，并将本实施例提出的方法，与定义离开概率统计区域为异常状态的对比方法进行了对比。

如图5所示，其为轨迹跟踪异常状态监测结果的示意图，对于本实施例提出的方法，设置数据规模N=1000，P=96%，因此

，

，观测周期t _w=1s，概率统计的预设概率p设置为90%。图5中的（a）为对比方法的混淆矩阵，图5中的（b）为本实施例方法的混淆矩阵，可以看出，本实施例的方法在保证异常状态监测的精度的前提下，提升了召回率。

本实施例的高斯过程回归是对采集到的历史轨迹进行回归得到误差带（由均值与误差两部分共同组成），该误差带会作为异常判断的依据。在此，对本实施例的高斯过程回归所涉及的部分公式进行介绍：

（1）含有噪音的高斯过程回归

高斯过程回归是基于高斯过程先验对数据进行回归分析的非参数模型，其算法原理图如图6所示。由n个样本观测值构成的样本数据集D如下：

；

其中，X为所有样本数据的输入特征向量组成的矩阵；y为所有样本数据的输出标签值组成的矩阵；x _i为第i个样本数据的输入特征向量，即第i个历史轨迹的每一采样点的位置坐标；y _i为第i个样本数据的输出标签值，即第i个历史轨迹的每一采样点的横向偏差。

假设样本数据集上存在噪声，并且该噪声服从标准高斯分布：

；

；

；

其中，f（x）是一种潜在函数，具体形式不确定，表示输入向量x对应的可能的函数值；

为噪声；

为对角线元素为

的关于噪声的对角线协方差矩阵，

为噪声的第n个协方差。

（2）高斯过程建模与预测方程

高斯过程是对函数的概率分布进行建模，并完全由均值函数和协方差函数确定：

；

其中，m（x）为一个随机过程f（x）的均值函数，在进行数据观测之前，由于并没有关于f（x）的均值的先验信息，通常假设均值函数为0；

为协方差函数，用来度量不同的输入向量之间的相似性，

代表样本输入特征向量的转置。

即有：

；

；

假设由n个训练样本构建的潜在函数为f，由m个测试样本构建的潜在函数为

，则根据多维高斯分布的性质，f和

的联合先验分布可以表示为：

；

其中，

为关于噪声的对角线协方差矩阵；I为单位矩阵；K，

以及

均为协方差矩阵的简写形式，具体的表达形式为：

；

；

；

其中，X _*为所有测试样本的输入特征向量组成的矩阵；

为第m个测试样本的输入特征向量。

根据观测得到的联合高斯先验分布，可以获得高斯过程回归的预测方程，预测方程是对于当前状态或未来状态的一个预测，判断其状态的一个概率分布：

；

其中，

表示函数

的均值，

表示函数

的协方差，具体表达式如下：

；

；

通过联合先验分布与预测方程，可以根据贝叶斯公式计算出样本数据的后验概率分布，完成对于测试集潜函数的估计与预测。

（3）建立训练集并误差分析

轨迹跟踪控制的运动执行是基于n维时间状态空间参数的一维曲线。这里采用基于高斯过程回归的方法，基于（1）与（2）的原理，对轨迹跟踪控制的执行情况进行频率分布函数的表达：

；

；

；

；

其中，p表示时刻t时观测点X ⁱ处的预测分布符合正态分布，

表示样本均值，

表示对角线为

的样本方差矩阵；X ⁱ=x ¹，...，

表示对于相同参考轨迹的J _i个观测值；

为观测点X ^j处的均值；

为观测点X ^j处的方差。该公式可以理解为具有高斯过程样本的样本均值和样本误差，它是高斯过程混合的高斯近似。

（4）确定高斯过程先验的协方差

根据（1）中所描述的高斯过程回归原理，结合（3）中所建立的轨迹跟踪控制的执行数据，构建高斯过程回归的先验概率。这里采用Matern协方差函数来构建高斯过程先验。已知Matern协方差的定义为：

；

其中，

为方差；

是伽马函数；

为平滑度参数；

表示两点之间的距离；l称为长度尺度，为正的d维向量，即

，对于不相关的输入，l可以取较大值；

为第二类贝塞尔函数。

本实施例采用的是Matern中

=3/2的协方差函数，上式可以表示为：

；

其中，

为信号的方差，用来调节协方差函数值；t ₁，t ₂为一个样本的输入特征向量在某个维度上的任意两个元素；

表示两个元素之间的几何距离。

（5）训练高斯过程回归模型，建立边缘似然函数

在高斯过程回归中，根据（4）中得到的协方差函数的参数与噪声

是可变的，这些参数被称为超参数，可以表示为

。超参数对高斯过程回归模型具有重要的影响，在建立模型的过程中需要确定最优超参数，本实施例的高斯过程回归采用最大边缘似然的策略获得最优超参数。

根据以上理论以及（3）中的公式，令

，假设

，得到训练样本集的边缘似然函数：

；

其中，K表示和函数的协方差，在（2）中确定；

表示噪音的方差，n表示训练集的维度。

为了进行分析，本实施例基于边缘似然函数对其进行取对数操作，建立负的对数边缘似然函数：

；

其中，

。

根据所列出的负对数边缘似然函数可知，右侧第一项包含样本数据，体现了模型的经验风险；右侧的第二项仅与回归模型有关，协方差矩阵越复杂其取值越高，体现了模型的结构风险。第三项为常数不变项。

（6）求解对数边缘似然函数的梯度以得到最优超参数

在上一步所求的基础上，以负的边缘似然函数为极小化目标函数，以超参数为优化变量，采用共轭梯度法进行优化，求解对数边缘似然函数的梯度以得到最优超参数。其对数边缘似然函数对各超参数求解偏导数的表达式：

；

；

；

其中，tr（.）表示矩阵的迹，即为矩阵的对角线上所有元素之和。

根据（1）与（2）的原理，按照（3）到（6）的过程得到高斯过程回归的训练结果。

以下，对本实施例所提出的Beta分布与二项分布进行介绍：

Beta分布是定义在[0，1]区间上的概率密度曲线，是二项分布、伯努利分布的共轭先验分布。通过贝叶斯统计原理相关内容，由先验分布推导后验分布可以发现，Beta分布的先验分布，其后验分布也是Beta分布，可以应用于多种场景，得到了广泛的应用，其概率密度函数的形式表达如下所示：

；

其中，x为随机量；u为积分变量；

表示gamma函数；B表示贝塔分布。

Beta分布的累计分布函数可以表示为：

；

其中，

为不完全B函数；

为正则不完全贝塔函数。

Beta分布的均值为：

；

其中，X为随机变量。

方差为：

；

二项分布是n次独立重复的伯努利实验中，事件A发生的次数的离散概率分布。每次试验成功的概率为p，n次实验中事件A恰好发生k次的概率可以表示为：

；

其中，X为随机变量；

表示二项式系数。

则如果随机变量X服从参数n、p的二项分布，可以记为：

；

二项分布的均值为：

；

方差为：

。

本实施例提出一种无人履带车辆轨迹跟踪控制过程中的状态监测方法，通过对轨迹跟踪历史数据的统计，生成了基于高斯过程回归的误差带，根据贝塔分布的概率统计以及积分阈值设定，实现了对轨迹跟踪异常状态的监测，对异常状态的监测进行了建模，为履带车辆提升越野工况下的轨迹跟踪的安全性提供了思路。本实施例的设计考虑到无人履带车辆在轨迹跟踪控制过程中的状态异常问题，通过数据学习的方式，应用高斯过程回归、贝塔概率分布对轨迹跟踪的状态监测进行建模，搭建了轨迹跟踪异常状态监测的架构，提升了越野工况下的履带车辆的轨迹跟踪的安全性。

实施例2：

本实施例用于提供一种无人履带车辆轨迹跟踪状态监测系统，如图7所示，所述状态监测系统包括：

历史轨迹获取模块M1，用于获取无人履带车辆跟踪目标路径的期望轨迹所形成的多条历史轨迹；所述历史轨迹包括多个采样点的位置坐标；

高斯过程回归模块M2，用于对所述多条历史轨迹进行高斯过程回归，得到误差带；所述误差带包括每一所述采样点的横向偏差的正常范围；

观测模块M3，用于获取上一观测周期所述无人履带车辆跟踪所述目标路径的期望轨迹所形成的观测轨迹，根据所述观测轨迹和所述误差带确定所述上一观测周期的正常点数量和异常点数量；

状态监测模块M4，用于根据所述正常点数量和所述异常点数量确定异常率的后验分布，并根据所述后验分布预测当前观测周期的状态。

实施例3：

本实施例用于提供一种无人履带车辆轨迹跟踪状态监测设备，包括：

处理器；以及

存储器，其中存储计算机可读程序指令，

其中，在所述计算机可读程序指令被所述处理器运行时执行实施例1所述的状态监测方法。

实施例4：

本实施例用于提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现实施例1所述状态监测方法的步骤。

本说明书中每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。