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

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

【技术实现步骤摘要】

本专利技术涉及协同定位算法领域,特别为一种基于相对方位的多移动机器人协同定 位算法。
技术介绍
多机器人协同系统是近年来机器人研究的热点,在工业、军事、航空航天等领域具 有非常广阔的应用前景。多机器人之间相互观测,通过信息交换,实现信息共享,减少对外 部环境的依赖性,增强机器人群中单个机器人的感知能力,从而可获得比单个机器人更精 确的定位信息,称为多机器人的协同定位。大多数机器人系统在环境探索时要求机器人有 自定位功能,而其中的自主移动机器人的自身定位问题尤为重要,是其最重要的能力之一。 D.Fox等人把单机器人马尔科夫定位方法扩展到多机器人,每个机器人使用概率 分布描述自身位姿信息,再用其它机器人的观测值重新定义概率分布。A.Howard等应用最 大似然估计和数值优化方法,提出了基于相对观测的、以自我为中心的机器人队列自定位 算法。文献1:王玲,刘云辉,万建伟.基于相对方位的多机器人协同合作定位算法.传感 器技术学报.2007, 20(4) :794-799。在此基础上研究了并行式结构中,利用扩展卡尔曼滤波 融合内部和外部相对观测信息,有效地提高了系统定位精度。 现有EKF的定位算法及其改进算法实时性好,但易产生误差积累,初始值不当和 系统的强非线性都易导致滤波发散。有学者提出了UKF及CDKF的机器人定位算法来解决 此问题,改善了算法的鲁棒性、可实现性,提高了定位精度,但在多机器人的高维系统中,协 方差传播会出现非正定性,进而导致数值不稳定。粒子滤波算法也被广泛应用于多机器人 系统定位中,PF定位精度较好但迭代算法的复杂性导致其耗时为EKF的2?3倍,而很多 实时应用中计算资源有限。在真实的多机器人系统中,由于现有技术手段、平台资源等实际 因素的制约,除定位精度外,系统对定位算法的实时性仍有较高要求。综上所述,现有算法 存在难以兼顾定位精度和实时性的问题。
技术实现思路
为了克服现有技术的不足,本专利技术提供一种基于平方根无迹卡尔曼滤波(square rootunscentedKalmanfilter,SR-UKF)的多移动机器人协同定位算法,能同时兼顾系统 的定位精度与实时性要求。 -种,包括以下步骤: 步骤1,根据机器人的运动学方程和基于相对方位的量测方程,给出多移动机器人 自定位的动态模型; 步骤1.1,收集机器人群中每个机器人的信息; 步骤1. 2,建立机器人Ri的状态方程; 步骤1. 3,建立量测方程。 步骤2,采用SR-UKF滤波算法,以相对方位为输入对系统状态进行整体更新,实现 多移动机器人的协同自定位; 步骤2.1,给定机器人群初始状态向量、初始协方差阵,以及初始协方差矩阵的 cholesky因子; 步骤2. 2,对位置状态以及初始协方差矩阵的cholesky因子进行扩维; 步骤2. 3,采用UT变换中的对称性采样,生成表示机器人群的Sigma点; 步骤2. 4,对生成的Sigma点进行位置状态变量的一步预测; 步骤2. 5,重新计算表示机器人氏的新Sigma点; 步骤2. 6,机器人氏新的采样点以及机器人新的采样点,通过非线性量测方程传播 得到量测变量预测值,加权求和得到输出预测; 步骤2. 7,计算自协方差矩阵的平方根因子预测值; 步骤2.8,更新机器人氏的位置状态向量和协方差矩阵的平方根因子。 本专利技术与现有技术相比,其优点在于:(1)用无迹变换(unscentedtransform, 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时刻机器人氏,iG[ 1,N]在t时刻的位置状态本文档来自技高网
...

【技术保护点】
一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法,其特征在于,包括以下步骤: 步骤1,根据机器人的运动学方程和基于相对方位的量测方程,给出多移动机器人自定位的动态模型; 步骤2,采用SR‑UKF滤波算法,以相对方位为输入对系统状态进行整体更新,实现多移动机器人的协同自定位。

【技术特征摘要】
1. 一种基于平方根无迹卡尔曼滤波的多机器人协同定位算法,其特征在于,包括以下 步骤: 步骤1,根据机器人的运动学方程和基于相对方位的量测方程,给出多移动机器人自定 位的动态模型; 步骤2,采用SR-UKF滤波算法,以相对方位为输入对系统状态进行整体更新,实现多移 动机器人的协同自定位。2. 根据权利要求1所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法,其特 征在于,步骤1的具体过程为: 步骤1. 1,收集机器人群中每个机器人的信息; 步骤1. 2,建立机器人Ri的状态方程; 步骤1.3,建立量测方程。3. 根据权利要求2所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法,其特 征在于,步骤I. 1收集的信息包括: t时刻机器人Ri, i G [1,N]在t时刻的位置状态X) = ,其中纟,乂,#分别 为机器人Ri在t时刻的横轴坐标,纵轴坐标和运动方向; t时刻整个机器人群的位置状态z, =()d2,...,xf)T; t时刻机器人Ri, i G [1,N]观测到的机器人Rj, j尹i,j > i,j G [1,N]的相对方位 角/f。4. 根据权利要求2所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法,其特 征在于,步骤1. 2中机器人Ri的状态方程为其中,f( ?)为状态方程,《丨=(Ca))1为机器人Ri的控制输入,贫为t到t+1时刻内 机器人Ri的位移,< 为t到t+1时刻内机器人Ri的旋转角度,W = (wK)T为非加性的单位时间内位移D的误差,〇 a为单位时间内旋转角的误差。5. 根据权利要求2所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法,其特 征在于,步骤1.3中量测方程为h(*)为量测方程,%为量测噪声,服从#(0,<),且与状态方程中的过程噪声w丨不相 关,《为t时刻机器人Ri与机器人Rj的相对方位,〇 $相对方位的误差。6.根据权利要求1所述的基于平方根无迹卡尔曼滤波的多机器人协同定位算法,其特...

【专利技术属性】
技术研发人员:王碧霞李银伢
申请(专利权)人:南京理工大学
类型:发明
国别省市:江苏;32

网友询问留言 已有0条评论
  • 还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。

1