申请/专利权人：上海师范大学

申请日：2021-10-15

公开（公告）日：2024-01-23

公开（公告）号：CN113884098B

主分类号：G01C21/20

分类号：G01C21/20

优先权：

专利状态码：有效-授权

法律状态：2024.01.23#授权;2022.01.21#实质审查的生效;2022.01.04#公开

摘要：本发明涉及一种基于具体化模型的可迭代的卡尔曼滤波定位方法，该方法包括以下步骤：步骤1：对地图进行初始化，即对地图的变量设置初值；步骤2：建立机器人的系统状态方程，并对运动模型初始化；步骤3：基于具体化模型的可迭代卡尔曼滤波算法进行位姿估计，以提高位姿估计的精度，以解决SLAM实时建图和定位的问题，提高位姿估计的精度，与现有技术相比，本发明具有能够有效减少运动模型累积误差、提高位姿估计的精度和鲁棒性好的特点等优点。

主权项：1.一种基于具体化模型的可迭代的卡尔曼滤波定位方法，其特征在于，该方法包括以下步骤：步骤1：对地图进行初始化，即对地图的变量设置初值；步骤2：建立机器人的系统状态方程，并对运动模型初始化；步骤3：基于具体化模型的可迭代卡尔曼滤波算法进行位姿估计，以提高位姿估计的精度；所述的步骤1中，地图的变量包括路标点的状态和地图的原点，并将路标点的状态的初值设为0，将机器人的初始位姿设为地图的原点；所述的步骤2中，对运动模型初始化具体为：设置初始时刻机器人的状态的初值和对应的协方差的初值；机器人的系统状态方程为：xt＝gut,xt-1+εtzt＝hxt+δt其中，xt为t时刻的状态向量，g·为运动模型，xt-1为t-1时刻的状态向量，ut为系统从t-1到t时刻的控制输入，εt为运动模型的噪声，服从高斯分布0,Qt，zt为t时刻路标点的观测向量，h·为观测模型，δt为观测模型的噪声，服从高斯分布0,Rt；所述的步骤3中，基于具体化模型的可迭代卡尔曼滤波算法进行位姿估计的过程包括以下步骤：步骤31：EKF预测；步骤32：EKF更新；步骤33：状态增广；所述的步骤31中，EKF预测的过程具体为：根据t-1时刻的状态估计值和t时刻的控制输入预测t时刻的系统位姿，得到先验状态估计值和对应的协方差矩阵所述的先验状态估计值的表达式为： 其中，为t时刻的先验状态估计值，g·为运动模型，ut为系统从t-1时刻到t时刻的控制输入，μt-1为t-1时刻的状态估计值；所述的协方差矩阵的表达式为： 其中，为t时刻的先验状态估计值对应的协方差矩阵，Gt为运动模型g·的雅克比矩阵，表示运动模型g·对状态估计值μt求偏导，Σt-1为t-1时刻的状态估计值μt-1对应的协方差矩阵，Rt为观测模型的噪声的协方差矩阵；所述的步骤32中，EKF更新的过程具体为：通过传感器获取t时刻先验状态估计值对应的协方差、观测模型的雅克比矩阵以及运动模型的噪声对应的协方差矩阵，进而计算卡尔曼增益Kt，并进行位姿的最优估计： 其中，Kt为t时刻的卡尔曼增益，Ht为观测模型h·的雅克比矩阵，为t时刻的先验状态估计值对应的协方差矩阵，Qt为运动模型的噪声的协方差矩阵，μt为t时刻的状态估计值，为t时刻的先验状态估计值，zt为t时刻的观测值，h·为观测模型，表示状态空间到测量空间的非线性映射，为观测估计值，为残差rt，为t时刻的状态估计值对应的协方差矩阵；所述的步骤33中，状态增广的过程具体为：在观测模型可逆时进行状态增广，即当机器人发现未观测到的路标点时，对新发现的路标点进行初始化，通过观测方程将新发现的路标点的状态加入地图，进而增大总的状态向量的大小，即EKF-SLAM中的滤波器是动态变化的。

全文数据：

权利要求：

百度查询： 上海师范大学 一种基于具体化模型的可迭代的卡尔曼滤波定位方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD