无人驾驶车辆实时跟踪行驶轨迹的计算方法

摘要

本发明涉及一种无人驾驶车辆实时跟踪行驶轨迹的计算方法，具体步骤如下：将无人驾驶车辆的非驱动桥左右车轮中心上安装旋转编码器机构，所述旋转编码器机构与计算机的解码器无线连接；标定：保持车辆以10公里/小时的速度匀速直线行驶50米，读取左右编码器脉冲读数CL、CR，计算得车轮在直线行驶时的滚动半径。有益效果：测量出左右车轮角位移，通过车轮滚动半径，可以计算得到左右车轮向前滚动的曲线距离，由于车辆在曲线行驶和直线行驶时，左右车轮滚动距离不同，可以计算获得车辆的航向角和车辆的具体坐标位置。准确地实时测量并计算车辆位置姿态数据。

主权项

1.一种无人驾驶车辆实时跟踪行驶轨迹的计算方法，具体步骤如下：(1)安装旋转编码器机构，将无人驾驶车辆的非驱动桥左右车轮中心上安装旋转编码器机构，所述旋转编码器机构与计算机的解码器无线连接；(2)标定保持车辆以10公里/小时的速度匀速直线行驶50米，读取左右编码器脉冲读数C_L、C_R，计算得车轮在直线行驶时的滚动半径r’_rL、r’_rR(单位：米)；$r_{\mathrm{rL}}^{\prime }=\frac{50\times 2000}{2\pi C_L}$$r_{\mathrm{rR}}^{\prime }=\frac{50\times 2000}{2\pi C_R}---\left(1\right)$(3)修正：车辆在弯道行驶时，侧向加速度会造成左右两侧车轮垂向载荷转移，引起车轮滚动半径发生变化，需要对采集到的数据进行修正，在滚动半径计算公式(1)中增加修正参数因子δ以补偿左右两侧车轮垂向载荷转移对车轮滚动半径的影响，修正因子δ的估算公式(2)：δ＝sign(a)×|a|^1/3×sc       (2)式中sc为无量纲修正因子，可通过弯道试验标定得到；实验过程：修正前左右编码器读数分别为C_Lbv、C_Rbv；保持车辆以20公里/小时的速度沿半径为50米的圆周逆时针匀速行驶一周，左右编码器读数分别为C_Lev、C_Rev；则读取左右编码器脉冲变化读数C_Lv＝C_Lev-C_Lbv＝、C_Rv＝C_Rev-C_Rbv，无量纲修正因子sc可由下式：$\mathrm{sc}=((\frac{100-B}{C_{\mathrm{LV}}{r}_{\mathrm{rL}}^{\prime }}-\frac{100+B}{C_{\mathrm{RV}}{r}_{\mathrm{rR}}^{\prime }})\times 500)/\left({({(200/36)}^2/(50+\frac{B}{2}))}^{1/3}\right)$式中a：$a=-\frac{B}{2}\frac{r_{\mathrm{rL}}^{\prime }(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})+r_{\mathrm{rR}}^{\prime }(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})}{r_{\mathrm{rL}}^{\prime }(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})-r_{\mathrm{rR}}^{\prime }(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})}$$\times {\left(\frac{r_{\mathrm{rR}}^{\prime }(E_{\mathrm{Rn}}-2E_{\mathrm{Rn}-1}+E_{\mathrm{Rn}-2})-r_{\mathrm{rL}}^{\prime }(E_{\mathrm{Ln}}-2E_{\mathrm{Ln}-1}+E_{\mathrm{Ln}-2})}{\mathrm{\Delta TB}}\right)}^2$a的第一部分反应车辆方向盘转角的影响因素，第二部分反应行驶车速的影响因素。式中E_in为第n个采样时刻编码器读数，下标i为L或R，分别代表左轮、右轮，因此，车轮滚动半径用公式(3)估算修正：r_rL＝r′_rL(1+δ)    r_rR＝r′_rR(1-δ)    (3)(4)车辆位置及航向角计算方法在第n个采样周期内，左右车轮分别滚动的曲线距离，即弧长由公式(4)计算：${\bar{L}}_n=r_{\mathrm{rL}}(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})\frac{2\pi }{2000}$(4)${\bar{R}}_n=r_{\mathrm{rR}}(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})\frac{2\pi }{2000}$在第n个采样周期内，车辆航向角的变化α_n由公式(5)计算：${\alpha }_n=\frac{{\bar{R}}_n}{{2R}_{2n}}=\frac{{\bar{L}}_n}{{2R}_{1n}}=\frac{({\bar{R}}_n-{\bar{L}}_n)}{2B}---\left(5\right)$在第n个采样周期内，左右车轮分别滚动的曲线弦长L_n、R_n由公式(6)计算$\mathrm{Sin}\left({\alpha }_n\right)=\frac{L_n}{{2R}_{1n}}=\frac{R_n}{2R_{2n}}$$L_n=2R_{1n}\mathrm{Sin}\left({\alpha }_n\right)=2\frac{{\bar{L}}_n}{{\alpha }_n}\mathrm{Sin}\left(\frac{({\bar{R}}_n-{\bar{L}}_n)}{2B}\right)=2\frac{{\bar{L}}_{n}B}{({\bar{R}}_n-{\bar{L}}_n)}\mathrm{Sin}\left(\frac{({\bar{R}}_n-{\bar{L}}_n)}{2B}\right)---\left(6\right)$$R_n=2R_{2n}\mathrm{Sin}\left({\alpha }_n\right)=2\frac{{\bar{R}}_n}{{\alpha }_n}\mathrm{Sin}\left(\frac{({\bar{R}}_n-{\bar{L}}_n)}{2B}\right)=2\frac{{\bar{R}}_{n}B}{({\bar{R}}_n-{\bar{L}}_n)}\mathrm{Sin}\left(\frac{({\bar{R}}_n-{\bar{L}}_n)}{2B}\right)$则车辆航向角和车辆左右两侧后轮位置由公式(7)计算得${\gamma }_n=\underset{i=1}{\overset{n}{\Sigma }}{\alpha }_i$$\begin{array}{cc}{x}_{\mathrm{Ln}}=\underset{i=1}{\overset{n}{\Sigma }}(L_i\mathrm{Sin}\left({\gamma }_i\right)-\frac{B}{2})& y_{\mathrm{Ln}}=\underset{i=1}{\overset{n}{\Sigma }}{L}_i\mathrm{Cos}\left({\gamma }_i\right)\end{array}$$\begin{array}{cc}{x}_{\mathrm{Rn}}=\underset{i=1}{\overset{n}{\Sigma }}(R_i\mathrm{Sin}\left({\gamma }_i\right)+\frac{B}{2})& y_{\mathrm{Rn}}=\underset{i=1}{\overset{n}{\Sigma }}{R}_i\mathrm{Cos}\left({\gamma }_i\right)---\left(7\right)\end{array}$(5)实际测试计算效果通过实车试验时采集的左右车轮编码器读数，经过上述计算方法可以获得高精度的计算效果。