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

Abstract

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

Description

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

技术领域

本发明涉及一种计算前方道路弯度/坡度的预估方法，属于道路导航技术领域。

背景技术

近几年来，我国的机动车数量一直处于快速增长状态，目前保有量已超过2亿辆。随着越来越多的汽车进入家庭，人们对于道路导航的需求也日益增加。因此，很多研究机构和企业都在道路自动导航技术上投入了很大的人力物力开展研究，已经取得了一定的研究成果。

例如在申请号为200910146468.0的中国发明专利申请中，英华达股份有限公司提出了一种辅助导航的转向判断方法。它是在沿一弯曲路径行进，且无法有效接收全球卫星定位系统的定位资讯时，撷取道路背景为多个影像，并加以处理运算，以相减比对的手法求出差异处的偏移方向，或捕捉特定物体影像作为定位标的，藉以判断转向以持续提供导航画面。

另外，在申请号为201010195063.9的中国发明专利申请中，上海交通大学提出了一种道路坡度的检测方法，包括以下步骤：获取待检测区域的等高线图和道路规划图；进行扫描和空间配准，得到等高线栅格分布图和道路规划栅格分布图；进行数字化处理，得到等高线矢量分布图和道路规划矢量分布图，并进行不规则三角网格转换处理，得到等高线不规则三角网格；提取所有节点，得到高程散点图，并进行空间插值处理，得到高程分布图；提取道路高程信息，添加到道路规划矢量分布图中，并进行扩充和均值平滑处理，得到扩充后的道路高程分布图；提取栅格的高程差和水平距离，从而得到每条道路的坡度信息。

但是，现有的道路导航技术依赖业已固定的数字地图，往往只能提供大致的前进方向或者对汽车当前所处位置的弯度/坡度进行测量计算，无法对前方道路的弯度/坡度做出提前预估。这一技术缺陷不利于汽车自动驾驶技术以及主动防碰撞技术的进一步发展。

发明内容

本发明所要解决的技术问题在于提供一种计算前方道路弯度/坡度的预估方法。

为实现上述的发明目的，本发明采用下述的技术方案：

一种计算前方道路弯度/坡度的预估方法，包括如下步骤：

首先，以行驶中的汽车当前所在位置为原点建立二维动态方位坐标系，搜索原点及周围的道路情况；

其次，按照汽车的长度l_car和道路拥堵程度d_jam确定弧长间隔在道路前后按照相邻两点间隔预定时间选择多个点(例如m个点)，根据各点的地理参数得到其在二维动态方位坐标系的坐标值，记为两个列向量 $\stackrel{\→}{X}={(x_1,...,x_m)}^T,\stackrel{\→}{Y}={(y_1,...,y_m)}^T;$

再次，基于已知列向量进行拟合得到如下的3维多项式及其一阶、二阶导函数：

$y=a_0+a_{1}x+a_2{x}^2+a_3{x}^3,\stackrel{\·}{y}=a_1+2a_{2}x+3a_3{x}^2,\stackrel{\·\·}{y}=2a_2+6a_{3}x$

曲率半径：

$R=\frac{{(1+{\stackrel{\·}{y}}^2)}^{3/2}}{\stackrel{\·\·}{y}}$

以及方向角：

