CN100504788C - 自主驾驶车辆行驶过程的软件仿真方法 - Google Patents

Abstract

本发明涉及一种自主驾驶车辆行驶过程的软件仿真方法，可在一台视窗操作***的计算机上进行自主驾驶车辆行驶过程的仿真。本发明首先选取了合适的状态变量和期望轨迹的描述形式，接着建立图形显示界面并显示期望轨迹和初始状态下的车辆的平面图，然后反复执行预瞄偏差求取、前轮转角控制量求取、状态变量刷新和图形显示刷新这一系列的步骤。本发明避免了实际自主驾驶车辆实验调试过程中可能存在的安全问题，无需使用任何外接设备，实现简便，并能较好地仿真自主驾驶车辆的行驶过程，对相关科技人员进行实际自主驾驶车辆实验调试起到指导作用。

Description

自主驾驶车辆行驶过程的软件仿真方法

技术领域

本发明涉及一种自主驾驶车辆行驶过程的软件仿真方法，可以在一台视窗操作***的计算机上进行自主驾驶车辆行驶过程的仿真，对相关科技人员进行实际自主驾驶车辆实验调试起到指导作用。

背景技术

汽车是现代社会最为有效、使用最为广泛的交通工具之一，是维持人们正常生活工作所不可或缺的一部分，而且，随着社会的不断发展，越来越多的汽车走进了人们的视野。然而，汽车的大量涌现，无论是公共客车，货车，还是越来越多的私家小轿车，给交通效率、交通安全以及环境保护带来了更多的挑战。日益频繁的交通堵塞和交通事故，严重影响到人们日常生活的方便和工作的效率，甚至危害到人们的生命安全。此外，排放的汽车尾气，尤其是交通堵塞时大量排放的汽车尾气，会加剧温室效应，污染环境，进而损害人们身体健康。

针对这些问题，人们采取了不少措施。如通过监测并管制客车司机的驾驶时间来减少甚至消除客车司机疲劳载客情况的发生；大力发展并鼓励人们多乘用公共交通；多修建公路等基础设施。诸如此类的管理层面上的方法在一定程度上缓解了严重的交通问题，但无法从根本上解决人为因素造成的交通问题。此外，采用更多的公共交通来提高交通效率的措施还限制了人们使用交通的自由程度。基于这些不足，发展技术层面上的措施，如能从根本上将人为不良因素排除在外的自主驾驶车辆(或称智能车辆、无人驾驶车辆)以及更进一步的智能交通***颇受人们青睐。

自主驾驶车辆的研究工作得到了不少国家的重视，有的国家在几十年前就已经开展了相关的研究工作。我国对无人驾驶车辆的研究起步较晚，不过目前已有部分高校研究并制作出了或运行于城市环境、或驰骋于高速公路的自主驾驶车辆样车(如上海交通大学的CyberC3车，清华大学的THMR-V车，以及吉林大学和国防科大的样车)。

自主驾驶的主要工作涉及两方面：位姿数据获取和车辆控制。位姿数据获取是指通过视觉摄像头、激光雷达、磁传感器、超声传感器、红外传感器、陀螺仪、编码器和GPS等设备获取车辆自身的位姿信息，以及道路、路边树木等外界环境的位姿信息。车辆控制通常是指利用的所获取的车辆位姿和环境位姿信息对车辆进行合适的转向控制和速度控制，使车辆尽可能沿一条期望轨迹行驶。这条期望轨迹可以是实际存在的道路标记，如公路中央的白色指引线、道路上铺设的磁钉、或路两旁的建筑物、墙、树、灌木等，也可以是规划出的一条假想轨迹。自主驾驶的过程就是反复循环完成上述两方面工作的过程。可见，车辆控制是实现车辆自主驾驶的重要部分。

对于相关科技工作者，要验证某种车辆控制关系式的效果，最直接的方法是在实际车辆上进行实验。然而，直接在实际车辆上进行实验却有一些不利之处，尤其是在对车辆控制关系式的稳定性还无法确定时，贸然进行实验可能会使车辆失控，造成人员伤亡和财物损失。因此，在车辆控制关系式设计初期，先通过软件仿真验证控制关系式的可靠性和表现，方便及时的发现控制关系式中隐藏的问题，以保证进一步实际车辆实验的成功，具有实际应用意义。目前尚未有这样一种自主驾驶车辆行驶过程的软件仿真方法公开报导。

