CN104330083A - 基于平方根无迹卡尔曼滤波的多机器人协同定位算法 - Google Patents

Abstract

本发明提供一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法，包括以下步骤：步骤1，根据机器人的运动学方程和基于相对方位的量测方程，给出多移动机器人自定位的动态模型；步骤2，采用SR-UKF滤波算法，以相对方位为输入对系统状态进行整体更新，实现多移动机器人的协同自定位。本发明提供的算法，能同时兼顾系统的定位精度与实时性要求，计算复杂度下降，大大缩短算法耗时的同时保证了协方差矩阵的半正定性和数值稳定性，可为后续多移动机器人的协同定位提供技术支撑。

Description

基于平方根无迹卡尔曼滤波的多机器人协同定位算法

技术领域

本发明涉及协同定位算法领域，特别为一种基于相对方位的多移动机器人协同定位算法。

背景技术

多机器人协同系统是近年来机器人研究的热点，在工业、军事、航空航天等领域具有非常广阔的应用前景。多机器人之间相互观测，通过信息交换，实现信息共享，减少对外部环境的依赖性，增强机器人群中单个机器人的感知能力，从而可获得比单个机器人更精确的定位信息,称为多机器人的协同定位。大多数机器人系统在环境探索时要求机器人有自定位功能，而其中的自主移动机器人的自身定位问题尤为重要，是其最重要的能力之一。

D.Fox等人把单机器人马尔科夫定位方法扩展到多机器人，每个机器人使用概率分布描述自身位姿信息，再用其它机器人的观测值重新定义概率分布。A.Howard等应用最大似然估计和数值优化方法，提出了基于相对观测的、以自我为中心的机器人队列自定位算法。文献1：王玲，刘云辉，万建伟.基于相对方位的多机器人协同合作定位算法.传感器技术学报.2007,20(4):794-799。在此基础上研究了并行式结构中，利用扩展卡尔曼滤波融合内部和外部相对观测信息，有效地提高了系统定位精度。

现有EKF的定位算法及其改进算法实时性好，但易产生误差积累，初始值不当和系统的强非线性都易导致滤波发散。有学者提出了UKF及CDKF的机器人定位算法来解决此问题，改善了算法的鲁棒性、可实现性，提高了定位精度，但在多机器人的高维系统中，协方差传播会出现非正定性，进而导致数值不稳定。粒子滤波算法也被广泛应用于多机器人系统定位中，PF定位精度较好但迭代算法的复杂性导致其耗时为EKF的2～3倍，而很多实时应用中计算资源有限。在真实的多机器人系统中，由于现有技术手段、平台资源等实际因素的制约，除定位精度外，系统对定位算法的实时性仍有较高要求。综上所述，现有算法存在难以兼顾定位精度和实时性的问题。

发明内容

为了克服现有技术的不足，本发明提供一种基于平方根无迹卡尔曼滤波(square root unscented Kalman filter,SR-UKF)的多移动机器人协同定位算法，能同时兼顾系统的定位精度与实时性要求。

一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法，包括以下步骤：

步骤1，根据机器人的运动学方程和基于相对方位的量测方程，给出多移动机器人自定位的动态模型；

步骤1.1，收集机器人群中每个机器人的信息；

步骤1.2，建立机器人R_i的状态方程；

步骤1.3，建立量测方程。

步骤2，采用SR-UKF滤波算法，以相对方位为输入对系统状态进行整体更新，实现多移动机器人的协同自定位；

步骤2.1，给定机器人群初始状态向量、初始协方差阵，以及初始协方差矩阵的cholesky因子；

步骤2.2，对位置状态以及初始协方差矩阵的cholesky因子进行扩维；

步骤2.3，采用UT变换中的对称性采样，生成表示机器人群的Sigma点；

步骤2.4，对生成的Sigma点进行位置状态变量的一步预测；

步骤2.5，重新计算表示机器人R_i的新Sigma点；

步骤2.6，机器人R_i新的采样点以及机器人新的采样点，通过非线性量测方程传播得到量测变量预测值，加权求和得到输出预测；

步骤2.7，计算自协方差矩阵的平方根因子预测值；

步骤2.8，更新机器人R_i的位置状态向量和协方差矩阵的平方根因子。

本发明与现有技术相比，其优点在于：(1)用无迹变换(unscented transform，UT)生成的Sigma点表示概率分布，避免了线性化采样点带来的近似误差，从而提高了定位精度；(2)采用相对方位信息作为量测输入量，进一步提高了定位精度；(3)为满足实时性要求，SR-UKF算法避免了计算雅克比矩阵，降低了计算复杂度，滤波增益的计算中采用自协方差阵平方根因子的嵌套逆，该因子是方阵且为三角阵，计算复杂度下降，大大缩短算法耗时；(4)更新中直接传递协方差矩阵的平方根因子，保证了协方差矩阵的半正定性和数值稳定性。

