[发明专利]扫地机及其行进角度的测量方法、装置

权利要求书

1.一种扫地机行进角度的测量方法，其特征在于，所述扫地机上设置有至少两个陀螺仪，且至少存在一个陀螺仪的水平安装角度与其他陀螺仪的水平安装角度不同，所述测量方法包括以下步骤：

获取每个陀螺仪的当前采集数据；

根据每个陀螺仪的水平安装角度对所有陀螺仪的当前采集数据建立陀螺仪随机漂移模型；

根据所述陀螺仪随机漂移模型设计卡尔曼滤波器；

利用所述卡尔曼滤波器对所有陀螺仪的当前采集数据进行卡尔曼滤波融合，以获得所述扫地机的行进角度的最优估计值。

2.如权利要求1所述的扫地机行进角度的测量方法，其特征在于，在根据每个陀螺仪的水平安装角度对所有陀螺仪的当前采集数据建立陀螺仪随机漂移模型之前，还分别对每个陀螺仪的当前采集数据进行滤波处理。

3.如权利要求2所述的扫地机行进角度的测量方法，其特征在于，所述陀螺仪为单轴陀螺仪或者多轴陀螺仪，其中，

当陀螺仪为单轴陀螺仪时，采用一维卡尔曼滤波分别对每个陀螺仪的当前采集数据进行滤波处理；

当陀螺仪为多轴陀螺仪时，采用互补滤波或卡尔曼滤波融合分别对每个陀螺仪的当前采集数据进行滤波处理。

4.如权利要求1所述的扫地机行进角度的测量方法，其特征在于，所述陀螺仪随机漂移模型通过如下状态空间模型表示：

其中，X_k＝[X_1k Λ X_ik Λ]，i＝1,2,Λ,N，X_ik表示第i个陀螺仪在k时刻的采集数据，Z_k表示k时刻的观测量，v_k表示k时刻的观测噪声，且v_k服从高斯分布N(0，R)，表示状态转移矩阵，τ_iG表示第i个陀螺仪的过程时间常数，表示观测矩阵，且c_i的取值根据陀螺仪的水平安装角度计算。

5.如权利要求4所述的扫地机行进角度的测量方法，其特征在于，所述利用所述卡尔曼滤波器对每个陀螺仪的当前采集数据进行卡尔曼滤波融合，包括以下步骤：

A，预估计

B，计算预估计协方差矩阵P_k：P_k＝AP_k-1A^T；

C，计算卡尔曼增益G_k：G_k＝P_k-1C^T(CP_k-1C^T+R)^-1；

D,更新所述行进角度的最优估计值

E，计算更新后估计协方差P_k；P_k＝(I-G_kC)P_k-1，其中，I表示单位矩阵。

6.如权利要求4所述的扫地机行进角度的测量方法，其特征在于，当所述扫地机中设置有两个陀螺仪，且两个陀螺仪的水平安装角度分别为α、β时，

7.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-6中任一项所述的扫地机行进角度的测量方法。

8.一种扫地机，其特征在于，包括存储器、处理器、存储在存储器上并可在处理器上运行的计算机程序及至少两个陀螺仪，且至少存在一个陀螺仪的水平安装角度与其他陀螺仪的水平安装角度不同，所述处理器执行所述程序时，实现如权利要求1-6中任一所述扫地机行进角度的测量方法。