CN110307836A - 一种用于无人清扫车辆贴边清扫的精确定位方法 - Google Patents

Abstract

本发明实施例涉及一种用于无人清扫车辆贴边清扫的精确定位方法，包括：接收IMU数据、轮速计脉冲数据、激光定位或GPS定位数据；根据IMU数据和轮速计脉冲数据对车辆状态进行监控，当车辆状态满足预设静止条件时，根据IMU输出的加速度数据计算车辆初始水平姿态角和三轴陀螺仪上电初始零偏值完成初始化；待完成初始化之后，采用IMU输出的角速度更新姿态四元数、加速度数据更新速度和位置，并建立相应滤波模型完成预测，当收到轮速计或激光定位或GPS定位数据时进行滤波得到误差修正量对当前四元数、位置和速度进行修正，得到车辆当前准确的姿态信息，并与轮速计进行航位推算，得到当前车辆在导航坐标系下的位置和姿态。

Description

一种用于无人清扫车辆贴边清扫的精确定位方法

技术领域

本发明涉及自动驾驶领域，尤其涉及一种用于无人清扫车辆贴边清扫的精确定位方法。

背景技术

近年来随着无人驾驶技术的不断发展与普及，应用于特定环境内的小型低速无人清扫车成为无人驾驶最先落地的商业化产品，对于公园、校园、大型商场、工业园区等场景的卫生清扫作业，由智能化无人清扫车替代人工清扫成为趋势，“贴边”清扫是无人清扫车特定环境下完成高效率清扫目标的重要技术手段之一。

小型低速无人清扫车同时配备高精度差分GNSS(Global Navigation SatelliteSystem,GNSS)板卡、激光雷达、IMU(Inertial Measurement Unit,IMU)以及轮速计等传感器，高精度差分GPS仅在开阔环境下给出高精度定位定向信息，激光定位方法在遮挡环境或者周边物体三维特征明显的情况下可以给出厘米级定位精度，采用多传感器数据融合和航位推算技术，可以获得车辆当前准确定位信息。

目前应用于车辆“贴边”行驶多采用视觉或者激光传感器进行路沿检测获得车辆相对定位的方法。

在智能无人驾驶系统中，高精地图是必不可少的先验信息，根据视觉或激光传感器采集到车辆周围的数据信息，提取特征并与高精地图进行匹配，获得车辆当前相对于“路沿”位置以达到车辆“贴边”行驶目的。基于视觉传感器的检测方案存在明显的缺陷,其一,图像识别的运算量大，其二,算法的效果依赖图像的成像质量,特别是如果遇到下雨、雾天、道路阴暗、对向强光等情况时会大大降低其识别效果。基于激光传感器的检测方案虽然可以适应道路阴暗、对向强光等场景，但同样易受雨雪等极端天气状况的影响，会导致其检测效果往往无法满足实用要求。此外，无论是采用视觉或者激光传感器进行路沿检测，均需要耗费大量计算资源并且对地图精度要求非常高。

发明内容

本发明的目的是针对现有技术的缺陷，提供一种用于无人清扫车辆贴边清扫的精确定位方法，高效的使用IMU、轮速计、GPS以及激光等传感器的数据进行融合获得车辆当前准确的姿态数据，采用航位推算方法获得车辆当前定位数据，保证车辆定位数据平滑可靠，并且在一定的时间和距离内绝对误差相对较小，以达到贴边清扫对于位置平滑性和精确性的要求。

有鉴于此，本发明实施例提供了一种用于无人清扫车辆贴边清扫的精确定位方法，包括：

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

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

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

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

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

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

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

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

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

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

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

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

进一步优选的，所述根据所述加速度计均值计算初始水平姿态角具体包括：根据下式计算初始水平姿态角：

roll＝a tan(-f_{x_mean}/f_{z_mean})

pitch＝a sin(f_{y_mean}/g)

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

优选的，在所述实时接收IMU数据，根据所述实时接收到的IMU数据输出角速度数据和加速度数据，根据所述角速度数据更新姿态四元数之前，所述方法还包括：

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

进一步优选的，所述根据所述角速度数据更新姿态四元数具体包括：

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

进一步优选的，所述根据角速度数据和所述三轴陀螺仪上电初始零偏值更新初始姿态四元数具体包括：

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

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

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

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

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

优选的，所述当收到轮速计数据、激光定位数据或GPS定位数据时，通过所述滤波模型进行滤波，得到误差修正量具体包括：

将接收到的轮速计测量速度作为当前速度观测值、激光定位数据作为当前航向观测值或GPS定位数据作为当前位置观测值；

将所述当前速度观测值、当前航向观测值或当前位置观测值输入所述误差模型进行EKF滤波得到所述误差修正量。

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

根据车辆当前的姿态信息和所述轮速计输出的当前车辆的速度数据计算当前所述车辆在导航坐标系下的速度数据；

根据当前所述车辆在导航坐标系下的速度数据计算出所述当前所述车辆在导航坐标系下的位置数据。

进一步优选的，所述根据车辆当前的姿态信息和所述轮速计输出的当前车辆的速度数据计算当前所述车辆在导航坐标系下的速度数据具体包括：

采用下面两个公式计算当前所述车辆在导航坐标系下的速度数据：

其中，R、P、H表示车辆当前的横滚角、俯仰角和航向角，表示车辆坐标系到导航坐标系姿态转换矩阵，V_eo、V_no、V_uo′表示车辆在导航坐标系内速度V_odom表示轮速计测量的当前车速。

进一步优选的，根据当前所述车辆在导航坐标系下的速度数据计算出所述当前所述车辆在导航坐标系下的位置数据具体包括：根据下面公式计算当前所述车辆在导航坐标系下的位置数据：

其中，其中，P_e、P_n、P_u为车辆当前在导航坐标系下的准确位置数据，ΔT为数据更新时间间隔。

本发明实施例提供的一种用于无人清扫车辆贴边清扫的精确定位方法，高效的使用IMU、轮速计、GPS以及激光等传感器的数据进行融合获得车辆当前准确的姿态数据，采用航位推算方法获得车辆当前定位数据，保证车辆定位数据平滑可靠，并且在一定的时间和距离内绝对误差相对较小，以达到贴边清扫对于位置平滑性和精确性的要求。

附图说明

图1为本发明实施例提供的设计思路示意图；

图2为本发明实施例提供的一种用于无人清扫车辆贴边清扫的精确定位方法流程图；

图3为本发明实施例提供的初始化的示意图。

具体实施方式

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

本发明实施例提供的一种精确定位方法应用于无人驾驶车辆，优选应用在低速无人清扫车中，如图1所示，本方法包括数据融合航位推算两部分，其中数据融合获得车辆当前准确的姿态信息，航位推算根据车辆当前的姿态信息和轮速数据，得到车辆当前准确、平滑、可靠的定位数据。

图2为本发明实施例提供的一种用于无人清扫车辆贴边清扫的精确定位方法流程图，如图2所示，包括：

步骤101，接收IMU数据和轮速计脉冲数据。

数据融合综合利用IMU、GPS、轮速计以及激光定位等传感器数据，最终计算输出车辆当前准确姿态信息，数据融合包括初始化、姿态解算以及滤波修正等步骤。

步骤101和步骤102是初始化的过程，下面结合图2至图3对初始化的过程进行介绍。

对车辆进行初始化，车辆上电后保持静止，接收IMU数据和轮速计脉冲数据，这里的IMU数据是指车辆初始化后车辆保持静止时的IMU数据，这里的轮速计脉冲数据车辆初始化后车辆保持静止时的轮速计脉冲数据。

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

具体而言，根据轮速计采集的原始脉冲数据判断车辆是否处于静止状态，本领域技术人员可以根据需要对静止状态的判断条件进行设定，在本例中，静止状态的判断条件为车轮角速率小于0.005°/s，当车辆的车轮角速率小于0.005°/s时，认为车辆处于静止状态，当车辆处于静止状态时，对初始IMU数据进行累加；继续判断车辆在预设时间内是否处于静止状态，这里的预设时间优选为3s，也就是说车辆连续3s满足静止状态判断条件认为车辆在预设时间内处于静止状态，此时，根据累加的IMU数据计算加速度计均值和陀螺仪均值；根据加速度计均值计算初始水平姿态角，并将陀螺仪均值为作为三轴陀螺仪上电初始零偏值。

其中，上述根据加速度计均值计算初始水平姿态角具体包括根据下面公式(1)和公式(2)计算初始水平姿态角：

roll＝a tan(-f_{x_mean}/f_{z_mean}) (1)

pitch＝a sin(f_{y_mean}/g) (2)

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

步骤103，实时接收IMU数据，根据实时接收到的IMU数据输出角速度数据和加速度数据，根据角速度数据更新姿态四元数，根据加速度数据更新速度数据和位置。

在步骤103之前，所述方法还包括根据初始水平姿态角输出初始姿态四元数。

待完成初始化之后，进行姿态解算，步骤103实现的姿态解算的过程，在步骤102之后，实时接收IMU数据，根据实时接收IMU数据得到陀螺仪数据，根据陀螺仪数据得到角速度数据，再根据角速度数据和三轴陀螺仪上电初始零偏值更新初始姿态四元数，具体而言，根据所述实时接收IMU数据得到陀螺仪数据，将陀螺仪数据和上电初始零偏值作差，得到修正后的陀螺仪数据；根据修正后的陀螺仪数据得到一个解算周期之内的变化四元数；根据一个解算周期之内的变化四元数和初始姿态四元数运用下面公式(3)进行姿态解算：

其中，Q_K为当前时刻四元数，Q_K-1为前一时刻四元数，Q(T)为一个解算周期之内的变化四元数，由陀螺仪输出数据计算得到。

需要说明的是，本发明的数据融合过程也可以采用无迹卡尔曼滤波(UKF)/粒子滤波(PF)等滤波方式。

步骤104，根据更新后的姿态四元数、速度数据和位置建立相应的滤波模型。

根据步骤103中的得到的更新后的姿态四元数、速度数据和位置，也就是当前姿态四元数、速度数据和位置建立相应的滤波模型，即误差模型，并根据当前姿态四元数、速度数据和位置完成预测，预测过程具体包括：

1)状态量一步预测，如下式(4)：

其中，为上一步计算得到状态量，X_k/k-1为当前预测状态量，PHI为状态转移矩阵，由当前姿态四元数、速度数据和位置得到。

2)协方差矩阵预测，如下式(5)：

P_k/k-1＝PHI·P_k-1/k-1·PHI^T+Q_k (5)

其中，P_k-1/k1为上一步计算得到协方差矩阵，P_k/k-1为当前预测协方差矩阵，Q_k为噪声驱动矩阵。

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

步骤105实现的是滤波修正的过程，具体而言，将接收到的轮速计测量速度作为当前速度观测值、激光定位数据作为当前航向观测值或GPS定位数据作为当前位置观测值；将当前速度观测值、当前航向观测值或当前位置观测值输入误差模型进行EKF滤波得到误差修正量。根据误差修正量对更新后的姿态四元数、速度数据和位置进行修正，得到车辆当前准确的姿态信息。

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

步骤106实现的是航位推算的过程，步骤105数据融合结果输出了车辆当前准确的姿态信息，轮速计输出当前车辆准确的速度信息，根据车辆当前的姿态信息和轮速计输出的当前车辆的速度数据计算当前车辆在导航坐标系下的速度数据；根据当前车辆在导航坐标系下的速度数据计算出当前车辆在导航坐标系下的位置数据。

具体而言，根据车辆当前的姿态信息和轮速计输出的当前车辆的速度数据计算当前车辆在导航坐标系下的速度数据具体包括采用下面两个公式(6)和公式(7)计算当前车辆在导航坐标系下的速度数据：

