CN106482736A

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

Info

Publication number: CN106482736A
Application number: CN201610538987.1A
Authority: CN
Inventors: 陈孟元; 李朕阳; 郎朗
Original assignee: Anhui Polytechnic University
Current assignee: Anhui Polytechnic University
Priority date: 2016-07-11
Filing date: 2016-07-11
Publication date: 2017-03-08
Anticipated expiration: 2036-07-11
Also published as: CN106482736B

Abstract

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

Description

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

技术领域

本发明涉及机器人协同定位领域，一种基于平方根容积卡尔曼滤波的多机器人协同定位算法。

背景技术

随着人们的研究领域不断拓宽，从陆地、海洋延伸到深海以至未知的外太空，多机器人协同系统在军事、航天、服务、工业等领域发挥着越来越重要的作用。能够对未知复杂外界环境信息感知、建模并确定自身的位置，是机器人自主导航的前提和基础。多机器人协同定位是指多机器人之间相互观测，不依赖于外部环境通过共享环境信息，实现在共同环境下确定各自的位姿信息。

目前，已有多种解决多机器人协同定位的方法，Jennings和Murray利用基于视觉的方法首次实现了机器人的协同定位。第一个机器人采用一种新的路标识别方法能够自主发现路标，第二个机器人将自己获取的图像帧与第一个机器人的图像帧进行比较，实现自身的相对定位。由于该方法依赖于准确的地图匹配，所以鲁棒性不强。Fox等人提出蒙特卡罗定位方法，首次将概率统计方法应用于多机器人协同定位。机器人所处的位置采用概率方式来描述，再交换共享其他机器人的相对观测信息，得到新的概率分布。但是在机器人识别等问题上该算法没有给出解决方案。Jo和lee等通过GPS数据差把不同机器人间的距离关联起来，进行机器人队列的相对定位。但是在GPS误差较大的情况下，该方法的实时性与精确性较差。王玲和邵金鑫等利用扩展卡尔曼滤波，将机器人内部传感器信息与队列中其他机器人之间的相对观测量相融合，以确定系统中的每个机器人的位置。该算法具有较好的实时性，但是该算法容易产生误差积累，并且其计算量会随地图的增大而急剧增大。

发明内容

本发明的目的在于提供一种基于SR-CKF的相对方位多机器人协同定位算法，以解决现有技术中导致的上述多项缺陷。

本发明实施例采用如下技术方案：

一种基于SR-CKF的相对方位多机器人协同定位算法，整个算法分为两步：预测和更新。首先建立机器人运动方程和观测方程，利用相对方位作为测量值，进一步得到多机器人协同定位的动态模型。预测阶段包括：计算容积点集；通过状态方程传播容积点；机器人位姿状态预估计和平方根因子预测。更新阶段包括：计算容积点集；计算卡尔曼增益；计算位姿信息和平方根因子；更新状态向量、协方差阵和位姿信息。

优选的，所述计算容积点集之前还包括：建立机器人运动方程。

在队列的行进过程中，向量为系统时刻的状态向量，为维的向量；向量表示地图特征路标集合矩阵，机器人的运动方程均相同，故以机器人为例其运动模型可以表示为：

其中表示控制输入，为维的向量；为运动噪声，其方差为，服从于的高斯分布。

优选的，所述计算容积点集之前还包括：建立机器人观测方程。

在机器人运动的过程中，表示时刻系统的观测向量矩阵，为维的向量，机器人在未知环境中存在两种观测值，地图环境特征点的观测值和对其他机器人的相对方位信息，机器人在时刻观测到机器人，由外部传感器得到它们之间的相对方位信息。，分别为它们各自的运动方向，为时刻机器人观测到机器人的相对方位角则：

可以得到一般形式的观测模型：

其中表示观测噪声矩阵，其方差为，服从于的高斯分布。

优选的，所述计算容积点集包括：

时刻的地图特征点信息、运动控制信息和机器人的位姿信息都包含于矩阵中，以到时刻为例，将误差协方差阵通过Cholesky分解可得，利用机器人控制输入对状态信息矩阵状态增广，可以得到：

优选的，所述通过状态方程传播容积点：

优选的，所述机器人位姿状态预估计和平方根因子预测包括：

时刻机器人的状态估计：由容积变换，可得：

使用容积变换进行，将特征误差向量进行分解，即可获得平方根因子矩阵：

优选的，所述更新阶段计算容积点集包括：

时刻机器人观测到特征点与其他机器人的相对方位信息，此时要根据观测值计算观测向量的后验概率分布。

优选的，所述计算卡尔曼增益包括：

其中，机器人结合自身新的容积点和机器人新的容积点利用观测方程对其进行更新，可以得到每个容积点的观测值。再根据容积变换，可以得到：

