[发明专利]一种用于无人清扫车辆贴边清扫的精确定位方法

权利要求书

1.一种用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，所述无人清扫车辆包括IMU、轮速计、GPS以及激光传感器；所述定位方法包括：

接收IMU数据和轮速计脉冲数据；

根据所述IMU数据和轮速计脉冲数据对车辆状态进行监控，当车辆状态满足预设静止条件时，根据所述IMU数据输出计算所述车辆的初始水平姿态角和三轴陀螺仪上电初始零偏值；

实时接收IMU数据，根据所述实时接收到的IMU数据输出角速度数据和加速度数据，根据所述角速度数据更新姿态四元数，根据所述加速度数据更新速度数据和位置；

根据所述更新后的姿态四元数、速度数据和位置建立相应的滤波模型；

当收到轮速计数据、激光定位数据或GPS定位数据时，通过所述滤波模型进行滤波，得到误差修正量，根据所述误差修正量对所述更新后的姿态四元数、速度数据和位置进行修正，得到车辆当前的姿态信息；

根据所述车辆当前的姿态信息和轮速计数据进行航位推算，得到当前车辆在导航坐标系下的位置和姿态信息。

2.根据权利要求1所述的用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，所述根据所述IMU数据和轮速计脉冲数据对车辆状态进行监控，当车辆状态满足预设静止条件时，根据所述IMU数据输出计算所述车辆的初始水平姿态角和三轴陀螺仪上电初始零偏值具体包括：

根据所述轮速计脉冲数据判断车辆是否处于静止状态；

当所述车辆处于静止状态时，对IMU数据进行累加；

判断所述车辆在预设时间内是否处于静止状态；

当所述车辆在预设时间内处于静止状态时，根据所述累加的IMU数据计算加速度计均值和陀螺仪均值；

根据所述加速度计均值计算初始水平姿态角，并将所述陀螺仪均值为作为三轴陀螺仪上电初始零偏值。

3.根据权利要求2所述的用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，所述根据所述加速度计均值计算初始水平姿态角具体包括：根据下式计算初始水平姿态角：

roll＝arctan(-f_{x_mean}/f_{z_mean})

pitch＝arcsin(f_{y_mean}/g)

其中，f_{x_mean}为X轴加速度计3s数据均值，f_{y_mean}为Y轴加速度计3s数据均值，f_{z_mean}为Z轴加速度计3s数据均值，g为当地重力加速度。

4.根据权利要求1所述的用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，在所述实时接收IMU数据，根据所述实时接收到的IMU数据输出角速度数据和加速度数据，根据所述角速度数据更新姿态四元数之前，所述方法还包括：

根据所述初始水平姿态角得到初始姿态四元数。

5.根据权利要求4所述的用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，所述根据所述角速度数据更新姿态四元数具体包括：

根据角速度数据和所述三轴陀螺仪上电初始零偏值更新初始姿态四元数。

6.根据权利要求5所述的用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，所述根据角速度数据和所述三轴陀螺仪上电初始零偏值更新初始姿态四元数具体包括：

根据所述实时接收IMU数据得到陀螺仪数据；

将所述陀螺仪数据和所述上电初始零偏值作差，得到修正后的陀螺仪数据；

根据所述修正后的陀螺仪数据得到一个解算周期之内的变化四元数；

根据所述一个解算周期之内的变化四元数和初始姿态四元数运用下式进行姿态解算：

其中，Q_K为当前时刻四元数，Q_K-1为前一时刻四元数，Q(T)为一个解算周期之内的变化四元数。