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

摘要

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

主权项

1.一种基于平方根容积卡尔曼滤波(Square Root Cubature Kalman Filtering，SR‑CKF)的多机器人协同定位算法，其特征在于，包括：预测阶段包括：计算容积点集；通过状态方程传播容积点；机器人位姿状态预估计和平方根因子预测；更新阶段包括：计算容积点集；计算卡尔曼增益；计算位姿信息和平方根因子；更新状态向量、协方差阵和位姿信息；每个机器人都配有可感知自身位姿信息的内部传感器和感知外部环境信息的外部传感器，通过携带的外部传感器可有效的探测到附近机器人对其的观测信息，通过通信协议每个机器人感知的外部环境信息可与其他机器人感知的数据之间进行交流融合；多个机器人组成队列在二维环境下移动，为机器人在k时刻的位姿，其中s_x,k、s_y,k和s_θ,k分别表示机器人R_a在k时刻的横坐标，纵坐标和运动方向，则k时机器人队列的位姿信息可以表示为：计算容积点集之前还包括：建立机器人运动方程；在队列的行进过程中，向量为系统k时刻的状态向量，为n_s维的向量；向量表示地图特征路标集合矩阵；机器人的运动方程均相同，故以机器人R_a为例其运动模型可以表示为：其中表示控制输入，为n_u维的向量；W_k为运动噪声，其方差为Q_k，服从于N(0,Q_k)的高斯分布；计算容积点集之前还包括：建立机器人观测方程；在机器人运动的过程中，表示k时刻系统的观测向量矩阵，为n_z维的向量，机器人R_a在k时刻观测到机器人R_b，由外部传感器得到它们之间的相对方位信息，分别为它们各自的运动方向，为k时刻机器人R_a观测到机器人R_b的相对方位角，则：可以得到一般形式的观测模型：其中Vk表示观测噪声矩阵，其方差为Rk，服从于N(0,Rk)的高斯分布；所述计算容积点集包括：k‑1时刻的地图特征点信息、运动控制信息和机器人的位姿信息都包含于矩阵中，以k‑1到k时刻为例，将误差协方差阵P_k‑1通过Cholesky分解可得P_k‑1＝L_k‑1L_k‑1^T，利用机器人控制输入u_k对状态信息矩阵S_k状态增广：所述通过状态方程传播容积点；所述机器人位姿状态预估计和平方根因子预测包括：k时刻机器人的状态估计：由容积变换，可得：使用容积变换进行，将特征误差向量Ak|k‑1进行分解，即可获得平方根因子矩阵Ck|k‑1；[Q R]＝qr(Ak|k‑1T)，Ck|k‑1＝RT所述计算容积点集包括：k时刻机器人观测到特征点与其他机器人的相对方位信息，此时要根据观测值计算观测向量的后验概率分布：所述计算卡尔曼增益包括：其中，机器人R_a结合自身新的容积点和机器人R_b新的容积点利用观测方程对其进行更新，可以得到每个容积点的观测值，再根据容积变换，可以得到：其中，为观测误差向量，矩阵为观测信息自相关协方差矩阵，矩阵为观测误差向量和地图特征误差向量的互相关协方差矩阵，可得卡尔曼滤波增益矩阵W_k：所述计算位姿信息和平方根因子包括：根据卡尔曼增益可计算得到机器人的位姿信息矩阵其中是机器人在k时刻通过外部传感器观测到队列中其他N‑1个机器人相对方位角的实际观测值；平方根因子矩阵可以由下式得到：所述更新状态向量、协方差阵和位姿信息包括:当同一时刻多个特征点同时被观测到时，需要对以下两个式子进行计算，然后再重复预测和更新的步骤：Sk|k‑1＝SkCk|k‑1＝Ck。