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

本实发明专利技术公开了一种基于平方根容积卡尔曼滤波（Square Root Cubature Kalman Filtering，SR‑CKF）的多机器人协同定位算法，属于机器人协同定位领域。整个算法分为两步：预测和更新。首先建立机器人运动方程和观测方程，利用相对方位作为测量值，进一步得到多机器人协同定位的动态模型。预测阶段包括：计算容积点集；通过状态方程传播容积点；机器人位姿状态预估计和平方根因子预测。更新阶段包括：计算容积点集；计算卡尔曼增益；计算位姿信息和平方根因子；更新状态向量、协方差阵和位姿信息。本发明专利技术实例在更新过程中直接传递目标状态均值和协方差矩阵的平方根因子，降低了计算的复杂度，确保了协方差矩阵的对称性和半正定性，改进了数值精度和稳定性。

【技术实现步骤摘要】

本专利技术涉及机器人协同定位领域，一种基于平方根容积卡尔曼滤波的多机器人协同定位算法。
技术介绍
随着人们的研究领域不断拓宽，从陆地、海洋延伸到深海以至未知的外太空，多机器人协同系统在军事、航天、服务、工业等领域发挥着越来越重要的作用。能够对未知复杂外界环境信息感知、建模并确定自身的位置，是机器人自主导航的前提和基础。多机器人协同定位是指多机器人之间相互观测，不依赖于外部环境通过共享环境信息，实现在共同环境下确定各自的位姿信息。目前，已有多种解决多机器人协同定位的方法，Jennings和Murray利用基于视觉的方法首次实现了机器人的协同定位。第一个机器人采用一种新的路标识别方法能够自主发现路标，第二个机器人将自己获取的图像帧与第一个机器人的图像帧进行比较，实现自身的相对定位。由于该方法依赖于准确的地图匹配，所以鲁棒性不强。Fox等人提出蒙特卡罗定位方法，首次将概率统计方法应用于多机器人协同定位。机器人所处的位置采用概率方式来描述，再交换共享其他机器人的相对观测信息，得到新的概率分布。但是在机器人识别等问题上该算法没有给出解决方案。Jo和lee等通过GPS数据差把不同机器人间的距离本文档来自技高网...

【技术保护点】
一种基于平方根容积卡尔曼滤波（Square Root Cubature Kalman Filtering，SR‑CKF）的多机器人协同定位算法，其特征在于，包括：预测阶段包括：计算容积点集；通过状态方程传播容积点；机器人位姿状态预估计和平方根因子预测；更新阶段包括：计算容积点集；计算卡尔曼增益；计算位姿信息和平方根因子；更新状态向量、协方差阵和位姿信息。

【技术特征摘要】
1.一种基于平方根容积卡尔曼滤波（SquareRootCubatureKalmanFiltering，SR-CKF）的多机器人协同定位算法，其特征在于，包括：预测阶段包括：计算容积点集；通过状态方程传播容积点；机器人位姿状态预估计和平方根因子预测；更新阶段包括：计算容积点集；计算卡尔曼增益；计算位姿信息和平方根因子；更新状态向量、协方差阵和位姿信息。2.根据权利要求1所述的一种基于平方根容积卡尔曼滤波的多机器人协同定位算法，其特征在于，每个机器人都配有可感知自身位姿信息的内部传感器和感知外部环境信息的外部传感器，通过携带的外部传感器可有效的探测到附近机器人对其的观测信息，通过通信协议每个机器人感知的外部环境信息可与其他机器人感知的数据之间进行交流融合；多个机器人组成队列在二维环境下移动，为机器人在时刻的位姿，其中、和分别表示机器人在时刻的横坐标，纵坐标和运动方向，则时机器人队列的位姿信息可以表示为：。3.根据权利要求1或2所述的一种基于平方根容积卡尔曼滤波的多机器人协同定位算法，其特征在于，计算容积点集之前还包括：建立机器人运动方程；在队列的行进过程中，向量为系统时刻的状态向量，为维的向量；向量表示地图特征路标集合矩阵；机器人的运动方程均相同，故以机器人为例其运动模型可以表示为：其中表示控制输入，为维的向量；为运动噪声，其方差为，服从于的高斯分布。4.根据权利要求1或2所述一种基于平方根容积卡尔曼滤波的多机器人协同定位算法，其特征在于，计算容积点集之前还包括：建立机器人观测方程；在机器人运动的过程中，表示时刻系统的观测向量矩阵，为维的向量，机器人在时刻观测到机器人，由外部传感器得到它们之间的相对方位信息，，分别为它们各自的运动方向，为时刻机器人观测到机器人的相对方位角，则：可以得到一般形式的观测模型：其中表示观测噪声矩阵，其方差为，服从于的高斯分布。5.根据权利要求3所述的一种基于平方根容积卡尔曼滤波的多机器人协同...

【专利技术属性】
技术研发人员：陈孟元，李朕阳，郎朗，
申请(专利权)人：安徽工程大学，
类型：发明
国别省市：安徽;34