其中，为观测误差向量，矩阵为观测信息自相关协方差矩阵，矩阵为观测误差向量和地图特征误差向量的互相关协方差矩阵。

可得，卡尔曼滤波增益矩阵：

优选的，所述计算位姿信息和平方根因子包括：

根据卡尔曼增益可计算得到机器人的位姿信息矩阵：

其中是机器人在时刻通过外部传感器观测到队列中其他个机器人相对方位角的实际观测值。

平方根因子矩阵可以由下式得到：

优选的，所述更新状态向量、协方差阵和位姿信息包括:

当同一时刻多个特征点同时被观测到时，需要对以下两个式子进行计算，然后再重复预测和更新的步骤：

本发明的优点在于：1、本发明算法将机器人通过传感器得到的自身位姿信息与其他机器人的相对方位信息融合起来，整个机器人队列内部共享这些观测信息提高了系统的鲁棒性和效率。2、在更新过程中直接传递目标状态均值和协方差矩阵的平方根因子，使得系统更稳定，有效的提高了定位的精度。3、在计算均值和方差时采用基于容积准则的数值积分方法，减少了截断误差，降低了计算复杂度，缩短了算法耗时，实时性强。

附图说明

图1为本发明一种基于平方根容积卡尔曼滤波的多机器人协同定位算法流程图；

图2为本发明一种基于平方根容积卡尔曼滤波的多机器人协同定位算法R_a、R_b之间的相对观测量示意图；

图3为本发明一种基于平方根容积卡尔曼滤波的多机器人协同定位算法协同定位方案示意图。

具体实施方式

为使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体实施方式，进一步阐述本发明。

根据本发明一个实施例，如图1所示，整个算法分为两步：预测和更新。首先建立机器人运动方程和观测方程，利用相对方位作为测量值，进一步得到多机器人协同定位的动态模型。预测阶段包括：计算容积点集；通过状态方程传播容积点；机器人位姿状态预估计和平方根因子预测。更新阶段包括：计算容积点集；计算卡尔曼增益；计算位姿信息和平方根因子；更新状态向量、协方差阵和位姿信息。

另外，如图2所示，当N个机器人组成队列在某一环境中沿不同方向运动，为了确保多机器人协同定位条件以及描述机器人之间的相对观测信息，对队列中机器人所需要的条件进行如下假设：

（1）每个机器人都配有可感知自身位姿信息的内部传感器和感知外部环境的外部传感器，通过携带的外部传感器可有效的探测到附近机器人对其的观测信息；

（2）通过通信协议每个机器人感知的外部环境可与其他机器人感知的数据之间进行交流融合；

（3）队列中每个机器人的外部传感器是相同的，因而可以用同一数学模型描述它们之间的观测信息。

多个机器人组成队列在二维环境下移动，为机器人在时刻的位姿，其中、和分别表示机器人在时刻的横坐标，纵坐标和运动方向，则时机器人队列的位姿信息可以表示为：

在机器人运动的过程中，表示时刻系统的观测向量矩阵，为维的向量，机器人在时刻观测到机器人，由外部传感器得到它们之间的相对方位信息。如图1所示，，分别为它们各自的运动方向，为时刻机器人观测到机器人的相对方位角则：

可以得到一般形式的观测模型：

其中表示观测噪声矩阵，其方差为，服从于的高斯分布。

如图3所示，当机器人通过可感知自身位姿信息的内部传感器和感知外部环境的外部传感器，获得运动数据时，通过运动模型结合传感器所获得的相对观测信息，完成对整个队列中机器人的位姿和平方根因子的更新。在时刻，由于每个机器人状态信息更新的流程是相同的，故以机器人为例详细说明提出的多机器人协同定位算法。

所述计算容积点集之前还包括：建立机器人运动方程。

在队列的行进过程中，向量为系统时刻的状态向量，为维的向量；向量表示地图特征路标集合矩阵，机器人的运动方程均相同，故以机器人为例其运动模型可以表示为

所述计算容积点集之前还包括：建立机器人观测方程。

在机器人运动的过程中，表示时刻系统的观测向量矩阵，为维的向量，机器人在时刻观测到机器人，由外部传感器得到它们之间的相对方位信息。，分别为它们各自的运动方向，为时刻机器人观测到机器人的相对方位角。则：

可以得到一般形式的观测模型：

其中表示观测噪声矩阵，其方差为，服从于的高斯分布。

所述计算容积点集包括：

所述通过状态方程传播容积点：

所述机器人位姿状态预估计和平方根因子预测包括：

时刻机器人的状态估计：由容积变换，可得：