发明内容

本发明的目的在于针对现有技术的不足，提出一种自主驾驶车辆行驶过程的软件仿真方法，实现简便，能较好地仿真自主驾驶车辆的行驶过程，避免实际自主驾驶车辆实验调试过程中可能存在的安全问题，为有关科技工作者对所设计的控制关系式的控制效果进行测试提供直接有效的测试手段。

为实现这一目的，本发明首先选取了合适的状态变量和期望轨迹的描述形式，接着建立图形显示界面并显示期望轨迹和初始状态下的车辆的平面图，然后反复执行预瞄偏差求取、前轮转角控制量求取、状态变量刷新和图形显示刷新这一系列的步骤，实现自主驾驶车辆行驶过程的仿真。

本发明所述的自主驾驶车辆行驶过程的软件仿真方法包括以下具体步骤：

第1步，取车辆位置、航向角、前轮转角作为描述自主驾驶车辆行驶过程的状态变量，并设定初始的状态变量值。

第2步，通过离散的样点来描述车辆行驶的期望轨迹，即在期望轨迹上每隔一定间距取一个样点，保存这些样点的位置信息。

本发明中，样点间距的大小可以根据设计需要灵活确定，间距取得越小，样点对期望轨迹的逼近能力越强，但保存样点位置信息的存储量也越大。可以根据需要在逼近能力与存储量间做一个权衡，选择一个合适的间距大小。

第3步，建立图形显示界面，利用初始状态变量值和所有样点的位置信息，将初始状态下的车辆的平面图和所有样点显示出来，并将相邻样点用直线连接；将初始的状态变量值作为当前控制时刻的状态变量值，进入自主驾驶车辆行驶过程的仿真过程。

第4步，选定一个预瞄距离，将车辆正前方与车辆后两轮中心距离等于预瞄距离的某点称为预瞄点；利用当前控制时刻的状态变量值和预瞄距离，确定预瞄点的位置；求取所有样点中与预瞄点距离最近的两个样点，由这两个样点的位置和预瞄点的位置计算预瞄偏差，即预瞄点与期望轨迹间的间距。

第5步，建立反映预瞄偏差和前轮转角控制量间关系的控制关系式，根据该控制关系式和预瞄偏差计算出前轮转角控制量。

值得说明的是，该步中，可根据实际需要采用各种具体的控制关系式，通过仿真，可以找出能很好地使自主驾驶车辆沿期望轨迹行驶的控制关系式，来作为实际自主驾驶车辆实验调试时的参考。但无论采用什么样具体的控制关系式，只要是由预瞄偏差求取前轮转角控制量，均能实现本发明。

第6步，建立车辆动态模型，导出反映下一控制时刻的状态变量值与当前控制时刻的状态变量值及前轮转角控制量之间关系的车辆状态动态关系式；根据当前控制时刻的状态变量值、前轮转角控制量和车辆状态动态关系式计算出下一控制时刻的状态变量值。

第7步，延时等待一段时间，用下一控制时刻的状态变量值刷新车辆在图形界面中的显示。

第8步，将下一控制时刻的状态变量值作为新一轮的当前控制时刻的状态变量值，回到第4步，进入新一轮的自主驾驶车辆行驶过程的仿真过程。

本发明避免了实际自主驾驶车辆实验调试过程中可能存在的安全问题，只需要在一台视窗操作***的计算机上进行仿真，无需使用任何外接设备，实现简便，并能较好地仿真自主驾驶车辆的行驶过程，为有关科技工作者对所设计的控制关系式的控制效果进行测试提供直接有效的测试手段，对相关科技人员进行实际自主驾驶车辆实验调试起到指导作用。

附图说明

图1是图形显示界面中车辆的平面图和车辆行驶的期望轨迹。

图2是自主驾驶车辆行驶过程的仿真示意图。