其中，上述式(6)和式(7)中R、P、H表示车辆当前的横滚角、俯仰角和航向角，表示车辆坐标系到导航坐标系姿态转换矩阵，V_eo、V_no、V_uo表示车辆在导航坐标系内速度V_odom表示轮速计测量的当前车速。

在给定车辆初始位置之后，根据下面公式(8)计算当前实时定位数据，即当前车辆在导航坐标系下的位置数据：

其中，P_e、P_n、P_u为车辆当前在导航坐标系下的准确位置数据，ΔT为数据更新时间间隔。

在给定车辆某一时刻准确定位数据之后，通过式(8)可以获得之后某一特定时间内的车辆准确定位，从而保证车辆定位数据平滑可靠，并且在一定的时间和距离内绝对误差相对较小，以达到贴边清扫对于位置平滑性和精确性的要求。

本发明实施例提供的一种用于无人清扫车辆贴边清扫的精确定位方法，高效的使用IMU、轮速计、GPS以及激光等传感器的数据进行融合获得车辆当前准确的姿态数据，采用航位推算方法获得车辆当前定位数据，保证车辆定位数据平滑可靠，并且在一定的时间和距离内绝对误差相对较小，以达到贴边清扫对于位置平滑性和精确性的要求。

专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RA用于无人清扫车辆贴边清扫的精确定位方法)、内存、只读存储器(RO用于无人清扫车辆贴边清扫的精确定位方法)、电可编程RO用于无人清扫车辆贴边清扫的精确定位方法、电可擦除可编程RO用于无人清扫车辆贴边清扫的精确定位方法、寄存器、硬盘、可移动磁盘、CD-RO用于无人清扫车辆贴边清扫的精确定位方法、或技术领域内所公知的任意其它形式的存储介质中。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，所述定位方法包括：

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

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

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

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

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

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

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

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

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

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

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

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

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

roll＝a tan(-f_{x_mean}/f_{z_mean})

pitch＝a sin(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)为一个解算周期之内的变化四元数。

7.根据权利要求1所述的用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，所述当收到轮速计数据、激光定位数据或GPS定位数据时，通过所述滤波模型进行滤波，得到误差修正量具体包括：

将接收到的轮速计测量速度作为当前速度观测值、激光定位数据作为当前航向观测值或GPS定位数据作为当前位置观测值；

将所述当前速度观测值、当前航向观测值或当前位置观测值输入所述误差模型进行EKF滤波得到所述误差修正量。

8.根据权利要求1所述的用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，根据所述车辆当前的姿态信息和轮速计数据进行航位推算，得到当前车辆在导航坐标系下的位置和姿态信息具体包括：

根据车辆当前的姿态信息和所述轮速计输出的当前车辆的速度数据计算当前所述车辆在导航坐标系下的速度数据；

根据当前所述车辆在导航坐标系下的速度数据计算出所述当前所述车辆在导航坐标系下的位置数据。

9.根据权利要求8所述的用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，所述根据车辆当前的姿态信息和所述轮速计输出的当前车辆的速度数据计算当前所述车辆在导航坐标系下的速度数据具体包括：

采用下面两个公式计算当前所述车辆在导航坐标系下的速度数据：

其中，R、P、H表示车辆当前的横滚角、俯仰角和航向角，表示车辆坐标系到导航坐标系姿态转换矩阵，P_eo、P_no、P_uo表示车辆在导航坐标系内速度V_odom表示轮速计测量的当前车速。

10.根据权利要求9所述的用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，根据当前所述车辆在导航坐标系下的速度数据计算出所述当前所述车辆在导航坐标系下的位置数据具体包括：根据下面公式计算当前所述车辆在导航坐标系下的位置数据：

其中，P_e、P_n、P_u为车辆当前在导航坐标系下的准确位置数据，ΔT为数据更新时间间隔。