CN106123908A - 车辆导航方法和系统 - Google Patents

Abstract

本发明提供了一种车辆导航方法，包括：测量车辆的加速度和角速度，根据测得的加速度和角速度通过捷联惯性导航算法得到车辆的航向角变化量的量测值和位置；测量车辆左右相对的两个车轮各自的行进距离，将测得的左右相对的两个车轮各自的行进距离之差除以这两个车轮的间距，计算得到所述航向角变化量的计算值；根据所述量测值和所述计算值得到所述航向角变化量的最优解；以及根据车辆的位置和所述航向角变化量的最优解来调整车辆运动，使车辆回到预设的行驶轨迹上。本发明还提供了一种车辆导航系统。

Description

车辆导航方法和系统

技术领域

本发明总体上涉及导航技术，更具体地涉及一种车辆导航方法和系统。

背景技术

随着智能技术的飞速发展，人们越来越希望在更多领域对其加以应用，以提高生产和管理效率。智能车辆导航是近期的研究热点，通过自动感测车辆位置和行驶方向，实现车辆自动导航。

目前的导航技术主要包括以下几种。

一种导航技术是基于磁条和磁传感器的导航技术。这种导航技术是在地面铺设磁条导轨，安装在车辆上的磁传感器能感知到与磁条的偏移量，从而通过调节车轮来实现循迹导航。

另一种导航技术是基于超声的导航技术。这种导航技术是由车辆上的超声发射器定向发射一定频率的超声波，通过计算反射波与入射波的时间差来计算与选定方向实体的距离，从而实现导航定位功能。

还有一种导航技术是基于激光定位的导航技术。这种导航技术是在车的行驶路径的周围安装位置精确的激光反射板，车辆通过激光扫描器发射激光束，同时采集由反射板反射的激光束，通过连续的三角几何运算来确定其当前的位置和航向。

但是，磁条导航技术需要预先在地面铺设磁条，施工难度较高，可扩展性差。超声导航技术和激光导航技术对环境要求较高，安装复杂，造价昂贵。

发明内容

本发明的一个方面提供了一种车辆导航方法，包括：测量车辆的加速度和角速度，根据测得的加速度和角速度通过捷联惯性导航算法得到车辆的航向角变化量的量测值和位置；测量车辆左右相对的两个车轮各自的行进距离，将测得的左右相对的两个车轮各自的行进距离之差除以这两个车轮的间距，计算得到所述航向角变化量的计算值；根据所述量测值和所述计算值得到所述航向角变化量的最优解；以及根据车辆的位置和所述航向角变化量的最优解来调整车辆运动，使车辆回到预设的行驶轨迹上。

本发明的另一个方面提供了一种车辆导航系统，包括：惯性测量单元，测量车辆的加速度和角速度；惯导计算单元，根据测得的加速度和角速度通过捷联惯性导航算法得到车辆的航向角变化量的量测值和位置；分别安装在车辆的左右两侧相对车轮上的两个编码器，车轮每转动一定位移，编码器发出一个脉冲；运动学计算单元，通过计算脉冲个数得到在采样时间内所述两个车轮各自的行进距离，将测得的左右相对的两个车轮各自的行进距离之差除以这两个车轮的间距，计算得到所述航向角变化量的计算值；融合单元，根据所述量测值和所述计算值得到所述航向角变化量的最优解；以及导航解算单元，根据车辆的位置和所述航向角变化量的最优解来调整车辆运动，使车辆回到预设的行驶轨迹上。

本发明的另一个方面提供了一种车辆导航系统，包括：惯性测量单元，测量车辆的加速度和角速度；分别安装在车辆的左右两侧相对车轮上的两个编码器，车轮每转动一定位移，编码器发出一个脉冲，通过计算脉冲个数得到在采样时间内所述两个车轮各自的行进距离；处理器；以及存储器，存储有机器可读指令，所述指令在被处理器执行时，使得处理器：根据测得的加速度和角速度通过捷联惯性导航算法得到车辆的航向角变化量的量测值和位置；将测得的左右相对的两个车轮各自的行进距离之差除以这两个车轮的间距，计算得到所述航向角变化量的计算值；根据所述量测值和所述计算值得到所述航向角变化量的最优解；以及根据车辆的位置和所述航向角变化量的最优解来调整车辆运动，使车辆回到预设的行驶轨迹上。

