一种应用于非高斯噪声环境下的移动机器人状态估计方法与流程

本发明属于状态估计领域，涉及一种应用于非高斯噪声环境下的移动机器人状态估计方法。

背景技术：

移动机器人在运动过程中，存在系统延时、噪声干扰以及不确定因素干扰等问题，使机器人不可能很好的按照规划好的路线行驶。在移动机器人跟踪定位系统中，环境中存在非高斯噪声，导致跟踪精度低、稳定性差。高斯和滤波器(gsf)常被用于解决非线性、非高斯系统的状态估计问题，gsf使用高斯分布的加权和来逼近状态概率密度函数，同时可以近似任意分布。但是传统高斯和滤波器存在严重的局限性，即每次迭代时用于近似密度函数的高斯项的数量都会增加，高斯项的增加会增加算法的复杂度，限制算法的使用。

目前解决高斯和滤波器中高斯项数目不断增加一般采用salmood提出的高斯项合并方法，该高斯项合并方法的效果并不理想。本发明基于gsf原理，同时利用并行的ckf进行状态预测与量测更新，采用凸组合融合方法，对高斯和滤波器产生的大量高斯项，融合了salmood高斯混合项合并方法和基于kl距离的高斯混合项合的方法b(i,j)准则。这样可以得到一种更好的高斯混合项合并方法更趋向于原分布，并且最终得到的高斯混合项的数目也比较理想，为解决移动机器人在非高斯状态估计中的跟踪精度低、稳定性差等问题，提供有效解决方法。

技术实现要素：

本发明的目的是针对现有机器人状态估计滤波算法在非线性非高斯环境下滤波估计的性能急剧下降，提供了一种应用于非高斯噪声环境下的移动机器人状态估计方法。

本发明解决技术问题所采取的技术方案为：

本发明包括以下步骤：

步骤一：考虑以下移动机器人运动模型。

xk＝f(xk-1)+ωk-1

zk＝h(xk)+vk

其中xk表示移动机器人在x轴方向的位置，zk为传感器对移动机器人状态的观测值，f(·)为移动机器人的状态转移矩阵，h(·)是传感器的观测模型，ωk-1为非高斯过程噪声，vk为非高斯量测噪声k表示时刻。

步骤二：时间更新：将非高斯过程噪声wk用高斯和表示为

其中分别代表第j个过程噪声分量的权重、均值、方差。则k时刻一步转移概率p(xk|xk-1)可以表示为：

其中为根据机器人运动模型得到的移动机器人状态的一步预测。

根据贝叶斯滤波递推公式可得k时刻xk的一步预测概率密度估计为p(xk|zk-1)为

k时刻有i个状态分量，一步预测后，每个状态分量又结合j个过程噪声分量，状态分量增加到i*j个，其中为分别为一步预测结束后第(i,j)个状态分量以及状态分量的协方差，表示它们的权重。

步骤三：量测更新：将非高斯量测噪声用高斯和原理表示为：

其中分别代表第l个量测噪声分量的权重、均值、方差。则可以得到似然概率密度p(zk|xk)为：

其中zk|k-1为观测值的预测。

当接收到k时刻量测值zk时，则可以得到后验概率密度p(xk|z1:k)为：

其中分别表示第(i,j,l)项状态分量与其协方差，为与的权重。

步骤四：利用凸组合融合，将基于马氏距离的salmond合并高斯项的方法与基于kl距离的b(i，j)方法的高斯项合并方法进行加权融合。

和分别为利用salmond合并高斯项的方法与利用b(i,j)方法合并后的高斯混合项中的第i个状态分量与其协方差。其中α1′、为利用salmond合并后的高斯项的权重，以及合并后x的概率密度。α′2、为利用b(i,j)方法合并后的高斯项的的权重，以及合并后x的概率密度。分别表示融合后第i个状态分量与其协方差，β′ii为与的权重。

最终融合得到k时刻的移动机器人的状态估计xk|k和估计误差协方差pk|k如下：

然后将k时刻的机器人状态估计作为参数输入到步骤二中的一步预测概率密度中，去估计k+1时刻的机器人的运动状态。这样一直进行迭代，便可估计移动机器人在时间t内的运动状态。

本发明的有益效果：本发明提出一种应用于非高斯噪声环境下的移动机器人状态估计方法，该算法采用容积卡尔曼滤波框架，并结合高斯和滤波过程，利用凸组合融合进行高斯和滤波中高斯项的合并，从而对高斯和滤波过程中产生大量的高斯项进行有效的合并，提升估计精度，能够解决非线性非高斯噪声环境下的机器人状态估计问题。

