CN114815820A - 一种基于自适应滤波的智能体小车线性路径规划方法 - Google Patents

Abstract

本发明公开了一种基于自适应滤波的智能体小车线性路径规划方法,包括以下步骤：S1.确定智能体小车的运行区域；S2.通过超宽带定位技术实时将智能体小车的未知信息进行解算，得到信息并进行汇总，汇总得到的信息包括：智能体小车在当前区域行进过程中实时测得的位置序列点信息；S3.根据智能体小车运行的线性轨迹，通过自适应滤波算法进行迭代训练，估计得到智能体小车的运行轨迹映射权重关系，从而预测智能体之后的运行轨迹。本发明通过迭代训练，可以得到具有更强稳定性的系统等价权值向量，用于估计预测智能体后续的运行轨迹。

Description

一种基于自适应滤波的智能体小车线性路径规划方法

技术领域

本发明涉及小车路径规划，特别是涉及一种基于自适应滤波的智能体小车线性路径规划方法。

背景技术

作为信号处理技术中的一项重要且基本的内容，滤波器设计一直都受到广泛关注。自适应滤波器由于其无需输入信号的先验知识、对未知环境较强的适应能力，可以动态调节滤波器参数等优点，因此更是被广泛应用于例如信道均衡、系统辨识、回声消除等场合。但伴随着自适应算法应用领域的拓展，自适应滤波算法也面临着一些新的问题：例如在自然界中广泛存在的非高斯噪声的影响等。因此，如何构建鲁棒性算法日渐成为了自适应滤波研究的核心问题。

发明内容

本发明的目的在于克服现有技术的不足，提供一种基于自适应滤波的智能体小车线性路径规划方法，通过迭代训练，可以得到具有更强稳定性的系统等价权值向量，用于估计预测智能体后续的运行轨迹。

本发明的目的是通过以下技术方案来实现的：一种基于自适应滤波的智能体小车线性路径规划方法，包括以下步骤：

S1.确定智能体小车的运行区域；

S2.通过超宽带(Ultra-Wide Band,UWB)定位技术实时将智能体小车的未知信息进行解算，得到信息并进行汇总，汇总得到的信息包括：智能体小车在当前区域行进过程中实时测得的位置序列点信息；

S3.根据智能体小车运行的线性轨迹，通过自适应滤波算法进行迭代训练，估计得到智能体小车的运行轨迹映射权重关系，从而预测智能体之后的运行轨迹。

其中，所述步骤S2中根据超宽带定位技术解算获得的智能体实时位置序列点信息，是以智能体在运行区域以线性方式测算得到的；

UWB空间四点的精准定位，其实是基于RSSI原理的，三维空间的四点定位算法描述如下：已知四个基站点的坐标(x₁,y₁),(x₂,y₂),(x₃,y₃),(x₄,y₄)和到未知点(x,y,)的距离R₁,R₂,R₃,R₄；所以四点定位使用矩阵求解方程的方式：

其中要求的点的坐标是(x,y)，已知的四个探针的位置为(x₁,y₁),(x₂,y₂),(x₃,y₃),(x₄,y₄)，这也即是四个基站的位置，R₁到R₄为所求的点到四个探针的距离；求解上述方程，首先分解：

然后相减：

2(x₁-x₂)x+2(y₁-y₂)y＝λ₁

2(x₁-x₃)x+2(y₁-y₃)y＝λ₂

2(x₁-x₄)x+2(y₁-y₄)y＝λ₃

其中：

这时候就能转换为矩阵相乘:

可以用A、B、C来表示上式，即

则上式可表示为AB＝C，那么此时只需要求解A的逆矩阵A^-1，然后把逆矩阵同时乘以等式两边，即B＝A^-1C，就能求得矩阵B，即小车的坐标x，y；x，y指的就是智能体小车在当前区域行进过程中实时测得的位置序列点信息，但实际在本算法中只需要小车的横纵坐标值，即x，y的值。在求解实际坐标的时候，即是通过四个基站同时测得的位置信息(x₁,y₁),(x₂,y₂),(x₃,y₃),(x₄,y₄)，然后通过上式求逆得到的。因为每一时刻四个基站可以获得位置信息是已知的，那之后便可以得到对应的A矩阵，接着对矩阵取逆就可以得到此时小车的实际位置坐标。

实物场景所采用的验证方案是采用4个基站位于小车运行范围的四角，用于接收解算小车的实时位置，另外将一个UWB置于小车上用于实时传输小车的位置信息，然后根据超宽带定位技术的四点定位法，得到智能体的线性运行轨迹点。

