CN110609539A - 路径跟踪控制方法、装置和系统及存储介质 - Google Patents
路径跟踪控制方法、装置和系统及存储介质 Download PDFInfo
- Publication number
- CN110609539A CN110609539A CN201811284321.3A CN201811284321A CN110609539A CN 110609539 A CN110609539 A CN 110609539A CN 201811284321 A CN201811284321 A CN 201811284321A CN 110609539 A CN110609539 A CN 110609539A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- steering angle
- increment
- model
- path
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 50
- 230000001133 acceleration Effects 0.000 claims abstract description 45
- 238000011156 evaluation Methods 0.000 claims abstract description 37
- 238000012545 processing Methods 0.000 claims abstract description 11
- 238000006243 chemical reaction Methods 0.000 claims description 54
- 230000006870 function Effects 0.000 claims description 49
- 239000000126 substance Substances 0.000 claims description 28
- 238000005070 sampling Methods 0.000 claims description 12
- 239000011159 matrix material Substances 0.000 claims description 11
- 238000004590 computer program Methods 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 8
- 238000010586 diagram Methods 0.000 description 6
- 238000010276 construction Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 4
- 238000009795 derivation Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 3
- 238000006467 substitution reaction Methods 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 150000001875 compounds Chemical class 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000008676 import Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
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/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- 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)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明实施例提供一种路径跟踪控制方法、装置和系统以及存储介质。路径跟踪控制方法包括：获取车辆在当前时刻的期望路径的路径数据；获取车辆在当前时刻的位姿数据；利用模型预测控制器处理路径数据和位姿数据，以预测车辆在当前时刻的转向角度，其中，在模型预测控制器中，状态量包括转向角度和转向角角速度，控制量包括转向角角加速度；以及基于当前时刻的转向角度控制车辆转向，以跟踪期望路径。上述方法、装置和系统及存储介质，在状态量中引入转向角角速度，在控制量中引入转向角角加速度，从而可以将转向角角速度、转向角角加速度添加到模型预测控制方法采用的评价函数的考量因素中。这使得兼顾路径跟踪的精度和平顺性成为可能。
Description
技术领域
本发明涉及车辆自动控制领域，更具体地涉及一种路径跟踪控制方法、装置和系统及存储介质。
背景技术
随着车辆智能化技术的发展，车辆自动转向控制技术逐渐成为车辆研究领域的一个热点。常见的车辆自动转向控制策略包括比例积分微分(Proportion IntegralDerivative，PID)，纯跟踪控制(Pure Pursuit，PP)，线性二次型调节器(Linear QuadraticRegulator，LQR)，模型预测控制(Model Predictive Control，MPC)等。其中，模型预测控制通过控制车辆转向角度(或说前轮偏角、前轮转向角)使车辆按照规划路径行驶。
现有的模型预测控制方法存在以下缺点：
1)模型预测控制中的控制量只考虑转向角度，忽略转向角角速度、转向角角加速度，因此算出的最优解存在转向角度不够平滑的现象，影响用户的乘车体验；
2)模型预测控制通常采用限制最大转向角角速度、转向角角加速度的方式来提升平滑性，但是在车速较快、转弯半径较小的情况下，容易造成转弯不及时等问题，从而带来较大的路径跟踪误差。
发明内容
考虑到上述问题而提出了本发明。本发明提供了一种路径跟踪控制方法、装置和系统以及存储介质。
根据本发明一方面，提供了一种路径跟踪控制方法。路径跟踪控制方法包括：获取车辆在当前时刻的期望路径的路径数据；获取车辆在当前时刻的位姿数据；利用模型预测控制器处理路径数据和位姿数据，以预测车辆在当前时刻的转向角度，其中，在模型预测控制器中，状态量包括转向角度和转向角角速度，控制量包括转向角角加速度；以及基于当前时刻的转向角度控制车辆转向，以跟踪期望路径。
示例性地，模型预测控制器包括预测模型和评价函数，方法还包括：构建模型预测控制器；其中，构建模型预测控制器包括：构建车辆的车辆线性离散模型；对车辆线性离散模型进行转化，使状态量包括转向角度和转向角角速度，控制量包括转向角角加速度，以获得转化模型；基于转化模型推导N步以内的状态量增量和控制量增量之间的关系，以获得预测模型，其中，N为大于0的整数；基于预测模型中N步以内的状态量增量和控制量增量之间的关系构建评价函数。
示例性地，车辆线性离散模型采用以下公式表示：
Δzk+1＝AkΔzk+BkΔδk
其中，
其中，Δzk和Δδk分别为车辆在t＝k时刻的线性离散模型状态量增量和线性离散模型控制量增量，(xk,yk)为车辆在t＝k时刻的位姿数据中的位置坐标，(Δxk，Δyk)为位置坐标(xk,yk)的增量，为车辆在t＝k时刻的位姿数据中的航向角，为航向角的增量，δk为车辆在t＝k时刻的转向角度，Δδk为转向角度δk的增量，L为车辆的轴距，vk为车辆在t＝k时刻的速度，T为采样时间间隔。
示例性地，转化模型采用以下公式表示：
其中，
其中，和分别为转换模型状态量增量和转化模型控制量增量，为车辆在t＝k时刻的转向角角速度的增量，为车辆在t＝k时刻的转向角角加速度的增量，I为单位矩阵。
示例性地，预测模型采用以下公式表示：
其中，
其中，ΔZ为N步以内的转化模型状态量增量，ΔU为N步以内的转化模型控制量增量，为转化模型初始状态量。
示例性地，评价函数采用以下公式表示：
其中，Q为状态量增量权重，Q0为第N步状态量增量权重，R为控制量权重。
示例性地，利用模型预测控制器处理路径数据和位姿数据，以预测车辆在当前时刻的转向角度包括：将路径数据和位姿数据代入预测模型，以计算N步以内的状态量增量和控制量增量之间的关系的表达式；将N步以内的状态量增量和控制量增量之间的关系的表达式代入评价函数，并对评价函数进行最小化，以获得N步以内的控制量增量；基于N步以内的控制量增量计算N步以内的状态量增量；基于N步以内的状态量增量中的第一个状态量增量确定当前时刻的转向角度增量；基于当前时刻的转向角度增量计算当前时刻的转向角度。
示例性地，构建车辆的车辆线性离散模型包括：构建车辆的车辆连续非线性运动学模型；使用泰勒级数，将车辆连续非线性运动学模型在期望路径上展开，并去除展开结果中的高阶项，以获得车辆线性模型；以及基于采样时间间隔将车辆线性模型离散化，以获得车辆线性离散模型。
根据本发明另一方面，提供了一种路径跟踪控制装置，包括：路径获取模块，用于获取车辆在当前时刻的期望路径的路径数据；位姿获取模块，用于获取车辆在当前时刻的位姿数据；预测控制模块，用于利用模型预测控制器处理路径数据和位姿数据，以预测车辆在当前时刻的转向角度，其中，在模型预测控制器中，状态量包括转向角度和转向角角速度，控制量包括转向角角加速度；以及转向控制模块，用于基于当前时刻的转向角度控制车辆转向，以跟踪期望路径。
根据本发明另一方面，提供了一种路径跟踪控制系统，包括处理器和存储器，其中，所述存储器中存储有计算机程序指令，所述计算机程序指令被所述处理器运行时用于执行上述路径跟踪控制方法。
根据本发明另一方面，提供了一种存储介质，在所述存储介质上存储了程序指令，所述程序指令在运行时用于执行上述路径跟踪控制方法。
根据本发明实施例的路径跟踪控制方法、装置和系统以及存储介质，当使用模型预测控制方法控制车辆时，在状态量中引入转向角角速度，在控制量中引入转向角角加速度，从而可以将转向角角速度、转向角角加速度添加到模型预测控制方法采用的评价函数的考量因素中。这种路径跟踪控制方法使得兼顾路径跟踪的精度和平顺性成为可能。
附图说明
通过结合附图对本发明实施例进行更详细的描述，本发明的上述以及其它目的、特征和优势将变得更加明显。附图用来提供对本发明实施例的进一步理解，并且构成说明书的一部分，与本发明实施例一起用于解释本发明，并不构成对本发明的限制。在附图中，相同的参考标号通常代表相同部件或步骤。
图1示出根据本发明一个实施例的路径跟踪控制方法的示意性流程图；
图2示出根据本发明一个实施例的期望路径以及车辆的示意图；
图3示出了根据本发明一个实施例的路径跟踪控制装置的示意性框图；以及
图4示出了根据本发明一个实施例的路径跟踪控制系统的示意性框图。
具体实施方式
为了使得本发明的目的、技术方案和优点更为明显，下面将参照附图详细描述根据本发明的示例实施例。显然，所描述的实施例仅仅是本发明的一部分实施例，而不是本发明的全部实施例，应理解，本发明不受这里描述的示例实施例的限制。
为了解决上述问题，本发明提出了一种路径跟踪控制方法。在该路径跟踪控制方法中，当使用模型预测控制方法控制车辆时，使状态量包括转向角度和转向角角速度，使控制量包括转向角角加速度，从而可以将转向角角速度、转向角角加速度添加到模型预测控制方法采用的评价函数的考量因素中。由此，在选取合适参数的情况下，可以使得车辆的控制能够兼顾路径跟踪的精度和平顺性。本发明实施例提供的路径跟踪控制方法可以应用于任何车辆的自动控制。
下面，结合图1描述本发明实施例提供的路径跟踪控制方法。图1示出根据本发明一个实施例的路径跟踪控制方法100的示意性流程图。如图1所示，路径跟踪方法100包括步骤S110、S120、S130和S140。
在步骤S110，获取车辆在当前时刻的期望路径的路径数据。
期望路径是车辆需要跟踪的行驶轨迹。图2示出根据本发明一个实施例的期望路径以及车辆的示意图。如图2所示，期望路径可以由一系列离散的点(即路径点)组成，其中每个路径点的属性数据可以包括该路径点的位置坐标车辆航向角和曲率半径ρ。位置坐标可以是在大地坐标系下的坐标。路径数据可以包括期望路径上的所有路径点的属性数据可选地，期望路径可以基于以下一项或者多项获得：车辆的规划路径、当前道路的延伸情况、用户的自定义路线等。在一个示例中，期望路径可以是通过车辆的车道线识别模块确定的道路中间线。在另一个示例中，期望路径可以是通过定位手段(例如GPS、激光雷达SLAM、二维码定位等)确定车辆位置，并结合路网文件得到的当前期望行驶轨迹。
在步骤S120，获取车辆在当前时刻的位姿数据。
如图2所示，车辆的位姿数据可以包括车辆的位置坐标(x，y)和航向角位置坐标(x，y)可以是在大地坐标系下的坐标。示例性地，位姿数据可以基于车辆的传感器采集到的传感器数据获得，所述感器可以包括一种或多种传感器，例如摄像头、激光雷达传感器、GPS传感器等。位姿数据可以通过一种或多种定位手段获得，例如GPS定位、即时定位与地图构建(SLAM)、二维码定位等。
在步骤S130，利用模型预测控制器处理路径数据和位姿数据，以预测车辆在当前时刻的转向角度，其中，在模型预测控制器中，状态量包括转向角度和转向角角速度，控制量包括转向角角加速度。
可以根据期望路径和车辆位姿，利用模型预测控制器来计算得到期望的转向角度，然后将期望的转向角度输出给车辆的转向电机控制模块，以控制车辆转向。在模型预测控制器中，可以使状态量包括转向角度和转向角角速度，控制量包括转向角角加速度，同时建立状态量与控制量之间的联系。这样，进行模型预测控制时，将转向角角速度和转向角角加速度加入考虑因素。
根据本发明实施例，模型预测控制器可以包括预测模型和评价函数，方法100还可以包括：构建模型预测控制器；其中，构建模型预测控制器包括：构建车辆的车辆线性离散模型；对车辆线性离散模型进行转化，使状态量包括转向角度和转向角角速度，控制量包括转向角角加速度，以获得转化模型；基于转化模型推导N步以内的状态量增量和控制量增量之间的关系，以获得预测模型，其中，N为大于0的整数；基于预测模型中N步以内的状态量增量和控制量增量之间的关系构建评价函数。
下面描述构建模型预测控制器的过程。
(1)、构建车辆线性离散模型。
示例性地，构建车辆的车辆线性离散模型可以包括：构建车辆的车辆连续非线性运动学模型；使用泰勒级数，将车辆连续非线性运动学模型在期望路径上展开，并去除展开结果中的高阶项，以获得车辆线性模型；以及基于采样时间间隔将车辆线性模型离散化，以获得车辆线性离散模型。
示例性地，可以以大地坐标系作为车辆参考系，使用(即车辆的位姿数据)来表示车辆的状态量，其中(x，y)为车辆的位置坐标，为车辆的航向角。由此，车辆连续非线性运动学模型可表示为：
其中，表示车辆的状态量随时间的变化率，δ为转向角度(控制量)，L为车辆的轴距，v为车辆的速度。
当车辆在期望路径(或说参考轨迹)上行驶时，可以定义此时的状态量为车辆状态量前馈此时对应的控制量为车辆控制量前馈 可使用如下公式求出：
其中，ρ为期望路径上的当前路径点的曲率半径。
当车辆不在期望路径上行驶时，可以首先查找期望路径上距离车辆最近的路径点并基于该路径点定义状态量前馈然后车辆的状态量可以由状态量前馈和状态量增量来表示，即：
状态量增量越小，表示车辆距离期望路径越近，跟踪精度越高。类似地，控制量δ可以由控制量前馈和控制量增量Δδ来表示，即：
其中，控制量增量Δδ可以使用本发明实施例提供的模型预测控制器计算获得。
假设车辆匀速运动，即v为常量，可以将上述车辆连续非线性运动学模型使用泰勒级数在期望路径上展开，并去除展开结果中的高阶项，获取车辆线性模型如下：
将上式写为矩阵形式如下：
随后，假设T为采样时间间隔，将车辆线性模型进行离散化，即将以下表达式代入上式：
其中，Δxk表示状态量增量Δx在t＝k时刻的大小。最终可以获得车辆线性离散模型，其用下式表示：
此车辆线性离散模型满足以下格式：
Δzk+1＝AkΔzk+BkΔδk
其中：
其中，Δzk和Δδk分别为车辆在t＝k时刻的线性离散模型状态量增量和线性离散模型控制量增量，(xk,yk)为车辆在t＝k时刻的位姿数据中的位置坐标，(Δxk，Δyk)为位置坐标(xk,yk)的增量，为车辆在t＝k时刻的位姿数据中的航向角，为航向角的增量，δk为车辆在t＝k时刻的转向角度，Δδk为转向角度δk的增量，L为车辆的轴距，vk为车辆在t＝k时刻的速度，T为采样时间间隔。
在车辆线性离散模型中，输入当前时刻的状态量增量、控制量增量，可以输出下一时刻的状态量增量。
(2)、转化模型。
上述推导中已经得到车辆线性离散模型Δzk+1＝AkΔzk+BkΔδk。假设控制量增量Δδk、控制量增量变化率控制量增量变化加速度之间的递推关系为：
将上式变形可获得：
将Δδk代入车辆线性离散模型，可以获得：
将上述等式统一为矩阵形式，获得转化模型：
其中，I为单位矩阵，满足性质IΔδk＝Δδk。此时，转化模型满足以下格式：
其中，
其中，和分别为转化模型状态量增量和转化模型控制量增量，为所述车辆在t＝k时刻的转向角角速度的增量，为所述车辆在t＝k时刻的转向角角加速度的增量，I为单位矩阵。
通过模型转化，可以将车辆线性离散模型由Δzk+1＝AkΔzk+BkΔδk转化为其中，状态量增量由Δzk转化成了控制量增量由Δδk转化成了示例性地，为了区分，可以将Δzk和Δδk分别称为线性离散模型状态量增量和线性离散模型控制量增量，将和分别称为转化模型状态量增量和转化模型控制量增量。因此，通过模型转化，在模型预测控制中不再将转向角度作为控制量，而是将转向角角加速度作为控制量，通过控制转向角角加速度来达到间接控制转向角度的目的。
(3)、预测模型。
上述推导中已经得到了转化模型假设则从开始的N步以内的转化模型状态量增量可由如下递推公式进行预测：
将所有的代入获得：
写成矩阵格式，如下：
以上矩阵满足以下格式：
其中：
其中，ΔZ为N步以内的转化模型状态量增量，ΔU为N步以内的转化模型控制量增量，为转化模型初始状态量。
(4)、最小化评价函数。
上述推导中已经得到转化模型的状态量增量、控制量增量分别为：
转化模型状态量增量中的Δxk、Δyk、越小则路线跟踪越精确，控制量越小则转向角角加速度越小，舒适度越高。为优化N步以内的状态量增量和控制量，可以设定评价函数JN如下：
其中，Q为状态量增量权重，Q0为第N步状态量增量权重，R为控制量权重。Q、Q0和R可以是预先设定好的。当评价函数JN达到最小时，状态量增量、控制量均达到最小，此时的预测路线为最优路线。
假设根据之前推导，评价函数JN可变形为：
其中，C是与ΔU无关的常量，不影响JN的最小值位置。评价函数JN满足以下格式：
JN＝ΔUTHΔU+2qTΔU+C
其中：
因此，评价函数JN是关于控制量增量ΔU的二次函数，可以使用MATLAB中的quadprog函数或YALMIP工具箱对此类函数的最小值位置进行求解，从而求出对应的控制量增量ΔU。
通过控制转向角角加速度来间接控制转向角度，并且将转向角角速度、转向角角加速度分别以状态量、控制量的形式添加到评价函数中，可以很方便地设置角转向角角加速度、转向角角速度的评价函数权重，方便跟踪轨迹的调整与优化，从而有利于兼顾路径跟踪的精度和平顺性。例如，当转向角角加速度的权重较大时，转向角度变化较慢，路径跟踪平顺性较好，但是跟踪精度较差；当状态量中坐标的权重较大时，路径跟踪精度较好，但是车辆平顺性较差。
下面描述转向角度的计算方式。
步骤S130可以包括：将路径数据和位姿数据代入预测模型，以计算N步以内的状态量增量和控制量增量之间的关系的表达式；将N步以内的状态量增量和控制量增量之间的关系的表达式代入评价函数，并对评价函数进行最小化，以获得N步以内的控制量增量；基于N步以内的控制量增量计算N步以内的状态量增量；基于N步以内的状态量增量中的第一个状态量增量确定当前时刻的转向角度增量；基于当前时刻的转向角度增量计算当前时刻的转向角度。
将路径数据和位姿数据代入上述预测模型，可以计算获得N步以内的状态量增量和控制量增量之间的关系的表达式。随后，可以通过将评价函数JN最小化来计算对应的转化模型控制量增量ΔU，而ΔU为N步以内所有转向角角加速度增量，可以将ΔU代入以计算N步以内的转化模型状态量增量ΔZ，其中，ΔZ的第一项为：
从ΔZ中可以得出在t＝0时刻(即当前时刻)的转向角度增量Δδ0。因此，当前时刻的转向角度δ0为：
通过以上方式，可以计算获得当前时刻的转向角度。
在步骤S440，基于当前时刻的转向角度控制车辆转向，以跟踪期望路径。
根据车辆的转向电机的安装位置，将当前时刻的转向角度做一定比例转换，可以得到转向电机的期望角度，并可以生成相应的控制信号，以控制转向电机转动。转向电机转动，可以带动车辆转向，进而可以跟踪期望路径。
在预测模型之前，不是直接使用车辆离散线性模型，而是先采用转化模型，将控制量由转向角度转化为转向角角加速度，通过控制转向角角加速度来间接控制转向角度。同时，将转向角角速度、转向角角加速度分别以状态量、控制量的形式添加到评价函数中，因此在最小化评价函数时，转向角角速度、转向角角加速度也都可以达到最小，从而能够达到兼顾路径跟踪的精度和平顺性的目的。
根据本发明实施例的路径跟踪控制方法，当使用模型预测控制方法控制车辆时，在状态量中引入转向角角速度，在控制量中引入转向角角加速度，从而可以将转向角角速度、转向角角加速度添加到模型预测控制方法采用的评价函数的考量因素中。这种路径跟踪控制方法使得兼顾路径跟踪的精度和平顺性成为可能。
根据本发明另一方面，提供一种路径跟踪控制装置。图3示出了根据本发明一个实施例的路径跟踪控制装置300的示意性框图。
如图3所示，根据本发明实施例的路径跟踪控制装置300包括路径获取模块310、位姿获取模块320、预测控制模块330和转向控制模块340。所述各个模块可分别执行上文中结合图1-2描述的路径跟踪控制方法的各个步骤/功能。以下仅对该路径跟踪控制装置300的各部件的主要功能进行描述，而省略以上已经描述过的细节内容。
路径获取模块310用于获取车辆在当前时刻的期望路径的路径数据。
位姿获取模块320用于获取车辆在当前时刻的位姿数据。
预测控制模块330用于利用模型预测控制器处理路径数据和位姿数据，以预测车辆在当前时刻的转向角度，其中，在模型预测控制器中，状态量包括转向角度和转向角角速度，控制量包括转向角角加速度。
转向控制模块340用于基于当前时刻的转向角度控制车辆转向，以跟踪期望路径。
示例性地，模型预测控制器包括预测模型和评价函数，路径跟踪控制装置还包括：预测器构建模块，用于构建模型预测控制器；其中，预测器构建模块包括：线性离散模型构建子模块，用于构建车辆的车辆线性离散模型；转化模块构建子模块，用于对车辆线性离散模型进行转化，使状态量包括转向角度和转向角角速度，控制量包括转向角角加速度，以获得转化模型；预测模块构建子模块，用于基于转化模型推导N步以内的状态量增量和控制量增量之间的关系，以获得预测模型，其中，N为大于0的整数；评价函数构建子模块，用于基于预测模型中N步以内的状态量增量和控制量增量之间的关系构建评价函数。
示例性地，车辆线性离散模型采用以下公式表示：
Δzk+1＝AkΔzk+BkΔδk
其中，
其中，Δzk和Δδk分别为车辆在t＝k时刻的线性离散模型状态量增量和线性离散模型控制量增量，(xk,yk)为车辆在t＝k时刻的位姿数据中的位置坐标，(Δxk，Δyk)为位置坐标(xk,yk)的增量，为车辆在t＝k时刻的位姿数据中的航向角，为航向角的增量，δk为车辆在t＝k时刻的转向角度，Δδk为转向角度δk的增量，L为车辆的轴距，vk为车辆在t＝k时刻的速度，T为采样时间间隔。
示例性地，转化模型采用以下公式表示：
其中，
其中，和分别为转换模型状态量增量和转化模型控制量增量，为车辆在t＝k时刻的转向角角速度的增量，为车辆在t＝k时刻的转向角角加速度的增量，I为单位矩阵。
示例性地，预测模型采用以下公式表示：
其中，
其中，ΔZ为N步以内的转化模型状态量增量，ΔU为N步以内的转化模型控制量增量，为转化模型初始状态量。
示例性地，评价函数采用以下公式表示：
其中，Q为状态量增量权重，Q0为第N步状态量增量权重，R为控制量权重。
示例性地，预测控制模块包括：第一代入子模块，用于将路径数据和位姿数据代入预测模型，以计算N步以内的状态量增量和控制量增量之间的关系的表达式；第二代入子模块，用于将N步以内的状态量增量和控制量增量之间的关系的表达式代入评价函数，并对评价函数进行最小化，以获得N步以内的控制量增量；第一计算子模块，用于基于N步以内的控制量增量计算N步以内的状态量增量；确定子模块，用于基于N步以内的状态量增量中的第一个状态量增量确定当前时刻的转向角度增量；第二计算子模块，用于基于当前时刻的转向角度增量计算当前时刻的转向角度。
示例性地，线性离散模型构建子模块包括：构建单元，用于构建车辆的车辆连续非线性运动学模型；展开单元，用于使用泰勒级数，将车辆连续非线性运动学模型在期望路径上展开，并去除展开结果中的高阶项，以获得车辆线性模型；以及离散化单元，用于基于采样时间间隔将车辆线性模型离散化，以获得车辆线性离散模型。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
图4示出了根据本发明一个实施例的路径跟踪控制系统400的示意性框图。路径跟踪控制系统400包括存储器410以及处理器420。
所述存储器410存储用于实现根据本发明实施例的路径跟踪控制方法中的相应步骤的计算机程序指令。
所述处理器420用于运行所述存储器410中存储的计算机程序指令，以执行根据本发明实施例的路径跟踪控制方法的相应步骤。
在一个实施例中，计算机程序指令被处理器420运行时用于执行以下步骤：获取车辆在当前时刻的期望路径的路径数据；获取车辆在当前时刻的位姿数据；利用模型预测控制器处理路径数据和位姿数据，以预测车辆在当前时刻的转向角度，其中，在模型预测控制器中，状态量包括转向角度和转向角角速度，控制量包括转向角角加速度；以及基于当前时刻的转向角度控制车辆转向，以跟踪期望路径。
示例性地，模型预测控制器包括预测模型和评价函数，计算机程序指令被处理器420运行时还用于执行以下步骤：构建模型预测控制器；其中，计算机程序指令被处理器420运行时所用于执行的构建模型预测控制器的步骤包括：构建车辆的车辆线性离散模型；对车辆线性离散模型进行转化，使状态量包括转向角度和转向角角速度，控制量包括转向角角加速度，以获得转化模型；基于转化模型推导N步以内的状态量增量和控制量增量之间的关系，以获得预测模型，其中，N为大于0的整数；基于预测模型中N步以内的状态量增量和控制量增量之间的关系构建评价函数。
示例性地，车辆线性离散模型采用以下公式表示：
ΔZk+1＝AkΔZk+BkΔδk
其中，
其中，Δzk和Δδk分别为车辆在t＝k时刻的线性离散模型状态量增量和线性离散模型控制量增量，(xk,yk)为车辆在t＝k时刻的位姿数据中的位置坐标，(Δxk，Δyk)为位置坐标(xk,yk)的增量，为车辆在t＝k时刻的位姿数据中的航向角，为航向角的增量，δk为车辆在t＝k时刻的转向角度，Δδk为转向角度δk的增量，L为车辆的轴距，vk为车辆在t＝k时刻的速度，T为采样时间间隔。
示例性地，转化模型采用以下公式表示：
其中，
其中，和分别为转换模型状态量增量和转化模型控制量增量，为车辆在t＝k时刻的转向角角速度的增量，为车辆在t＝k时刻的转向角角加速度的增量，I为单位矩阵。
示例性地，预测模型采用以下公式表示：
其中，
其中，ΔZ为N步以内的转化模型状态量增量，ΔU为N步以内的转化模型控制量增量，为转化模型初始状态量。
示例性地，评价函数采用以下公式表示：
其中，Q为状态量增量权重，Q0为第N步状态量增量权重，R为控制量权重。
示例性地，计算机程序指令被处理器420运行时所用于执行的利用模型预测控制器处理路径数据和位姿数据，以预测车辆在当前时刻的转向角度的步骤包括：将路径数据和位姿数据代入预测模型，以计算N步以内的状态量增量和控制量增量之间的关系的表达式；将N步以内的状态量增量和控制量增量之间的关系的表达式代入评价函数，并对评价函数进行最小化，以获得N步以内的控制量增量；基于N步以内的控制量增量计算N步以内的状态量增量；基于N步以内的状态量增量中的第一个状态量增量确定当前时刻的转向角度增量；基于当前时刻的转向角度增量计算当前时刻的转向角度。
示例性地，计算机程序指令被处理器420运行时所用于执行的构建车辆的车辆线性离散模型的步骤包括：构建车辆的车辆连续非线性运动学模型；使用泰勒级数，将车辆连续非线性运动学模型在期望路径上展开，并去除展开结果中的高阶项，以获得车辆线性模型；以及基于采样时间间隔将车辆线性模型离散化，以获得车辆线性离散模型。
此外，根据本发明实施例，还提供了一种存储介质，在所述存储介质上存储了程序指令，在所述程序指令被计算机或处理器运行时用于执行本发明实施例的路径跟踪控制方法的相应步骤，并且用于实现根据本发明实施例的路径跟踪控制装置中的相应模块。所述存储介质例如可以包括智能电话的存储卡、平板电脑的存储部件、个人计算机的硬盘、只读存储器(ROM)、可擦除可编程只读存储器(EPROM)、便携式紧致盘只读存储器(CD-ROM)、USB存储器、或者上述存储介质的任意组合。
在一个实施例中，所述程序指令在被计算机或处理器运行时可以使得计算机或处理器实现根据本发明实施例的路径跟踪控制装置的各个功能模块，并和/或者可以执行根据本发明实施例的路径跟踪控制方法。
在一个实施例中，所述程序指令在运行时用于执行以下步骤：获取车辆在当前时刻的期望路径的路径数据；获取车辆在当前时刻的位姿数据；利用模型预测控制器处理路径数据和位姿数据，以预测车辆在当前时刻的转向角度，其中，在模型预测控制器中，状态量包括转向角度和转向角角速度，控制量包括转向角角加速度；以及基于当前时刻的转向角度控制车辆转向，以跟踪期望路径。
示例性地，模型预测控制器包括预测模型和评价函数，所述程序指令在运行时还用于执行以下步骤：构建模型预测控制器；其中，所述程序指令在运行时所用于执行的构建模型预测控制器的步骤包括：构建车辆的车辆线性离散模型；对车辆线性离散模型进行转化，使状态量包括转向角度和转向角角速度，控制量包括转向角角加速度，以获得转化模型；基于转化模型推导N步以内的状态量增量和控制量增量之间的关系，以获得预测模型，其中，N为大于0的整数；基于预测模型中N步以内的状态量增量和控制量增量之间的关系构建评价函数。
示例性地，车辆线性离散模型采用以下公式表示：
Δzk+1＝AkΔzk+BkΔδk
其中，
其中，Δzk和Δδk分别为车辆在t＝k时刻的线性离散模型状态量增量和线性离散模型控制量增量，(xk,yk)为车辆在t＝k时刻的位姿数据中的位置坐标，(Δxk，Δyk)为位置坐标(xk,yk)的增量，为车辆在t＝k时刻的位姿数据中的航向角，为航向角的增量，δk为车辆在t＝k时刻的转向角度，Δδk为转向角度δk的增量，L为车辆的轴距，vk为车辆在t＝k时刻的速度，T为采样时间间隔。
示例性地，转化模型采用以下公式表示：
其中，
其中，和分别为转换模型状态量增量和转化模型控制量增量，为车辆在t＝k时刻的转向角角速度的增量，为车辆在t＝k时刻的转向角角加速度的增量，I为单位矩阵。
示例性地，预测模型采用以下公式表示：
其中，
其中，ΔZ为N步以内的转化模型状态量增量，ΔU为N步以内的转化模型控制量增量，为转化模型初始状态量。
示例性地，评价函数采用以下公式表示：
其中，Q为状态量增量权重，Q0为第N步状态量增量权重，R为控制量权重。
示例性地，所述程序指令在运行时所用于执行的利用模型预测控制器处理路径数据和位姿数据，以预测车辆在当前时刻的转向角度的步骤包括：将路径数据和位姿数据代入预测模型，以计算N步以内的状态量增量和控制量增量之间的关系的表达式；将N步以内的状态量增量和控制量增量之间的关系的表达式代入评价函数，并对评价函数进行最小化，以获得N步以内的控制量增量；基于N步以内的控制量增量计算N步以内的状态量增量；基于N步以内的状态量增量中的第一个状态量增量确定当前时刻的转向角度增量；基于当前时刻的转向角度增量计算当前时刻的转向角度。
示例性地，所述程序指令在运行时所用于执行的构建车辆的车辆线性离散模型的步骤包括：构建车辆的车辆连续非线性运动学模型；使用泰勒级数，将车辆连续非线性运动学模型在期望路径上展开，并去除展开结果中的高阶项，以获得车辆线性模型；以及基于采样时间间隔将车辆线性模型离散化，以获得车辆线性离散模型。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同系统来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
类似地，应当理解，为了精简本发明并帮助理解各个发明方面中的一个或多个，在对本发明的示例性实施例的描述中，本发明的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而，并不应将该本发明的系统解释成反映如下意图：即所要求保护的本发明要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说，如相应的权利要求书所反映的那样，其发明点在于可以用少于某个公开的单个实施例的所有特征的特征来解决相应的技术问题。因此，遵循具体实施方式的权利要求书由此明确地并入该具体实施方式，其中每个权利要求本身都作为本发明的单独实施例。
应该注意的是上述实施例对本发明进行说明而不是对本发明进行限制，并且本领域技术人员在不脱离所附权利要求的范围的情况下可设计出替换实施例。在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。本发明可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。
以上所述，仅为本发明的具体实施方式或对具体实施方式的说明，本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。本发明的保护范围应以权利要求的保护范围为准。
Claims (10)
1.一种路径跟踪控制方法，包括：
获取车辆在当前时刻的期望路径的路径数据；
获取所述车辆在所述当前时刻的位姿数据；
利用模型预测控制器处理所述路径数据和所述位姿数据，以预测所述车辆在所述当前时刻的转向角度，其中，在所述模型预测控制器中，状态量包括转向角度和转向角角速度，控制量包括转向角角加速度；以及
基于所述当前时刻的转向角度控制所述车辆转向，以跟踪所述期望路径。
2.如权利要求1所述的方法，其中，所述模型预测控制器包括预测模型和评价函数，所述方法还包括：构建所述模型预测控制器；
其中，所述构建所述模型预测控制器包括：
构建所述车辆的车辆线性离散模型；
对所述车辆线性离散模型进行转化，使状态量包括转向角度和转向角角速度，控制量包括转向角角加速度，以获得转化模型；
基于所述转化模型推导N步以内的状态量增量和控制量增量之间的关系，以获得所述预测模型，其中，N为大于0的整数；
基于所述预测模型中N步以内的状态量增量和控制量增量之间的关系构建所述评价函数。
3.如权利要求2所述的方法，其中，所述车辆线性离散模型采用以下公式表示：
Δzk+1＝AkΔzk+BkΔδk
其中，
其中，Δzk和Δδk分别为所述车辆在t＝k时刻的线性离散模型状态量增量和线性离散模型控制量增量，(xk,yk)为所述车辆在t＝k时刻的位姿数据中的位置坐标，(Δxk，Δyk)为位置坐标(xk,yk)的增量，为所述车辆在t＝k时刻的位姿数据中的航向角，为航向角的增量，δk为所述车辆在t＝k时刻的转向角度，Δδk为转向角度δk的增量，L为所述车辆的轴距，vk为所述车辆在t＝k时刻的速度，T为采样时间间隔。
4.如权利要求3所述的方法，其中，所述转化模型采用以下公式表示：
其中，
其中，和分别为转换模型状态量增量和转化模型控制量增量，为所述车辆在t＝k时刻的转向角角速度的增量，为所述车辆在t＝k时刻的转向角角加速度的增量，I为单位矩阵。
5.如权利要求4所述的方法，其中，所述预测模型采用以下公式表示：
其中，
其中，ΔZ为N步以内的转化模型状态量增量，ΔU为N步以内的转化模型控制量增量，为转化模型初始状态量。
6.如权利要求5所述的方法，其中，所述评价函数采用以下公式表示：
其中，Q为状态量增量权重，Q0为第N步状态量增量权重，R为控制量权重。
7.如权利要求2至6任一项所述的方法，其中，所述利用模型预测控制器处理所述路径数据和所述位姿数据，以预测所述车辆在所述当前时刻的转向角度包括：
将所述路径数据和所述位姿数据代入所述预测模型，以计算N步以内的状态量增量和控制量增量之间的关系的表达式；
将所述N步以内的状态量增量和控制量增量之间的关系的表达式代入所述评价函数，并对所述评价函数进行最小化，以获得N步以内的控制量增量；
基于所述N步以内的控制量增量计算N步以内的状态量增量；
基于所述N步以内的状态量增量中的第一个状态量增量确定所述当前时刻的转向角度增量；
基于所述当前时刻的转向角度增量计算所述当前时刻的转向角度。
8.一种路径跟踪控制装置，包括：
路径获取模块，用于获取车辆在当前时刻的期望路径的路径数据；
位姿获取模块，用于获取所述车辆在所述当前时刻的位姿数据；
预测控制模块，用于利用模型预测控制器处理所述路径数据和所述位姿数据，以预测所述车辆在所述当前时刻的转向角度，其中，在所述模型预测控制器中，状态量包括转向角度和转向角角速度，控制量包括转向角角加速度；以及
转向控制模块，用于基于所述当前时刻的转向角度控制所述车辆转向，以跟踪所述期望路径。
9.一种路径跟踪控制系统，包括处理器和存储器，其中，所述存储器中存储有计算机程序指令，所述计算机程序指令被所述处理器运行时用于执行如权利要求1至7任一项所述的路径跟踪控制方法。
10.一种存储介质，在所述存储介质上存储了程序指令，所述程序指令在运行时用于执行如权利要求1至7任一项所述的路径跟踪控制方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811284321.3A CN110609539B (zh) | 2018-10-31 | 2018-10-31 | 路径跟踪控制方法、装置和系统及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811284321.3A CN110609539B (zh) | 2018-10-31 | 2018-10-31 | 路径跟踪控制方法、装置和系统及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110609539A true CN110609539A (zh) | 2019-12-24 |
| CN110609539B CN110609539B (zh) | 2023-06-23 |
Family
ID=68888841
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811284321.3A Active CN110609539B (zh) | 2018-10-31 | 2018-10-31 | 路径跟踪控制方法、装置和系统及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110609539B (zh) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112422184A (zh) * | 2020-09-28 | 2021-02-26 | 东方红卫星移动通信有限公司 | 一种用于空间光通信的粗指向装置的转动控制方法及装置 |
| CN113495559A (zh) * | 2020-03-18 | 2021-10-12 | 百度（美国）有限责任公司 | 用于自动驾驶的基于学习的控制器 |
| CN113734182A (zh) * | 2020-05-29 | 2021-12-03 | 比亚迪股份有限公司 | 一种车辆自适应控制方法及其装置 |
| WO2023142794A1 (zh) * | 2022-01-30 | 2023-08-03 | 中国第一汽车股份有限公司 | 一种车辆控制方法、装置、设备以及存储介质 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108334086A (zh) * | 2018-01-25 | 2018-07-27 | 江苏大学 | 一种基于软约束二次规划mpc的无人驾驶车辆路径跟踪控制方法 |
| CN108426581A (zh) * | 2018-01-08 | 2018-08-21 | 深圳市易成自动驾驶技术有限公司 | 车辆位姿确定方法、装置及计算机可读存储介质 |
-
2018
- 2018-10-31 CN CN201811284321.3A patent/CN110609539B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108426581A (zh) * | 2018-01-08 | 2018-08-21 | 深圳市易成自动驾驶技术有限公司 | 车辆位姿确定方法、装置及计算机可读存储介质 |
| CN108334086A (zh) * | 2018-01-25 | 2018-07-27 | 江苏大学 | 一种基于软约束二次规划mpc的无人驾驶车辆路径跟踪控制方法 |
Non-Patent Citations (4)
| Title |
|---|
| 余如: "无人驾驶车辆的自动转向控制", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 * |
| 刘卉等: "基于模型预测控制的轨迹跟踪算法研究", 《控制工程》 * |
| 孙传扬等: "极限工况下自动驾驶车辆转向控制器的设计", 《华南理工大学学报(自然科学版)》 * |
| 李金良等: "基于模型预测控制的无人驾驶汽车的轨迹跟踪", 《汽车工程师》 * |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113495559A (zh) * | 2020-03-18 | 2021-10-12 | 百度（美国）有限责任公司 | 用于自动驾驶的基于学习的控制器 |
| CN113734182A (zh) * | 2020-05-29 | 2021-12-03 | 比亚迪股份有限公司 | 一种车辆自适应控制方法及其装置 |
| CN113734182B (zh) * | 2020-05-29 | 2023-11-14 | 比亚迪股份有限公司 | 一种车辆自适应控制方法及其装置 |
| CN112422184A (zh) * | 2020-09-28 | 2021-02-26 | 东方红卫星移动通信有限公司 | 一种用于空间光通信的粗指向装置的转动控制方法及装置 |
| CN112422184B (zh) * | 2020-09-28 | 2022-05-27 | 东方红卫星移动通信有限公司 | 一种用于空间光通信的粗指向装置的转动控制方法及装置 |
| WO2023142794A1 (zh) * | 2022-01-30 | 2023-08-03 | 中国第一汽车股份有限公司 | 一种车辆控制方法、装置、设备以及存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110609539B (zh) | 2023-06-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110609539A (zh) | 路径跟踪控制方法、装置和系统及存储介质 | |
| KR102292277B1 (ko) | 자율 주행 차량에서 3d cnn 네트워크를 사용하여 솔루션을 추론하는 lidar 위치 추정 | |
| CN109489675A (zh) | 用于自动驾驶车辆的基于成本的路径规划 | |
| CN109491377A (zh) | 用于自动驾驶车辆的基于dp和qp的决策和规划 | |
| CN109491376A (zh) | 用于自动驾驶车辆的基于动态规划和梯度下降的决策和规划 | |
| CN101871782B (zh) | 基于set2fnn的gps/mems-ins组合导航系统定位误差预测方法 | |
| JP2018534205A (ja) | 車両の運動を制御する方法、及び車両の制御システム | |
| KR20200096411A (ko) | 자율 주행 차량에서 rnn 및 lstm을 사용하여 시간적 평활화를 수행하는 lidar 위치 추정 | |
| CN110389580A (zh) | 用于规划自动驾驶车辆的路径的漂移校正的方法 | |
| US20200363816A1 (en) | System and method for controlling autonomous vehicles | |
| JP2019505423A (ja) | 比例、積分及び微分（ｐｉｄ）コントローラを用いた自律走行車のステアリング制御方法及びシステム | |
| CN112180925B (zh) | Agv轨迹跟随方法、装置及处理设备 | |
| WO2017167387A1 (en) | Method for predicting a motion of an object | |
| CN110688920A (zh) | 一种无人驾驶控制方法、装置及服务器 | |
| CN112977478B (zh) | 一种车辆控制方法及系统 | |
| CN113805610B (zh) | 一种轨迹跟踪控制方法、装置及存储介质 | |
| CN114942642A (zh) | 一种无人驾驶汽车轨迹规划方法 | |
| CN115639823A (zh) | 崎岖起伏地形下机器人地形感知与移动控制方法及系统 | |
| Hartman et al. | Robust finite-time parameter estimation using a hybrid systems framework | |
| CN116520847A (zh) | 路径规划方法、装置、移动机器人、电子设备及存储介质 | |
| US20230288886A1 (en) | Model-Based Control with Uncertain Motion Model | |
| CN115454053A (zh) | 自动导引车控制方法、系统、装置和计算机设备 | |
| Vincke et al. | Static and dynamic fusion for outdoor vehicle localization | |
| Zhang et al. | Improved grid mapping technology based on Rao-Blackwellized particle filters and the gradient descent algorithm | |
| CN115008450A (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 | ||
| REG | Reference to a national code |
Ref country code: HK Ref legal event code: DE Ref document number: 40011446 Country of ref document: HK |
|
| REG | Reference to a national code |
Ref country code: HK Ref legal event code: DE Ref document number: 40019587 Country of ref document: HK |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |