阅读说明：本技术 一种基于imu和码盘融合的车辆定位方法 (Vehicle positioning method based on IMU and coded disc fusion ) 是由 许文渊 祝露峰 任卓恒 何韵 邓盛兰 杨芬 许天宏 于 2019-12-25 设计创作，主要内容包括：本发明涉及车辆、系统定位技术领域,具体公开了一种基于IMU和码盘融合的车辆定位方法,包括如下步骤,包括如下步骤,步骤1,启动系统；步骤2,获取码盘数据,计算当前位置和航向角θ；步骤3,获取IMU数据计算当前航向角；步骤4,以码盘的x,y坐标和IMU的航向角为基准,进行位置更新。本发明使用码盘和IMU相结合的方法来组成定位系统,计算过程简单,避免繁琐的计算,相较于视觉里程计方法,计算效率得到提高,达到实时性的要求。(The invention relates to the technical field of vehicle and system positioning, and particularly discloses a vehicle positioning method based on IMU and coded disc fusion, which comprises the following steps of 1, starting a system; step 2, code disc data are obtained, and the current position and the course angle theta are calculated; step 3, obtaining IMU data and calculating a current course angle; and 4, updating the position by taking the x and y coordinates of the code disc and the course angle of the IMU as the reference. The positioning system is formed by combining the coded disc and the IMU, the calculation process is simple, complicated calculation is avoided, the calculation efficiency is improved compared with a visual odometer method, and the real-time requirement is met.)

一种基于IMU和码盘融合的车辆定位方法

技术领域

本发明涉及车辆、系统定位技术领域。

背景技术

机器人的定向方法具有两大难点：精度和实时性。传统的基于里程计定向方 法由于机器人在行进中打滑等异常情况从而导致定向不准确；现有定向系统主要 包含有基于轮式里程计定向系统，基于IMU的定向系统，基于GPS的定向系统， 基于视觉里程计的定向系统，但存在不足：

1、基于轮式里程计定向系统的缺点：基于轮式里程计定向系统是通过里程 计获取机器人的移动信息来计算其当前姿态。但是由于码盘精度低以及轮子存在 打滑、侧翻的现象，轮式里程计的定向效果很差。

2、基于IMU的定向系统的缺点：基于IMU的定向系统是通过IMU获取机 器人的加速度信息，然后积分求得其当前姿态。但是由于其姿态角是通过积分得 到，会随着时间而导致其误差增大，其定向在机器人长时间移动中精度较差。

3、基于GPS的定向系统的缺点：基于GPS的定向系统是通过计算机接收 卫星发射的信号进行定位定向的。但是GPS易受到信号的限制，当信号被遮挡 时或反射时，其精度下降严重，尤其是当机器人在室内封闭环境下将无法实现定 位定向。

4、基于视觉里程计的定向系统的缺点：基于视觉里程计的定向系统是通过 对获取的图像进行匹配，然后根据特征点的运动变化估计机器人的运动参数。其 缺点在于视觉里程计常采用稀疏特征点匹配技术，当检测到的特征点少时，容易 造成误匹配。当场景变化较大时，也难以进行匹配。而且，处理视觉图像需要花 费更多的时间，难以达到实时性的要求。

发明内容

本发明的目的在于提供一种基于IMU和码盘融合的车辆定位方法。

为了达到上述目的，本发明的基础方案提供一种基于IMU和码盘融合的车 辆定位方法，包括如下步骤，

步骤1，启动系统；

步骤2，获取码盘数据，计算当前位置和航向角θ；

步骤3，获取IMU数据计算当前航向角；

步骤4，以码盘的x,y坐标和IMU的航向角为基准，进行位置更新。

进一步，步骤3具体为通过对姿态进行获取并利用惯导计算航向角。

进一步，步骤2中，设车辆两轮的轮式里程计给出的车左轮、右轮转动距离 Δd_rL、Δd_rR，设车轮轮中心为M_r，其瞬时速度为v_r，则满足如下公式

进一步，步骤3中，设是t_m时刻的姿态四元数,简记为 是t_m-1时刻的姿态四元数,简记为 是以n系为参考坐标系时，b 系从t_m-1时刻到t_m的变换四元数，的计算利用到载体坐标系相对于导航 坐标系的转动角速度记η_m是从t_m-1时刻到t_m时刻载体坐标系b相对于导 航坐标系n的等效旋转矢量，则满足如下公式

本发明的优点如下：

(1)本发明使用码盘和IMU相结合的方法来组成定位系统，计算过程简单， 避免繁琐的计算，相较于视觉里程计方法，计算效率得到提高，达到实时性的要 求。

