[发明专利]一种面向UWB SLAM的Kalman/R-T-S混合定位方法及系统

权利要求书

1.一种面向UWB SLAM的Kalman/R-T-S混合定位方法，其特征在于，包括：

将x和y方向的位置、速度、航向角和UWB参考节点的位置作为扩展卡尔曼滤波器的状态向量；

将UWB测量的机器人与UWB参考节点之间的距离作为扩展卡尔曼滤波器的观测向量进行数据融合,得到预估的UWB参考节点的位置；

利用R-T-S平滑算法对扩展卡尔曼滤波器预估的UWB参考节点的位置进行平滑，对各个时刻平滑过的UWB参考节点位置取平均值，最终得到移动机器人和UWB参考节点的最优预估。

2.如权利要求1所述的一种面向UWB SLAM的Kalman/R-T-S混合定位方法，其特征在于，所述扩展卡尔曼滤波器的状态方程为：

其中，(x_k,y_k)为移动机器人k时刻在x和y方向的位置；(x_i,y_i),i∈[1,2,...,g]为UWB参考节点k时刻在x和y方向的位置；V_k为k时刻的速度；为k时刻的航向；T为采样周期，ω_n为k时刻的系统噪声，其协方差阵为Q。

3.如权利要求1所述的一种面向UWB SLAM的Kalman/R-T-S混合定位方法，其特征在于，扩展卡尔曼滤波器的观测方程为：

其中，d_i,k为k时刻UWB测量得到的机器人与第i个参考节点之间的距离；(x_k,y_k)为k时刻的机器人位置；ν_k为观测噪声，其协方差阵为R。

4.如权利要求1所述的一种面向UWB SLAM的Kalman/R-T-S混合定位方法，其特征在于，利用R-T-S平滑算法对扩展卡尔曼滤波器预估的UWB参考节点的位置进行平滑，具体为：

其中，为平滑的误差增益、P_k|k为k时刻的误差矩阵、为系统矩阵的转置、P_k+1|k为由k-1到k时刻的误差矩阵、为k时刻的平滑状态预估、为k时刻的状态预估、为k+1时刻的平滑状态预估、为由k-1到k时刻的平滑状态预估、为k时刻的平滑误差矩阵、为k+1时刻的平滑误差矩阵、P_k|k-1为由k-1到k时刻的误差矩阵。

5.一种面向UWB SLAM的Kalman/R-T-S混合定位系统，其特征在于，包括：

用于将x和y方向的位置、速度、航向角和UWB参考节点的位置作为扩展卡尔曼滤波器的状态向量的装置；

用于将UWB测量的机器人与UWB参考节点之间的距离作为扩展卡尔曼滤波器的观测向量进行数据融合,得到预估的UWB参考节点的位置的装置；

用于利用R-T-S平滑算法对扩展卡尔曼滤波器预估的UWB参考节点的位置进行平滑，对各个时刻平滑过的UWB参考节点位置取平均值，最终得到移动机器人和UWB参考节点的最优预估的装置。

6.一种终端设备，其包括处理器和计算机可读存储介质，处理器用于实现各指令；计算机可读存储介质用于存储多条指令，其特征在于，所述指令适于由处理器加载并执行权利要求1-4任一项所述的面向UWB SLAM的Kalman/R-T-S混合定位方法。

7.一种计算机可读存储介质，其中存储有多条指令，其特征在于，所述指令适于由终端设备的处理器加载并执行权利要求1-4任一项所述的面向UWB SLAM的Kalman/R-T-S混合定位方法。