其中，步骤S3中所述的智能体运行轨迹映射权重关系的估计，包括自适应滤波算法的引入和线性系统关系的预测。

所述步骤S3中，自适应滤波算法的引入就是通过自适应滤波算法对系统的等价权值系数进行迭代训练的过程，具体包括：

S301.在实物场景中，通过手机蓝牙遥控让智能体小车沿着指定矩阵范围的一条对角线匀速行进；

通过在对角处建立原点坐标系，得到智能体小车实际的运行轨迹方程曲线；按照步骤S2，在智能体小车运行过程中，得到智能体小车的轨迹点；

所述智能小车的轨迹点记为(x₁,y₁),(x₂,y₂),...,(x_N,y_N)，其中(x_n,y_n)表示第n个时刻的小车实际位置坐标，n＝1,2,...,N；

S302.构建自适应滤波模型，将智能体小车的轨迹点的横坐标x_n作为模型输入x(n)，将智能体小车的轨迹点的纵坐标y_n作为模型期望输出y(n)，通过对自适应滤波器权重向量w(n)进行训练，得到成熟的自适应滤波模型：

A1、初始化参数e(1)，w(1)，M(1)＝δ^-1I，初始化n＝2，其中δ为预设的参数；

A2、将x(n)输入自适应滤波模型y(n)＝w(n-1)^Tx(n)中，得到对应的模型估计输出y(n)，计算误差：

e(n)＝y(n)-y(n)＝y(n)-w(n-1)^Tx(n)

A3、计算中间参数H(n)、M(n)：

H(n)＝M(n-1)x(n)[λ²/φ₀+x^T(n)M(n-1)x(n)]^-1

M(n)＝λ^-2[M(n-1)-H(n)x^T(n)M(n-1)]

其中，

参数λ为遗忘因子，q₁,ρ₁和q₂,ρ₂分别q高斯函数

和

的形状参数和核宽参数，η为混合系数；

A4、对自适应滤波器权重向量进行更新：

w(n)＝w(n-1)+H(n)e(n)；

即当前时刻更新得到的权重向量w(n)，等于当前时刻H(n)与e(n)的乘积加上前一时刻权重向量w(n-1)；

A5、对n进行更新，更新后的n等于更新前的n加1。然后判断n是否大于N：

若是，将步骤A4中得到的w(n)作为最终的权重向量；

若否，跳转至步骤A2；

S304.利用最终的权重向量，对小车之后的行进轨迹进行预测；对于要预测的位置点，将小车需要预测的位置点的横坐标作为输入，通过和最终的权重向量进行相乘得到小车预测位置点的纵坐标点，从而得到小车预测的位置点信息。

本发明的有益效果是：本发明通过迭代训练，可以得到具有更强稳定性的系统等价权值向量，用于估计预测智能体后续的运行轨迹。

附图说明

图1为本发明的方法流程图；

图2为实施例中自适应滤波器结构示意图。

具体实施方式

下面结合附图进一步详细描述本发明的技术方案，但本发明的保护范围不局限于以下所述。

本发明针对传统的自适应滤波算法只在高斯噪声环境下具有最优性能，提出了基于混合q高斯广义误差熵准则的递归混合q高斯最小误差熵算法。该算法采用q高斯密度函数来取代普通高斯函数，并结合混合熵的思想，并推导提出了具有递归形式的递归混合q高斯最小误差熵算法，收敛性分析也验证了算法的稳定性。并且算法仿真以实物验证实验都表明：该算法在非高斯噪声干扰下能够达到更好性能。而且，通过算法的迭代训练，可以得到具有更强稳定性的系统等价权值向量，用于估计预测智能体后续的运行轨迹；具体地：

如图1所示，一种基于自适应滤波的智能体小车线性路径规划方法，包括以下步骤：

S1.确定智能体小车的运行区域；

S2.通过超宽带定位技术(Ultra-Wide Band,UWB)实时将智能体小车的未知信息进行解算，得到信息并进行汇总，汇总得到的信息包括：智能体小车在当前区域行进过程中实时测得的位置序列点信息；

S3.根据智能体小车运行的线性轨迹，通过自适应滤波算法进行迭代训练，估计得到智能体小车的运行轨迹映射权重关系，从而预测智能体之后的运行轨迹。

