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

本发明专利技术涉及一种应用于非高斯噪声环境下的移动机器人状态估计方法。针对现有滤波器在非线非高斯噪声环境下其估计精度大打折扣，本发明专利技术将利用高斯和容积卡尔曼滤波解决非高斯非线性滤波问题，采用凸组合融合，对高斯和滤波器产生的大量高斯项，将Salmood高斯混合项合并方法和基于KL距离的高斯混合项合的方法B(i,j)准则进行加权融合，进而得到一种更好的高斯项合并方法。本发明专利技术能够有效提高移动机器人在非高斯非线性环境下的滤波精度与稳定性。

A state estimation method for mobile robot in non Gaussian noise environment

【技术实现步骤摘要】
一种应用于非高斯噪声环境下的移动机器人状态估计方法
本专利技术属于状态估计领域，涉及一种应用于非高斯噪声环境下的移动机器人状态估计方法。
技术介绍
移动机器人在运动过程中，存在系统延时、噪声干扰以及不确定因素干扰等问题，使机器人不可能很好的按照规划好的路线行驶。在移动机器人跟踪定位系统中，环境中存在非高斯噪声，导致跟踪精度低、稳定性差。高斯和滤波器(GSF)常被用于解决非线性、非高斯系统的状态估计问题，GSF使用高斯分布的加权和来逼近状态概率密度函数，同时可以近似任意分布。但是传统高斯和滤波器存在严重的局限性，即每次迭代时用于近似密度函数的高斯项的数量都会增加，高斯项的增加会增加算法的复杂度，限制算法的使用。目前解决高斯和滤波器中高斯项数目不断增加一般采用Salmood提出的高斯项合并方法，该高斯项合并方法的效果并不理想。本专利技术基于GSF原理，同时利用并行的CKF进行状态预测与量测更新，采用凸组合融合方法，对高斯和滤波器产生的大量高斯项，融合了Salmood高斯混合项合并方法和基于KL距离的高斯混合项合的方法B(i,j)准则。这样可以得到一种更好的高斯混合项合并方法更趋向于原分布，并且最终得到的高斯混合项的数目也比较理想，为解决移动机器人在非高斯状态估计中的跟踪精度低、稳定性差等问题，提供有效解决方法。
技术实现思路
本专利技术的目的是针对现有机器人状态估计滤波算法在非线性非高斯环境下滤波估计的性能急剧下降，提供了一种应用于非高斯噪声环境下的移动机器人状态估计方法。本专利技术解决技术问题所采取的技术方案为：本专利技术包括以下步骤：步骤一：考虑以下移动机器人运动模型。xk＝f(xk-1)+ωk-1zk＝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-1zk＝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为状态向量维数，cr为球面径向规则确定的容积点。步骤三：量测更新：非高斯量测噪声用高斯和原理表示为：其中分别代表第l个量测噪声分量的权重、均值、方差。对进行cholesky分解得到容积点估计与传播计算新息协方差和状态与量测交互协方差矩阵计算k时刻滤波增益其中zk为真实的量测值。步骤四：高斯和滤波算法过程中开始有I个高斯项，第一步估计结束后项数增加到I·J·L个，第k步估计结束后，高斯项增加到I·Jk·Lk个。随时间的推移，高斯和的项数一直增加，这样便会导致计算量不停的增加，限制了该算法的实用性。利用凸组合融合，将基于马氏距离的Salmond合并高斯项的方法与基于KL距离的B(i，j)方法的高斯项合并方法进行加权融合。Salmond:对于两个高斯项马氏距离小于一定阈值的高斯项进行合并原则如下：其合并方法如下：α1本文档来自技高网...

【技术保护点】
1.一种应用于非高斯噪声环境下的移动机器人状态估计方法，其特征在于该方法包括以下步骤：/n步骤一：建立移动机器人运动模型/nx

【技术特征摘要】
1.一种应用于非高斯噪声环境下的移动机器人状态估计方法，其特征在于该方法包括以下步骤：
步骤一：建立移动机器人运动模型
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的一步预测概率密度估计为：



k时刻有I个状态分量，一步预测后，每个状态分量又结合J个过程噪声分量，状态分量增加到I*J个，其中为分别为一步预测结束后第(i,j)个状态分量以及状态分量的协方差，表示与的权重；
步骤三：量测更新
将非高斯量测噪声用高斯和原理表示为：



其中分别代表第l个量测噪声分量...

【专利技术属性】
技术研发人员：王贺彬，葛泉波，刘华平，付东洋，申兴发，刘洺辛，
申请(专利权)人：广东海洋大学深圳研究院，
类型：发明
国别省市：广东;44