车辆导航方法和系统与流程

技术特征：

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所述的系统，其中，所述处理器还执行指令，以：

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

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