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

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

A Precise Location Method for Unmanned Vehicle Edge Cleaning

【技术实现步骤摘要】
一种用于无人清扫车辆贴边清扫的精确定位方法
本专利技术涉及自动驾驶领域，尤其涉及一种用于无人清扫车辆贴边清扫的精确定位方法。
技术介绍
近年来随着无人驾驶技术的不断发展与普及，应用于特定环境内的小型低速无人清扫车成为无人驾驶最先落地的商业化产品，对于公园、校园、大型商场、工业园区等场景的卫生清扫作业，由智能化无人清扫车替代人工清扫成为趋势，“贴边”清扫是无人清扫车特定环境下完成高效率清扫目标的重要技术手段之一。小型低速无人清扫车同时配备高精度差分GNSS(GlobalNavigationSatelliteSystem,GNSS)板卡、激光雷达、IMU(InertialMeasurementUnit,IMU)以及轮速计等传感器，高精度差分GPS仅在开阔环境下给出高精度定位定向信息，激光定位方法在遮挡环境或者周边物体三维特征明显的情况下可以给出厘米级定位精度，采用多传感器数据融合和航位推算技术，可以获得车辆当前准确定位信息。目前应用于车辆“贴边”行驶多采用视觉或者激光传感器进行路沿检测获得车辆相对定位的方法。在智能无人驾驶系统中，高精地图是必不可少的先验信息，根据视觉或激光传感器采集到车辆周围的数据信息，提取特征并与高精地图进行匹配，获得车辆当前相对于“路沿”位置以达到车辆“贴边”行驶目的。基于视觉传感器的检测方案存在明显的缺陷,其一,图像识别的运算量大，其二,算法的效果依赖图像的成像质量,特别是如果遇到下雨、雾天、道路阴暗、对向强光等情况时会大大降低其识别效果。基于激光传感器的检测方案虽然可以适应道路阴暗、对向强光等场景，但同样易受雨雪等极端天气状况的影响，会导致其检测效果往往无法满足实用要求。此外，无论是采用视觉或者激光传感器进行路沿检测，均需要耗费大量计算资源并且对地图精度要求非常高。
技术实现思路
本专利技术的目的是针对现有技术的缺陷，提供一种用于无人清扫车辆贴边清扫的精确定位方法，高效的使用IMU、轮速计、GPS以及激光等传感器的数据进行融合获得车辆当前准确的姿态数据，采用航位推算方法获得车辆当前定位数据，保证车辆定位数据平滑可靠，并且在一定的时间和距离内绝对误差相对较小，以达到贴边清扫对于位置平滑性和精确性的要求。有鉴于此，本专利技术实施例提供了一种用于无人清扫车辆贴边清扫的精确定位方法，包括：接收IMU数据和轮速计脉冲数据；根据所述IMU数据和轮速计脉冲数据对车辆状态进行监控，当车辆状态满足预设静止条件时，根据所述IMU数据输出计算所述车辆的初始水平姿态角和三轴陀螺仪上电初始零偏值；实时接收IMU数据，根据所述实时接收到的IMU数据输出角速度数据和加速度数据，根据所述角速度数据更新姿态四元数，根据所述加速度数据更新速度数据和位置；根据所述更新后的姿态四元数、速度数据和位置建立相应的滤波模型；当收到轮速计数据、激光定位数据或GPS定位数据时，通过所述滤波模型进行滤波，得到误差修正量，根据所述误差修正量对所述更新后的姿态四元数、速度数据和位置进行修正，得到车辆当前的姿态信息；根据所述车辆当前的姿态信息和轮速计数据进行航位推算，得到当前车辆在导航坐标系下的位置和姿态信息。优选的，所述根据所述IMU数据和轮速计脉冲数据对车辆状态进行监控，当车辆状态满足预设静止条件时，根据所述IMU数据输出计算所述车辆的初始水平姿态角和三轴陀螺仪上电初始零偏值具体包括：根据所述轮速计脉冲数据判断车辆是否处于静止状态；当所述车辆处于静止状态时，对IMU数据进行累加；判断所述车辆在预设时间内是否处于静止状态；当所述车辆在预设时间内处于静止状态时，根据所述累加的IMU数据计算加速度计均值和陀螺仪均值；根据所述加速度计均值计算初始水平姿态角，并将所述陀螺仪均值为作为三轴陀螺仪上电初始零偏值。进一步优选的，所述根据所述加速度计均值计算初始水平姿态角具体包括：根据下式计算初始水平姿态角：roll＝atan(-fx_mean/fz_mean)pitch＝asin(fy_mean/g)其中，fx_mean为X轴加速度计3s数据均值，fy_mean为Y轴加速度计3s数据均值，fz_mean为Z轴加速度计3s数据均值，g为当地重力加速度。优选的，在所述实时接收IMU数据，根据所述实时接收到的IMU数据输出角速度数据和加速度数据，根据所述角速度数据更新姿态四元数之前，所述方法还包括：根据所述初始水平姿态角得到初始姿态四元数。进一步优选的，所述根据所述角速度数据更新姿态四元数具体包括：根据角速度数据和所述三轴陀螺仪上电初始零偏值更新初始姿态四元数。进一步优选的，所述根据角速度数据和所述三轴陀螺仪上电初始零偏值更新初始姿态四元数具体包括：根据所述实时接收IMU数据得到陀螺仪数据；将所述陀螺仪数据和所述上电初始零偏值作差，得到修正后的陀螺仪数据；根据所述修正后的陀螺仪数据得到一个解算周期之内的变化四元数；根据所述一个解算周期之内的变化四元数和初始姿态四元数运用下式进行姿态解算：其中，QK为当前时刻四元数，QK-1为前一时刻四元数，Q(T)为一个解算周期之内的变化四元数。优选的，所述当收到轮速计数据、激光定位数据或GPS定位数据时，通过所述滤波模型进行滤波，得到误差修正量具体包括：将接收到的轮速计测量速度作为当前速度观测值、激光定位数据作为当前航向观测值或GPS定位数据作为当前位置观测值；将所述当前速度观测值、当前航向观测值或当前位置观测值输入所述误差模型进行EKF滤波得到所述误差修正量。优选的，根据所述车辆当前的姿态信息和轮速计数据进行航位推算，得到当前车辆在导航坐标系下的位置和姿态信息具体包括：根据车辆当前的姿态信息和所述轮速计输出的当前车辆的速度数据计算当前所述车辆在导航坐标系下的速度数据；根据当前所述车辆在导航坐标系下的速度数据计算出所述当前所述车辆在导航坐标系下的位置数据。进一步优选的，所述根据车辆当前的姿态信息和所述轮速计输出的当前车辆的速度数据计算当前所述车辆在导航坐标系下的速度数据具体包括：采用下面两个公式计算当前所述车辆在导航坐标系下的速度数据：其中，R、P、H表示车辆当前的横滚角、俯仰角和航向角，表示车辆坐标系到导航坐标系姿态转换矩阵，Veo、Vno、Vuo′表示车辆在导航坐标系内速度Vodom表示轮速计测量的当前车速。进一步优选的，根据当前所述车辆在导航坐标系下的速度数据计算出所述当前所述车辆在导航坐标系下的位置数据具体包括：根据下面公式计算当前所述车辆在导航坐标系下的位置数据：其中，其中，Pe、Pn、Pu为车辆当前在导航坐标系下的准确位置数据，ΔT为数据更新时间间隔。本专利技术实施例提供的一种用于无人清扫车辆贴边清扫的精确定位方法，高效的使用IMU、轮速计、GPS以及激光等传感器的数据进行融合获得车辆当前准确的姿态数据，采用航位推算方法获得车辆当前定位数据，保证车辆定位数据平滑可靠，并且在一定的时间和距离内绝对误差相对较小，以达到贴边清扫对于位置平滑性和精确性的要求。附图说明图1为本专利技术实施例提供的设计思路示意图；图2为本专利技术实施例提供的一种用于无人清扫车辆贴边清扫的精确定位方法流程图；图3为本专利技术实施例提供的初始化的示意图。具体实施方式下面通过附图和实施例，对本专利技术的技术方案做进一步的详细描述。本专利技术实施例提供的一种精确定位方本文档来自技高网...

