一种面向UWB SLAM的Kalman/R-T-S混合定位方法及系统

摘要

本发明公开了一种面向UWB SLAM的Kalman/R‑T‑S混合定位方法及系统，包括：将x和y方向的位置、速度、航向角和UWB参考节点的位置作为扩展卡尔曼滤波器的状态向量；将UWB测量的机器人与UWB参考节点之间的距离作为扩展卡尔曼滤波器的观测向量进行数据融合,得到预估的UWB参考节点的位置；利用R‑T‑S平滑算法对扩展卡尔曼滤波器预估的UWB参考节点的位置进行平滑，对各个时刻平滑过的UWB参考节点位置取平均值，最终得到移动机器人和UWB参考节点的最优预估。本发明利用R‑T‑S平滑算法对扩展卡尔曼滤波器预估的UWB参考节点的位置进行平滑，不再像传统UWB定位算法需要依赖UWB参考节点的位置信息；R‑T‑S平滑算法的引入增加了参考节点位置信息的预估精度。