CN102495631A - 一种无人驾驶车辆跟踪预定轨迹的智能控制方法 - Google Patents

Abstract

本发明公开了一种无人驾驶车辆跟踪预定轨迹的智能控制方法，特征是先建立车身坐标系，确定车辆自身位置信息；依据弧度值计算道路曲率特性的隶属度函数，定义模糊规则计算输出决策速度；按行驶1.5～2.5秒的距离在预定轨迹的坐标序列中搜索大于该距离并距车辆最近的轨迹点；采用自适应比例-微分-积分控制算法公式计算车辆运动的曲率；最后判断当前控制周期目标点是否为路点序列中被选中的最后一点：若是，则向刹车伺服控制系统输出信号启动刹车使车辆减速；若否，则使用控制器局域网模块输出电压信号保持控制油门开度使车辆继续前进。采用本发明能够使无人驾驶车辆实现对预定轨迹的跟踪功能，且对道路曲率特性变化具有一定的自适应能力。

Description

一种无人驾驶车辆跟踪预定轨迹的智能控制方法

技术领域

本发明属于车辆自动控制方法技术领域，具体涉及无人驾驶车辆跟踪预定轨迹的控制方法。

背景技术

无人驾驶车辆是验证机器感知与认知理论和关键技术的最佳实验平台之一。它在军事领域的侦查、运输及排爆等场合具有很大的应用前景。同时，其涉及的感知、决策与控制等关键技术，对于智能交通领域和汽车电子领域相关产品的研发，也起到了极大的推动作用。世界主要发达国家将无人驾驶车辆作为展示人工智能技术水准和引领车辆工业未来的重要平台，纷纷开展无人驾驶车辆的研究。

中国专利申请公开号CN101758855A提出的一种无人驾驶车辆转向装置及其控制方法，主要目的是实现无人驾驶车辆的自动转向控制。中国专利申请公开号CN101797917A设计的一种无人驾驶车辆的制动装置，主要目的是实现无人驾驶车辆对制动装置的控制。这两种装置涉及对车辆转向以及制动装置的改装方法，并采用伺服控制技术实现了对转向和制动装置的自动控制，是实现车辆无人驾驶控制的必要前提条件。中国专利申请号200910234933.6提出的一种“无人驾驶汽车”，包括车体、设于车体外的车载摄像头和传感器，设于车体内的GPS卫星定位系统和计算机控制系统；其中的传感器用于感受汽车前进道路上的障碍物，并将障碍物信息传送给计算机控制系统，计算机控制系统根据从车载摄像头、传感器和GPS卫星定位系统接收到的各种信息控制汽车的启动、速度变换及方向变换等功能。但该技术方案仅给出了一种无人驾驶汽车的感知和导航系统的配置方案，并没有给出如何控制车辆跟踪预期路径的方法。国防科技大学博士论文《自主驾驶汽车智能控制系统》提出了四层递阶式自主驾驶的系统结构，并对高速公路工况下的自主驾驶做了实验研究。但该论文中并没有充分公开控制算法中参数的取值。由于其针对的是高速公路工况，路面比较平直，该论文中并没有探讨对道路几何形状的定量估计问题，而道路的几何形状是决定车辆行驶速度的关键因素。

发明内容

本发明的目的是提出一种无人驾驶车辆跟踪预定轨迹的智能控制方法，以克服现有技术的上述缺陷，实现无人驾驶车辆对预定轨迹的精确跟踪。

本发明无人驾驶车辆跟踪预定轨迹的智能控制方法，在装有车载计算机和确定车辆相对预定轨迹位置的传感器GPS/INS定位系统的车辆上，车辆的转向机构采用电机驱动、涡轮蜗杆传动，电机轴与蜗杆直接连接，与蜗杆啮合的涡轮与方向盘转向柱固联；车辆的刹车机构采用电机驱动，采用滚珠丝杠传动，该电机的轴与滚珠丝杠机构的螺杆相连，滚珠丝杠机构的螺母与制动踏板固联；转向与制动控制系统采用编码器测量电机轴的转角，采用一体化的电机控制和驱动系统作为转向机构的控制单元，通过控制器局域网络(简称CAN)总线接收车载计算机发送的电机转角信息；采用控制器局域网络总线输出模拟电压信号来控制油门开度；采用路点坐标序列描述预定轨迹，采用等采样周期的计算机控制方式；其特征在于：每个控制周期的操作步骤如下：