(2)本发明使用码盘和IMU相结合的方法来组成定向系统，通过组合方式 获得最优定向结果。相较于单纯使用里程计和IMU的方法，精度得到提高。

(3)本发明使用码盘和IMU相结合的方法来组成定向系统，相较于视觉里 程计、GPS等方法，制作成本更低，满足低成本机器人的制造。

(4)本发明使用码盘和IMU相结合的方法来组成定向系统，在计算航向角 的过程中，计算过程简单，计算效率高，达到实时性的要求。对求出的航向角进 行组合的方式，获得最优结果，可以获得更高精度的定向结果。

附图说明

图1为本发明一种基于IMU和码盘融合的车辆定位方法的逻辑框图；

图2为本发明方法和视觉里程计方法计算效率对比结果图；

图3为本发明方法和轮式里程计方法平均误差对比结果图。

具体实施方式

下面通过具体实施方式进一步详细的说明：

实施例：

如图1所示，一种基于IMU和码盘融合的车辆定位方法，包括如下步骤，

步骤1，启动系统；

步骤2，获取码盘数据，计算当前位置和航向角θ；

步骤3，获取IMU数据计算当前航向角；

步骤4，以码盘的x,y坐标和IMU的航向角为基准，进行位置更新。

具体的操作方法如下：

1、基于车辆码盘计算当前航向角

根据机器人两轮的轮式里程计给出的车轮转动距离Δd_rL和Δd_rR，对于车轮轮 中心M_r，其瞬时速度为v_r，满足如下公式：

其中航向角θ为：

为了简化计算，设车轮中心处M_r的速度v_r为左右后轮瞬时速度的平均值， 即：

将(3)式代入(1)式，合并(2)式，整理得：

式中w为后轮轴轮距。

2、基于IMU计算当前航向角

利用惯导计算航向角是通过对姿态进行获取的到的。利用四元数的链乘规则 可写出姿态四元数更新的递推式：

其中：

是t_m时刻的姿态四元数,简记为

是t_m-1时刻的姿态四元数,简记为

是以n系为参考坐标系时,b系从t_m-1时刻到t_m的变换四元数,它的 计算和有关。

的计算利用到载体坐标系相对于导航坐标系的转动角速度记η_m是从t_m-1时刻到t_m时刻载体坐标系b相对于导航坐标系n的等效旋转矢量, 则有:

由于相对来说变化很小,若认为和在t_m-1≤t≤t_m时间内基本不变,并取为t_m-1时刻的值,则有:

其中

是t_m-1时刻的姿态阵,是t_m-1时刻导航系相对于惯性空间的转 动角速度,均为已知量,并记：

Φ_m的计算可用不同子样数的角增量算法。得到η_m后利用式

可求得完成姿态更新。

姿态更新完毕后，获得当前姿态四元数。将四元素转换为欧拉角获得当前位 置的航向角θ_IMU。

3、车辆位置更新

当里程计计算出来的航向角稳定在一定范围内，则直接采用里程计计算出来 的航向进行更新。当里程计计算出来的航向角大于某个阈值时，则以码盘的x,y 坐标和IMU的航向角为基准，进行位置更新。

如图2，展示了本发明方法(图中下面的折线)和视觉里程计方法(图中上 面的折线)计算效率对比结果。在十组实验中，对两种方法每帧数据的平均计算 进行了对比，获得其计算效率对比图。很明显，本文方法相较于视觉里程计定位 有这更加高效的计算，能够达到实时性的要求。本发明方法只需要进行简单的数 学计算，而基于视觉里程计的定向方法，需要处理大量的图片数据，并进行提点 与匹配，计算效率较低。

如图3，展示了本发明方法(图中下面的折线)和轮式里程计方法(图中上 面的折线)平均误差对比结果。在十组实验中，对两种方法每帧数据的平均误差 进行了对比，获得其定向误差的对比图。很明显，基于轮式里程计的定向方法其 平均误差更大，本发明方法拥有更高的定向精度。轮式里程计由于轮子存在打滑、 侧翻的现象，导致其定向精度下降。而本发明算法通过与IMU获取的数据进行 组合方式，得到优化后的航向角，提高其定向精度。

以上所述的仅是本发明的实施例，方案中公知的具体结构及特性等常识在此 未作过多描述。应当指出，对于本领域的技术人员来说，在不脱离本发明结构的 前提下，还可以作出若干变形和改进，这些也应该视为本发明的保护范围，这些 都不会影响本发明实施的效果和专利的实用性。本申请要求的保护范围应当以其 权利要求的内容为准，说明书中的具体实施方式等记载可以用于解释权利要求的 内容。