[发明专利]基于平方根无迹卡尔曼滤波的多机器人协同定位算法

说明书

技术领域

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

背景技术

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

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滤波算法实现的流程图。

具体实施方式