在图1和图2中，平面坐标系中每单位长度对应实际中的10米。

具体实施方式

下面将参照附图并结合一个具体实施例进行详细说明，以便对本发明的目的、技术方案有更深入的理解。

具体实施步骤说明如下：

第1步，取车辆位置、航向角、前轮转角作为描述自主驾驶车辆行驶过程的状态变量，并设定初始的状态变量值。

用符号x、y表示车辆位置(x表示横坐标、y表示纵坐标)，ψ表示航向角，δ表示前轮转角。x、y、ψ、δ即构成了描述自主驾驶车辆行驶过程的状态变量，本实施例中，对状态变量赋初值为x＝0，y＝0，ψ＝0，δ＝0。

第2步，通过离散的样点来描述车辆行驶的期望轨迹，即在期望轨迹上每隔一定间距取一个样点，保存这些样点的位置信息。

本发明中采用通过离散的样点来描述期望轨迹的方式。具体做法是，在期望轨迹上每隔一定间距取一个样点，保存这些样点的位置信息。仿真过程中，期望轨迹上非样点的位置可以根据样点位置按线性插值的方式得到。间距取得越小，样点对期望轨迹的逼近能力越强，但需要保存位置的样点数也越多。可以根据需要在逼近能力与存储量间做一个权衡，选择一个合适的间距大小。

本实施例中，车辆行驶的期望轨迹由两部分组成：一段长为10米的直线AB和一段曲率半径为10米的90度圆弧BC。在直线上，只需要两个样点就可以完全确定这条直线，所以对于直线AB，仅取直线AB的两端点A点、B点作为样点。在圆弧上，需要较多的样点才能较好地反映圆弧的形状，所以对于圆弧BC，取样间距仅为1米，即该段圆弧曲率半径的十分之一。如图1。

第3步，建立图形显示界面，利用初始状态变量值和所有样点的位置信息，将初始状态下的车辆的平面图和所有样点显示出来，并将相邻样点用直线连接；将初始的状态变量值作为当前控制时刻的状态变量值，进入自主驾驶车辆行驶过程的仿真过程。

显示的具体做法是，在一显示窗口中，显示一个平面坐标系，将表示期望轨迹的所有样点显示在此坐标系中，相邻样点间用直线连接并显示，根据初始的状态变量值，将车辆的平面图也显示在这个平面坐标系中。如图1。

第4步，选定一个预瞄距离，将车辆正前方与车辆后两轮中心距离等于预瞄距离的某点称为预瞄点；利用当前控制时刻的状态变量值和预瞄距离，确定预瞄点的位置；求取所有样点中与预瞄点距离最近的两个样点，由这两个样点的位置和预瞄点的位置计算预瞄偏差，即预瞄点与期望轨迹间的间距。

该步的具体做法是，首先采用如下公式计算预瞄点位置：

px＝x(n)+L cos ψ(n)

py＝y(n)+L sin ψ(n)

其中，px、py表示预瞄点的位置。x(n)、y(n)为当前控制时刻的车辆位置，ψ(n)为当前控制时刻的航向角，L表示预瞄距离。本实施例中，取L＝4米。

接着求取所有样点中与预瞄点距离最近的两个样点。

最后采用如下公式计算预瞄偏差：

$e=\frac{|(p2x-p1x)*\mathrm{py}-(p2y-p1y)*\mathrm{px}+p2y*p1x-p1y*p2x|}{\sqrt{{(p2x-p1y)}^2+{(p2y-p1y)}^2}}$

其中，e表示预瞄偏差，px、py表示预瞄点的位置，(p1x，p1y)和(p2x，p2y)表示所有样点中与预瞄点距离最近的两个样点的位置。

第5步，建立反映预瞄偏差和前轮转角控制量间关系的控制关系式，根据该控制关系式和预瞄偏差计算出前轮转角控制量。

在本实施例中，控制关系式取如下具体的公式形式：

u＝2*e

其中，u表示前轮转角控制量。