其中，所述步骤S2中根据超宽带定位技术解算获得的智能体实时位置序列点信息，是以智能体在运行区域以线性方式测算得到的；

UWB空间四点的精准定位，其实是基于RSSI原理的，三维空间的四点定位算法描述如下：已知四个基站点的坐标(x₁,y₁),(x₂,y₂),(x₃,y₃),(x₄,y₄)和到未知点(x,y)的距离R₁,R₂,R₃,R₄；所以四点定位使用矩阵求解方程的方式：

其中要求的点的坐标是(x,y,)，已知的四个探针的位置为(x₁,y₁),(x₂,y₂),(x₃,y₃),(x₄,y₄)，R₁到R₄为所求的点到四个探针的距离；求解上述方程，首先分解：

然后相减：

2(x₁-x₂)x+2(y₁-y₂)y＝λ₁

2(x₁-x₃)x+2(y₁-y₃)y＝λ₂

2(x₁-x₄)x+2(y₁-y₄)y＝λ₃

其中：

这时候就能转换为矩阵相乘:

假设上面矩阵为A*B＝C，那么此时只需要求解A的逆矩阵，然后把逆矩阵同时乘以等式两边，就能求得x，y；

x，y指的就是智能体小车在当前区域行进过程中实时测得的位置序列点信息，但实际在本算法中只需要小车的横纵坐标值，即x，y的值。在求解实际坐标的时候，即是通过四个基站同时测得的位置信息(x₁,y₁),(x₂,y₂),(x₃,y₃),(x₄,y₄)，然后通过上式求逆得到的。因为每一时刻四个基站可以获得位置信息是已知的，那之后便可以得到对应的A矩阵，接着对矩阵取逆就可以得到此时小车的实际位置坐标。(在实际场景中也可以通过UWB的上位机自动导出的)

实物场景所采用的验证方案是采用4个基站位于小车运行范围的四角，用于接收解算小车的实时位置，另外将一个UWB置于小车上用于实时传输小车的位置信息，然后根据超宽带定位技术的四点定位法，得到智能体的线性运行轨迹点。

其中，步骤S3中所述的智能体运行轨迹映射权重关系的估计，包括自适应滤波算法的引入和线性系统关系的预测。

所述步骤S3中，通过自适应滤波算法进行迭代训练的过程包括：

S301.在实物场景中，通过手机蓝牙遥控让智能体小车沿着指定矩阵范围的一条对角线匀速行进；

通过在对角处建立原点坐标系，得到智能体小车实际的运行轨迹方程曲线；通过UWB定位解算技术，在智能体小车运行过程中，得到智能体小车的轨迹点；

S302.将UWB解算获得的横纵坐标序列点值作为自适应滤波算法的输入x(n)与输出y(n)；滤波算法的目的是使得坐标值能够尽量和轨迹曲线贴合，通过对自适应滤波器权重向量的训练，用y(n)＝x^T(n)w(n)的方式来估算得到滤波后的纵坐标y(n)：

在自适应滤波系统中，具有有限脉冲响应(FIR)的每个线性系统可以描述为等价权值向量w_o。未知系统w_o输出的带噪声测量信号定义为

其中d(n)是系统的输出信号，u(n)＝[u(n),u(n-1),…,u(n-L+1)]^T表示系统的输入向量，v(n)则是系统中的噪声，L表示脉冲响应的长度。

线性滤波器在n时间的误差可定义为

e(n)＝d(n)-w^T(n)u(n) (1)

其中w是w_o的估计值。

对于自适应滤波算法中基于信息理论学习的方式，熵的公式将量化模型输出与期望输出之间误差的平均不确定性或多样性。因此，最小误差熵准则(Minimum ErrorEntropy,MEE)的最小化将使得误差集中。香农熵的一个著名推广是Renyi熵，即

分别用于离散和连续随机变量，其中α是一个自由参数。

Renyi熵通常采用Parzen窗方法从样本非参数地估计密度函数。当给样本{e₁,e₂,…,e_L}，可以使用核函数来获得随机变量e的概率密度估计。当选用高斯函数作为核函数，并且α取为2时，上式可以表示为

其中σ为核函数的标准差，L为滑动窗口的长度。此时，对数函数中的累计项V(e)称为信息势能。

然后，将q高斯分布(1<q<3)来代替普通的高斯函数，并定义q高斯广义误差熵。在给定的样本序列中，同样使用样本均值估计来近似信息势能

其中，