第一步、以车身安装GPS/INS定位系统处为坐标原点，以车辆首尾轴线为y轴，以车辆左右侧轴线为x轴，建立车身坐标系；

第二步、确定车辆自身位置信息，从预定轨迹中选择三个轨迹点，一个是车辆坐标点后5～10m处的轨迹点记为后轨迹点A，另一个是车辆坐标点前方0～1m的轨迹点记为前近轨迹点B，再一个是车辆坐标点前方20～30m的轨迹点记为前远轨迹点C；计算这三个轨迹点所形成的角度∠ABC的弧度θ，根据对∠ABC的弧度θ值的辨识来定义各段道路的曲率特性：若θ≤1.57，则该道路段为弯道(t)段；若1.57＜θ≤2.356，则该道路段为弧线(a)段；若2.356＜θ≤3.14，则该道路段为直线(l)段；

第三步、依据∠ABC的弧度θ值，按照下面给出的对直线段道路的隶属度函数，对弧线段道路的隶属度函数，和对弯道段道路的隶属度函数，计算当前道路曲率特性对直线段的隶属度函数值s_tf、弧线段的隶属度函数值s_af和弯道段的隶属度函数值s_lf，记为隶属度函数值向量s_f＝(s_tf，s_af，s_lf)，其中，

对直线段道路的隶属度函数 $S_T=\left\{\begin{array}{cc}1& \mathrm{\&theta;}\&le;0.78\\ \frac{1.57-\mathrm{\&theta;}}{0.78}& 0.78<\mathrm{\&theta;}\&le;1.57\\ 0& \mathrm{\&theta;}>1.57\end{array}\right.---\left(1\right),$

对弧线段道路的隶属度函数 $S_A=\left\{\begin{array}{cc}\frac{2(\mathrm{\&theta;}-0.78)}{1.57}& 0.78<\mathrm{\&theta;}\&le;1.57\\ \frac{2(2.236-\mathrm{\&theta;})}{1.57}& 1.57<\mathrm{\&theta;}\&le;3.14\end{array}\right.---\left(2\right),$

对弯道段道路的隶属度函数 $S_L=\left\{\begin{array}{cc}0& \mathrm{\&theta;}\&le;1.57\\ \frac{\mathrm{\&theta;}-1.57}{0.78}& 1.57<\mathrm{\&theta;}\&le;2.356\\ 1& \mathrm{\&theta;}>2.356\end{array}\right.---\left(3\right),$

定义车速的集合：以车速快(VQ)、车速中等(VM)和车速慢(VL)分别对应速度30km/h、20km/h和10km/h；并定义模糊规则为：当道路段的曲率为直线段时，设定的车速快；当道路段的曲率为弧线段时，设定的车速中等；当道路段的曲率为弯道段时，设定的车速慢；生成模糊关系矩阵

$R=\left[\begin{array}{ccc}1& & \\ & 1& \\ & & 1\end{array}\right],$

计算模糊响应值R_V＝s_f×R；

按照加权平均算法进行模糊判决，计算输出决策速度

V＝(10，20，30)^T×R_V    (4)；

第四步、按车辆以当前车速行驶1.5～2.5秒通过的距离，在预定轨迹的坐标序列中搜索大于该距离并距车辆最近的轨迹点；

第五步、采用自适应比例-微分-积分控制(PID)算法公式

$\frac{1}{R}=\frac{2}{d^2}(k_p\&CenterDot;e\left(k\right)+k_d(e\left(k\right)-e(k-1)))---\left(5\right),$

计算车辆运动的曲率，式中，d为当前控制周期车辆到当前控制周期目标点的距离，e(k)为当前控制周期的x轴向误差；e(k-1)上一控制周期的x轴向误差；当通过的道路段为弧线段和弯道段时，选择比例系数k_p为0.3～0.5，微分系数k_d为4～7；当通过的道路段为直线段时选择比例系数k_p为0.1，微分系数k_d为2～4；

第六步、根据计算出的车辆运动曲率，采用阿克尔曼几何关系

$\mathrm{\&delta;}=\frac{1}{R}\&CenterDot;i\&CenterDot;L---\left(6\right)$

推算方向盘的转角δ，式中，i为转向系传动比，L为轴距；

第七步、判断当前控制周期目标点是否为路点序列中被选中的最后一点，若是，则向刹车伺服控制系统输出信号，启动刹车使车辆减速，当x轴向和y轴向误差为0时，完全刹车至车辆停止；若否，则使用控制器局域网络(CAN)模块输出电压信号保持控制油门开度使车辆继续前进。

采用本发明无人驾驶车辆跟踪预定轨迹的智能控制方法，能够利用目标点相对车辆的位置，利用自适应PID算法确定方向盘的转向角度，使车辆跟踪预定轨迹，功能通用性强，且具有对道路环境自适应性。

采用本发明无人驾驶车辆跟踪预定轨迹的智能控制方法的意义在于：

一、无人驾驶汽车对预定轨迹的跟踪能力，是评价无人驾驶汽车性能的重要指标之一，也是无人驾驶汽车能够安全、有效的完成预定任务的重要保障。采用本发明无人驾驶车辆跟踪预定轨迹的智能控制方法，能够简单有效的实现无人驾驶车辆对预期轨迹的跟踪，且能够达到较好的跟踪效果；

二、由于采用本发明方法在控制过程中仅需要目标点相对车辆的坐标，而并不局限于某一种固定的传感器和定位手段，获取目标点相对车辆的坐标，既可以使用GPS系统，也可以采用雷达、光学等传感器，因此本发明方法具有较好的普适性、通用性，能够适用于安装不同传感器的无人驾驶车辆。

三、本发明采用在轨迹序列中选取前近轨迹点、前远轨迹点和后轨迹点形成的角度判断道路的几何形状，并借鉴已有常识的驾驶经验，判断当前控制周期的速度、选择控制参数，提高了行驶质量和无人车的智能性。

四、相比于二值逻辑的判断方法，本发明采用模糊决策的方法判断车辆当前控制周期的车速方法，既可以将人类已有常识的驾驶经验采用模糊规则描述，以指导速度决策，又避免了二值逻辑判断过程中发生的判断结果突变而导致车辆抖动，提高了行驶的稳定性。

五、由于本发明无人驾驶车辆跟踪预定轨迹的智能控制方法中的控制算法采用经典的PID控制算法，实现简单，鲁棒性和可靠性较高；同时算法能够根据车辆速度，自动选择目标点，具有一定的自适应能力。

附图说明

图1为本发明无人驾驶车辆跟踪预定轨迹的智能控制方法的工作流程框图。

图2为本发明无人驾驶车辆跟踪预定轨迹的智能控制方法中的全局坐标系X_GO_GY_G与跟踪坐标系X_LO_LY_L示意图。

图3为采用本发明无人驾驶车辆跟踪预定轨迹的智能控制方法对预定轨迹跟踪的效果图。

具体实施方式

实施例1：以奇瑞“瑞虎”车改装的无人驾驶车辆对弧线轨迹跟踪

本实施例以奇瑞“瑞虎”车改装的无人驾驶车为例，具体说明如何采用本发明方法实现对预定轨迹的跟踪及其功能效果。

在本实施例所采用的以奇瑞“瑞虎”车改装的无人驾驶车上，装有诺瓦泰(NovAtel)公司提供的产品名称为SPAN-CPT的GPS/INS组合定位系统，能够获取车辆所在位置的经纬度、车辆速度信号和车辆航向角信号，在使用差分定位的情况下，定位精度可以达到0.1m。为了实现自动驾驶的功能，车辆的转向机构采用电机驱动、涡轮蜗杆传动，电机的轴与蜗杆直接连接，与蜗杆啮合的涡轮与方向盘转向柱固联；车辆的刹车机构采用电机驱动，采用滚珠丝杠传动，该电机的轴与滚珠丝杠机构的螺杆相连，滚珠丝杠机构的螺母与制动踏板固联；转向与制动控制系统采用编码器测量电机轴的转角，采用maxon公司生产的一体化的电机控制和驱动系统，型号为epos24/5作为转向机构的控制单元，通过控制器局域网络(简称CAN)总线接收车载计算机发送的电机转角信息。

本实施例中采用伺服控制技术，从而可以通过给定位置控制信号驱动伺服机构使上述装置运动到给定位置，实现对方向盘和刹车的控制。通过改装油门，采用周立功模拟量输出模块(iCAN-4210)，可以通过给定电压信号，确定节气门的开度，实现对车速的控制。目前无人驾驶状态设计的最大安全速度为36km/h。

图1为本发明无人驾驶车辆跟踪预定轨迹的智能控制方法的工作流程框图；图2为全局坐标系X_GO_GY_G与跟踪坐标系X_LO_LY_L示意图。以下结合附图对本发明无人驾驶车辆跟踪预定轨迹的智能控制方法的操作步骤进行具体详细的说明。

首先采用路点坐标序列描述预定轨迹。预定轨迹是由n个目标点组成的序列，目标点序列采用一n×2维的数组描述，每一行的两个元素分别代表轨迹中一点的经纬度。

采用等采样周期的计算机控制方式来实现对预定轨迹的跟踪，每个控制周期按如下步骤进行：

第一步、建立车身坐标系：该车身坐标系是以车身安装GPS/INS定位系统处为坐标原点，以车辆首尾轴线为y轴，以车辆左右侧轴线为x轴；车辆运动过程中的坐标系如图2中所示，其中X_GO_GY_G为全局坐标系，X_LO_LY_L为车身坐标系。

第二步、确定车辆自身位置信息，辨识车辆当前位置附近的道路曲率特性，按如下步骤进行：

①从所述预定轨迹的坐标序列中选择三个轨迹点：一个是车辆坐标点后5m处的轨迹点记为后轨迹点A，另一个是车辆坐标点前方1m的轨迹点记为前近轨迹点B，再一个是车辆坐标点前方30m的轨迹点记为前远轨迹点C；

例如，本实施例中选择车辆位置前方1m的前近轨迹点B为117.12443，31.848936，航向角为176.79°，选择车辆后轨迹点A的坐标为117.124428，31.84905，选择车辆前方30m附近的前远轨迹点C坐标为117.124695，31.848801。

②计算∠ABC的弧度θ，根据θ值给出对于道路段的辨识结果，按照道路段曲率的不同，定性描述道路段]为直线段(l)、弧线段(a)或弯道段(t)，描述规则为：若θ≤1.57，则该道路段为弯道段；若1.57＜θ≤2.356，则该道路段为弧线段；若2.356＜θ≤3.14，则该道路段为直线段。根据三点的坐标可以计算∠ABC的弧度θ＝1.3421根据描述规则，可以判断前方道路段为弯道段。

可以看出，采用这样的方法可以定性判断道路的几何形状，而识别道路的几何形状是决定车辆速度和方向盘转动量的关键。

第三步、采用模糊决策的方法决定车辆在当前控制周期的车辆当前速度，依据道路段为直线段、弧线段或弯道段的辨识结果来决定车辆在当前控制周期的车辆当前速度，采用马丹尼(Mamdani)型模糊系统为决策模型，决策当前控制周期车辆速度，其具体步骤如下：

①将道路曲率特性模糊化，定义描述道路曲率特性(F)的模糊集合“直线段(L)”，“曲线段(A)”，“弯道段(T)”。按照下面给出的对直线段道路的隶属度函数，对弧线段道路的隶属度函数，和对弯道段道路的隶属度函数，计算当前道路曲率特性对直线段的隶属度函数值s_tf、弧线段的隶属度函数值s_af和弯道段的隶属度函数值s_lf，记为隶属度函数值向量s_f＝(s_tf，s_af，s_lf)，其中，

对直线段道路的隶属度函数 $S_T=\left\{\begin{array}{cc}1& \mathrm{\&theta;}\&le;0.78\\ \frac{1.57-\mathrm{\&theta;}}{0.78}& 0.78<\mathrm{\&theta;}\&le;1.57\\ 0& \mathrm{\&theta;}>1.57\end{array}\right.---\left(1\right),$

对弧线段道路的隶属度函数 $S_A=\left\{\begin{array}{cc}\frac{2(\mathrm{\&theta;}-0.78)}{1.57}& 0.78<\mathrm{\&theta;}\&le;1.57\\ \frac{2(2.236-\mathrm{\&theta;})}{1.57}& 1.57<\mathrm{\&theta;}\&le;3.14\end{array}\right.---\left(2\right),$

对弯道段道路的隶属度函数 $S_L=\left\{\begin{array}{cc}0& \mathrm{\&theta;}\&le;1.57\\ \frac{\mathrm{\&theta;}-1.57}{0.78}& 1.57<\mathrm{\&theta;}\&le;2.356\\ 1& \mathrm{\&theta;}>2.356\end{array}\right.---\left(3\right),$

由当前θ值，计算当前道路曲率对模糊集合“直线段(L)”，“曲线段(A)”，“弯道段(T)”的隶属度函数值，结果为[0.292，0.716，0]。

②编写模糊决策规则，生成模糊变换器。定义车速的集合车速快(VQ)、车速中等(VM)、车速慢(VL)，分别对应速度30km/h，20km/h，10km/h。模糊规则为：当道路曲率为直线段，车速快；当道路曲率为曲线段，车速中等；当道路曲率为弯道段，车速慢。

则模糊关系矩阵R为

$R=\left[\begin{array}{ccc}1& & \\ & 1& \\ & & 1\end{array}\right]$

模糊隶属度函数值经过模糊变换矩阵的计算，得到模糊响应值R_V为[0.292，0.716，0]。

③按照加权平均判决法，进行模糊判决，输出决策速度。根据定义的车速集合，计算决策出的速度为V＝0.292×10+0.716×20+0×30＝17.24km/h。

可以看出，用上述方法决策车辆当前控制周期的速度可以避免二值逻辑决策中发生的速度突变，比如从10km/h突变到30km/h，从而能保持较好的稳定性。

第四步、根据车辆当前速度，决定当前控制周期的目标点与车辆当前位置之间的距离S1，如图2中所示的点O_LT之间的直线1，根据所述距离S1确定当前控制周期的目标点，如图2中T点所示，计算出当前控制周期的目标点在车身坐标系中的坐标，把车辆以当前车速行驶2秒通过的距离确定为当前控制周期的目标点与车辆当前位置之间距离，在预定轨迹的坐标序列中搜索大于该距离并距车辆最近的轨迹点确定为当前控制周期目标点。通过GPS/INS组合定位系统，检测得车辆当前速度为16.2km/h，可以计算预瞄距离为9m，从坐标序列中搜索得当前控制周期目标点的坐标为117.124489，31.848817。

第五步、计算当前控制周期目标点相对于车身坐标系x轴向的误差为3.7m，横向控制算法采用基于先验知识的自适应PID算法，车辆的转向曲率是由横向误差经由横向控制算法计算获得；基于先验知识的自适应PID方法是按如下步骤进行：

采用自适应PID算法计算车辆运动的曲率，控制算法的输出按照自适应比例-微分-积分控制(PID)算法公式式(5)

$\frac{1}{R}=\frac{2}{d^2}(k_p\&CenterDot;e\left(k\right)+k_d(e\left(k\right)-e(k-1)))---\left(5\right)$

获得；式中，d为当前控制周期车辆到当前控制周期目标点的距离，e(k)为当前控制周期的x轴向误差；e(k-1)上一控制周期的x轴向误差；当通过的道路段为弧线段和弯道段时，选择比例系数k_p为0.3～0.5，微分系数k_d为4～7；当通过的道路段为直线段时选择比例系数k_p为0.1，微分系数k_d为2～4；在本实施例中，当判断道路曲率特性为弯道段和弧线段时，取k_p＝0.3，k_d＝5；当判断道路曲率特性为直线段时，取k_p＝0.1，k_d＝3。计算车辆运动曲率约为0.07。

第六步、根据计算出的车辆运动曲率，采用阿克尔曼几何关系

$\mathrm{\&delta;}=\frac{1}{R}\&CenterDot;i\&CenterDot;L---\left(6\right)$

推算方向盘的转角δ，式中i为转向系传动比，L为轴距，得出方向盘转角约为188.35度。采用上述控制动作时，车辆实际行驶轨迹距离该点的误差约为0.1m。说明该方法能够较准确的跟踪预定的轨迹。在本发明中利用该自适应PID算法计算车辆运动的曲率时，车辆能够在不同的道路曲率特性下，采用不同的控制参数，以维持较高的精度，从而使本发明方法具有一定的自适应能力。

第七步、判断当前控制周期目标点是否为路点序列中被选中的最后一点，若是，启动刹车使车辆减速，当y轴向误差为0时，完全刹车至车辆停止；若否，则使用控制器局域网络(CAN)模块输出电压信号，保持控制油门开度保持当前速度继续前进。在本实施例中，轨迹序列被选中的最后一点坐标为31.849798，117.126435，当搜索到该点时，首先使车辆减速；当y轴误差为0时完全刹车使车辆停止，最终车辆停止于31.849802，117.126435坐标点，该车辆停止坐标点距离车辆预定位置约为0.25m。

图3为采用本发明无人驾驶车辆跟踪预定轨迹的智能控制方法对预定轨迹跟踪的效果图。从图3中可以看出，实际行驶轨迹虚点曲线1和预定轨迹实曲线2几乎重合，说明采用本发明方法有较高的跟踪精度。

Claims (1)

1.一种无人驾驶车辆跟踪预定轨迹的智能控制方法，在装有车载计算机和确定车辆相对预定轨迹位置的传感器GPS/INS定位系统的车辆上，车辆的转向机构采用电机驱动、涡轮蜗杆传动，电机轴与蜗杆直接连接，与蜗杆啮合的涡轮与方向盘转向柱固联；车辆的刹车机构采用电机驱动，采用滚珠丝杠传动，该电机的轴与滚珠丝杠机构的螺杆相连，滚珠丝杠机构的螺母与制动踏板固联；转向与制动控制系统采用编码器测量电机轴的转角，采用一体化的电机控制和驱动系统作为转向机构的控制单元，通过控制器局域网络总线接收车载计算机发送的电机转角信息；采用控制器局域网络总线输出模拟电压信号来控制油门开度；采用路点坐标序列描述预定轨迹，采用等采样周期的计算机控制方式；其特征在于：每个控制周期的操作步骤如下：

第一步、以车身安装GPS/INS定位系统处为坐标原点，以车辆首尾轴线为y轴，以车辆左右侧轴线为x轴，建立车身坐标系；

第二步、确定车辆自身位置信息，从预定轨迹中选择三个轨迹点，一个是车辆坐标点后5～10m处的轨迹点记为后轨迹点A，另一个是车辆坐标点前方0～1m的轨迹点记为前近轨迹点B，再一个是车辆坐标点前方20～30m的轨迹点记为前远轨迹点C；计算这三个轨迹点所形成的角度∠ABC的弧度θ，根据对∠ABC的弧度θ值的辨识来定义各段道路的曲率特性：若θ≤1.57，则该道路段为弯道段；若1.57＜θ≤2.356，则该道路段为弧线段；若2.356＜θ≤3.14，则该道路段为直线段；

第三步、依据∠ABC的弧度θ值，按照下面给出的对直线段道路的隶属度函数，对弧线段道路的隶属度函数，和对弯道段道路的隶属度函数，计算当前道路曲率特性对直线段的隶属度函数值s_tf、弧线段的隶属度函数值s_af和弯道段的隶属度函数值s_lf，记为隶属度函数值向量s_f＝(s_tf，s_af，s_lf)，其中，

对直线段道路的隶属度函数 $S_T=\left\{\begin{array}{cc}1& \mathrm{\&theta;}\&le;0.78\\ \frac{1.57-\mathrm{\&theta;}}{0.78}& 0.78<\mathrm{\&theta;}\&le;1.57\\ 0& \mathrm{\&theta;}>1.57\end{array}\right.,$

对弧线段道路的隶属度函数 $S_A=\left\{\begin{array}{cc}\frac{2(\mathrm{\&theta;}-0.78)}{1.57}& 0.78<\mathrm{\&theta;}\&le;1.57\\ \frac{2(2.236-\mathrm{\&theta;})}{1.57}& 1.57<\mathrm{\&theta;}\&le;3.14\end{array}\right.,$

对弯道段道路的隶属度函数 $S_L=\left\{\begin{array}{cc}0& \mathrm{\&theta;}\&le;1.57\\ \frac{\mathrm{\&theta;}-1.57}{0.78}& 1.57<\mathrm{\&theta;}\&le;2.356\\ 1& \mathrm{\&theta;}>2.356\end{array}\right.;$

定义车速的集合：以车速快、车速中等和车速慢分别对应速度30km/h、20km/h和10km/h；并定义模糊规则为：当道路段的曲率为直线段时，设定的车速快；当道路段的曲率为弧线段时，设定的车速中等；当道路段的曲率为弯道段时，设定的车速慢；生成模糊关系矩阵

$R=\left[\begin{array}{ccc}1& & \\ & 1& \\ & & 1\end{array}\right],$

计算模糊响应值R_V＝s_f×R；

按照加权平均算法进行模糊判决，计算输出决策速度V＝(10，20，30)^T×R_V；

第四步、按车辆以当前车速行驶1.5～2.5秒通过的距离，在预定轨迹的坐标序列中搜索大于该距离并距车辆最近的轨迹点；

第五步、采用自适应比例-微分-积分控制算法公式

$\frac{1}{R}=\frac{2}{d^2}(k_p\&CenterDot;e\left(k\right)+k_d(e\left(k\right)-e(k-1)))$

计算车辆运动的曲率，式中，d为当前控制周期车辆到当前控制周期目标点的距离，e(k)为当前控制周期的x轴向误差；e(k-1)上一控制周期的x轴向误差；当通过的道路段为弧线段和弯道段时，选择比例系数k_p为0.3～0.5，微分系数k_d为4～7；当通过的道路段为直线段时选择比例系数k_p为0.1，微分系数k_d为2～4；

第六步、根据计算出的车辆运动曲率，采用阿克尔曼几何关系

$\mathrm{\&delta;}=\frac{1}{R}\&CenterDot;i\&CenterDot;L$

推算方向盘的转角δ，式中，i为转向系传动比，L为轴距；

第七步、判断当前控制周期目标点是否为路点序列中被选中的最后一点，若是，则向刹车伺服控制系统输出信号，启动刹车使车辆减速，当x轴向和y轴向误差为0时，完全刹车至车辆停止；若否，则使用控制器局域网络模块输出电压信号保持控制油门开度使车辆继续前进。

Images