值得说明的是，该具体的公式形式并不作为本发明的限制，在此采用该具体的公式形式只是作为一个例子，方便理解。由预瞄偏差求取前轮转角控制量，可以根据实际需要采用现有技术中其它的控制关系式。

第6步，建立车辆动态模型，导出反映下一控制时刻的状态变量值与当前控制时刻的状态变量值及前轮转角控制量之间关系的车辆状态动态关系式；根据当前控制时刻的状态变量值、前轮转角控制量和车辆状态动态关系式计算出下一控制时刻的状态变量值。

建立如下由运动学模型和一阶转向动态模型构成的车辆动态模型

$\left\{\begin{array}{c}\stackrel{.}{x}=v\cos \mathrm{\ψ}\\ \stackrel{.}{y}=v\sin \mathrm{\ψ}\\ \stackrel{.}{\mathrm{\ψ}}=\frac{v}{l}\tan \mathrm{\δ}\\ \stackrel{.}{\mathrm{\δ}}=-\frac{1}{T}\mathrm{\δ}+\frac{1}{T}u\end{array}\right.$

其中，x、y表示车辆位置，ψ表示车辆航向角，δ表示前轮转角，l表示车辆前后轮轴间距，v表示行驶速度，T表示转向电机响应时间常数，u表示前轮转角控制量。

对上述连续微分方程组描述下的车辆动态模型进行离散化处理，推得车辆状态动态关系式，如下所示：

$\left\{\begin{array}{c}x(n+1)=x\left(n\right)+v*T_0*\cos \mathrm{\ψ}\left(n\right)\\ y(n+1)=y\left(n\right)+v*T_0*\sin \mathrm{\ψ}\left(n\right)\\ \mathrm{\ψ}(n+1)=\mathrm{\ψ}\left(n\right)+\frac{v*T_0}{l}\tan \mathrm{\δ}\left(n\right)\\ \mathrm{\δ}(n+1)=\frac{T}{T+T_0}\mathrm{\δ}\left(n\right)+\frac{T_0}{T+T_0}u\end{array}\right.$

其中，l表示车辆前后轮轴间距；v表示行驶速度；T表示转向电机响应时间常数；T₀表示对车辆进行控制的控制周期。x(n)、y(n)、ψ(n)、δ(n)表示当前控制时刻的状态变量值。x(n+1)、y(n+1)、ψ(n+1)、δ(n+1)表示下一控制时刻的状态变量值。

本实施例中，l＝2米，v＝3米/秒，T＝0.6秒，T₀＝0.1秒。

再根据当前控制时刻的状态变量值、前轮转角控制量和上述车辆状态动态关系式计算出下一控制时刻的状态变量值。

第7步，延时等待一段时间，用下一控制时刻的状态变量值刷新车辆在图形界面中的显示。

本实施例中，延迟等待的时间取与控制周期相同，即0.1秒。延迟等待的时间也可以取别的值，如果小于控制周期，则仿真出来的会是一种“快放”的效果；如果大于控制周期，则是一种“慢放”的效果。

第8步，将下一控制时刻的状态变量值作为新一轮的当前控制时刻的状态变量值，回到第4步，进入新一轮的自主驾驶车辆行驶过程的仿真过程。

上述具体实施步骤下的自主驾驶车辆行驶过程的仿真示意图如图2所示，图2是从仿真的动画中截取的四张图片，显示出车辆在行驶过程中四个不同时刻的形态。采用本发明中提供的方法，可以有效地仿真自主驾驶车辆的行驶过程。在上述第5步中，设计者采用的具体的控制关系式不同，仿真出的自主驾驶车辆行驶的效果也不同。设计者可以尝试各种各样的控制关系式，通过仿真，可以及时发现哪些控制关系式会使自主驾驶车辆在行驶过程脱离期望轨迹，在实际自主驾驶车辆实验调试时就可以避免采用这样的控制关系式。也可以通过仿真找出哪种控制关系式能很好地使自主驾驶车辆沿期望轨迹行驶，那么在实际自主驾驶车辆实验调试时采用这种控制关系式，成功的可能性就更高。这样，本发明的自主驾驶车辆行驶过程的软件仿真方法就能对相关科技人员进行实际自主驾驶车辆实验调试起到了指导作用。

Claims (6)

1、一种自主驾驶车辆行驶过程的软件仿真方法，其特征在于包括以下步骤：

1)取车辆位置、航向角、前轮转角作为描述自主驾驶车辆行驶过程的状态变量，并设定初始的状态变量值；

2)通过离散的样点来描述车辆行驶的期望轨迹，即在期望轨迹上每隔一定间距取一个样点，保存这些样点的位置信息；