下面结合附图进一步说明本发明。

附图说明

图1是本发明的仿真系统中所用相对方位观测量的含义图；

图2是本发明中SR-UKF滤波算法实现的流程图。

具体实施方式

结合图2，一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法，首先考虑N个机器人组成的队列在二维环境中沿不同方向运动的情况，且N个机器人在环境探索时均处于同等地位。为了满足协同定位以及系统可观测要求，每个机器人需具备下列条件：(1)在二维环境中无障碍运动，每个机器人配有测量自身位置变化的内置传感器(采用旋转编码器测量旋转角，直尺编码器测量单位时间内位移)；(2)每个机器人都配有外部传感器，能探测并识别周围的机器人并测量相对方位信息(采用激光测距仪探测，用全景摄像机配合机器人上不同的可视标记来确定测得的信息来源于哪个机器人)；(3)每个机器人配有通信设备，能进行必要的信息交互，掌握自身在机器人群中的相对位置。

一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法，包括以下步骤：

步骤1，根据机器人的运动学方程和基于相对方位的量测方程，给出多移动机器人自定位的动态模型；

步骤2，采用SR-UKF滤波算法，以相对方位为输入对系统状态进行整体更新，实现多移动机器人的协同自定位。

步骤1的具体过程为：

步骤1.1，收集机器人群中每个机器人的信息，包括：

t时刻机器人R_i，i∈[1,N]在t时刻的位置状态其中 分别为机器人R_i在t时刻的横轴坐标，纵轴坐标和运动方向；

t时刻整个机器人群的位置状态

t时刻机器人R_i，i∈[1,N]观测到的机器人R_j,j≠i,j＞i,j∈[1,N]的相对方位角

步骤1.2，建立机器人R_i的状态方程

$X_{t+1}^i=f(X_t^i,u_t^i,w_t^i)=\left[\begin{array}{c}{x}_t^i+(D_t^i+w_D^i)\cos ({\mathrm{\θ}}_t^i+{\mathrm{\α}}_t^i+w_{\mathrm{\α}}^i)\\ y_t^i+(D_t^i+w_D^i)\sin ({\mathrm{\θ}}_t^i+{\mathrm{\α}}_t^i+w_{\mathrm{\α}}^i)\\ {\mathrm{\θ}}_t^i+{\mathrm{\α}}_t^i+w_{\mathrm{\α}}^i\end{array}\right],$