$\mathrm{\&psi;}=\left\{\begin{array}{ccc}\arctan \left(\stackrel{\&CenterDot;}{y}\right)& \mathrm{if}& x\&GreaterEqual;0\\ \arctan \left(\stackrel{\&CenterDot;}{y}\right)+\mathrm{\&pi;}& \mathrm{if}& x<0,y\&GreaterEqual;0\\ \arctan \left(\stackrel{\&CenterDot;}{y}\right)-\mathrm{\&pi;}& \mathrm{if}& x<0,y<0.\end{array}\right.$

最后，计算每一点相对上一点的地理参数改变量，根据该地理参数改变量预估前方道路的弯度/坡度。

其中，当需要预估前方道路的弯度时，所述地理参数为经纬度参数；当需要预估前方道路的坡度时，所述地理参数为海拔高度参数。

利用本发明，可以对前方道路的弯度/坡度做出提前预估，从而让汽车内部的主动防碰撞系统等提前采取相对应的技术措施，避免碰撞类交通事故的发生。

附图说明

下面结合附图和具体实施方式对本发明作进一步的详细说明。

图1为利用本发明所提供的方法预估前方道路弯度的示意图。

具体实施方式

对于汽车主动防碰撞技术而言，很有必要预先知道前方道路的弯度/坡度。为此，本发明提出了如下的技术解决方案：

如图1所示，首先，假设行驶中的汽车当前位于C点，以该点为原点建立二维动态方位坐标系，其中横坐标x轴指向东，纵坐标y轴指向北。在现有技术中，已经做到任意给定一个点，可以搜索到最近的道路(具体可以参考鲍远律、刘振安合著的《卫星定位、交通监控与数字地图》，ISBN：7118046221，国防工业出版社2006年出版)。因此，利用现有技术可以获得C点所在及周围的道路情况，在此就不详细赘述了。

其次，按照汽车的长度l_car和道路拥堵程度d_jam确定弧长间隔在道路前后按照相邻两点间隔Δs选择多个点，例如车后选择A,B两点，车前选择D,E,F,G四点，则可根据A,B,C,D,E,F,G各点的经纬度得到其在动态方位坐标系的坐标值，记两个列向量

$\stackrel{\&RightArrow;}{X}={(x_A,x_B,x_C,x_D,x_E,x_F,x_G)}^T,\stackrel{\&RightArrow;}{Y}={(y_A,y_B,y_C,y_D,y_E,y_F,y_G)}^T.$

再次，基于上述已知列向量进行拟合得到如下形式的3维多项式及其一阶、二阶导函数：

$y=a_0+a_{1}x+a_2{x}^2+a_3{x}^3,\stackrel{\&CenterDot;}{y}=a_1+2a_{2}x+3a_3{x}^2,\stackrel{\&CenterDot;\&CenterDot;}{y}=2a_2+6a_{3}x---\left(1\right)$

其中

${(a_0,a_1,a_2,a_3)}^T={\left(\begin{array}{cccc}m& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^2& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^3\\ \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^2& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^3& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^4\\ \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^2& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^3& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^4& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^5\\ \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^3& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^4& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^5& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^6\end{array}\right)}^{-1}{(\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{y}_i,\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i{y}_i,\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^2{y}_i,\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^3{y}_i)}^T$

上式中，m表示点的个数。

曲率半径：

$R=\frac{{(1+{\stackrel{\&CenterDot;}{y}}^2)}^{3/2}}{\stackrel{\&CenterDot;\&CenterDot;}{y}}---\left(2\right)$

以及方向角：

$\mathrm{\&psi;}=\left\{\begin{array}{ccc}\arctan \left(\stackrel{\&CenterDot;}{y}\right)& \mathrm{if}& x\&GreaterEqual;0\\ \arctan \left(\stackrel{\&CenterDot;}{y}\right)+\mathrm{\&pi;}& \mathrm{if}& x<0,y\&GreaterEqual;0\\ \arctan \left(\stackrel{\&CenterDot;}{y}\right)-\mathrm{\&pi;}& \mathrm{if}& x<0,y<0.\end{array}\right.---\left(3\right)$

然后，计算每一点相对上一点的方向改变量Δψ，写成列向量形式为

$\mathrm{\&Delta;}\stackrel{\&RightArrow;}{\mathrm{\&psi;}}={(*,{\mathrm{\&psi;}}_B-{\mathrm{\&psi;}}_A,{\mathrm{\&psi;}}_C-{\mathrm{\&psi;}}_B,{\mathrm{\&psi;}}_D-{\mathrm{\&psi;}}_C,{\mathrm{\&psi;}}_E-{\mathrm{\&psi;}}_D,{\mathrm{\&psi;}}_F-{\mathrm{\&psi;}}_E,{\mathrm{\&psi;}}_G-{\mathrm{\&psi;}}_F)}^T---\left(4\right)$

当Δψ＞0时意味着前方道路在向左拐，当Δψ＜0时意味着前方道路在向右拐。

在汽车主动防碰撞技术的实践中，可以不必精确掌握方向改变量的具体取值，而是根据取值大小分为5档：左急弯；左小弯；无弯；右小弯；右急弯。根据前方道路的弯曲程度，汽车内部的主动防碰撞系统可以提前采取相对应的技术措施，如自动减速、提前报警等。

类似的方法可以用来预估前方道路的坡度，此时将上述的经纬度参数改为海拔高度参数即可。在计算前方道路的坡度时，可以对前方道路的海拔高度随弧长的变化进行多项式拟合，也可以利用三角函数进行更简单的近似计算，例如对于图1中的B、D、F三点而言：

${\mathrm{\&theta;}}_B=\arctan \left(\frac{h_C-h_A}{2\mathrm{\&Delta;s}}\right),{\mathrm{\&theta;}}_D=\arctan \left(\frac{h_E-h_C}{2\mathrm{\&Delta;s}}\right),{\mathrm{\&theta;}}_F=\arctan \left(\frac{h_G-h_E}{2\mathrm{\&Delta;s}}\right)---\left(5\right)$

同样地，在汽车主动防碰撞技术的实践中，可以根据前方道路的坡度大小分为5档：下陡坡；下缓坡；无坡；上缓坡；上陡坡。汽车内部的主动防碰撞系统可以提前采取相对应的技术措施，如加大油门、利用发动机进行制动等。

上面对本发明所述的计算前方道路弯度/坡度的预估方法进行了详细的说明，但显然本发明的具体实现形式并不局限于此。对于本技术领域的一般技术人员来说，在不背离本发明的精神和权利要求范围的情况下对它进行的各种显而易见的改变都在本发明的保护范围之内。

Claims (3)

1.一种计算前方道路弯度/坡度的预估方法，其特征在于包括如下步骤：

首先，以行驶中的汽车当前所在位置为原点建立二维动态方位坐标系，搜索原点及周围的道路情况；

其次，按照汽车的长度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}{\mathrm{\&Sigma;}}}{x}_i& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^2& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^3\\ \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^2& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^3& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^4\\ \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^2& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^3& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^4& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^5\\ \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^3& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^4& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^5& \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^6\end{array}\right)}^{-1}{(\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{y}_i,\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i{y}_i,\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^2{y}_i,\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i^3{y}_i)}^T$

曲率半径：

$R=\frac{{(1+{\stackrel{.}{y}}^2)}^{3/2}}{\stackrel{..}{y}}---\left(2\right)$

以及方向角：

$\mathrm{\&psi;}=\left\{\begin{array}{ccc}\arctan \left(\stackrel{.}{y}\right)& \mathrm{if}& x\&GreaterEqual;0\\ \arctan \left(\stackrel{.}{y}\right)+\mathrm{\&pi;}& \mathrm{if}& x<0,y\&GreaterEqual;0\\ \arctan \left(\stackrel{.}{y}\right)-\mathrm{\&pi;}& \mathrm{if}& x<0,y<0.\end{array}\right.---\left(3\right)$

最后，计算每一点相对上一点的地理参数改变量，根据该地理参数改变量预估前方道路的弯度/坡度。

2.如权利要求1所述的计算前方道路弯度/坡度的预估方法，其特征在于：

当需要预估前方道路的弯度时，所述地理参数为经纬度参数。

3.如权利要求1所述的计算前方道路弯度/坡度的预估方法，其特征在于：

当需要预估前方道路的坡度时，所述地理参数为海拔高度参数。