附图说明

为了更完整地理解本发明及其优势，现在将参考结合附图的以下描述，其中：

图1示意性示出了根据本发明实施例的车辆和车辆所处环境；

图2示意性示出了根据本发明实施例在车辆上安装的编码器；

图3示意性示出了根据本发明实施例在环境地面上布置的二维码阵列；以及

图4示意性示出了根据本发明实施例的车辆导航系统的结构框图。

具体实施方式

根据结合附图对本发明示例性实施例的以下详细描述，本发明的其它方面、优势和突出特征对于本领域技术人员将变得显而易见。

在本发明中，术语“包括”和“含有”及其派生词意为包括而非限制；术语“或”是包含性的，意为和/或。

在本说明书中，下述用于描述本发明原理的各种实施例只是说明，不应该以任何方式解释为限制发明的范围。参照附图的下述描述用于帮助全面理解由权利要求及其等同物限定的本发明的示例性实施例。下述描述包括多种具体细节来帮助理解，但这些细节应认为仅仅是示例性的。因此，本领域普通技术人员应认识到，在不背离本发明的范围和精神的情况下，可以对本文中描述的实施例进行多种改变和修改。此外，为了清楚和简洁起见，省略了公知功能和结构的描述。此外，贯穿附图，相同参考数字用于相似功能和操作。

如图1所示，以车辆所在地面(例如仓库地面)的平面为环境坐标系的XY平面，车辆的位置信息是环境坐标系中的X轴和Y轴坐标，车辆的姿态信息包括车头方向与X轴的夹角(航向角)的信息。本文中，将车辆的位置和姿态统称为位姿。

通过捷联惯性导航算法解算出车辆的航向角变化量的量测值Δθ_kg，

Δθ_kg＝h_k+v_k h_k＝ω_k·Δt

其中，ω_k为车辆在状态k的瞬时角速度，v_k为IMU的量测噪声，例如是高斯白噪声。k为车辆的状态编号，按时间先后从小到大依次取值。Δt是采样时间间隔，例如可以是两个状态之间的时间间隔。

根据本发明的实施例，在车辆左右相对的两个车轮(例如左前轮和右前轮、或左后轮和右后轮)上各安装有一个编码器，如图2所示。当车轮转动时，编码器会发出脉冲信号，通过计算脉冲信号的个数，可以得知相应车轮在测量时间内行走的距离，再除以测量时间，即可得出车辆的即时速度信息。根据本发明的实施例，所述编码器安装在车轮上，车轮每转动一定位移，编码器发出一个脉冲信号，通过计算脉冲个数即可知道车轮在采样时间内的总位移。

解车辆的运动学方程可以得到车辆的航向角变化量，解算公式为：

u_k＝U_R(k)-U_L(k)/W

其中，k为车辆的状态编号，按时间先后从小到大依次取值。U_R(k)和U_L(k)分别为状态k-1到状态k之间，右车轮和左车轮行进的距离，W为右车轮和左车轮的车轮间距。

将测得的左右相对的两个车轮各自的行进距离之差除以这两个车轮的间距，得到航向角变化量计算值Δθ_ke，

Δθ_ke＝u_k+w_k

其中，w_k为系统噪声，是高斯白噪声。k为车辆的状态编号，按时间先后从小到大依次取值。

于是，得到预测方程为：

$\left\{\begin{array}{c}\mathrm{\Δ}{\widehat{\mathrm{\θ}}}_k=u_{k-1}\\ {\hat{P}}_k=P_{k-1}+Q\end{array}\right.$

其中P为预测均方误差阵，代表预测均方误差阵估计值，k-1表示状态k的前一状态。Q为系统噪声的方差矩阵。

初始状态的预测均方误差阵P₀可以通过对具体环境分析给出，本文中初始状态通过下文描述的二维码阵列获取，通过估算或实测等方式可以得到P₀，状态k下的预测均方误差阵Pk则可以通过迭代方法解算。

更新方程为：

$\left\{\begin{array}{c}{K}_k={\hat{P}}_k/({\hat{P}}_k+R)\\ {\mathrm{\Δ\θ}}_k=\mathrm{\Δ}{\widehat{\mathrm{\θ}}}_k+K_k(h_k-\mathrm{\Δ}{\widehat{\mathrm{\θ}}}_k)\\ P_k=(1-K_k){\hat{P}}_k\end{array}\right.$

其中，R为量测噪声的方差矩阵。

这样，将编码器和惯导解算出的航向角变化量进行融合，可以得到状态k的航向角变化量的最优解Δθ_k，根据这个最优解可以得到与预定轨迹的偏差，相应地调整车辆运动，可以使车辆回归到正确的轨迹上。根据本发明的实施例，将初始航向角与后续各个状态的航向角变化量最优解累加，可以得到当前状态的航向角。车辆的位置和航向角已知，则可以使用现有导航技术将调整车辆行驶状态，使其回到预设路线。

根据本发明的实施例，可以在地面铺设二维码方阵，如图3所示。通过设置在车辆中心的摄像头获取二维码，经过分析可以得到该二维码的路标点ID，进而得到车辆的X、Y坐标。通过分析二维码图像还可以得到车辆经过该二维码时的航向角信息。

根据本发明的实施例，已知每个二维码在环境坐标系中的位置坐标和该二维码的尺寸信息。由于车辆的航向角和车辆在X、Y轴上相对于二维码的偏移量会使得拍摄到的图像有所不同，所以通过对拍摄到的图像进行数据处理，可以得到拍摄时刻车辆在X、Y轴上的偏移量以及航向角。将车辆在X、Y轴上相对于二维码的的偏移量与二维码的X、Y坐标分别求和，可以得到车辆的X、Y坐标。

将车辆的X、Y坐标和航向角作为进一步导航的初始值，从而实现误差消除。

根据本发明的实施例，二维码间距在1.5-2米之间，车速最高1.2m/s，因此在不超过2s的时间内车辆的位姿信息会进行一次校正，用以补偿惯性导航和编码器的累积误差，达到精定位的要求。

根据本发明的实施例，从扫描二维码到得到车辆的位置和航向角数据的延时为精准的20ms，因此可以对这段时间的车辆位姿信息进行追溯，方法有以下两种。

第一种方法是从扫描二维码的时刻开始，每隔预定时间(例如10ms)对车辆的位置和航向角变化量进行保存，当得到根据二维码确定的位置和航向角数据时，将此前20ms内车辆的位置和航向角变化量提取出来，对根据二维码确定的位置和航向角数据进行补偿，即可得到当前车辆的位置和航向角。

第二种方法是通过对扫描二维码时刻车辆加速度和角速度进行提取，认为车在20ms内加速度角速度信息不变，即可算出这段时间内车的位移和航向角变化量，再将其与根据二维码确定的位置和航向角数据求和，即可得到当前车辆的位置和航向角。

根据车辆的当前位置和航向角，可以即时调整车辆的运动，引导车辆运行在正确的轨道上。

图4示意性示出了根据本发明实施例的车辆导航系统400的结构框图。

如图4所示，车辆导航系统400包括惯性测量单元(IMU)401、惯导计算单元402、分别安装在车辆的左右两侧相对车轮上的两个编码器403、运动学计算单元404、融合单元405、导航解算单元406、以及运动控制器407。

根据本发明的实施例，可以在车体的几何中心点安装IMU 401，用于采集车辆运动的角速度和加速度信息。根据本发明的实施例，IMU 401包括可以测量车辆的角速度的三轴陀螺仪和可以测量车辆的加速度信息的三轴加速计。惯导计算单元402基于车辆的加速度和角速度信息，以环境坐标系为导航坐标系，可以通过捷联惯性导航算法解算出车辆的位置以及航向角信息。

通过捷联惯性导航算法解算出车辆的航向角变化量的量测值Δθ_kg，

Δθ_kg＝h_k+v_k h_k＝ω_k·Δt

其中，ω_k为车辆的瞬时角速度，v_k为IMU 401的量测噪声，例如是高斯白噪声。k为车辆的状态编号，按时间先后从小到大依次取值。Δt是采样时间间隔，例如可以是两个状态之间的时间间隔。

根据本发明的实施例，在车辆左右相对的两个车轮(例如左前轮和右前轮)上各安装有一个编码器403，如图2所示。当车轮转动时，编码器会发出脉冲信号，运动学计算单元404接收到这个信号后，通过计算脉冲信号的个数，可以得知相应车轮在测量时间内行走的距离，再除以测量时间，即可得出车辆的即时速度信息。根据本发明的实施例，所述编码器安装在车轮上，车轮每转动一定位移，编码器发出一个脉冲，通过计算脉冲个数即可知道车轮在采样时间内的总位移。

解车辆的运动学方程可以得到车辆的航向角变化量，解算公式为：

u_k＝U_R(k)-U_L(k)/W

其中，k为车辆的状态编号，按时间先后从小到大依次取值。U_R(k)和U_L(k)分别为状态k-1到状态k之间，右车轮和左车轮行进的距离，W为右车轮和左车轮的车轮间距。

根据测得的左右相对的两个车轮各自的行进距离得到航向角变化量计算值Δθ_ke，

Δθ_ke＝u_k+w_k

其中，w_k为系统噪声，是高斯白噪声。k为车辆的状态编号，按时间先后从小到大依次取值。

融合单元405基于惯导计算单元402得到的航向角变化量量测值和运动学计算单元404得到的航向角变化量计算值，得到航向角变化量最优值。

预测方程为：

$\left\{\begin{array}{c}\mathrm{\Δ}{\widehat{\mathrm{\θ}}}_k=u_{k-1}\\ {\hat{P}}_k=P_{k-1}+Q\end{array}\right.$

其中P为预测均方误差阵，代表预测均方误差阵估计值，k-1表示状态k的前一状态。Q为系统噪声的方差矩阵。

初始状态的预测均方误差阵P₀可以通过对具体环境分析给出，本文中初始状态通过下文描述的二维码阵列获取，通过估算或实测等方式可以得到P₀，状态k下的预测均方误差阵Pk则可以通过迭代方法解算。

更新方程为：

$\left\{\begin{array}{c}{K}_k={\hat{P}}_k/({\hat{P}}_k+R)\\ {\mathrm{\Δ\θ}}_k=\mathrm{\Δ}{\widehat{\mathrm{\θ}}}_k+K_k(h_k-\mathrm{\Δ}{\widehat{\mathrm{\θ}}}_k)\\ P_k=(1-K_k){\hat{P}}_k\end{array}\right.$

其中，R为量测噪声的方差矩阵。

这样，将编码器和惯导解算出的航向角变化量进行融合，可以得到状态k的航向角变化量的最优解Δθ_k，根据这个最优解可以得到与预定轨迹的偏差，相应地调整车辆运动，可以使车辆回归到正确的轨迹上。

根据本发明的实施例，导航解算单元406根据车辆航向角变化量的最优解Δθ_k，得到当前车体位姿信息。运动控制器407根据该位姿信息调整车辆运动，使车辆回到正确的轨迹上。

根据本发明的实施例，可以在地面铺设二维码方阵，如图3所示。通过设置在车辆上的二维码采集器408获取二维码，经过精确位姿计算单元409处理，可以得到该二维码的路标点ID，进而得到车辆的X、Y坐标。通过分析二维码图像还可以得到车辆经过二维码时的航向角信息。根据本发明的实施例，二维码采集器408例如可以设置在车辆底部中心的摄像头。

根据本发明的实施例，已知每个二维码在环境坐标系中的位置坐标和该二维码的尺寸信息。由于车辆的航向角和车辆在X、Y轴上相对于二维码的偏移量会使得拍摄到的图像有所不同，所以通过对拍摄到的图像进行数据处理，可以得到拍摄时刻车辆在X、Y轴上的偏移量以及航向角。将车辆在X、Y轴上相对于二维码的的偏移量与二维码的X、Y坐标分别求和，可以得到车辆的X、Y坐标。

将车辆的X、Y坐标和航向角发送给当前位置确定单元406，导航解算单元将该信息作为进一步导航的初始值，从而实现误差消除。

根据本发明的实施例，二维码间距在1.5-2米之间，车速最高1.2m/s，因此在不超过2s的时间内车辆的位姿信息会进行一次校正，用以补偿惯性导航和编码器的累积误差，达到精定位的要求。

根据本发明的实施例，从扫描二维码到得到车辆的位置和航向角数据的延时为精准的20ms，精确位姿计算单元409可以对这段时间的车辆位姿信息进行追溯，方法有以下两种。

第一种方法是从扫描二维码的时刻开始，精确位姿计算单元409每隔预定时间(例如10ms)对车辆的位置和航向角变化量进行保存，当得到根据二维码确定的位置和航向角数据时，将此前20ms内车辆的位置和航向角变化量变化量提取出来，对根据二维码确定的位置和航向角数据进行补偿，即可得到当前车辆的位置和航向角。

第二种方法是通过对扫描二维码时刻车辆加速度和角速度进行提取，认为车在20ms内加速度角速度信息不变，精确位姿计算单元409即可算出这段时间内车的位移和航向角变化量，再将其与根据二维码确定的位置和航向角数据求和，即可得到当前车辆的位置和航向角。

根据本发明的实施例，精确位姿计算单元409根据二维码确定车辆精确位姿。运动控制器407根据该位姿信息调整车辆运动，使车辆回到正确的轨迹上。运动控制器407根据该精确位姿调整车辆运动，使车辆回到正确的轨迹上。

根据本发明的实施例，运动控制器407例如是车载运动控制器。车辆轮系采用差速固定舵方式，驱动电机为伺服电机，运动控制器407通过控制两个电机的转速从而实现对车的速度控制和姿态控制。

根据本发明的实施例，可以在车的行进过程中实时给出精确的位置和航向信息，运动控制器通过这个信息可以即时调整轮系运动，从而导引车运行在正确的轨道上。本发明无需在库房实施复杂的磁条铺设等施工，结构简单、成本低、可移植性强，适用各种室内环境。

根据本发明各实施例的上述方法和各个单元(例如，惯导计算单元、运动学计算单元、融合单元、导航解算单元、精确位姿计算单元等)可以通过有计算能力的电子设备执行包含计算机指令的软件来实现。该系统可以包括存储设备，以实现上文所描述的各种存储。所述有计算能力的电子设备可以包含通用处理器、数字信号处理器、专用处理器、可重新配置处理器等能够执行计算机指令的装置，但不限于此。执行这样的指令使得电子设备被配置为执行根据本发明的上述各项操作。上述各设备和/或模块可以在一个电子设备中实现，也可以在不同电子设备中实现。这些软件可以存储在计算机可读存储介质中。计算机可读存储介质存储一个或多个程序(软件模块)，所述一个或多个程序包括指令，当电子设备中的一个或多个处理器执行所述指令时，所述指令使得电子设备执行本发明的方法。

这些软件可以存储为易失性存储器或非易失性存储装置的形式(比如类似ROM等存储设备)，不论是可擦除的还是可重写的，或者存储为存储器的形式(例如RAM、存储器芯片、设备或集成电路)，或者被存储在光可读介质或磁可读介质上(比如，CD、DVD、磁盘或磁带等等)。应该意识到，存储设备和存储介质是适于存储一个或多个程序的机器可读存储装置的实施例，所述一个程序或多个程序包括指令，当所述指令被执行时，实现本发明的实施例。实施例提供程序和存储这种程序的机器可读存储装置，所述程序包括用于实现本发明的任何一项权利要求所述的装置或方法的代码。此外，可以经由任何介质(比如，经由有线连接或无线连接携带的通信信号)来电传递这些程序，多个实施例适当地包括这些程序。

根据本发明各实施例的方法、装置、单元和/或模块还可以使用例如现场可编程门阵列(FPGA)、可编程逻辑阵列(PLA)、片上系统、基板上的系统、封装上的系统、专用集成电路(ASIC)或可以以用于对电路进行集成或封装的任何其他的合理方式等硬件或固件来实现，或以软件、硬件以及固件三种实现方式的适当组合来实现。该系统可以包括存储设备，以实现上文所描述的存储。在以这些方式实现时，所使用的软件、硬件和/或固件被编程或设计为执行根据本发明的相应上述方法、步骤和/或功能。本领域技术人员可以根据实际需要来适当地将这些系统和模块中的一个或多个，或其中的一部分或多个部分使用不同的上述实现方式来实现。这些实现方式均落入本发明的保护范围。

尽管已经参照本发明的特定示例性实施例示出并描述了本发明，但是本领域技术人员应该理解，在不背离所附权利要求及其等同物限定的本发明的精神和范围的情况下，可以对本发明进行形式和细节上的多种改变。因此，本发明的范围不应该限于上述实施例，而是应该不仅由所附权利要求来进行确定，还由所附权利要求的等同物来进行限定。

Claims (18)

1.一种车辆导航方法，包括：

测量车辆的加速度和角速度，根据测得的加速度和角速度通过捷联惯性导航算法得到车辆的航向角变化量的量测值和位置；

测量车辆左右相对的两个车轮各自的行进距离，将测得的左右相对的两个车轮各自的行进距离之差除以这两个车轮的间距，计算得到所述航向角变化量的计算值；

根据所述量测值和所述计算值得到所述航向角变化量的最优解；以及

根据车辆的位置和所述航向角变化量的最优解来调整车辆运动，使车辆回到预设的行驶轨迹上。

2.根据权利要求1所述的方法，其中：

航向角变化量的量测值Δθ_kg＝h_k+v_k，h_k＝ω_k·Δt；以及

航向角变化量的计算值Δθ_kg＝u_k+w_k，u_k＝U_R(k)-U_L(k)/W，

其中：v_k为量测噪声，w_k为系统噪声，k为车辆的状态编号，按时间先后从小到大依次取值，ω_k为车辆在状态k的瞬时角速度，U_R(k)和U_L(k)分别为状态k-1到状态k之间，右车轮和左车轮行进的距离，W为右车轮和左车轮的车轮间距。

3.根据权利要求2所述的方法，其中，根据所述量测值和所述计算值得到车辆航向角变化量的最优解包括：

使用预测方程和更新方程得到状态k的航向角变化量的最优解Δθ_k，其中：

所述预测方程为：

$\left\{\begin{array}{c}\mathrm{\Δ}\widehat{\mathrm{\θ}}=u_{k-1}\\ {\hat{P}}_k=P_{k-1}+Q\end{array}\right.$

所述更新方程为：

$\left\{\begin{array}{c}{K}_k={\hat{P}}_k/\left({\hat{P}}_k+R\right)\\ {\mathrm{\Δ\θ}}_k=\mathrm{\Δ}{\widehat{\mathrm{\θ}}}_k+K_k\left(h_k-\mathrm{\Δ}{\widehat{\mathrm{\θ}}}_k\right)\\ P_k=\left(1-K_k\right){\hat{P}}_k\end{array}\right.$

其中P为预测均方误差阵，代表预测均方误差阵估计值，k-1表示状态k的前一状态，Q为系统噪声的方差矩阵，初始状态的预测均方误差阵P₀通过对具体环境分析给出，状态k下的预测均方误差阵P_k则通过迭代方法解算，R为量测噪声的方差矩阵。

4.根据权利要求1所述的方法，还包括：

在车辆行驶经过在地面铺设的二维码阵列中的二维码时，获取二维码图像；

分析所述二维码图像，得到车辆经过所述二维码时的精确位置和精确航向角；以及

根据所述精确位置和精确航向角来调整车辆运动，使车辆回到预设的行驶轨迹上。

5.根据权利要求4所述的方法，还包括：

从获取二维码的时刻开始，每隔预定时间对车辆的位置和航向角变化量进行保存；

当得到根据二维码确定的精确位置和精确航向角时，将当前时刻与获取二维码的时刻之间车辆的位置和航向角变化量提取出来，对所述精确位置和精确航向角进行补偿，得到当前车辆的位置和航向角。

6.根据权利要求4所述的方法，还包括：

对扫描二维码时刻车辆加速度和角速度进行提取；

认为在得到根据二维码确定的精确位置和精确航向角的时刻与获取二维码的时刻之间，车辆加速度角速度信息不变，算出这段时间内车的位移和航向角变化量，将其与根据二维码确定的精确位置和精确航向角数据求和，得到当前车辆的位置和航向角。

7.一种车辆导航系统，包括：

惯性测量单元，测量车辆的加速度和角速度；

惯导计算单元，根据测得的加速度和角速度通过捷联惯性导航算法得到车辆的航向角变化量的量测值和位置；

分别安装在车辆的左右两侧相对车轮上的两个编码器，车轮每转动一定位移，编码器发出一个脉冲；

运动学计算单元，通过计算脉冲个数得到在采样时间内所述两个车轮各自的行进距离，将测得的左右相对的两个车轮各自的行进距离之差除以这两个车轮的间距，计算得到所述航向角变化量的计算值；

融合单元，根据所述量测值和所述计算值得到所述航向角变化量的最优解；以及

导航解算单元，根据车辆的位置和所述航向角变化量的最优解来调整车辆运动，使车辆回到预设的行驶轨迹上。

8.根据权利要求7所述的系统，其中：

航向角变化量的量测值Δθ_kg＝h_k+v_k，h_k＝ω_k·Δt；以及

航向角变化量的计算值Δθ_kg＝u_k+w_k，u_k＝U_R(k)-U_L(k)/W，

其中：v_k为量测噪声，w_k为系统噪声，k为车辆的状态编号，按时间先后从小到大依次取值，ω_k为车辆在状态k的瞬时角速度，U_R(k)和U_L(k)分别为状态k-1到状态k之间，右车轮和左车轮行进的距离，W为右车轮和左车轮的车轮间距。

9.根据权利要求8所述的系统，其中，根据所述量测值和所述计算值得到车辆航向角变化量的最优解包括：

使用预测方程和更新方程得到状态k的航向角变化量的最优解Δθ_k，其中：

所述预测方程为：

$\left\{\begin{array}{c}\mathrm{\Δ}\widehat{\mathrm{\θ}}=u_{k-1}\\ {\hat{P}}_k=P_{k-1}+Q\end{array}\right.$

所述更新方程为：

$\left\{\begin{array}{c}{K}_k={\hat{P}}_k/\left({\hat{P}}_k+R\right)\\ {\mathrm{\Δ\θ}}_k=\mathrm{\Δ}{\widehat{\mathrm{\θ}}}_k+K_k\left(h_k-\mathrm{\Δ}{\widehat{\mathrm{\θ}}}_k\right)\\ P_k=\left(1-K_k\right){\hat{P}}_k\end{array}\right.$

其中户为预测均方误差阵，代表预测均方误差阵估计值，k-1表示状态k的前一状态，Q为系统噪声的方差矩阵，初始状态的预测均方误差阵P₀通过对具体环境分析给出，状态k下的预测均方误差阵P_k则通过迭代方法解算，R为量测噪声的方差矩阵。

10.根据权利要求7所述的系统，还包括：

二维码采集器，在车辆行驶经过在地面铺设的二维码阵列中的二维码时，获取二维码图像；以及

精确位姿计算单元，分析所述二维码图像，得到车辆经过所述二维码时的精确位置和精确航向角，

其中，所述导航解算单元根据所述精确位置和精确航向角来调整车辆运动，使车辆回到预设的行驶轨迹上。

11.根据权利要求10所述的系统，其中，所述精确位姿计算单元：

从获取二维码的时刻开始，每隔预定时间对车辆的位置和航向角变化量进行保存；以及

当得到根据二维码确定的精确位置和精确航向角时，将当前时刻与获取二维码的时刻之间车辆的位置和航向角变化量提取出来，对所述精确位置和精确航向角进行补偿，得到当前车辆的位置和航向角。

12.根据权利要求10所述的系统，其中，所述精确位姿计算单元：

对扫描二维码时刻车辆加速度和角速度进行提取；

认为在得到根据二维码确定的精确位置和精确航向角的时刻与获取二维码的时刻之间，车辆加速度角速度信息不变，算出这段时间内车的位移和航向角变化量，将其与根据二维码确定的精确位置和精确航向角数据求和，得到当前车辆的位置和航向角。

13.一种车辆导航系统，包括：

惯性测量单元，测量车辆的加速度和角速度；

分别安装在车辆的左右两侧相对车轮上的两个编码器，车轮每转动一定位移，编码器发出一个脉冲，通过计算脉冲个数得到在采样时间内所述两个车轮各自的行进距离；

处理器；以及

存储器，存储有机器可读指令，所述指令在被处理器执行时，使得处理器：

根据测得的加速度和角速度通过捷联惯性导航算法得到车辆的航向角变化量的量测值和位置；

将测得的左右相对的两个车轮各自的行进距离之差除以这两个车轮的间距，计算得到所述航向角变化量的计算值；

根据所述量测值和所述计算值得到所述航向角变化量的最优解；以及

根据车辆的位置和所述航向角变化量的最优解来调整车辆运动，使车辆回到预设的行驶轨迹上。

14.根据权利要求13所述的系统，其中：

航向角变化量的量测值Δθ_kg＝h_k+v_k，h_k＝ω_k·Δt；以及

航向角变化量的计算值Δθ_kg＝u_k+w_k，u_k＝U_R(k)-U_L(k)/W，

其中：v_k为量测噪声，w_k为系统噪声，k为车辆的状态编号，按时间先后从小到大依次取值，ω_k为车辆在状态k的瞬时角速度，U_R(k)和U_L(k)分别为状态k-1到状态k之间，右车轮和左车轮行进的距离，W为右车轮和左车轮的车轮间距。

15.根据权利要求14所述的系统，其中，根据所述量测值和所述计算值得到车辆航向角变化量的最优解包括：

使用预测方程和更新方程得到状态k的航向角变化量的最优解Δθ_k，其中：

所述预测方程为：

$\left\{\begin{array}{c}\mathrm{\Δ}\widehat{\mathrm{\θ}}=u_{k-1}\\ {\hat{P}}_k=P_{k-1}+Q\end{array}\right.$

所述更新方程为：

$\left\{\begin{array}{c}{K}_k={\hat{P}}_k/\left({\hat{P}}_k+R\right)\\ {\mathrm{\Δ\θ}}_k=\mathrm{\Δ}{\widehat{\mathrm{\θ}}}_k+K_k\left(h_k-\mathrm{\Δ}{\widehat{\mathrm{\θ}}}_k\right)\\ P_k=\left(1-K_k\right){\hat{P}}_k\end{array}\right.$

其中P为预测均方误差阵，代表预测均方误差阵估计值，k-1表示状态k的前一状态，Q为系统噪声的方差矩阵，初始状态的预测均方误差阵P₀通过对具体环境分析给出，状态k下的预测均方误差阵P_k则通过迭代方法解算，R为量测噪声的方差矩阵。

16.根据权利要求16所述的系统，还包括：

二维码采集器，在车辆行驶经过在地面铺设的二维码阵列中的二维码时，获取二维码图像，

其中，所述处理器还执行指令，以：

分析所述二维码图像，得到车辆经过所述二维码时的精确位置和精确航向角；以及

根据所述精确位置和精确航向角来调整车辆运动，使车辆回到预设的行驶轨迹上。

17.根据权利要求16所述的系统，其中，所述处理器还执行指令，以：

从获取二维码的时刻开始，每隔预定时间对车辆的位置和航向角变化量进行保存；以及

当得到根据二维码确定的精确位置和精确航向角时，将当前时刻与获取二维码的时刻之间车辆的位置和航向角变化量提取出来，对所述精确位置和精确航向角进行补偿，得到当前车辆的位置和航向角。

18.根据权利要求16所述的系统，其中，所述处理器还执行指令，以：

对扫描二维码时刻车辆加速度和角速度进行提取；

认为在得到根据二维码确定的精确位置和精确航向角的时刻与获取二维码的时刻之间，车辆加速度角速度信息不变，算出这段时间内车的位移和航向角变化量，将其与根据二维码确定的精确位置和精确航向角数据求和，得到当前车辆的位置和航向角。