式(4)的误差熵反映了q高斯核在高维空间中引起的误差最小化。这个新的准则称为q高斯广义最小误差熵准则(q-GaussianGeneralized Minimum Error Entropy,QGMEE)。

在自适应系统训练中，由于对数函数的单调性，最小化Renyi熵等价于最大化信息势能。因此，可以式(4)作为最优解。根据q高斯密度函数，可以证明当q→1时，QGMEE将退化为普通MEE。故通过选择合适的形状参数q，QGMEE准则可以更好地模拟外部随机干扰，并获得比MEE更好的鲁棒性。

另一方面，内核宽度通常时熵的关键参数之一。较小的核宽使算法具有较低的稳态误差，但收敛速度较慢；而较大的核宽则相反。为了获得更好的性能，本节提出了一种新的相似性度量思想。通过混合熵利用熵的特性保持对异常值的稳定性，同时结合不同核宽的优点，使算法具有更好的收敛趋势和更低的稳态误差。同样，将混合熵的概念推广到q高斯广义误差熵中。其中，混合误差熵使用两个q高斯函数的混合作为代价函数，表达式定义如下

其中，0≤η≤1是混合系数，x,y都表示随机变量,q₁,ρ₁和q₂,ρ₂分别q高斯函数

和

的形状参数和核宽参数。

当混合系数η取适当值时，混合q高斯广义最小误差熵(MQGMEE)的性能将优于原视误差熵准则，因此混合误差熵是一种更灵活的相似性度量。

故基于QGMEE准则的带指数遗忘因子λ的代价函数是

代入提出的MQGMEE准则作为误差拟合准则，可得到一个新的鲁棒自适应滤波公式

其中，e(i)是第i次迭代期间计算得到的残差。则通过递归方式的推导，递归混合q高斯最小误差熵(Recursive Mixture q-Gaussian Minimum Error Entropy,RMQGMEE)的权值系数更新方程可通过以下公式获得

其中，e(0)的值通过滑动窗口不断更新。

通过以上推导，本章所提出的RMQGMEE算法的迭代过程可以总结为

其中，参数λ,q₁,ρ₁,q₂,ρ₂,η都是需要预先定义的。在本实验中，具体为：参数λ为遗忘因子，可取为0.9999，q₁,ρ₁和q₂,ρ₂分别q高斯函数

和

的形状参数和核宽参数，可分别取值为1.2，0.5556，，2.9，10，η为混合系数，可取为0.9999。

通过算法的训练，可以迭代得到系统等价的权值向量w。在算法迭代过程中，可以得到算法与真实轨迹之间的误差将会不断减小。当算法收敛稳定后，通过算法估计得到的坐标值将会更加符合真实轨迹值。从训练过程可以得到，算法的估计值基本都能够很好去贴合小车的估计曲线。而且，RMQGMEE算法相较于传统的递归最小误差熵(RecursiveMinimum Error Entropy,RMEE)算法达到更低的误差，即该算法的性能更好；自适应滤波器的结构如图2所示。

在自适应系统完成了对于系统等价权值向量的训练之后，那智能体接下来的轨迹预测将可以使用该等价权值向量进行预测，以滤除UWB定位解算误差以及环境噪声所噪声系统误差，规划得到更符合真实运行轨迹的位置序列点。

上述说明示出并描述了本发明的一个优选实施例，但如前所述，应当理解本发明并非局限于本文所披露的形式，不应看作是对其他实施例的排除，而可用于各种其他组合、修改和环境，并能够在本文所述发明构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本发明的精神和范围，则都应在本发明所附权利要求的保护范围内。

Claims (4)

1.一种基于自适应滤波的智能体小车线性路径规划方法，其特征在于：包括以下步骤：

S1.确定智能体小车的运行区域；

S2.通过超宽带定位技术实时将智能体小车的位置信息进行解算，将得到的信息进行汇总，汇总得到的信息包括：智能体小车在当前区域行进过程中实时测得的位置序列点信息；

S3.根据智能体小车运行的线性轨迹，通过自适应滤波算法进行迭代训练，估计得到智能体小车的运行轨迹映射权重关系，从而预测智能体之后的运行轨迹。

2.根据权利要求1所述的一种基于自适应滤波的智能体小车线性路径规划方法，其特征在于：所述步骤S2中根据超宽带定位技术解算获得的智能体实时位置序列点信息，是以智能体在运行区域以线性方式测算得到的；