所述更新阶段计算容积点集包括：

所述计算卡尔曼增益包括：

可得，卡尔曼滤波增益矩阵：

所述计算位姿信息和平方根因子包括：

根据卡尔曼增益可计算得到机器人的位姿信息矩阵：

平方根因子矩阵可以由下式得到：

所述更新状态向量、协方差阵和位姿信息包括:

由技术常识可知，本实用发明可以通过其它的不脱离其精神实质或必要特征的实施方案来实现。因此，上述公开的实施方案，就各方面而言，都只是举例说明，并不是仅有的。所有在本发明范围内或在等同于本实用发明的范围内的改变均被本发明包含。

Claims

1.一种基于平方根容积卡尔曼滤波（Square Root Cubature Kalman Filtering，SR-CKF）的多机器人协同定位算法，其特征在于，包括：

预测阶段包括：计算容积点集；通过状态方程传播容积点；机器人位姿状态预估计和平方根因子预测；

更新阶段包括：计算容积点集；计算卡尔曼增益；计算位姿信息和平方根因子；更新状态向量、协方差阵和位姿信息。

2.根据权利要求1所述的一种基于平方根容积卡尔曼滤波的多机器人协同定位算法，其特征在于，每个机器人都配有可感知自身位姿信息的内部传感器和感知外部环境信息的外部传感器，通过携带的外部传感器可有效的探测到附近机器人对其的观测信息，通过通信协议每个机器人感知的外部环境信息可与其他机器人感知的数据之间进行交流融合；

。

3.根据权利要求1或2所述的一种基于平方根容积卡尔曼滤波的多机器人协同定位算法，其特征在于，计算容积点集之前还包括：建立机器人运动方程；

在队列的行进过程中，向量为系统时刻的状态向量，为维的向量；向量表示地图特征路标集合矩阵；机器人的运动方程均相同，故以机器人为例其运动模型可以表示为：

4.根据权利要求1或2所述一种基于平方根容积卡尔曼滤波的多机器人协同定位算法，其特征在于，计算容积点集之前还包括：建立机器人观测方程；

在机器人运动的过程中，表示时刻系统的观测向量矩阵，为维的向量，机器人在时刻观测到机器人，由外部传感器得到它们之间的相对方位信息，，分别为它们各自的运动方向，为时刻机器人观测到机器人的相对方位角，则：

可以得到一般形式的观测模型：

其中表示观测噪声矩阵，其方差为，服从于的高斯分布。

5.根据权利要求3所述的一种基于平方根容积卡尔曼滤波的多机器人协同定位算法，其特征在于，所述计算容积点集包括：

时刻的地图特征点信息、运动控制信息和机器人的位姿信息都包含于矩阵中，以到时刻为例，将误差协方差阵通过Cholesky分解可得，利用机器人控制输入对状态信息矩阵状态增广。

6.根据权利要求5所述的一种基于平方根容积卡尔曼滤波的多机器人协同定位算法，其特征在于，所述通过状态方程传播容积点。

7.根据权利要求6所述一种基于平方根容积卡尔曼滤波的多机器人协同定位算法，其特征在于，所述机器人位姿状态预估计和平方根因子预测包括：

时刻机器人的状态估计：由容积变换，可得：

使用容积变换进行，将特征误差向量进行分解，即可获得平方根因子矩阵。

8.根据权利要求4所述一种基于平方根容积卡尔曼滤波的多机器人协同定位算法，其特征在于，所述计算容积点集包括：

9.根据权利要求8所述一种基于平方根容积卡尔曼滤波的多机器人协同定位算法，其特征在于，所述计算卡尔曼增益包括：

其中，机器人结合自身新的容积点和机器人新的容积点利用观测方程对其进行更新，可以得到每个容积点的观测值，再根据容积变换，可以得到：

其中，为观测误差向量，矩阵为观测信息自相关协方差矩阵，矩阵为观测误差向量和地图特征误差向量的互相关协方差矩阵，可得卡尔曼滤波增益矩阵。

10.根据权利要求9所述一种基于平方根容积卡尔曼滤波的多机器人协同定位算法，其特征在于，所述计算位姿信息和平方根因子包括：

根据卡尔曼增益可计算得到机器人的位姿信息矩阵：

其中是机器人在时刻通过外部传感器观测到队列中其他个机器人相对方位角的实际观测值；

平方根因子矩阵可以由下式得到。

11.根据权利要求10所述一种基于平方根容积卡尔曼滤波的多机器人协同定位算法，其特征在于，所述更新状态向量、协方差阵和位姿信息包括:

当同一时刻多个特征点同时被观测到时，需要对以下两个式子进行计算，然后再重复预测和更新的步骤。