【技术保护点】
1.一种用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，所述定位方法包括：接收IMU数据和轮速计脉冲数据；根据所述IMU数据和轮速计脉冲数据对车辆状态进行监控，当车辆状态满足预设静止条件时，根据所述IMU数据输出计算所述车辆的初始水平姿态角和三轴陀螺仪上电初始零偏值；实时接收IMU数据，根据所述实时接收到的IMU数据输出角速度数据和加速度数据，根据所述角速度数据更新姿态四元数，根据所述加速度数据更新速度数据和位置；根据所述更新后的姿态四元数、速度数据和位置建立相应的滤波模型；当收到轮速计数据、激光定位数据或GPS定位数据时，通过所述滤波模型进行滤波，得到误差修正量，根据所述误差修正量对所述更新后的姿态四元数、速度数据和位置进行修正，得到车辆当前的姿态信息；根据所述车辆当前的姿态信息和轮速计数据进行航位推算，得到当前车辆在导航坐标系下的位置和姿态信息。

【技术特征摘要】
1.一种用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，所述定位方法包括：接收IMU数据和轮速计脉冲数据；根据所述IMU数据和轮速计脉冲数据对车辆状态进行监控，当车辆状态满足预设静止条件时，根据所述IMU数据输出计算所述车辆的初始水平姿态角和三轴陀螺仪上电初始零偏值；实时接收IMU数据，根据所述实时接收到的IMU数据输出角速度数据和加速度数据，根据所述角速度数据更新姿态四元数，根据所述加速度数据更新速度数据和位置；根据所述更新后的姿态四元数、速度数据和位置建立相应的滤波模型；当收到轮速计数据、激光定位数据或GPS定位数据时，通过所述滤波模型进行滤波，得到误差修正量，根据所述误差修正量对所述更新后的姿态四元数、速度数据和位置进行修正，得到车辆当前的姿态信息；根据所述车辆当前的姿态信息和轮速计数据进行航位推算，得到当前车辆在导航坐标系下的位置和姿态信息。2.根据权利要求1所述的用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，所述根据所述IMU数据和轮速计脉冲数据对车辆状态进行监控，当车辆状态满足预设静止条件时，根据所述IMU数据输出计算所述车辆的初始水平姿态角和三轴陀螺仪上电初始零偏值具体包括：根据所述轮速计脉冲数据判断车辆是否处于静止状态；当所述车辆处于静止状态时，对IMU数据进行累加；判断所述车辆在预设时间内是否处于静止状态；当所述车辆在预设时间内处于静止状态时，根据所述累加的IMU数据计算加速度计均值和陀螺仪均值；根据所述加速度计均值计算初始水平姿态角，并将所述陀螺仪均值为作为三轴陀螺仪上电初始零偏值。3.根据权利要求2所述的用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，所述根据所述加速度计均值计算初始水平姿态角具体包括：根据下式计算初始水平姿态角：roll＝atan(-fx_mean/fz_mean)pitch＝asin(fy_mean/g)其中，fx_mean为X轴加速度计3s数据均值，fy_mean为Y轴加速度计3s数据均值，fz_mean为Z轴加速度计3s数据均值，g为当地重力加速度。4.根据权利要求1所述的用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，在所述实时接收IMU数据，根据所述实时接收到的IMU数据输出角速度数据和加速度数据，根据所述角速度数据更新姿态四元数之前，所述方法还包括：根据所述初始水平姿态角得到初始姿态四元数。5.根据权利要求4所述的用于无人清扫车辆贴边清扫的精确定位方法，其特征在于，所述根据所述...

【专利技术属性】
技术研发人员：李波，黄友，张国龙，张放，李晓飞，张德兆，王肖，霍舒豪，
申请(专利权)人：北京智行者科技有限公司，
类型：发明
国别省市：北京,11