附图说明

图1.本发明流程图。

具体实施方式

以下结合附图对本发明作进一步说明。

如图1所示，本发明方法具体实施方式是：

步骤一：给定移动机器人具体运动模型，其进行转弯运动，目的为了在非高斯噪声下估计移动机器人的运动状态：

xk＝f(ω)xk-1+wk-1

zk＝h(xk)+vk

其中：

移动机器人状态向量其中(xk,yk)表示移动机器人在k时刻的位置，分别表示x轴方向的分速度与y轴方向的分速度。h(xk)为传感器的观测模型，zk为传感器在k时刻对移动机器人状态的观测值。状态转移矩阵f(ω)是将机器人由一个状态转为另一个状态，其中的ω表示移动机器人运动的角速度，δ为采样周期。wk-1为非高斯过程噪声，vk为非高斯量测噪声。

步骤二：时间更新：高斯和原理：任意分布的概率密度函数都能够用有限个高斯项的累加进行近似。

假设已知移动机器人k-1时刻的运动状态，将k-1时刻运动状态用高斯混合模型可以表示为：

其中分别表示第i个状态分量以及其协方差，为其权重。非高斯过程噪声wk用高斯和表示为

其中分别代表第j个过程噪声分量的权重、均值、方差。对进行cholesky分解得到即：

容积点传播

其中各个高斯项权重为：

各个高斯项状态的一步预测为：

各个高斯分量的状态一步预测误差协方差矩阵为：

其中β^r为容积点权重，nx为状态向量维数，c^r为球面径向规则确定的容积点。

步骤三：量测更新：非高斯量测噪声用高斯和原理表示为：

其中分别代表第l个量测噪声分量的权重、均值、方差。对进行cholesky分解得到

容积点估计与传播

计算新息协方差和状态与量测交互协方差矩阵

计算k时刻滤波增益

其中zk为真实的量测值。

步骤四：高斯和滤波算法过程中开始有i个高斯项，第一步估计结束后项数增加到i·j·l个，第k步估计结束后，高斯项增加到i·j^k·l^k个。随时间的推移，高斯和的项数一直增加，这样便会导致计算量不停的增加，限制了该算法的实用性。利用凸组合融合，将基于马氏距离的salmond合并高斯项的方法与基于kl距离的b(i，j)方法的高斯项合并方法进行加权融合。

salmond:对于两个高斯项马氏距离小于一定阈值的高斯项进行合并原则如下：

其合并方法如下：

α1′＝α^m+αⁿ

μ1′＝(α^mμ^m+αⁿμⁿ)/(α^m+αⁿ)

∑′1＝(α^m∑^m+αⁿ∑ⁿ)/(α^m+αⁿ)

其中d²(m,n)为两个高斯项的马氏距离，(α^m,μ^m,∑^m)与(αⁿ,μⁿ,∑ⁿ)分别表示合并前第m个和第n个高斯项的权重、均值、方差。

α1′、μ1′、∑′1为利用salmond方法合并后高斯项的权重、均值、方差。

b(i,j)方法：对于两个高斯项kl距离小于一定阈值的高斯项进行合并原则如下：

其合并方法如下：

α′2＝α^m+αⁿ

μ′2＝αn|mnμ^m+αm|mnμⁿ

∑mn＝αn|mn∑n+αm|mn∑m+αn|mnαm|mn(μ^m-μⁿ)(μ^m-μⁿ)^t

α′2、μ′2、∑′2为利用b(i,j)准则合并后高斯项的权重、均值、方差。

其中相对权重αm|mn、αn|mn定义如下：

αm|mn＝αm/(αm+αn)

αn|mn＝αn/(αm+αn)

对来自两者的高斯项进行凸组合融合：

其中α1′、α′2、分别为第一种和第二种高斯项合并方法所得到的权重，以及x的概率密度。分别表示融合后第i个状态分量与其协方差，β′ii为其权重。和分别为利用salmond方法与利用b(i,j)方法合并后的高斯混合项中的第i个状态分量与其协方差。

最终融合得到k时刻的移动机器人的状态估计xk|k和估计误差协方差pk|k如下：

然后将k时刻的机器人状态估计作为参数输入到步骤二中的一步预测概率密度中，去估计k+1时刻的机器人的运动状态。这样一直进行迭代，便可估计移动机器人在时间t内的运动状态。

综上，一种应用于非高斯噪声环境下的移动机器人状态估计方法，可以有效的解决非线性非高斯噪声环境下的移动机器人状态估计问题，能够有效提升状态估计的精度与稳定性。