一种计算前方道路弯度/坡度的预估方法

摘要

本发明公开了一种计算前方道路弯度/坡度的预估方法。首先，以行驶中的汽车当前所在位置为原点建立二维动态方位坐标系，搜索原点及周围的道路情况；其次，按照汽车的长度和道路拥堵程度确定弧长间隔，在道路前后按照相邻两点间隔预定时间选择多个点，根据各点的地理参数得到其在二维动态方位坐标系的坐标值，记为列向量；再次，基于已知列向量进行拟合得到3维多项式及其一阶、二阶导函数；最后，计算每一点相对上一点的地理参数改变量，根据该地理参数改变量预估前方道路的弯度/坡度。利用本发明，可以对前方道路的弯度/坡度做出提前预估，从而提前采取相对应的技术措施，避免碰撞类交通事故的发生。

主权项

一种计算前方道路弯度/坡度的预估方法，其特征在于包括如下步骤：首先，以行驶中的汽车当前所在位置为原点建立二维动态方位坐标系，搜索原点及周围的道路情况；其次，按照汽车的长度l_car和道路拥堵程度d_jam确定弧长间隔在道路前后按照相邻两点间隔预定时间选择多个点，根据各点的地理参数得到其在二维动态方位坐标系的坐标值，记为列向量；再次，基于已知列向量进行拟合得到如下的3维多项式及其一阶、二阶导函数：$y=a_3{x}^3+a_2{x}^2+a_{1}x+a_0,\stackrel{.}{y}=3a_3{x}^2+{2a}_{2}x+a_1,\stackrel{..}{y}=6a_{3}x+2a_2---\left(1\right)$其中${(a_0,a_1,a_2,a_3)}^T={\left(\begin{array}{cccc}m& \underset{i=1}{\overset{m}{\Sigma }}{x}_i& \underset{i=1}{\overset{m}{\Sigma }}{x}_i^2& \underset{i=1}{\overset{m}{\Sigma }}{x}_i^3\\ \underset{i=1}{\overset{m}{\Sigma }}{x}_i& \underset{i=1}{\overset{m}{\Sigma }}{x}_i^2& \underset{i=1}{\overset{m}{\Sigma }}{x}_i^3& \underset{i=1}{\overset{m}{\Sigma }}{x}_i^4\\ \underset{i=1}{\overset{m}{\Sigma }}{x}_i^2& \underset{i=1}{\overset{m}{\Sigma }}{x}_i^3& \underset{i=1}{\overset{m}{\Sigma }}{x}_i^4& \underset{i=1}{\overset{m}{\Sigma }}{x}_i^5\\ \underset{i=1}{\overset{m}{\Sigma }}{x}_i^3& \underset{i=1}{\overset{m}{\Sigma }}{x}_i^4& \underset{i=1}{\overset{m}{\Sigma }}{x}_i^5& \underset{i=1}{\overset{m}{\Sigma }}{x}_i^6\end{array}\right)}^{-1}{(\underset{i=1}{\overset{m}{\Sigma }}{y}_i,\underset{i=1}{\overset{m}{\Sigma }}{x}_i{y}_i,\underset{i=1}{\overset{m}{\Sigma }}{x}_i^2{y}_i,\underset{i=1}{\overset{m}{\Sigma }}{x}_i^3{y}_i)}^T$曲率半径：$R=\frac{{(1+{\stackrel{.}{y}}^2)}^{3/2}}{\stackrel{..}{y}}---\left(2\right)$以及方向角：$\psi =\left\{\begin{array}{ccc}\arctan \left(\stackrel{.}{y}\right)& \mathrm{if}& x\ge 0\\ \arctan \left(\stackrel{.}{y}\right)+\pi & \mathrm{if}& x<0,y\ge 0\\ \arctan \left(\stackrel{.}{y}\right)-\pi & \mathrm{if}& x<0,y<0.\end{array}\right.---\left(3\right)$最后，计算每一点相对上一点的地理参数改变量，根据该地理参数改变量预估前方道路的弯度/坡度。