UWB空间四点的精准定位，其实是基于RSSI原理的，三维空间的四点定位算法描述如下：已知四个基站点的坐标(x₁,y₁),(x₂,y₂),(x₃,y₃),(x₄,y₄)和到未知点(x,y)的距离R₁,R₂,R₃,R₄；所以四点定位使用矩阵求解方程的方式：

其中要求的点的坐标是(x,y)，已知的四个探针的位置为(x₁,y₁),(x₂,y₂),(x₃,y₃),(x₄,y₄)，也就是四个基站的位置，R₁到R₄为所求的点到四个探针的距离；求解上述方程，首先分解：

然后相减：

2(x₁-x₂)x+2(y₁-y₂)y＝λ₁

2(x₁-x₃)x+2(y₁-y₃)y＝λ₂

2(x₁-x₄)x+2(y₁-y₄)y＝λ₃

其中：

这时候就能转换为矩阵相乘:

可以用A、B、C来表示上式，即

则上式可表示为AB＝C，那么此时只需要求解A的逆矩阵A^-1，然后把逆矩阵同时乘以等式两边，即B＝A^-1C，就能求得矩阵B，即小车的坐标x，y；

x，y指的就是智能体小车在当前区域行进过程中实时测得的位置序列点信息，在求解实际坐标的时候，即是通过四个基站同时测得的位置信息(x₁,y₁),(x₂,y₂),(x₃,y₃),(x₄,y₄)，然后通过式B＝A^-1C求逆得到的；

因为每一时刻四个基站可以获得位置信息是已知的，那之后便能够得到对应的A矩阵，接着对矩阵取逆得到此时小车的实际位置坐标。

3.根据权利要求1所述的一种基于自适应滤波的智能体小车线性路径规划方法，其特征在于：步骤S3中所述的智能体运行轨迹映射权重关系的估计，包括自适应滤波算法的引入和线性系统关系的预测。

4.根据权利要求1所述的一种基于自适应滤波的智能体小车线性路径规划方法，其特征在于：所述步骤S3中，自适应滤波算法的引入就是通过自适应滤波算法对系统的等价权值系数进行迭代训练的过程，具体包括：

S301.在实物场景中，通过手机蓝牙遥控让智能体小车沿着指定矩阵范围的一条对角线匀速行进；

通过在对角处建立原点坐标系，得到智能体小车实际的运行轨迹方程曲线；按照步骤S2，在智能体小车运行过程中，得到智能体小车的轨迹点；

所述智能小车的轨迹点记为(x₁,y₁),(x₂,y₂),...,(x_N,y_N)，其中(x_n,y_n)表示第n个时刻的小车实际位置坐标，n＝1,2,...,N；

S302.构建自适应滤波模型，将智能体小车的轨迹点的横坐标x_n作为模型输入x(n)，将智能体小车的轨迹点的纵坐标y_n作为模型期望输出y(n)，通过对自适应滤波器权重向量w(n)进行训练，得到成熟的自适应滤波模型：

A1、初始化参数e(1)，w(1)，M(1)＝δ^-1I，初始化n＝2，其中δ为预设的参数；

A2、将x(n)输入自适应滤波模型y(n)＝w(n-1)^Tx(n)中，得到对应的模型估计输出y(n)，计算误差：

e(n)＝y(n)-y(n)＝y(n)-w(n-1)^Tx(n)

A3、计算中间参数H(n)、M(n)：

H(n)＝M(n-1)x(n)[λ²/φ₀+x^T(n)M(n-1)x(n)]^-1

M(n)＝λ^-2[M(n-1)-H(n)x^T(n)M(n-1)]

其中，

参数λ为遗忘因子，q₁,ρ₁和q₂,ρ₂分别q高斯函数

和

的形状参数和核宽参数，η为混合系数；

A4、对自适应滤波器权重向量进行更新：

w(n)＝w(n-1)+H(n)e(n)；

即当前时刻更新得到的权重向量w(n)，等于当前时刻H(n)与e(n)的乘积加上前一时刻权重向量w(n-1)；

A5、对n进行更新，更新后的n等于更新前的n加1；然后判断n是否大于N：

若是，将步骤A4中得到的w(n)作为最终的权重向量；

若否，跳转至步骤A2；

S304.利用最终的权重向量，对小车之后的行进轨迹进行预测；对于要预测的位置点，将小车需要预测的位置点的横坐标作为输入，通过和最终的权重向量进行相乘得到小车预测位置点的纵坐标点，从而得到小车预测的位置点信息。

Images