3)建立图形显示界面，利用初始状态变量值和所有样点的位置信息，将初始状态下的车辆的平面图和所有样点显示出来，并将相邻样点用直线连接；将初始的状态变量值作为当前控制时刻的状态变量值，进入自主驾驶车辆行驶过程的仿真过程；

4)选定一个预瞄距离，将车辆正前方与车辆后两轮中心距离等于预瞄距离的某点称为预瞄点；利用当前控制时刻的状态变量值和预瞄距离，确定预瞄点的位置；求取所有样点中与预瞄点距离最近的两个样点，由这两个样点的位置和预瞄点的位置计算预瞄偏差，即预瞄点与期望轨迹间的间距；

5)建立反映预瞄偏差和前轮转角控制量间关系的控制关系式，根据该控制关系式和预瞄偏差计算出前轮转角控制量；

6)建立车辆动态模型，导出反映下一控制时刻的状态变量值与当前控制时刻的状态变量值及前轮转角控制量之间关系的车辆状态动态关系式；根据当前控制时刻的状态变量值、前轮转角控制量和车辆状态动态关系式计算出下一控制时刻的状态变量值；

7)延时等待一段时间，用下一控制时刻的状态变量值刷新车辆在图形界面中的显示；

8)将下一控制时刻的状态变量值作为新一轮的当前控制时刻的状态变量值，回到步骤4)，进入新一轮的自主驾驶车辆行驶过程的仿真过程。

2、根据权利要求1的自主驾驶车辆行驶过程的软件仿真方法，其特征在于确定预瞄点的位置时，采用公式：

px＝x(n)+Lcosψ(n)

py＝y(n)+Lsinψ(n)

其中，px、py表示预瞄点的位置，x(n)、y(n)为当前控制时刻的车辆位置，ψ(n)为当前控制时刻的航向角，L表示预瞄距离。

3、根据权利要求1的自主驾驶车辆行驶过程的软件仿真方法，其特征在于计算预瞄偏差时，采用公式：

$e=\frac{|(p2x-p1x)*\mathrm{py}-(p2y-p1y)*\mathrm{px}+p2y*p1x-p1y*p2x|}{\sqrt{{(p2x-p1x)}^2+{(p2y-p1y)}^2}}$

其中，e表示预瞄偏差，px、py表示预瞄点的位置，(plx，ply)和(p2x，p2y)表示所有样点中与预瞄点距离最近的两个样点的位置。

4、根据权利要求1的自主驾驶车辆行驶过程的软件仿真方法，其特征在于计算下一控制时刻的状态变量值时，采用的车辆状态动态关系式为：

$\left\{\begin{array}{c}x(n+1)=x\left(n\right)+v*T_0*\cos \mathrm{\ψ}\left(n\right)\\ y(n+1)=y\left(n\right)+v*T_0*\sin \mathrm{\ψ}\left(n\right)\\ \mathrm{\ψ}(n+1)=\mathrm{\ψ}\left(n\right)+\frac{v*T_0}{l}\tan \mathrm{\δ}\left(n\right)\\ \mathrm{\δ}(n+1)=\frac{T}{T+T_0}\mathrm{\δ}\left(n\right)+\frac{T_0}{T+T_0}u\end{array}\right.$

其中，l表示车辆前后轮轴间距，v表示行驶速度，u表示前轮转角控制量，T表示转向电机响应时间常数，T₀表示对车辆进行控制的控制周期，x(n)、y(n)、ψ(n)、δ(n)表示当前控制时刻的状态变量值，x(n+1)、y(n+1)、ψ(n+1)、δ(n+1)表示下一控制时刻的状态变量值。

Images