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

说明书

本发明公开了一种基于陀螺仪的扫地机器人角度最优估算方法包括：在全局坐标系{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；当系统进入下一时刻，重复步骤二至步骤六，实现自回归运算。本方法，融合了驱动轮旋转编码器数据和陀螺仪数据，在每个采样时间点计算得到当前时刻角度的最优估计值，对系统的随机误差有较好的抑制作用。

技术领域

本发明公开了一种基于陀螺仪的扫地机器人角度最优估算方法，涉及自动控制技术领域，具体涉及一种控制估算方法。

背景技术

双轮驱动扫地机器人的前进与转向由独立的直流电机驱动实现。现有技术中，双轮移动转向的测定大多数依靠陀螺仪的测量数据来处理完成。

例如申请号为CN201610081865的中国专利申请，公开了一种电动车驱动轴扭矩的观测方法及相关观测控制系统，所述方法由传感器模块检测驱动系统的电机转速、轮速；将传感器模块检测到的电机转速、轮速数据传输给UKF观测模块进行仿真运算，并输出驱动轴扭矩估计结果。其优点是：可以实现对驱动轴扭矩的观测估计，且将这种观测方法运用到驱动系统控制中，实现了汽车控制器对驱动轴扭矩的提前预判和处理，起到了对汽车驱动系统的可靠控制的目的。但是，双轮驱动扫地机器人与汽车相比无论是在硬件结构还是数据测量处理上均大不相同，电动车驱动轴扭矩的观测方法对照在双轮驱动扫地机器人中并不适用。

又例如，申请号为CN201010132143的中国专利申请，公开了一种轮式倒立摆的传感器处理与平衡控制算法，包括传感器处理算法，平衡控制算法，过速度保护算法，应急处理算法；所述传感器处理算法，包括加速度传感器与陀螺仪数据融合算法以及滤波算法；加速度传感器与陀螺仪的数据融合算法，利用加速度传感器的低频特性，和陀螺仪输出的高频特性，用以得到一个在动态情况下稳定的倒立摆偏转角；所述平衡控制算法，通过使用当前倒立摆的偏转角与倒立摆的偏转角速度，来控制当前车体的加速度和角度；控制过程的输入参数是当其车体运动的速度，当其车体的偏转角，以及偏转角速度；输出为车体的加速度；所述过速度保护算法，当速度高于一定速度阈值时，该算法会输出一个与速度有关的角度偏移量；该偏移量将附加在所述传感器融合算法所计算出来的角度上，用于整个车体的减速，当速度减小到一定阈值时，将逐渐撤去该偏移量，使得系统恢复正常；所述的应急处理算法是在紧急情况下，系统将自动启动应急处理算法，其速度阈值为0附近，控制目标是将车体的速度迅速降下来，以保证车载设备的安全。该申请虽然提及了加速度传感器与陀螺仪的数据融合算法，但其计算和处理方式主要是针对解决两轮平衡车的稳定性问题，所公开的内容对双轮驱动扫地机器人这种使用场景来说帮助并不大。

双轮驱动扫地机器人在前进和转向过程中，由于陀螺仪测量信号会产生漂移，如果单纯依靠陀螺仪数据会导致角度测量误差较大。

发明内容

本发明所要解决的技术问题是：针对现有技术的缺陷，提供一种基于陀螺仪的扫地机器人角度最优估算方法，所述方法融合了驱动轮旋转编码器数据和陀螺仪数据，在每个采样时间点计算得到当前时刻角度的最优估计值，对系统的随机误差有较好的抑制作用。

本发明为解决上述技术问题采用以下技术方案：

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

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