其中，f(·)为状态方程，为机器人R_i的控制输入，为t到t+1时刻内机器人R_i的位移，为t到t+1时刻内机器人R_i的旋转角度，为非加性的高斯过程噪声，其均值为 $Q_{w_t^i}=\mathrm{diag}\{{\mathrm{\σ}}_D^2,{\mathrm{\σ}}_{\mathrm{\α}}^2\},$ ${\mathrm{\δ}}_{\mathrm{tl}}=\left\{\begin{array}{c}1,t=l\\ 0,t\≠l\end{array}\right.,$ σ_D为单位时间内位移D的误差，σ_α为单位时间内旋转角的误差；

步骤1.3，建立量测方程，结合图1，在机器人群移动过程中，t时刻机器人R_i通过外部传感器探测到机器人R_j，并测量与机器人R_j的相对方位如图1所示，其相对方位角为

${\mathrm{\γ}}_t^{\mathrm{ij}}=h(X_t^i,X_t^j)+v_{\mathrm{\φ}}^i={\mathrm{\φ}}_t^{\mathrm{ij}}={\tan }^{-1}\left(\frac{y_t^j-y_t^i}{x_t^j-x_t^i}\right)-{\mathrm{\θ}}_t^i+v_{\mathrm{\φ}}^i$

其中，为t时刻R_i观测到的机器人R_j的相对方位角，h(·)为量测方程，为量测噪声，服从且与状态方程中的过程噪声不相关，为t时刻机器人R_i与机器人R_j的相对方位，σ_φ相对方位φ的误差。

步骤2的具体过程为：

步骤2.1，给定机器人群初始状态向量初始协方差阵以及初始协方差矩阵的cholesky因子

${\hat{X}}_0^i=E\left[X_0^i\right]$

$P_0^i=E\left[(X_0^i-{\hat{X}}_0^i){(X_0^i-{\hat{X}}_0^i)}^T\right]$

$S_0^{X,i}=\mathrm{chol}\left\{P_0^i\right\}$

状态维数为n_x。

步骤2.2，对位置状态以及进行扩维，即

${\hat{X}}_0^{a,i}=E[X_0^i,w_0^i]=[{\hat{X}}_0^i,0]$

$S_0^{a,i}=\left[\begin{array}{cc}{S}_0^{X,i}& 0\\ 0& S_0^{w_0^i,i}\end{array}\right]$

其中 $S_0^{w_0^i,i}=\mathrm{chol}\left\{Q_{w_0^i}\right\},$ $Q_{w_0^i}=\mathrm{diag}\{{\mathrm{\σ}}_D^2,{\mathrm{\σ}}_{\mathrm{\α}}^2\},$ 的实际维数为n_a＝n_x+n_w，n_w为过程噪声的维数。

步骤2.3，生成表示机器人R_i的Sigma点：采用UT变换中的对称性采样，选取一系列确定的采样点对状态向量的概率密度函数进行近似，计算相应权值，具体过程为：

由位置状态变量的实际维数n_a计算权值

${\mathrm{\ωa}}_0^m={\mathrm{\λ}}_a/(n_a+{\mathrm{\λ}}_a)$

${\mathrm{\ωa}}_0^c={\mathrm{\λ}}_a/(n_a+{\mathrm{\λ}}_a)+(1-{{\mathrm{\α}}_2}^2+\mathrm{\β})$

${\mathrm{\ωa}}_{\mathrm{ui}}^m={\mathrm{\ωa}}_{\mathrm{ui}}^c={\mathrm{\λ}}_a/({2n}_a+{2\mathrm{\λ}}_a)$

其中，和分别为第一个采样点的位置状态权值和误差协方差阵权值，和表示第ui个采样点对应的位置状态权值和误差协方差阵权值，ui＝1,2,...,2n_a；β为描述先验分布信息，表示最小化高阶项影响的参数；其中，λ_a＝α₂ ²(n_a+κ)-n_a，κ为尺度参数，α₂为正值比例缩放因子；

经UT变换后t时刻机器人R_i的Sigma点为

${\mathrm{\χ}}_t^i=\left[\begin{array}{ccc}{\hat{X}}_t^{a,i}& {\hat{X}}_t^{a,i}+{\mathrm{\η}}_a{S}_t^{a,i}& {\hat{X}}_t^{a,i}-{\mathrm{\η}}_a{S}_t^{a,i}\end{array}\right],$

其中 ${\mathrm{\η}}_a=\sqrt{(n_a+{\mathrm{\λ}}_a)};$

机器人群所有机器人经过UT变换后得到的Sigma点组合成机器人群的Sigma点。

步骤2.4，对生成的Sigma点进行位置状态变量的一步预测，具体过程为：

通过非线性状态函数f(·)传播为加权求和得机器人R_i位置状态的一步预测值

${\mathrm{\χ}}_{t+1/t}^{*i}=f({\mathrm{\χ}}_t^i,u_t^i)$

${\hat{X}}_{t+1/t}^i=\underset{\mathrm{ui}=0}{\overset{{2n}_a}{\mathrm{\Σ}}}{\mathrm{\ωa}}_{\mathrm{ui}}^m{\mathrm{\χ}}_{\mathrm{ui},t+1/t}^{*i};$

误差协方差阵的平方根一步预测：

$S_{t+1|t}^i=\mathrm{qr}\left\{\right[\sqrt{{\mathrm{\ωa}}_{1:2n_a}^c}({\mathrm{\χ}}_{1:2n_a,t+1|t}^{*i}-{\hat{X}}_{t+1|t}^i)\left]\right\}$

$S_{t+1|t}^i=\mathrm{cholupdate}\{S_{t+1|t}^i,{\mathrm{\χ}}_{0,t+1|t}^{*i}-{\hat{X}}_{t+1|t}^i,{\mathrm{\ωa}}_0^c\}$

其中qr{S}返回矩阵P的cholesky因子，P＝SS^T，因子更新cholupdate{S,B,±v}为返回矩阵的上三角cholesky因子。

步骤2.5，利用和重新计算表示机器人R_i的新Sigma点，其维数为n_b＝n_x，个数为L_b＝2n_b+1，相应权值计算，具体过程为：

${\mathrm{\ωb}}_0^m={\mathrm{\λ}}_b/(n_b+{\mathrm{\λ}}_b)$

${\mathrm{\ωb}}_0^c={\mathrm{\λ}}_b/(n_b+{\mathrm{\λ}}_b)+(1-{{\mathrm{\α}}_2}^2+\mathrm{\β})$

${\mathrm{\ωb}}_{\mathrm{ui}}^m={\mathrm{\ωb}}_{\mathrm{ui}}^c={\mathrm{\λ}}_b/({2n}_b+{2\mathrm{\λ}}_b)$

其中，和为第一个新采样点的位置状态权值和误差协方差阵权值，和表示第ui个新采样点对应的的位置状态权值和误差协方差阵权值，ui＝1,2,...,2n_b，λ_b＝α₂ ²(n_b+κ)-n_b，n_b＝n_x；

新Sigma点 ${\mathrm{\χ}}_{t+1|t}^i=\left[\begin{array}{ccc}{\hat{X}}_{t+1|t}^i& {\hat{X}}_{t+1|t}^i+{\mathrm{\η}}_b{S}_{t+1|t}^i& {\hat{X}}_{t+1|t}^i-{\mathrm{\η}}_b{S}_{t+1|t}^i\end{array}\right],$ 其中 ${\mathrm{\η}}_b=\sqrt{(n_b+{\mathrm{\λ}}_b)}.$

步骤2.6，结合机器人R_i新的采样点以及机器人R_j新的采样点通过非线性量测方程传播得到量测变量预测值加权求和得到输出预测

${\mathrm{\γ}}_{\mathrm{ui},t+1|t}^i=h({\mathrm{\χ}}_{t+1|t}^i,{\mathrm{\χ}}_{t+1|t}^j)+v_{\mathrm{\φ}}^i$

${\hat{Y}}_{t+1|t}^i=\underset{\mathrm{ui}=0}{\overset{{2n}_b}{\mathrm{\Σ}}}{\mathrm{\ωb}}_{\mathrm{ui}}^m{\mathrm{\γ}}_{\mathrm{ui},t+1|t}^i.$

步骤2.7，计算自协方差矩阵的平方根因子预测值

$S_{t+1}^{Y,i}=\mathrm{qr}\left\{\right[\sqrt{{\mathrm{\ωb}}_{1:2n_b}^c}({\mathrm{\γ}}_{1:2n_b,t+1|t}^i-{\hat{Y}}_{t+1|t}^i)\sqrt{R_{\mathrm{\φ}}^i}\left]\right\}$

$S_{t+1}^{Y,i}=\mathrm{cholupdate}\{S_{t+1}^{Y,i},{\mathrm{\γ}}_{0,t+1|t}^i-{\hat{Y}}_{t+1|t}^i,{\mathrm{\ωb}}_0^c\}$

其中为t时刻机器人R_i观测其他机器人的量测噪声的协方差矩阵

$R_{\mathrm{\φ}}^i=\mathrm{diag}\left\{\right[{\mathrm{\σ}}_{{\mathrm{\φ}}_{i1}}^2...{\mathrm{\σ}}_{{\mathrm{\φ}}_{\mathrm{ij}}}^2...{\mathrm{\σ}}_{{\mathrm{\φ}}_{\mathrm{iN}}}^2\left]\right\},i\≠j,j=\mathrm{1,2},...,N,$

其中表示第i个机器人与第j个机器人之间相对方位φ_ij的误差；

计算互协方差阵 $P_{X_{t+1}{Y}_{t+1}}^i=\underset{\mathrm{ui}=0}{\overset{{2n}_b}{\mathrm{\Σ}}}{\mathrm{\ωb}}_{\mathrm{ui}}^{c,i}({\mathrm{\χ}}_{\mathrm{ui},t+1|t}^{*i}-{\hat{X}}_{t+1|t}^i){({\mathrm{\γ}}_{\mathrm{ui},t+1|t}^i-{\hat{Y}}_{t+1|t}^i)}^T$

计算滤波增益阵 $K_{t+1}=(P_{X_{t+1}{Y}_{t+1}}^i/{\left(S_{t+1}^{Y,i}\right)}^T)/S_{t+1}^{Y,i}.$

步骤2.8，更新机器人R_i的位置状态向量和协方差矩阵的平方根因子即

${\hat{X}}_{t+1}^i={\hat{X}}_{t+1|t}^i+K_{t+1}(Y_{t+1}^{\mathrm{measure},i}-{\hat{Y}}_{t+1|t}^i)$

$U=K_{t+1}{S}_{t+1}^{Y,i}$

$S_{t+1}^{X,i}=\mathrm{cholupdate}\{S_{t+1|t}^i,U,-1\}$

其中是机器人R_i在t+1时刻观测其它N-1个机器人的相对方位角的实际量测值。

每当机器人R_i获得与其它机器人的相对量测信息，其状态信息就会得到更新，机器人间的相对关系就被体现在了机器人的位置估计中；反之当量测信息缺失时，机器人R_i的状信息在该时刻就不更新，但其它机器人的状态会更新。而在下一时刻，通过新的量测更新，其它机器人更新的结果能影响到前一时刻量测信息缺失的那个机器人。由于i＝1,2,...N,t∈{0,1,2,...,∞}，在每个时刻t其它机器人的状态信息也是按上述流程更新的。

Claims (6)

1.一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，包括以下步骤： 

步骤1，根据机器人的运动学方程和基于相对方位的量测方程，给出多移动机器人自定位的动态模型； 

步骤2，采用SR-UKF滤波算法，以相对方位为输入对系统状态进行整体更新，实现多移动机器人的协同自定位。 

2.根据权利要求1所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，步骤1的具体过程为： 

步骤1.1，收集机器人群中每个机器人的信息； 

步骤1.2，建立机器人R_i的状态方程； 

步骤1.3，建立量测方程。 

3.根据权利要求2所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，步骤1.1收集的信息包括： 

t时刻机器人R_i，i∈[1,N]在t时刻的位置状态其中 分别为机器人R_i在t时刻的横轴坐标，纵轴坐标和运动方向； 

t时刻整个机器人群的位置状态

t时刻机器人R_i，i∈[1,N]观测到的机器人R_j,j≠i,j＞i,j∈[1,N]的相对方位角

4.根据权利要求2所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，步骤1.2中机器人R_i的状态方程为 

其中，f(·)为状态方程，为机器人R_i的控制输入，为t到t+1时刻内机器人R_i的位移，为t到t+1时刻内机器人R_i的旋转角度， 为非加性的高斯过程噪声，其均值为 σ_D为单位时间内位移D的误差，σ_α为单位时间内旋转角的误差。 

5.根据权利要求2所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，步骤1.3中量测方程为 

h(·)为量测方程，为量测噪声，服从且与状态方程中的过程噪声不相关，为t时刻机器人R_i与机器人R_j的相对方位，σ_φ相对方位φ的误差。 

6.根据权利要求1所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法，其特征在于，步骤2的具体过程为： 

步骤2.1，给定机器人群初始状态向量初始协方差阵以及初始协方差矩阵的cholesky因子

状态维数为n_x； 

步骤2.2，对位置状态以及进行扩维，即 

其中 的实际维数为n_a＝n_x+n_w，n_w为过程噪声的维数； 

步骤2.3，采用UT变换中的对称性采样，生成表示机器人群的Sigma点， 具体过程为： 

由位置状态变量的实际维数n_a计算权值 

其中，和分别为第一个采样点的位置状态权值和误差协方差阵权值，和表示第ui个采样点对应的位置状态权值和误差协方差阵权值，ui＝1,2,...,2n_a； 

β为描述先验分布信息，表示最小化高阶项影响的参数； 

其中，κ为尺度参数，α₂为正值比例缩放因子； 

经UT变换后t时刻机器人R_i的Sigma点 其中

机器人群所有机器人经过UT变换后得到的Sigma点组合成机器人群的Sigma点； 

步骤2.4，对生成的Sigma点进行位置状态变量的一步预测，具体过程为： 

通过非线性状态函数f(·)传播为加权求和得机器人R_i位置状态的一步预测值

误差协方差阵的平方根一步预测： 

其中qr{S}返回矩阵P的cholesky因子，P＝SS^T，因子更新 cholupdate{S,B,±v}为返回矩阵的上三角cholesky因子； 

步骤2.5，重新计算表示机器人R_i的新Sigma点，具体过程为： 

其中，和为第一个新采样点的位置状态权值和误差协方差阵权值， 和表示第ui个新采样点对应的的位置状态权值和误差协方差阵权值，ui＝1,2,...,2n_b，λ_b＝α₂ ²(n_b+κ)-n_b，n_b＝n_x； 

新Sigma点其中

步骤2.6，机器人R_i新的采样点以及机器人R_j新的采样点通过非线性量测方程传播得到量测变量预测值加权求和得到输出预测

步骤2.7，计算自协方差矩阵的平方根因子预测值 

其中为t时刻机器人R_i观测其他机器人的量测噪声的协方差矩阵 

计算互协方差阵

计算滤波增益阵

步骤2.8，更新机器人R_i的位置状态向量和协方差矩阵的平方根因子 即 

其中是机器人R_i在t+1时刻观测其它N-1个机器人的相对方位角的实际量测值。