CN110989625B - 一种车辆路径跟踪控制方法 - Google Patents

一种车辆路径跟踪控制方法 Download PDF

Info

Publication number
CN110989625B
CN110989625B CN201911360652.5A CN201911360652A CN110989625B CN 110989625 B CN110989625 B CN 110989625B CN 201911360652 A CN201911360652 A CN 201911360652A CN 110989625 B CN110989625 B CN 110989625B
Authority
CN
China
Prior art keywords
vehicle
path
time
point
points
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201911360652.5A
Other languages
English (en)
Other versions
CN110989625A (zh
Inventor
徐彪
王俊懿
胡满江
秦兆博
边有钢
谢国涛
秦晓辉
王晓伟
尹冲
丁荣军
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan University
Original Assignee
Hunan University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hunan University filed Critical Hunan University
Priority to CN201911360652.5A priority Critical patent/CN110989625B/zh
Publication of CN110989625A publication Critical patent/CN110989625A/zh
Application granted granted Critical
Publication of CN110989625B publication Critical patent/CN110989625B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Steering Control In Accordance With Driving Conditions (AREA)

Abstract

本发明公开了一种车辆路径跟踪控制方法，该方法包括：S1，根据已有的参考路径点，获得一条路径点更密集的新参考路径；S2，获得车辆状态信息；S3，在新参考路径上找出最近路径点；S4，以最近路径点为起点，在新参考路径上向车辆行驶的前方搜索N个预瞄点；S5，构建预测模型、目标函数以及***约束，根据当前测量信息和预测模型，预测车辆未来动态，在线求解满足所述目标函数和约束条件的优化问题，获取N个预瞄点所对应的期望前轮转向角构成的最优控制序列；S6，根据最优控制序列，控制车辆直到下一采样时刻到达，下一观测时刻到达时，重复步骤S2至S5。本发明提供的方法跟踪精度较高，同时也能够保证在控制过程中的舒适性，不会产生控制量的突变。

Description

一种车辆路径跟踪控制方法
技术领域
本发明涉及车辆控制技术领域，特别是关于一种车辆路径跟踪控制方法。
背景技术
无人驾驶车辆路径跟踪控制是指通过无人驾驶车辆的底层控制器，控制车辆转向角度，使车辆始终沿着期望的路径行驶。因此无人驾驶车辆的路径跟踪能力是无人驾驶车辆安全行驶的重要保证。路径跟踪控制的目标是减小车辆在行驶过程中与参考路径的偏差，同时保证车辆转向平稳。
目前主要的无人驾驶车辆路径跟踪控制方法主要包括模糊控制、预瞄控制以及模型预测控制等方法。
比如：公开号为CN107942663A的专利申请提出了模糊PID控制方法，该方法利用了模糊控制算法建立模糊控制规则表，同时利用PID控制方法获得比列系数、微分系数和积分系数，计算得到转向控制器的控制值。该专利文件提供的控制方法将模糊控制与PID控制结合在一起，但是该方法没有使用准确的物理模型，也没有加入车辆约束，在复杂情况下很难实现良好的跟踪效果。
再比如：公开号为CN109318905A的专利申请提出了两种控制方法，其中一种利用车辆的运动学约束，建立车辆的横向控制预瞄运动学模型，向车辆行驶的前方选取一个距离车辆一定预瞄距离的的路径点作为预瞄点，再结合PID控制方法对车辆进行横向控制。但是该方法只能选取一个预瞄点，仅能使所取的单一预瞄点附近的路径偏离误差较小，而无法兼顾整条路径的跟踪误差。
还比如：专利公开号为CN108973769A的专利申请提出了基于模型控制论(MPC，Model Predictive Control)的控制方法，构建了车辆路径跟踪的动力学模型，其通过求解模型预测控制器获得最优控制序列。但是该方法仅利用当前车辆与路径的偏差作为输入条件，并未充分利用整段路径的位置和方向信息。
综上所述，现有技术提供的方法均无法解决大曲率路径车辆路径跟踪控制的问题，因此需要有一种能充分利用路径信息的方法来提高路径跟踪精度。
发明内容
本发明的目的在于提供一种车辆路径跟踪控制方法来克服或至少减轻现有技术的上述缺陷中的至少一个。
为实现上述目的，本发明提供一种车辆路径跟踪控制方法，该方法包括：
S1，对已有的参考路径点进行插值，获得一条路径点更加密集的新参考路径；
S2，获得车辆状态信息；
S3，遍历S1获得的新参考路径上的所有路径点pi(xi，yi)，找出最近路径点p0；
S4，在S1获得的新参考路径上，以S3找到的最近路径点p0为起点，根据S2获取的车辆运动速度v(t)，向车辆行驶的前方搜索N个预瞄点；
S5，构建预测模型和下式(5)所示的目标函数，根据S2获取的当前车辆状态信息和预测模型，预测车辆未来动态，在线求解满足所述目标函数和约束条件的优化问题，获取S4搜索得到的N个预瞄点中每一个预瞄点对应的期望前轮转向角，N个预瞄点中每一个预瞄点对应的期望前轮转向角构成式(20)所表示的最优控制序列；
Figure GDA0002660294070000021
式(5)中：
Q为权重系数；
Δδ(k+t|t)为在t时刻预测的的k+1+t时刻的车辆前轮转向角与上一预测时刻k+t时刻的车辆前轮转向角之差；
e(k+t|t)为在t时刻预测到的k+t时刻的车辆后轴中心与在t时刻搜索的第k个预瞄点切线的距离，其表示为式(6)：
Figure GDA0002660294070000022
式(6)中：
x(k+t|t)为在t时刻预测到的k+t时刻的车辆后轴中心点的横坐标；
y(k+t|t)为在t时刻预测到的k+t时刻的车辆后轴中心点的纵坐标；
xp(k)为S4中搜索得到的第k个预瞄点的横坐标；
yp(k)为S4中搜索得到的第k个预瞄点的纵坐标；
θp(k)为S4中搜索得到的第k个预瞄点的航向角；
Figure GDA0002660294070000031
式(20)中，
Figure GDA0002660294070000032
表示在t时刻的k+t时刻的前轮转向角；
S6，使用
Figure GDA0002660294070000033
控制车辆直到下一采样时刻到达，下一观测时刻t+1到达时，重复步骤S2至S5，利用S2提供的新的车辆状态刷新问题，如此循环直至到达路径终点。
进一步地，S4中的“向车辆行驶的前方搜索N个预瞄点”具体包括如下方法：
以最近路径点p0为起点，同时，以v(t)ΔT为搜索距离，沿新参考路径向车辆行驶的前方搜索N个路径点作为预瞄点(xp(k)，yp(k))，k＝1，2，…，N，ΔT为离散的时间步长，当搜索到达新参考路径终点时，停止搜索。
进一步地，S4中的“向车辆行驶的前方搜索N个预瞄点”具体包括如下方法：
以最近路径点p0为起点，同时，以v(t)ΔT为搜索距离，沿新参考路径向车辆行驶的前方搜索N个路径点作为预瞄点(xp(k)，yp(k))，k＝1，2，…，N，ΔT为离散的时间步长，当搜索到达设定的最大预测步数N时，停止搜索。
进一步地，S5在每一个控制周期内求解如下式(13)所示的优化问题：
Figure GDA0002660294070000034
式(13)中：
minJ(k)是式(5)提供的目标函数的最小值；
x(k+1+t|t)为在t时刻预测到的k+1+t时刻的车辆后轴中心点的横坐标；
y(k+1+t|t)为在t时刻预测到的k+1+t时刻的车辆后轴中心点的纵坐标；
Figure GDA0002660294070000035
为在t时刻预测到的k+t时刻的车身航向角；
Figure GDA0002660294070000036
为在t时刻预测到的k+1+t时刻的车身航向角；
v(t)为S2获得的车辆速度；
Δt为每个预测步长的时间；
δ(k+t|t)为在t时刻预测的k+t时刻的前轮转向角；
δ(k+1+t|t)为在t时刻预测的k+t+1时刻的前轮转向角；
L为车辆的轴距；
k为第k个预测步；
Δmax为相邻时刻内车辆的前轮转向角的最大变化量；
δmax为车辆前轮最大转向角。
进一步地，式(13)所示的优化问题求解之前，预测时域内的前轮转向角的初始值的具体计算方法包括：
S51.利用步骤S2获得的车辆状态信息，取与步骤S4中相同的步长ΔT，以步骤S3和步骤S4中得到的最近路径点p0以及搜索到的前N-1个预瞄点作为Stanley方法对应时刻的最近路径点(xp(k)，yp(k))，k＝1，2，…，N；
S52.计算当前t时刻车辆后轴中心(xr(t)，yr(t))与对应的最近路径点p0(xp(0)，yp(0))的横向偏差efa(t)：
Figure GDA0002660294070000041
式(14)中，θp(0)为路径点p0的航向角；
S53.计算当前t时刻车身航向角与最近路径点航向角的角度误差θe(t)：
Figure GDA0002660294070000042
式(15)中，
Figure GDA0002660294070000043
为车身航向角，θp(0)为路径点p0的航向角；
S54.计算当前t时刻前轮的期望转向角δ′(t)：
Figure GDA0002660294070000044
式(16)中：
K为权重系数；
θe(t)为期望转向角δ′(t)的角度误差部分；
S55.利用式(16)求得的δ′(t)和已知的车辆后轴中心坐标(xr(t)，yr(t))、速度v、车身航向角
Figure GDA0002660294070000051
前轮转向角δ(t)信息以及车辆运动学公式(17)至式(19)，得到下一时刻t+ΔT的车辆状态信息：
Figure GDA0002660294070000052
Figure GDA0002660294070000053
Figure GDA0002660294070000054
S56.以式(17)至式(19)所得到的车辆状态信息更新车辆状态，以t+ΔT时刻作为当前时刻，选取第一个预瞄点(xp(1)，yp(1))作为最近路径点，重复步骤S52至S55，递推出之后每一个预瞄点对应的期望转向角δ′(t+k*ΔT)，并将其作为公式(13)所示优化问题中δ(k+t|t)的迭代初始值。
进一步地，S3中的“最近路径点p0”的方法包括：
利用式(1)计算路径点pi(xi，yi)与当前时刻t下的车辆后轴中心(xr(t)，yr(t))的距离平方Di，并找出距离平方Di最小的路径点，该路径点记为最近路径点p0：
Di＝(xr(t)-xi)2+(yr(t)-yi)2 (1)。
进一步地，S1采用三次样条插值方法，其具体包括如下步骤：
S11，在参考路径上选定一个路径点，根据大地坐标系下与该选定的路径点相邻的路径点的横摆角信息，计算出坐标系旋转向角度；
S12，对大地坐标系进行旋转，旋转向角度为步骤S11计算得到的坐标系旋转向角度，并计算旋转后的S11中的两个所述路径点的新坐标和航向角；
S13，根据S12计算得到的S11中的两个所述路径点的新坐标和航向角，计算得到该两个路径点之间的拟合三次样条插值曲线表达式；
S14，对S13得到的三次样条插值曲线表达式的横坐标进行等间距离散，获得高密度的插值路径点，进而获得新参考路径。其中，各插值路径点的一阶导数即为该点航向角信息；
S15，对S14的插值路径点进行坐标系旋转，以还原至大地坐标系中，得到大地坐标系下原相邻路径点及其间插值路径点的坐标及航向角信息，其中：旋转方向与S12的旋转方向相反，旋转的角度大小为S11计算得到的坐标系旋转向角度。
本发明由于采取以上技术方案，其具有以下优点：1.选取路径点作为预瞄点时，不需要一条可解析的路径，即使是离散的路径点、非可解析的路径，也能很好地找到预瞄点，这在工程运用中更加符合实际工况。2.同时在选取预瞄点的个数上，相比于传统的基于预瞄的控制方法只能选取一个预瞄点，本方法采用了多点预瞄的方法，多个预瞄点相对于单预瞄点，能够预测更长的步数与距离，能兼顾整条路径的跟踪误差，从而提高了跟踪的精度，同时也保证了在控制过程中的舒适性，不会产生控制量的突变。3.将预瞄控制和模型预测控制相结合，能够顾及由于模型失配、时变、干扰等引起的不确定性，及时进行弥补，始终把最新的优化建立在实际的基础上，保持实际上的最优。
附图说明
图1为本发明实施例提供的车辆路径跟踪控制方法的流程示意图；
图2为图1中的参考路径点插值的步骤提供的三次样条插值法的流程示意图；
图3为本发明实施例提供的路径跟踪示意图；
图4为图1中的计算最近路径点的步骤提供的获取最近路径点的原理示意图；
图5为图1中的计算最近路径点的步骤提供的路径跟踪控制器的结构示意图。
具体实施方式
下面结合附图和实施例对本发明进行详细的描述。
如图1所示，本发明实施例所提供的车辆路径跟踪控制方法包括以下步骤：
S1，对已有的参考路径点进行插值，获得一条路径点更加密集的新参考路径。其中，“参考路径”比如可以通过用GPS***记录路径的方式获得。但是，获得的该参考路径有可能会是离散的稀疏路径点，为了获得更密的路径点以保证跟踪的精度，本实施例使用三次样条插值方法对参考路径上起始路径点与目标路径点之间的所有路径点进行插值处理，以获得大地坐标系下由起始点至目标点之间的参考路径上各路径点的坐标(x，y)及航向角θp(如图2所示)。三次样条插值方法具体包括如下步骤：
S11，在参考路径上选定一个路径点，根据大地坐标系下与该选定的路径点相邻的路径点的横摆角信息，计算出坐标系旋转向角度。
S12，对大地坐标系进行旋转，旋转向角度为步骤S11计算得到的坐标系旋转向角度，并计算旋转后的S11中的两个所述路径点的新坐标和航向角。
S13，根据S12计算得到的S11中的两个所述路径点的新坐标和航向角，计算得到该两个路径点之间的拟合三次样条插值曲线表达式f(x)＝a(x-x′i-1)3+b(x-x′i-1)2+c(x-x′i-1)+d，式中，i-1和i分别表示S11中的两个所述路径点的序号，x′i-1表示第i-1个点在旋转后的坐标系下的横坐标，a、b、c、d为旋转坐标系下S11中的两个所述路径点之间的三次样条插值曲线表达式的系数，即为S13待计算的参数。
S14，对S13得到的三次样条插值曲线表达式的横坐标进行等间距离散，获得高密度的插值路径点，进而获得新参考路径。其中，各插值路径点的一阶导数即为该点航向角信息。
S15，对S14的插值路径点进行坐标系旋转，以还原至大地坐标系中，得到大地坐标系下原相邻路径点及其间插值路径点的坐标及航向角信息，其中：旋转方向与S12的旋转方向相反，旋转的角度大小为S11计算得到的坐标系旋转向角度。
S2，获得车辆状态信息。图3示意了车辆的位置与参考路径，如图3所示，通过车辆上安装的GPS传感器、惯性测量单元(IMU)以及安装的其它传感器，获得当前观测时刻t下的车辆后轴中心的坐标(xr(t)，yr(t))和速度v(t)、车身航向角
Figure GDA0002660294070000071
和前轮转向角δ(t)。
S3，计算S1获得的新参考路径上与车辆后轴中心的最近路径点。
如图4所示，遍历新参考路径上的所有路径点pi(xi，yi)，利用式(1)计算路径点pi(xi，yi)与当前时刻t下的车辆后轴中心(xr(t)，yr(t))的距离平方Di，并找出距离平方Di最小的路径点，该路径点记为最近路径点p0：
Di＝(xr(t)-xi)2+(yr(t)-yi)2 (1)。
S4，在S1获得的新参考路径上，以S3找到的最近路径点p0为起点，根据S2获取的车辆运动速度v(t)，向车辆行驶的前方搜索N个预瞄点。此处的“向车辆行驶的前方”指的是车头的前方。
其中：S4中的“向车辆行驶的前方搜索N个预瞄点”具体包括如下方法：
以最近路径点p0为起点，同时，以v(t)ΔT为搜索距离，向车辆行驶的前方搜索N个路径点作为预瞄点(xp(k)，yp(k))，k＝1，2，…，N。也就是说，第一个预瞄点(xp(1)，yp(1))与最近路径点p0沿新参考路径的距离为v(t)ΔT，第二个预瞄点(xp(2)，yp(2))与第一个预瞄点(xp(1)，yp(1))沿新参考路径的距离为v(t)ΔT，以此类推，第i个预瞄点(xp(j)，yp(j))与第i-1个预瞄点(xp(j-1)，yp(j-1))沿新参考路径的距离为v(t)ΔT，第N个预瞄点(xp(N)，yp(N))与第N-1个预瞄点(xp(N-1)，yp(N-1))沿新参考路径的距离为v(t)ΔT，ΔT为离散的时间步长。
上述实施例中，当搜索到达新参考路径终点时，停止搜索。当然，也可以根据设定的预测步数进行控制搜索操作，比如：当搜索到达设定的最大预测步数N时，停止搜索。
S5，构建模型预测控制器，模型预测控制器基本示意图如图5所示，模型预测控制器包括预测模型、目标函数以及***约束，根据当前车辆状态信息和预测模型，预测车辆未来动态，在线求解满足目标函数和约束条件的优化问题，获取S4搜索得到的N个预瞄点中每一个预瞄点(xp(k)，yp(k))对应的前轮转向角的最优控制序列。
根据车辆运动学公式，所述预测模型设置为式(2)至式(4)：
Figure GDA0002660294070000081
Figure GDA0002660294070000082
Figure GDA0002660294070000083
式(2)至式(4)中：
x(k+t|t)为在t时刻预测到的k+t时刻的车辆后轴中心点的横坐标；
x(k+1+t|t)为在t时刻预测到的k+1+t时刻的车辆后轴中心点的横坐标；
y(k+t|t)为在t时刻预测到的k+t时刻的车辆后轴中心点的纵坐标；
y(k+1+t|t)为在t时刻预测到的k+1+t时刻的车辆后轴中心点的纵坐标；
Figure GDA0002660294070000084
为在t时刻预测到的k+t时刻的车身航向角；
Figure GDA0002660294070000085
为在t时刻预测到的k+1+t时刻的车身航向角；
v(t)为S2获取得到的车辆速度；
Δt为每个预测步长的时间；
δ(k+t|t)为在t时刻预测的k+t时刻的车辆前轮转向角；
L为车辆的轴距；
k为第k个预测步。
根据车辆跟踪精度和稳定的需求，目标函数设置为式(5)：
Figure GDA0002660294070000091
式(5)中：
Q为权重系数，其需要根据实际试验以及仿真得到，本实施例中取Q＝150。
e(k+t|t)为在t时刻预测到的k+t时刻的车辆后轴中心与在t时刻搜索的第k个预瞄点切线的距离，即图3中所示距离efa，该距离表示车辆与参考路径的横向偏差，优化过程中应尽可能地使其e(k+t|t)取最小值，来保证车辆跟踪的精度。e(k+t|t)计算公式为式(6)：
Figure GDA0002660294070000092
式(6)中：
x(k+t|t)为在t时刻预测到的k+t时刻的车辆后轴中心点的横坐标；
y(k+t|t)为在t时刻预测到的k+t时刻的车辆后轴中心点的纵坐标；
xp(k)为S4中搜索得到的第k个预瞄点的横坐标；
yp(k)为S4中搜索得到的第k个预瞄点的纵坐标；
θp(k)为S4中搜索得到的第k个预瞄点的航向角。
Δδ(k+t|t)为在t时刻预测的k+1+t时刻的车辆前轮转向角与上一预测时刻k+t时刻的车辆前轮转向角之差，引入该目标，是为了在保证跟踪精度的情况下限制前轮转向角的变化幅度，从而防止方向盘产生较大抖动，保证跟踪的稳定性。Δδ(k+t|t)计算公式为式(7)：
Δδ(k+t|t)＝δ(k+1+t|t)-δ(k+t|t) (7)
式(7)中：
δ(k+t|t)为在t时刻预测的k+t时刻的前轮转向角；
δ(k+1+t|t)为在t时刻预测的k+t+1时刻的前轮转向角。
由于车辆作为一个机械***，存在一些结构上的约束，方向盘转向角不能超过允许的最大转向角，同时车辆作为运载工具，需要保证乘客的乘坐舒适性，方向盘转向角变化率不宜过大，鉴于此，式(6)的可行状态与控制变量空间约束设置为式(8)至式(12)：
Figure GDA0002660294070000101
Figure GDA0002660294070000102
Figure GDA0002660294070000103
-Δmax≤δ(k+1+t|t)-δ(k+t|t)≤Δmax (11)
-δmax≤δ(k+t|t)≤δmax (12)
式(8)至式(12)中，Δmax为相邻时刻内前轮转向角的最大变化量，δmax为车辆前轮最大转向角，机械结构的限制。
在每一个控制周期内求解优化问题：
根据前面步骤建立的目标函数和约束条件，模型预测控制器需要在每一个周期内求解的带约束优化问题如下所示：
Figure GDA0002660294070000104
式(13)中，minJ(k)是式(5)提供的目标函数的最小值。
在求解优化问题之前，先使用Stanley方法通过车辆运动学模型仿真计算出预测时域内的前轮转向角的初始值，具体计算方法如下所示：
S51.利用步骤S2获得的车辆状态信息，取与步骤S4中相同的步长ΔT，以步骤S3和步骤S4中得到的最近路径点p0以及搜索到的前N-1个预瞄点作为Stanley方法对应时刻的最近路径点(xp(k)，yp(k))，k＝1，2，…，N。
S52.计算当前t时刻车辆后轴中心(xr(t)，yr(t))与对应的最近路径点p0(xp(0)，yp(0))的横向偏差efa(t)：
Figure GDA0002660294070000105
式(14)中，θp(0)为路径点p0的航向角。
S53.计算当前t时刻车身航向角与最近路径点航向角的角度误差θe(t)：
Figure GDA0002660294070000111
式(15)中，
Figure GDA0002660294070000112
为车身航向角，θp(0)为路径点p0的航向角。
S54.计算当前t时刻前轮的期望转向角δ′(t)：
Figure GDA0002660294070000113
式(16)中：
K为权重系数，权重系数需要根据不同的情况来调整，具体给出的方法是靠测试和仿真得到一个较优的权重系数，本实施例中取K＝1；
θe(t)为期望转向角δ′(t)的角度误差部分，计算如公式(15)所示；
S55.利用式(16)求得的δ′(t)和已知的车辆后轴中心坐标(xr(t)，yr(t))、速度v(t)、车身航向角
Figure GDA0002660294070000118
前轮转向角δ(t)信息以及车辆运动学公式(17)至式(19)，得到下一时刻t+ΔT的车辆状态信息：
Figure GDA0002660294070000114
Figure GDA0002660294070000115
Figure GDA0002660294070000116
S56.以式(17)至式(19)所得到的车辆状态信息更新车辆状态，以t+ΔT时刻作为当前时刻，选取第一个预瞄点(xp(1)，yp(1))作为最近路径点，重复步骤S52至S55，递推出之后每一个预瞄点对应的期望转向角δ′(t+k*ΔT)，并将其作为公式(13)所示优化问题中δ(k+t|t)的迭代初始值。
初始值的引用，能提高求解过程中的收敛速度以及获得最优解的成功率，并且避免问题陷入局部最优。
根据步骤S51至S56求解得到的预测时域内的前轮转向角的初始值，对式(13)提供的优化问题进行求解，进而获取N个预瞄点中每一个预瞄点(xp(k)，yp(k))对应的前轮转向角的控制量，即N个预瞄点中每一个预瞄点(xp(k)，yp(k))对应的期望前轮转向角，N个预瞄点中每一个预瞄点对应的期望前轮转向角构成式(20)所表示的最优控制序列：
Figure GDA0002660294070000117
式(20)中，
Figure GDA0002660294070000121
表示在t时刻预测的k+t时刻的前轮转向角。
上述最优控制序列中，将其中的第一个元素用作N步中该步的控制量。
S6，使用
Figure GDA0002660294070000122
控制车辆直到下一采样时刻到达，下一观测时刻t+1到达时，重复步骤S2至S5，利用S2提供的新的车辆状态刷新问题，如此循环直至到达路径终点。
最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (6)

1.一种车辆路径跟踪控制方法，其特征在于，包括：
S1，对已有的参考路径点进行插值，获得一条路径点更加密集的新参考路径；
S2，获得车辆状态信息；
S3，遍历S1获得的新参考路径上的所有路径点pi(xi，yi)，找出最近路径点p0；
S4，在S1获得的新参考路径上，以S3找到的最近路径点p0为起点，根据S2获取的车辆状态信息中的车辆运动速度v(t)，向车辆行驶的前方搜索N个预瞄点；
S5，构建由式(2)至式(4)表示的预测模型和下式(5)所示的目标函数，根据S2获取的当前车辆状态信息和预测模型，预测车辆未来动态，在线求解满足所述目标函数和约束条件的优化问题，获取S4搜索得到的N个预瞄点中每一个预瞄点对应的期望前轮转向角，N个预瞄点中每一个预瞄点对应的期望前轮转向角构成式(20)所表示的最优控制序列；
Figure FDA0002660294060000011
Figure FDA0002660294060000012
Figure FDA0002660294060000013
Figure FDA0002660294060000014
式(5)中：
Q为权重系数；
Δδ(k+t|t)为在t时刻预测的k+1+t时刻的车辆前轮转向角与上一预测时刻k+t时刻的车辆前轮转向角之差；
e(k+t|t)为在t时刻预测到的k+t时刻的车辆后轴中心与在t时刻搜索的第k个预瞄点切线的距离，其表示为式(6)：
Figure FDA0002660294060000015
式(6)中：
x(k+t|t)为在t时刻预测到的k+t时刻的车辆后轴中心点的横坐标；
y(k+t|t)为在t时刻预测到的k+t时刻的车辆后轴中心点的纵坐标；
xp(k)为S4中搜索得到的第k个预瞄点的横坐标；
yp(k)为S4中搜索得到的第k个预瞄点的纵坐标；
θp(k)为S4中搜索得到的第k个预瞄点的航向角；
Figure FDA0002660294060000021
式(20)中，
Figure FDA0002660294060000022
表示在t时刻预测到的k+t时刻的前轮转向角；
在每一个控制周期内求解如下式(13)所示的优化问题：
Figure FDA0002660294060000023
式(13)中：
minJ(k)是式(5)提供的目标函数的最小值；
x(k+1+t|t)为在t时刻预测到的k+1+t时刻的车辆后轴中心点的横坐标；
y(k+1+t|t)为在t时刻预测到的k+1+t时刻的车辆后轴中心点的纵坐标；
Figure FDA0002660294060000025
为在t时刻预测到的k+t时刻的车身航向角；
Figure FDA0002660294060000026
为在t时刻预测到的k+1+t时刻的车身航向角；
v(t)为S2获得的车辆速度；
Δt为每个预测步长的时间；
δ(k+t|t)为在t时刻预测的k+t时刻的前轮转向角；
δ(k+1+t|t)为在t时刻预测的k+t+1时刻的前轮转向角；
L为车辆的轴距；
k为第k个预测步；
Δmax为相邻时刻内车辆的前轮转向角的最大变化量；
δmax为车辆前轮最大转向角；
S6，使用
Figure FDA0002660294060000024
控制车辆直到下一采样时刻到达，下一观测时刻t+1到达时，重复步骤S2至S5，利用S2提供的新的车辆状态刷新问题，如此循环直至到达路径终点。
2.如权利要求1所述的车辆路径跟踪控制方法，其特征在于，S4中的“向车辆行驶的前方搜索N个预瞄点”具体包括如下方法：
以最近路径点p0为起点，同时，以v(t)ΔT为搜索距离，沿新参考路径向车辆行驶的前方搜索N个路径点作为预瞄点(xp(k)，yp(k))，k＝1，2，…，N，ΔT为离散的时间步长，当搜索到达新参考路径终点时，停止搜索。
3.如权利要求1所述的车辆路径跟踪控制方法，其特征在于，S4中的“向车辆行驶的前方搜索N个预瞄点”具体包括如下方法：
以最近路径点p0为起点，同时，以v(t)ΔT为搜索距离，沿新参考路径向车辆行驶的前方搜索N个路径点作为预瞄点(xp(k)，yp(k))，k＝1，2，…，N，ΔT为离散的时间步长，当搜索到达设定的最大预测步数N时，停止搜索。
4.如权利要求2或3所述的车辆路径跟踪控制方法，其特征在于，式(13)所示的优化问题求解之前，预测时域内的前轮转向角求解迭代的初始值的具体计算方法包括：
S51.利用步骤S2获得的车辆状态信息，取与步骤S4中相同的步长ΔT，以步骤S3和步骤S4中得到的最近路径点p0以及搜索到的前N-1个预瞄点作为Stanley方法对应时刻的最近路径点(xp(k)，yp(k))，k＝0，2，…，N-1；
S52.计算当前t时刻车辆后轴中心(xr(t)，yr(t))与对应的最近路径点p0(xp(0)，yp(0))的横向偏差efa(t)：
Figure FDA0002660294060000031
式(14)中，θp(0)为路径点p0的航向角；
S53.计算当前t时刻车身航向角与最近路径点航向角的角度误差θe(t)：
Figure FDA0002660294060000041
式(15)中，
Figure FDA0002660294060000042
为车身航向角，θp(0)为路径点p0的航向角；
S54.计算当前t时刻前轮的期望转向角δ′(t)：
Figure FDA0002660294060000043
式(16)中：
K为权重系数；
θe(t)为期望转向角δ′(t)的角度误差部分；
S55.利用式(16)求得的δ′(t)和已知的车辆后轴中心坐标(xr(t)，yr(t))、速度v、车身航向角
Figure FDA0002660294060000044
前轮转向角δ(t)信息以及车辆运动学公式(17)至式(19)，得到下一时刻t+ΔT的车辆状态信息：
Figure FDA0002660294060000045
Figure FDA0002660294060000046
Figure FDA0002660294060000047
S56.以式(17)至式(19)所得到的车辆状态信息更新车辆状态，以t+ΔT时刻作为当前时刻，选取第一个预瞄点(xp(1)，yp(1))作为最近路径点，重复步骤S52至S55，递推出之后每一个预瞄点对应的期望转向角δ′(t+k*ΔT)，并将其作为公式(13)所示优化问题中δ(k+t|t)的迭代初始值。
5.如权利要求1或2或3所述的车辆路径跟踪控制方法，其特征在于，S3中的“最近路径点p0”的方法包括：
利用式(1)计算路径点pi(xi，yi)与当前时刻t下的车辆后轴中心(xr(t)，yr(t))的距离平方Di，并找出距离平方Di最小的路径点，该路径点记为最近路径点p0：
Di＝(xr(t)-xi)2+(yr(t)-yi)2 (1)。
6.如权利要求1或2或3所述的车辆路径跟踪控制方法，其特征在于，S1采用三次样条插值方法，其具体包括如下步骤：
S11，在参考路径上选定一个路径点，根据大地坐标系下与该选定的路径点相邻的路径点的横摆角信息，计算出坐标系旋转向角度；
S12，对大地坐标系进行旋转，旋转向角度为步骤S11计算得到的坐标系旋转向角度，并计算旋转后的S11中的两个所述路径点的新坐标和航向角；
S13，根据S12计算得到的S11中的两个所述路径点的新坐标和航向角，计算得到该两个路径点之间的拟合三次样条插值曲线表达式；
S14，对S13得到的三次样条插值曲线表达式的横坐标进行等间距离散，获得高密度的插值路径点，进而获得新参考路径，其中，各插值路径点的一阶导数即为该点航向角信息；
S15，对S14的插值路径点进行坐标系旋转，以还原至大地坐标系中，得到大地坐标系下原相邻路径点及其间插值路径点的坐标及航向角信息，其中：旋转方向与S12的旋转方向相反，旋转的角度大小为S11计算得到的坐标系旋转向角度。
CN201911360652.5A 2019-12-25 2019-12-25 一种车辆路径跟踪控制方法 Active CN110989625B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911360652.5A CN110989625B (zh) 2019-12-25 2019-12-25 一种车辆路径跟踪控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911360652.5A CN110989625B (zh) 2019-12-25 2019-12-25 一种车辆路径跟踪控制方法

Publications (2)

Publication Number Publication Date
CN110989625A CN110989625A (zh) 2020-04-10
CN110989625B true CN110989625B (zh) 2020-11-27

Family

ID=70077025

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911360652.5A Active CN110989625B (zh) 2019-12-25 2019-12-25 一种车辆路径跟踪控制方法

Country Status (1)

Country Link
CN (1) CN110989625B (zh)

Families Citing this family (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111487966B (zh) * 2020-04-13 2022-09-09 哈尔滨工程大学 一种基于航路点的水面无人艇自适应路径跟踪控制方法
CN111504340B (zh) * 2020-05-22 2022-04-29 北京汽车研究总院有限公司 车辆的路径规划方法、装置、车辆
CN112000090B (zh) * 2020-07-03 2023-06-27 中国北方车辆研究所 一种用于非结构化道路的轨迹队列管理方法
CN114384902B (zh) * 2020-10-19 2024-04-12 中车株洲电力机车研究所有限公司 一种自动循迹控制方法及其***
CN113110045B (zh) * 2021-03-31 2022-10-25 同济大学 一种基于计算图的模型预测控制实时优化并行计算方法
CN113204236B (zh) * 2021-04-14 2022-05-20 华中科技大学 一种智能体路径跟踪控制方法
CN113276833B (zh) * 2021-05-11 2022-07-29 上汽通用五菱汽车股份有限公司 车辆的横向运动控制方法、控制终端及存储介质
CN113390431B (zh) * 2021-06-17 2022-09-30 广东工业大学 动态生成参考线的方法、装置、计算机设备和存储介质
CN113306573B (zh) * 2021-06-23 2022-08-16 湖南大学 一种自动驾驶车辆学习型路径跟踪预测控制方法
CN113253741B (zh) * 2021-06-29 2021-11-23 浙江华睿科技股份有限公司 一种agv叉车的控制方法、装置及计算机可读介质
CN113635892B (zh) * 2021-10-18 2022-02-18 禾多科技（北京）有限公司 车辆控制方法、装置、电子设备和计算机可读介质
CN113848953B (zh) * 2021-10-25 2023-12-08 济南大学 一种大型液压植保机无人驾驶路径跟踪控制方法
CN113985895B (zh) * 2021-11-29 2024-05-03 佛山市毕佳索智能科技有限公司 一种基于最优化的agv路径跟踪方法
CN113978550B (zh) * 2021-12-30 2022-03-25 北京慧拓无限科技有限公司 一种无人矿车倒车控制方法

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0788044A1 (en) * 1996-02-01 1997-08-06 MANNESMANN Aktiengesellschaft An automated guided vehicle navigation steering control system
CN102358287A (zh) * 2011-09-05 2012-02-22 北京航空航天大学 一种用于车辆自动驾驶机器人的轨迹跟踪控制方法
US8437890B2 (en) * 2009-03-05 2013-05-07 Massachusetts Institute Of Technology Integrated framework for vehicle operator assistance based on a trajectory prediction and threat assessment
CN104977933A (zh) * 2015-07-01 2015-10-14 吉林大学 一种自主驾驶车辆的区域型路径跟踪控制方法
CN107161207A (zh) * 2017-05-08 2017-09-15 江苏大学 一种基于主动安全的智能汽车轨迹跟踪控制***及控制方法
CN107942663A (zh) * 2017-11-21 2018-04-20 山东省计算中心（国家超级计算济南中心） 基于模糊pid算法的农机自动转向控制方法
CN108248605A (zh) * 2018-01-23 2018-07-06 重庆邮电大学 一种智能车辆轨迹跟随的横纵向协调控制方法
CN108732921A (zh) * 2018-04-28 2018-11-02 江苏大学 一种自动驾驶汽车横向可拓预瞄切换控制方法
CN108973769A (zh) * 2018-06-15 2018-12-11 吉林大学 一种全线控电动汽车路径跟踪控制方法
CN109318905A (zh) * 2018-08-22 2019-02-12 江苏大学 一种智能汽车路径跟踪混合控制方法
CN109795502A (zh) * 2018-09-27 2019-05-24 吉林大学 智能电动汽车路径跟踪模型预测控制方法
CN109857098A (zh) * 2018-12-11 2019-06-07 东南大学 一种基于mpc的自动驾驶农机轨迹跟踪***及方法
CN110598311A (zh) * 2019-09-06 2019-12-20 广东工业大学 一种自动驾驶车辆轨迹跟踪方法

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0788044A1 (en) * 1996-02-01 1997-08-06 MANNESMANN Aktiengesellschaft An automated guided vehicle navigation steering control system
US8437890B2 (en) * 2009-03-05 2013-05-07 Massachusetts Institute Of Technology Integrated framework for vehicle operator assistance based on a trajectory prediction and threat assessment
CN102358287A (zh) * 2011-09-05 2012-02-22 北京航空航天大学 一种用于车辆自动驾驶机器人的轨迹跟踪控制方法
CN104977933A (zh) * 2015-07-01 2015-10-14 吉林大学 一种自主驾驶车辆的区域型路径跟踪控制方法
CN107161207A (zh) * 2017-05-08 2017-09-15 江苏大学 一种基于主动安全的智能汽车轨迹跟踪控制***及控制方法
CN107942663A (zh) * 2017-11-21 2018-04-20 山东省计算中心（国家超级计算济南中心） 基于模糊pid算法的农机自动转向控制方法
CN108248605A (zh) * 2018-01-23 2018-07-06 重庆邮电大学 一种智能车辆轨迹跟随的横纵向协调控制方法
CN108732921A (zh) * 2018-04-28 2018-11-02 江苏大学 一种自动驾驶汽车横向可拓预瞄切换控制方法
CN108973769A (zh) * 2018-06-15 2018-12-11 吉林大学 一种全线控电动汽车路径跟踪控制方法
CN109318905A (zh) * 2018-08-22 2019-02-12 江苏大学 一种智能汽车路径跟踪混合控制方法
CN109795502A (zh) * 2018-09-27 2019-05-24 吉林大学 智能电动汽车路径跟踪模型预测控制方法
CN109857098A (zh) * 2018-12-11 2019-06-07 东南大学 一种基于mpc的自动驾驶农机轨迹跟踪***及方法
CN110598311A (zh) * 2019-09-06 2019-12-20 广东工业大学 一种自动驾驶车辆轨迹跟踪方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
Vehicle tracking control on piecewise-clothoidal trajectories by MPC with guaranteed error bounds;S. Di Cairano等;《2016 IEEE 55th Conference on Decision and Control (CDC)》;20161214;第709-714页 *
基于MPC算法的预判型ESP控制策略研究;李萧良;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20190215;第C035-237页 *
基于多点预瞄最优控制的智能车辆路径跟踪;黄海洋;《汽车技术》;20181231(第10期);第6-9页 *
基于模型预测控制的移动机器人路径跟踪控制;刘洋;《中国优秀硕士学位论文全文数据库 信息科技辑》;20160930(第09(2016)期);第I140-87页 *
智能汽车横向轨迹跟踪的离散时间模型预测控制;高洪波，等;《指挥与控制学报》;20181231;第4卷(第4期);第297-305页 *

Also Published As

Publication number Publication date
CN110989625A (zh) 2020-04-10

Similar Documents

Publication Publication Date Title
CN110989625B (zh) 一种车辆路径跟踪控制方法
CN110955250B (zh) 一种车辆倒车路径跟踪控制方法
CN110568758B (zh) 一种自动驾驶汽车的参数自适应横向运动lqr控制方法
CN112733270B (zh) 车辆行驶轨迹预测和轨迹偏离危险度评估的***与方法
CN108614426B (zh) 一种基于干扰观测器的多移动机器人编队鲁棒控制方法
CN112693449B (zh) 一种无人车辆极限工况下横纵向耦合控制方法
CN110398969A (zh) 自动驾驶车辆自适应预测时域转向控制方法及装置
CN108710294B (zh) 一种无人车路径跟踪前馈控制方法
CN109683614B (zh) 用于无人矿用车辆的车辆路径控制方法及装置
JP2008143269A (ja) 車両用制御装置
CN111538328B (zh) 一种用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法
CN107289938B (zh) 一种地面无人平台局部路径规划方法
CN111258218B (zh) 基于最大相关熵准则的智能车辆路径跟踪方法
Kebbati et al. Lateral control for autonomous wheeled vehicles: A technical review
CN113467231A (zh) 基于侧滑补偿ilos制导律的无人艇路径跟踪方法
CN113220021A (zh) 基于虚拟领导者的飞行编队协同自适应跟踪控制方法
CN106647264A (zh) 一种基于控制约束的扩展鲁棒h∞的无人机控制方法
CN113311845B (zh) 基于路径曲率的纯跟踪控制误差补偿方法及装置
Lorenz et al. A decoupled approach for trajectory generation for an unmanned rotorcraft
US20240067185A1 (en) Technology for dead time compensation during transverse and longitudinal guidance of a motor vehicle
CN113306573B (zh) 一种自动驾驶车辆学习型路径跟踪预测控制方法
CN116691668A (zh) 一种大型智能车辆的横向控制方法
CN114740845A (zh) 一种基于浸入与不变流形车辆跟踪控制方法
CN114047748A (zh) 用于农机自动驾驶的自适应前馈模型预测控制方法及***
CN113960998A (zh) 一种无人艇模糊预测控制***及方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant