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

Abstract

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

Description

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

技术领域

本发明属于智能车辆工程，尤其涉及一种无人驾驶车辆实时跟踪行驶轨迹的计算方法。

背景技术

无人驾驶汽车集自动控制、体系结构、人工智能、视觉计算等众多技术于一体，是计算机科学、模式识别和智能控制技术高度发展的产物。目前，在智能交通技术、车辆工程研究领域都需要实时确定车辆的行驶位置。试验场通常采用安装第五轮来测量数据，虽然可以获得精确的结果，但是受实验条件的局限，不能大规模使用；此外是采用全球定位系统(GPS)，该方法虽然应用方便，但是定位精确度较差(10米)、数据获得频率过低(几十赫兹)，不能满足车辆行驶轨迹实时跟踪的要求。造成目前智能车辆行驶轨迹不能严格跟踪目标行驶路径，产生较大偏差，极易引发事故。发达国家及我国均在进行无人驾驶汽车的研究，目前在可行性和实用化方面都取得了突破性的进展。因此，智能车辆工程领域亟待配合开发出一种无人驾驶车辆行驶轨迹的实时跟踪装置和计算方法，以满足无人驾驶汽车的发展需求。

发明内容

本发明是为了克服现有技术中的不足，提供一种无人驾驶车辆实时跟踪行驶轨迹的计算方法，能够准确测量并计算出车辆实际行驶位置坐标，而且响应频率高达100Hz以上，很好满足使用要求。

本发明为实现上述目的，通过以下技术方案实现，一种无人驾驶车辆实时跟踪行驶轨迹的计算方法，具体步骤如下：

1、安装旋转编码器机构，

将无人驾驶车辆的非驱动桥左右车轮中心上安装旋转编码器机构，所述旋转编码器机构与计算机的解码器无线连接；

2、标定

保持车辆以10公里/小时的速度匀速直线行驶50米，读取左右编码器脉冲读数C_L、C_R，计算得车轮在直线行驶时的滚动半径r’_rL、r’_rR(单位：米)；

$r_{\mathrm{rL}}^{\′}=\frac{50\×2000}{2\mathrm{\π}{C}_L}$ $r_{\mathrm{rR}}^{\′}=\frac{50\×2000}{2\mathrm{\π}{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}}^{\′}}-\frac{100+B}{C_{\mathrm{RV}}{r}_{\mathrm{rR}}^{\′}})\×500)/\left({({(200/36)}^2/(50+\frac{B}{2}))}^{1/3}\right)$

式中a：

$a=-\frac{B}{2}\frac{r_{\mathrm{rL}}^{\′}(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})+r_{\mathrm{rR}}^{\′}(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})}{r_{\mathrm{rL}}^{\′}(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})-r_{\mathrm{rR}}^{\′}(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})}$

$\×{\left(\frac{r_{\mathrm{rR}}^{\′}(E_{\mathrm{Rn}}-2E_{\mathrm{Rn}-1}+E_{\mathrm{Rn}-2})-r_{\mathrm{rL}}^{\′}(E_{\mathrm{Ln}}-2E_{\mathrm{Ln}-1}+E_{\mathrm{Ln}-2})}{\mathrm{\Δ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)计算：

${\stackrel{\‾}{L}}_n=r_{\mathrm{rL}}(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})\frac{2\mathrm{\π}}{2000}$

(4)

${\stackrel{\‾}{R}}_n=r_{\mathrm{rR}}(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})\frac{2\mathrm{\π}}{2000}$

在第n个采样周期内，车辆航向角的变化α_n由公式(5)计算：

${\mathrm{\α}}_n=\frac{{\stackrel{\‾}{R}}_n}{{2R}_{2n}}=\frac{{\stackrel{\‾}{L}}_n}{{2R}_{1n}}=\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}---\left(5\right)$

在第n个采样周期内，左右车轮分别滚动的曲线弦长L_n、R_n由公式(6)计算

$\mathrm{Sin}\left({\mathrm{\α}}_n\right)=\frac{L_n}{{2R}_{1n}}=\frac{R_n}{2R_{2n}}$

$L_n=2R_{1n}\mathrm{Sin}\left({\mathrm{\α}}_n\right)=2\frac{{\stackrel{\‾}{L}}_n}{{\mathrm{\α}}_n}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)=2\frac{{\stackrel{\‾}{L}}_{n}B}{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)---\left(6\right)$

$R_n=2R_{2n}\mathrm{Sin}\left({\mathrm{\α}}_n\right)=2\frac{{\stackrel{\‾}{R}}_n}{{\mathrm{\α}}_n}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)=2\frac{{\stackrel{\‾}{R}}_{n}B}{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)$

则车辆航向角和车辆左右两侧后轮位置由公式(7)计算得

${\mathrm{\γ}}_n=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\α}}_i$

$x_{\mathrm{Ln}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(L_i\mathrm{Sin}\left({\mathrm{\γ}}_i\right)-\frac{B}{2})$ $y_{\mathrm{Ln}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\mathrm{Cos}\left({\mathrm{\γ}}_i\right)$

$x_{\mathrm{Rn}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(R_i\mathrm{Sin}\left({\mathrm{\γ}}_i\right)+\frac{B}{2})$ $y_{\mathrm{Rn}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{R}_i\mathrm{Cos}\left({\mathrm{\γ}}_i\right)---\left(7\right)$

5、实际测试计算效果

通过实车试验时采集的左右车轮编码器读数，经过上述计算方法可以获得高精度的计算效果。

所述旋转编码器机构包括U型支架、固定块、编码器支架、编码器、联轴器和延伸轴，所述固定块固接在车体上，所述固定块纵向设有互为平行的圆孔，所述U型支架与固定块圆孔滑动连接，所述U型支架上固接有编码器固定支架，所述编码器固定支架的竖直侧面上固接有编码器，所述编码器的输出轴通过挠性联轴器与延伸轴连接，所述延伸轴与车轴末端同轴固定连接。

所述U型支架之间通过轴承连接有旋转销，所述旋转销与延伸轴垂直呈水平安装位置。

有益效果：测量出左右车轮角位移，通过车轮滚动半径，可以计算得到左右车轮向前滚动的曲线距离，由于车辆在曲线行驶和直线行驶时，左右车轮滚动距离不同，可以计算获得车辆的航向角和车辆的具体坐标位置。准确地实时测量并计算车辆位置姿态数据，提高智能车辆跟踪目标轨迹的准确程度，且成本低，结构简单，应用范围广。

附图说明

图1是旋转编码器机构安装位置俯视图；

图2是本发明的计算方法原理图；

图3是在实车试验时左右车轮编码器读数与车轮滚过距离的换算图；

图4是显示车辆航向角变化示意图；

图5是计算轨迹和实际行驶轨迹的偏差与行驶距离的比值坐标图；

图6是旋转编码器机构结构示意图；

图7是U型支架通过固定块连接在车身的结构图。

图中：1、U型支架，2、固定块，3、编码器支架，4、编码器，5、编码器信号线，6、编码器轴，7、挠性联轴器，8、旋转销，9、延伸轴。

具体实施方式

以下结合较佳实施例，对依据本发明提供的具体实施方式详述如下：详见附图，一种无人驾驶车辆实时跟踪行驶轨迹的计算方法，具体步骤如下：

1、安装旋转编码器机构，

将无人驾驶车辆的非驱动桥左右车轮中心上安装旋转编码器机构，所述旋转编码器机构与计算机的解码器无线连接；

2、标定

保持车辆以10公里/小时的速度匀速直线行驶50米，读取左右编码器脉冲读数C_L、C_R，计算得车轮在直线行驶时的滚动半径r’_rL、r’_rR(单位：米)；

$r_{\mathrm{rL}}^{\′}=\frac{50\×2000}{2\mathrm{\π}{C}_L}$ $r_{\mathrm{rR}}^{\′}=\frac{50\×2000}{2\mathrm{\π}{C}_R}---\left(1\right)$

3、修正：车辆在弯道行驶时，侧向加速度会造成左右两侧车轮垂向载荷转移，引起车轮滚动半径发生变化，需要对采集到的数据进行修正，

在滚动半径计算公式(1)中增加修正参数因子δ以补偿左右两侧车轮垂向载荷转移对车轮滚动半径的影响，

修正因子δ的估算公式(2)：

$\mathrm{\δ}=\frac{(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})-(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})}{(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})+(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})}\×{\left(\frac{(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})+(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})}{2\mathrm{\Δt}}\right)}^{2/3}\×\mathrm{sc}---\left(2\right)$

式中sc为无量纲修正因子，E_in为第n个采样时刻编码器读数，下标i为L或R，分别代表左轮、右轮，

因此，车轮滚动半径用公式(3)估算修正：

r_rL＝r′_rL(1+δ) r_rR＝r′_rR(1-δ)    (3)

4、车辆位置及航向角计算方法

在第n个采样周期内，左右车轮分别滚动的曲线距离，即弧长由公式(4)计算：

${\stackrel{\‾}{L}}_n=r_{\mathrm{rL}}(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})\frac{2\mathrm{\π}}{2000}$

(4)

${\stackrel{\‾}{R}}_n=r_{\mathrm{rR}}(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})\frac{2\mathrm{\π}}{2000}$

在第n个采样周期内，车辆航向角的变化α_n由公式(5)计算：

${\mathrm{\α}}_n=\frac{{\stackrel{\‾}{R}}_n}{{2R}_{2n}}=\frac{{\stackrel{\‾}{L}}_n}{{2R}_{1n}}=\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}---\left(5\right)$

在第n个采样周期内，左右车轮分别滚动的曲线弦长L_n、R_n由公式(6)计算

$\mathrm{Sin}\left({\mathrm{\α}}_n\right)=\frac{L_n}{{2R}_{1n}}=\frac{R_n}{2R_{2n}}$

$L_n=2R_{1n}\mathrm{Sin}\left({\mathrm{\α}}_n\right)=2\frac{{\stackrel{\‾}{L}}_n}{{\mathrm{\α}}_n}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)=2\frac{{\stackrel{\‾}{L}}_{n}B}{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)---\left(6\right)$

$R_n=2R_{2n}\mathrm{Sin}\left({\mathrm{\α}}_n\right)=2\frac{{\stackrel{\‾}{R}}_n}{{\mathrm{\α}}_n}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)=2\frac{{\stackrel{\‾}{R}}_{n}B}{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)$

则车辆航向角和车辆左右两侧后轮位置由公式(7)计算得

${\mathrm{\γ}}_n=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\α}}_i$

$x_{\mathrm{Ln}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(L_i\mathrm{Sin}\left({\mathrm{\γ}}_i\right)-\frac{B}{2})$ $y_{\mathrm{Ln}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\mathrm{Cos}\left({\mathrm{\γ}}_i\right)$

$x_{\mathrm{Rn}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(R_i\mathrm{Sin}\left({\mathrm{\γ}}_i\right)+\frac{B}{2})$ $y_{\mathrm{Rn}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{R}_i\mathrm{Cos}\left({\mathrm{\γ}}_i\right)---\left(7\right)$

5、实际测试计算效果

通过实车试验时采集的左右车轮编码器读数，经过上述计算方法可以获得高精度的计算效果。

所述旋转编码器机构包括U型支架1、固定块2、编码器支架3、编码器4、联轴器7和延伸轴9，所述固定块固接在车体上，所述固定块纵向设有互为平行的圆孔，所述U型支架与固定块圆孔滑动连接，所述U型支架上固接有编码器固定支架，所述编码器固定支架的竖直侧面上固接有编码器，所述编码器的输出轴通过挠性联轴器与延伸轴连接，所述延伸轴与车轴末端同轴固定连接。所述U型支架之间通过轴承连接有旋转销8，所述旋转销与延伸轴垂直呈水平安装位置。

延伸轴9固定在车轴末端，随车轴一起旋转，延伸轴9与车轴安装时要求同轴；旋转销8水平安装，与延伸轴9相垂直，两者之间有轴承连接，可以保证延伸轴9随车轮一起滚动，而旋转销8仍保持水平位置；编码器固定支架3与U型支架1焊接；编码器4通过端部连接螺栓固定在编码器固定支架3的竖直侧面；编码器轴6与延伸轴9之间有挠性联轴器7连接；旋转销8与U型支架1之间有轴承连接，可保证固定在一起的编码器4、编码器固定支架3，U型支架1总成能够一同绕旋转销8的轴线左右摆动，以补偿车轮跳动引起的编码器轴6与延伸轴9的不同轴；固定块2通过螺钉固定在车身上；U型支架1穿过固定块2的孔，U型支架1可相对固定块2上下滑动，以补偿车轮上下跳动。考虑到车轮滚动半径主要受车轮驱动力和车轮垂向载荷影响较大，旋转编码器安装在非驱动轮上，可避免受驱动力的影响；车辆在直线行驶时，左右车轮垂直载荷变化不大，但是车辆在弯道上行驶时，会造成左右车轮垂直载荷较大转移，外侧车轮垂直载荷增加，内侧车轮垂直载荷减小，但是增大与较小的数值是一样的，该数值与侧向加速度具有固定函数关系，可以计算得到。侧向加速度可通过旋转编码器测得的左右车轮转速计算可得。故可通过左右编码器读数修正车轮滚动半径，消除弯道时垂直载荷对车轮半径的影响，提高了精确度。

工作原理：在车轮上安装了微型旋转编码器(2000P/R)，编码器的信号线5与计算机的解码器无线连接，通过车轮滚动半径，测量左右车轮角位移，可以计算得到左右车轮向前滚动的曲线距离，由于车辆在曲线行驶和直线行驶时，左右车轮滚动距离不同，即可计算获得车辆的航向角和车辆的具体坐标位置。

实施例

工作过程：

总行驶距离：1.49133公里，车辆停止位置的计算误差0.024公里。四段路分别为A、B、C、D段，每段路在大地测量时的航向角为分别为0°、104.34°、193.23°、284.35°。

一次标定实例

标定前左右编码器读数分别为C_Lb＝1432、C_Rb＝243；保持车辆以10公里/小时的速度匀速直线行驶50米后，左右编码器读数分别为C_Le＝55352、C_Re＝56524；则读取左右编码器脉冲变化读数C_L＝C_Le-C_Lb＝53920、C_R＝C_Re-C_Rb＝56281，计算得车轮在直线行驶时的滚动半径r’_rL、r’_rR(单位：米)； $r_{\mathrm{rL}}^{\′}=\frac{50\×2000}{2\mathrm{\π}{C}_L}=0.295168663$ $r_{\mathrm{rR}}^{\′}=\frac{50\×2000}{2\mathrm{\π}{C}_R}=0.282786274---\left(1\right)$

3、修正：车辆在弯道行驶时，侧向加速度会造成左右两侧车轮垂向载荷转移，引起车轮滚动半径发生变化，故需要在滚动半径计算公式(1)中增加修正参数因子δ以补偿左右两侧车轮垂向载荷转移对车轮滚动半径的影响。

侧向加速度大小反映在左右车轮编码器的读数差值和差值的变化率上，差值越大，说明侧向加速度越大，反之则越小，而侧向加速度综合反映了车辆转弯时的速度和瞬时的转弯半径大小。修正因子δ与左右车轮编码器的读数差值和差值的变化率存在函数关系，在滚动半径计算公式(1)中增加修正参数因子δ以补偿左右两侧车轮垂向载荷转移对车轮滚动半径的影响，

修正因子δ的估算公式(2)：

δ＝sign(a)×|a|^1/3×sc      (2)

式中sc为无量纲修正因子，可通过弯道试验标定得到；

一次修正实例

修正前左右编码器读数分别为C_Lbv＝2873、C_Rbv＝1367；保持车辆以20公里/小时的速度沿半径为50米的圆周匀速行驶一周，左右编码器读数分别为C_Lev＝337596、C_Rev＝350747；则读取左右编码器脉冲变化读数C_Lv＝C_Lev-C_Lbv＝＝191478、C_Rv＝C_Rev-C_Rbv＝492625，车宽为1.2米，无量纲修正因子sc：

$\mathrm{sc}=((\frac{100-B}{C_{\mathrm{LV}}{r}_{\mathrm{rL}}^{\′}}-\frac{100+B}{C_{\mathrm{RV}}{r}_{\mathrm{rR}}^{\′}})\×500)/\left({({(200/36)}^2/(50+\frac{B}{2}))}^{1/3}\right)$

$=0.06$

实验过程：修正前左右编码器读数分别为C_Lbv、C_Rbv；保持车辆以20公里/小时的速度沿半径为50米的圆周逆时针匀速行驶一周，左右编码器读数分别为C_Kev、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}}^{\′}}-\frac{100+B}{C_{\mathrm{RV}}{r}_{\mathrm{rR}}^{\′}})\×500)/\left({({(200/36)}^2/(50+\frac{B}{2}))}^{1/3}\right)$

式中

$a=-\frac{B}{2}\frac{r_{\mathrm{rL}}^{\′}(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})+r_{\mathrm{rR}}^{\′}(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})}{r_{\mathrm{rL}}^{\′}(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})-r_{\mathrm{rR}}^{\′}(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})}$

$\×{\left(\frac{r_{\mathrm{rR}}^{\′}(E_{\mathrm{Rn}}-2E_{\mathrm{Rn}-1}+E_{\mathrm{Rn}-2})-r_{\mathrm{rL}}^{\′}(E_{\mathrm{Ln}}-2E_{\mathrm{Ln}-1}+E_{\mathrm{Ln}-2})}{\mathrm{\ΔTB}}\right)}^2$

a的第一部分反应车辆方向盘转角的影响因素，第二部分反应行驶车速的影响因素。

式中E_in为第n个采样时刻编码器读数，下标i为L或R，分别代表左轮、右轮，ΔT为采样周期，B为轮距；

因此，车轮滚动半径用公式(3)估算修正：

r_rL＝r′_rL(1+δ)    r_rR＝r′_rR(1-δ)    (3)

4、车辆位置及航向角计算方法

以一个采样周期内的计算数据为例的计算实例：

采样周期为10ms，所测得的数据分别为：

E_Ln＝24356，E_Ln-1＝24379，E_Ln-2＝24402

E_Rn＝22342，E_Rn-1＝22386，E_Rn-2＝22410

$a=-\frac{B}{2}\frac{r_{\mathrm{rL}}^{\′}(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})+r_{\mathrm{rR}}^{\′}(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})}{r_{\mathrm{rL}}^{\′}(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})-r_{\mathrm{rR}}^{\′}(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})}$

$\×{\left(\frac{r_{\mathrm{rR}}^{\′}(E_{\mathrm{Rn}}-2E_{\mathrm{Rn}-1}+E_{\mathrm{Rn}-2})-r_{\mathrm{rL}}^{\′}(E_{\mathrm{Ln}}-2E_{\mathrm{Ln}-1}+E_{\mathrm{Ln}-2})}{\mathrm{\ΔTB}}\right)}^2$

$=0.00223$

则修正因子

δ＝sign(a)×|a|^1/3×sc＝0.007839018879139

因此车轮滚动半径可用公式(3)估算修正：

r_rL＝r′_rL(1+δ)＝0.297482495721787

                                                    (3)

r_rR＝r′_rR(1-δ)＝0.280569507059353

车辆位置及航向角计算

${\stackrel{\‾}{L}}_n=r_{\mathrm{rL}}(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})\frac{2\mathrm{\π}}{2000}=0.021495082932016$

${\stackrel{\‾}{R}}_n=r_{\mathrm{rR}}(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})\frac{2\mathrm{\π}}{2000}=0.021544424527751$

车辆航向角的变化α_n可由公式(5)计算：

${\mathrm{\α}}_n=\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}=2.055899822291559e-0.005\mathrm{rad};$

左右车轮分别滚动的曲线弦长L_n、R_n由公式(6)计算

$L_n=2\frac{{\stackrel{\‾}{L}}_{n}B}{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)=0.021495074322535$

$R_n=2\frac{{\stackrel{\‾}{R}}_{n}B}{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)=0.021544419452454$

则在本采样周期内车辆航向角和车辆左右两侧后轮位置变化量计算得

γ_n＝2.055899822291559e-005rad

x_Ln＝0.0000042432    y_Ln＝0.021495073903724

x_Rn＝0.0000046324    y_Rn＝0.021544418954433

根据公式(7)，按照采样周期顺序，将所获得的车辆航向角及车辆左右两侧后轮位置变化量累加。即可得到每一时刻车辆的姿态数据。

在第n个采样周期内，左右车轮分别滚动的曲线距离，即弧长

由公式(4)计算：

${\stackrel{\‾}{L}}_n=r_{\mathrm{rL}}(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})\frac{2\mathrm{\π}}{2000}$

(4)

${\stackrel{\‾}{R}}_n=r_{\mathrm{rR}}(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})\frac{2\mathrm{\π}}{2000}$

在第n个采样周期内，车辆航向角的变化α_n由公式(5)计算：

${\mathrm{\α}}_n=\frac{{\stackrel{\‾}{R}}_n}{{2R}_{2n}}=\frac{{\stackrel{\‾}{L}}_n}{{2R}_{1n}}=\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}---\left(5\right)$

在第n个采样周期内，左右车轮分别滚动的曲线弦长L_n、R_n由公式(6)计算

$L_n=2R_{1n}\mathrm{Sin}\left({\mathrm{\α}}_n\right)=2\frac{{\stackrel{\‾}{L}}_n}{{\mathrm{\α}}_n}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)=2\frac{{\stackrel{\‾}{L}}_{n}B}{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)$

(6)

$R_n=2R_{2n}\mathrm{Sin}\left({\mathrm{\α}}_n\right)=2\frac{{\stackrel{\‾}{R}}_n}{{\mathrm{\α}}_n}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)=2\frac{{\stackrel{\‾}{R}}_{n}B}{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)$

则车辆航向角和车辆左右两侧后轮位置由公式(7)计算得

${\mathrm{\γ}}_n=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\α}}_i$

$\begin{array}{cc}{x}_{\mathrm{Ln}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(L_i\mathrm{Sin}\left({\mathrm{\γ}}_i\right)-\frac{B}{2})& y_{\mathrm{Ln}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\mathrm{Cos}\left({\mathrm{\γ}}_i\right)\end{array}$

$\begin{array}{cc}{x}_{\mathrm{Rn}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(R_i\mathrm{Sin}\left({\mathrm{\γ}}_i\right)+\frac{B}{2})& y_{\mathrm{Rn}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{R}_i\mathrm{Cos}\left({\mathrm{\γ}}_i\right)---\left(7\right)\end{array}$

5、实际测试计算效果

通过实车试验时采集的左右车轮编码器读数，经过上述计算方法可以获得高精度的计算效果。

通过n个采样周期，如附图5所示，计算轨迹和实际行驶轨迹的偏差与行驶距离的比值坐标图，实际测试计算效果，在行驶1.5公里时偏差仅为1.6％。

以上所述，仅是本发明的较佳实施例而已，并非对本发明的结构作任何形式上的限制。凡是依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明的技术方案的范围内。

Claims (3)

1.一种无人驾驶车辆实时跟踪行驶轨迹的计算方法，具体步骤如下：

(1)安装旋转编码器机构，

将无人驾驶车辆的非驱动桥左右车轮中心上安装旋转编码器机构，所述旋转编码器机构与计算机的解码器无线连接；

(2)标定

保持车辆以10公里/小时的速度匀速直线行驶50米，读取左右编码器脉冲读数C_L、C_R，计算得车轮在直线行驶时的滚动半径r’_rL、r’_rR(单位：米)；

$r_{\mathrm{rL}}^{\′}=\frac{50\×2000}{2\mathrm{\π}{C}_L}$ $r_{\mathrm{rR}}^{\′}=\frac{50\×2000}{2\mathrm{\π}{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}}^{\′}}-\frac{100+B}{C_{\mathrm{RV}}{r}_{\mathrm{rR}}^{\′}})\×500)/\left({({(200/36)}^2/(50+\frac{B}{2}))}^{1/3}\right)$

式中a：

$a=-\frac{B}{2}\frac{r_{\mathrm{rL}}^{\′}(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})+r_{\mathrm{rR}}^{\′}(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})}{r_{\mathrm{rL}}^{\′}(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})-r_{\mathrm{rR}}^{\′}(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})}$

$\×{\left(\frac{r_{\mathrm{rR}}^{\′}(E_{\mathrm{Rn}}-2E_{\mathrm{Rn}-1}+E_{\mathrm{Rn}-2})-r_{\mathrm{rL}}^{\′}(E_{\mathrm{Ln}}-2E_{\mathrm{Ln}-1}+E_{\mathrm{Ln}-2})}{\mathrm{\Δ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)计算：

${\stackrel{\‾}{L}}_n=r_{\mathrm{rL}}(E_{\mathrm{Ln}}-E_{\mathrm{Ln}-1})\frac{2\mathrm{\π}}{2000}$

(4)

${\stackrel{\‾}{R}}_n=r_{\mathrm{rR}}(E_{\mathrm{Rn}}-E_{\mathrm{Rn}-1})\frac{2\mathrm{\π}}{2000}$

在第n个采样周期内，车辆航向角的变化α_n由公式(5)计算：

${\mathrm{\α}}_n=\frac{{\stackrel{\‾}{R}}_n}{{2R}_{2n}}=\frac{{\stackrel{\‾}{L}}_n}{{2R}_{1n}}=\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}---\left(5\right)$

在第n个采样周期内，左右车轮分别滚动的曲线弦长L_n、R_n由公式(6)计算

$\mathrm{Sin}\left({\mathrm{\α}}_n\right)=\frac{L_n}{{2R}_{1n}}=\frac{R_n}{2R_{2n}}$

$L_n=2R_{1n}\mathrm{Sin}\left({\mathrm{\α}}_n\right)=2\frac{{\stackrel{\‾}{L}}_n}{{\mathrm{\α}}_n}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)=2\frac{{\stackrel{\‾}{L}}_{n}B}{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)---\left(6\right)$

$R_n=2R_{2n}\mathrm{Sin}\left({\mathrm{\α}}_n\right)=2\frac{{\stackrel{\‾}{R}}_n}{{\mathrm{\α}}_n}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)=2\frac{{\stackrel{\‾}{R}}_{n}B}{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}\mathrm{Sin}\left(\frac{({\stackrel{\‾}{R}}_n-{\stackrel{\‾}{L}}_n)}{2B}\right)$

则车辆航向角和车辆左右两侧后轮位置由公式(7)计算得

${\mathrm{\γ}}_n=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\α}}_i$

$\begin{array}{cc}{x}_{\mathrm{Ln}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(L_i\mathrm{Sin}\left({\mathrm{\γ}}_i\right)-\frac{B}{2})& y_{\mathrm{Ln}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{L}_i\mathrm{Cos}\left({\mathrm{\γ}}_i\right)\end{array}$

$\begin{array}{cc}{x}_{\mathrm{Rn}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(R_i\mathrm{Sin}\left({\mathrm{\γ}}_i\right)+\frac{B}{2})& y_{\mathrm{Rn}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{R}_i\mathrm{Cos}\left({\mathrm{\γ}}_i\right)---\left(7\right)\end{array}$

(5)实际测试计算效果

通过实车试验时采集的左右车轮编码器读数，经过上述计算方法可以获得高精度的计算效果。

2.根据权利要求1所述的无人驾驶车辆实时跟踪行驶轨迹的计算方法，其特征是：所述旋转编码器机构包括U型支架、固定块、编码器支架、编码器、联轴器和延伸轴，所述固定块固接在车体上，所述固定块纵向设有互为平行的圆孔，所述U型支架与固定块圆孔滑动连接，所述U型支架上固接有编码器固定支架，所述编码器固定支架的竖直侧面上固接有编码器，所述编码器的输出轴通过挠性联轴器与延伸轴连接，所述延伸轴与车轴末端同轴固定连接。

3.根据权利要求2所述的无人驾驶车辆实时跟踪行驶轨迹的计算方法，其特征是：所述U型支架之间通过轴承连接有旋转销，所述旋转销与延伸轴垂直呈水平安装位置。

Images