[发明专利]基于陀螺仪的扫地机器人角度最优估算方法

权利要求书

1.一种基于陀螺仪的扫地机器人角度最优估算方法，其特征在于，所述方法包括如下步骤：

步骤一、在全局坐标系{O,X,Y}中，建立双轮驱动扫地机器人的连续运动学模型和离散化的运动学模型；

步骤二、基于系统上一时刻的状态，利用系统的过程模型，预测系统当前k时刻的状态；

步骤三、利用过程噪声Q更新滤波均方误差，得到当前时刻的预测均方误差P_k|k-1；

步骤四、基于预测均方误差P_k|k-1和测量噪声R，计算卡尔曼增益K_k；

步骤五、结合预测值θ_k|k-1和陀螺仪的测量值Z_k，得到当前时刻的角度最优估算值θ_k|k；

步骤六、更新当前时刻的滤波均方误差，得到最优估计值P_k|k；

步骤七、当系统进入下一时刻，重复步骤二至步骤六，实现自回归运算。

2.如权利要求1所述的一种基于陀螺仪的扫地机器人角度最优估算方法，其特征在于：所述步骤一中，双轮驱动扫地机器人的坐标表示为P＝[x y θ]^T，其中，P点为两个驱动轮轴的中心，θ为两个驱动轮轴与x轴的夹角；

设机器人前进的线速度为υ，转向的角速度为ω，连续运动学模型表示为：

其中，υ_L和υ_R分别为左、右驱动轮的线速度，L是驱动轮之间的距离；

选取T为采样周期时间，离散化的运动学模型为：

其中，θ_k-1为机器人前一时刻的角度，ω_k-1为前一时刻的转向角速度，θ_k为当前时刻角度的计算值；

通过读取采样周期内左、右驱动轮的旋转编码器脉冲个数N_R和N_L，得到新的角度状态表达式：

其中，r为驱动轮半径，N为编码器旋转一周的脉冲个数，w_k为过程噪声；

依据陀螺仪，读取当前时刻的角度测量值Z_k：

Z_k＝θ_k+v_k (4)

其中，v_k为陀螺仪的测量噪声。

3.如权利要求2所述的一种基于陀螺仪的扫地机器人角度最优估算方法，其特征在于：所述过程噪声w_k以及陀螺仪的测量噪声v_k均为高斯白噪声。

4.如权利要求2所述的一种基于陀螺仪的扫地机器人角度最优估算方法，其特征在于：所述步骤二中，当前k时刻的状态θ_k|k-1的表达式为：

式(5)中，θ_k|k-1是利用上一状态预测的结果，即当前时刻角度的预测值，θ_k-1|k-1为上一时刻角度的最优估计值。

5.如权利要求2所述的一种基于陀螺仪的扫地机器人角度最优估算方法，其特征在于：所述步骤三中，所述当前时刻的预测均方误差P_k|k-1的表达式为：

P_k|k-1＝P_k-1|k-1+Q (6)

式(6)中，P_k-1|k-1为上一时刻的最优均方误差，P_k|k-1为当前时刻的预测均方误差。

6.如权利要求2所述的一种基于陀螺仪的扫地机器人角度最优估算方法，其特征在于：所述步骤四中，所述卡尔曼增益K_k的表达式为：

K_k＝P_k|k-1(P_k|k-1+R)^-1。 (7)。