[发明专利]考虑采样周期随机变化的机器人紧组合导航方法及系统

权利要求书

1.考虑采样周期随机变化的机器人紧组合导航方法，其特征在于，包括：

获取机器人x方向和y方向的位置误差、速度误差、航向角以及当前时刻的采样周期数据；

将上述获取的数据作为扩展卡尔曼滤波器的状态向量；

将航迹推算和激光雷达分别测量的机器人与特征点之间距离的平方的差以及当前时刻的采样周期作为扩展卡尔曼滤波器的观测向量进行数据融合，最终得到机器人的最优的状态预估，实现对于机器人的路径导航；

所述扩展卡尔曼滤波器的状态方程具体为：

其中，(δx_n,δy_n)、(δx_n-1,δy_n-1)分别为n时刻和n-1时刻机器人在x和y方向的位置误差；δV_n、δV_n-1分别为n时刻和n-1时刻的速度误差；分别为n时刻和n-1时刻的航向角；Δs_n、Δs_n-1分别为n时刻和n-1时刻的采样周期，ω_n-1为n-1时刻的系统噪声；

所述扩展卡尔曼滤波器的观测方程具体为：

其中，δd_i,n为n时刻航迹推算与激光雷达分别测量得到的机器人与第i个角特征点之间距离的平方的差；和分别为n时刻航迹推算与激光雷达分别测量得到的机器人与第i个角特征点之间的距离；i＝1,2,…,g；(x^D,y^D)为航迹推算的机器人位置，(x_i,y_i)为第i个角特征点的坐标；ν_n为观测噪声，其协方差阵为R。

2.如权利要求1所述的考虑采样周期随机变化的机器人紧组合导航方法，其特征在于，在使用激光雷达进行定位时，将室内环境中的角特征点作为特征点。

3.如权利要求1所述的考虑采样周期随机变化的机器人紧组合导航方法，其特征在于，所述扩展卡尔曼滤波器k时刻的状态协方差矩阵具体为：

P_k＝(I-K_kH_k)P_k|k-1

P_k|k-1＝A_k-1P_k-1(A_k-1)^T+Q

其中，K_k为卡尔曼增益；H_k为状态方程是线性方程时的转换矩阵；P_k-1为k-1时刻的状态协方差矩阵；Q为过程噪声的方差；A_k-1为状态方程是非线性方程时k-1时刻的状态转移矩阵。

4.如权利要求1所述的考虑采样周期随机变化的机器人紧组合导航方法，其特征在于，所述扩展卡尔曼滤波器k时刻一步状态估计矩阵具体为：

X_k|k-1＝A_k-1X_k-1+ω_k-1

其中，A_k-1为状态方程是非线性方程时k-1时刻的状态转移矩阵；X_k-1为k-1时刻的状态矩阵；ω_k-1为过程噪声。