CN114940164B - 一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及*** - Google Patents

一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及*** Download PDF

Info

Publication number
CN114940164B
CN114940164B CN202210552336.3A CN202210552336A CN114940164B CN 114940164 B CN114940164 B CN 114940164B CN 202210552336 A CN202210552336 A CN 202210552336A CN 114940164 B CN114940164 B CN 114940164B
Authority
CN
China
Prior art keywords
vehicle
representing
pose
point
kth
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
CN202210552336.3A
Other languages
English (en)
Other versions
CN114940164A (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.)
Chongqing University of Post and Telecommunications
Original Assignee
Chongqing University of Post and Telecommunications
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 Chongqing University of Post and Telecommunications filed Critical Chongqing University of Post and Telecommunications
Priority to CN202210552336.3A priority Critical patent/CN114940164B/zh
Publication of CN114940164A publication Critical patent/CN114940164A/zh
Application granted granted Critical
Publication of CN114940164B publication Critical patent/CN114940164B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
    • B60W30/06—Automatic manoeuvring for parking
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W50/0098—Details of control systems ensuring comfort, safety or stability not otherwise provided for
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W60/00—Drive control systems specially adapted for autonomous road vehicles
    • B60W60/001—Planning or execution of driving tasks
    • B60W60/0011—Planning or execution of driving tasks involving control alternatives for a single driving scenario, e.g. planning several paths to avoid obstacles
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0019—Control system elements or transfer functions
    • B60W2050/0028—Mathematical models, e.g. for simulation
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0019—Control system elements or transfer functions
    • B60W2050/0028—Mathematical models, e.g. for simulation
    • B60W2050/0031—Mathematical model of the vehicle
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0043—Signal treatments, identification of variables or parameters, parameter estimation or state estimation
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B60W2520/10—Longitudinal speed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2554/00—Input parameters relating to objects
    • B60W2554/80—Spatial relation or speed relative to objects
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/04—Constraint-based CAD
    • 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)
  • Automation & Control Theory (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Theoretical Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Computer Hardware Design (AREA)
  • Geometry (AREA)
  • General Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Evolutionary Computation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明属于无人驾驶车辆技术领域，具体涉及一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及***；该方法包括：获取全局泊车路径；根据车辆状态参数构建整车运动学模型；根据整车运动学模型和全局泊车路径，以泊车所耗时间最短为优化目标，以自动泊车起始点位姿、目标点位姿、行驶车速、整车运动学以及避障安全距离为基本约束参数，构建行驶轨迹优化模型；采用Levenberg‑Marquardt(LM)方法对行驶轨迹优化模型进行求解，得到优化的局部行驶轨迹；根据优化的局部行驶轨迹实现泊车场景下的完整车辆行驶轨迹优化；本发明提高了在泊车场景下，无人驾驶车辆行驶的安全性，实用性高。

Description

一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及***
技术领域
本发明属于无人驾驶车辆技术领域，具体涉及一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及***。
背景技术
近年来随着汽车智能化的不断演进，无人驾驶车辆技术受到越来越多的关注。自主规划是无人驾驶车辆的关键属性，它不仅要求基于算法找到一条从起始点到目标点的无碰撞路径，还要求无人驾驶车辆在复杂环境中完成自适应避障。自主泊车是无人驾驶车辆最可能实现商用化的场景之一，然而仅使用全局路径规划算法，如Dijkstrs算法、A*算法、RRT算法等不能满足泊车场景下的无人驾驶车辆驾驶需求，而已有的局部路径规划算法即全局路径优化算法，如Lattice Planner算法、DWA算法和TEB算法等大多适用于两轮机器人，较少考虑无人驾驶车辆在实际行驶过程中的运动学、速度、加速度、允许转向半径及避障安全距离等约束条件，对于无人驾驶车辆的使用具有较大的安全隐患。
综上所述，亟需一种一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及***，以解决泊车场景下，现有全局路径规划算法及其优化算法不能满足无人驾驶车辆安全行驶需求的问题。
发明内容
针对现有技术存在的不足，本发明提出了一种基于用户意识和演化博弈的信息传播控制方法，该方法包括：
S1：获取全局泊车路径；
S2：根据车辆状态参数构建整车运动学模型；
S3：根据整车运动学模型和全局泊车路径，以泊车所耗时间最短为优化目标，以自动泊车起始点位姿、目标点位姿、行驶车速、整车运动学以及避障安全距离为基本约束参数，构建行驶轨迹优化模型；
S4：采用LM方法对行驶轨迹优化模型进行求解，得到优化的局部行驶轨迹；根据优化的局部行驶轨迹实现泊车场景下的完整车辆行驶轨迹优化。
优选的，整车运动学模型为：
Figure BDA0003655390910000021
其中，x(t)表示t时刻车辆后轴中点横坐标，y(t)表示t时刻车辆后轴中点纵坐标，
Figure BDA0003655390910000022
表示t时刻车辆航向角，δ(t)表示t时刻前轮转向角，L表示轴距，v(t)表示t时刻后轴车速。
优选的，行驶车速约束为：
Figure BDA0003655390910000023
其中，vk表示车辆在第k个位姿点的行驶车速，Rk表示车辆在第k个位姿点的转向半径，
Figure BDA0003655390910000024
表示车辆在第k个位姿点的与前一位姿点的航向角变化量，ΔTk表示车辆在第k个位姿点的与前一位姿点的时间间隔，Υ(Sk,Sk+1)表示车辆从位姿点Sk到位姿点Sk+1的速度方向，dk表示车辆在位姿点Sk指向位姿点Sk+1的方向向量。
优选的，整车运动学约束为：
Figure BDA0003655390910000025
Figure BDA0003655390910000026
其中，hk(Sk+1,Sk)表示从位姿点Sk到位姿点Sk+1的连续迁移指数，
Figure BDA0003655390910000031
表示第k个位姿点的车辆航向角，/>
Figure BDA0003655390910000032
表示第k+1个位姿点的车辆航向角，dk表示车辆在位姿点Sk指向位姿点Sk+1的方向向量，Rk表示车辆在第k个位姿点的转向半径，/>
Figure BDA0003655390910000033
表示车辆在第k个位姿点的与前一位姿点的航向角变化量，Rmin表示车辆的最小转向半径。
优选的，避障安全距离约束为：
ok(Sk)＝[σ(Sk,O1),σ(Sk,O2),…,σ(Sk,Om)]T-[σmin,σmin,…,σmin]T≥0
其中，ok(Sk)表示车辆在位姿点Sk的避障安全距离，σ(Sk,Om)表示车辆在位姿点Sk与障碍物Om间的避障安全距离，σmin表示最小避障安全距离。
优选的，行驶轨迹优化模型为：
Figure BDA0003655390910000034
s.t.S1＝Sc,Sn＝Sf,0≤ΔTk≤ΔTmax
Figure BDA0003655390910000035
ok(Sk)≥0
vk(Sk+1,Sk,ΔTk)≥0,(k＝1,2,…,n-1)
ak(Sk+2,Sk+1,Sk,ΔTk+1,ΔT)≥0,(k＝2,3,…,n-2)
a1(S2,S1,ΔT1)≥0,an(Sn,Sn-1,ΔTn-1)≥0
其中，ΔTk表示车辆在第k个位姿点的与前一位姿点的时间间隔，ΔTmax表示位姿点间最大时间间隔，n表示位姿点数量，S1表示初始位姿，Sc表示车辆初始状态，Sn表示目标位姿，Sf表示车辆最终状态，hk(Sk+1,Sk)表示从位姿点Sk到位姿点Sk+1的连续迁移指数，
Figure BDA0003655390910000036
表示从位姿点Sk到位姿点Sk+1的允许转向半径，ok(Sk)表示车辆在位姿点Sk的避障安全距离，vk(Sk+1,Sk,ΔTk)表示车辆在第k个位姿点的行驶车速约束，ak(Sk+2,Sk+1,Sk,ΔTk+1,ΔT)表示车辆在第k个位姿点的加速度，a1(S2,S1,ΔT1)表示初始位姿加速度，an(Sn,Sn-1,ΔTn-1)表示目标位姿加速度。
优选的，对行驶轨迹优化模型进行求解的过程包括：引入权重因子，对行驶轨迹优化模型中的约束进行转化，得到行驶轨迹优化目标函数；在图优化理论g2o框架下，采用LM方法对行驶轨迹优化目标函数进行求解，得到最优解向量；根据最优解向量，得到优化的局部行驶轨迹。
进一步的，行驶轨迹优化目标函数为：
Figure BDA0003655390910000041
Figure BDA0003655390910000042
其中，B*表示行驶轨迹最优解向量，
Figure BDA0003655390910000043
表示中间函数，S1表示初始位姿，Sn表示目标位姿，n表示位姿点数量，ΔTk表示车辆在第k个位姿点的与前一位姿点的时间间隔，φ(hk,ξh)表示连续迁移指数约束，/>
Figure BDA0003655390910000044
表示转向半径约束，χ(vk,ξv)表示行驶车速约束，χ(ok,ξo)表示避障安全距离约束，χ(ak,ξa)表示第k个位姿点的加速度约束，χ(an,ξa)表示目标位姿加速度约束；hk表示车辆在第k个位姿点的连续迁移指数，/>
Figure BDA0003655390910000045
表示车辆在第k个位姿点的允许转向半径，vk表示车辆在第k个位姿点的行驶车速，ok表示车辆在第k个位姿点的避障安全距离，ak表示车辆在第k个位姿点的加速度，an表示车辆在目标位姿点的加速度；ξh表示连续迁移指数约束权重，ξr表示转向半径约束权重，ξv表示行驶车速约束权重，ξo表示避障安全距离约束权重，ξa表示加速度约束权重。
一种面向泊车场景的无人驾驶车辆行驶轨迹优化***，包括：信息获取模块、行驶轨迹优化模型构建模块、解算模块和控制模块；
所述信息获取模块用于获取全局泊车路径；
所述行驶轨迹优化模型构建模块用于根据全局泊车路径和车辆状态参数构建行驶轨迹优化模型；
所述解算模块用于对行驶轨迹优化模型进行解算，得到优化的车辆行驶轨迹；
所述控制模块用于根据优化的车辆行驶轨迹进行泊车操作。
本发明的有益效果为：本发明设计的面向泊车场景的无人驾驶车辆行驶轨迹优化方法基于局部路径规划算法思想，以泊车所耗时间最短为优化目标，以自动泊车起始点位姿、目标点位姿、行驶车速、整车运动学以及避障安全距离为基本约束条件，构建行驶轨迹优化模型，通过对行驶轨迹优化模型进行求解，得到优化的局部行驶轨迹，进而实现完整的车辆行驶轨迹优化；与采用现有的全局路径规划算法及其优化算法对车辆轨迹进行优化相比，本发明优化的车辆行驶轨迹考虑了实际泊车过程中符合整车行驶特点的约束条件，满足分层导航体系机构中的局部优化需求，提高了在泊车场景下，无人驾驶车辆行驶的安全性，实用性高。
附图说明
图1为本发明中面向泊车场景的无人驾驶车辆行驶轨迹优化方法流程图；
图2为本发明中车辆运动学模型示意图；
图3为本发明中运动学位移姿态迁移图；
图4为本发明中泊车行驶轨迹优化效果示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明提出了一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法，如图1所示，所述方法包括以下内容：
S1：获取全局泊车路径。
以已知的全局泊车路径为基础，***若干个整车位姿点，并在相邻位姿点之间加入时间间隔信息。
S2：根据车辆状态参数构建整车运动学模型。
车辆状态参数包括车辆后轴中点坐标、前轮转向角、轴距、车辆航向角、后轴车速(行驶车速)等。如图2所示，以车辆后轴中点为整车状态参考点，整车的位姿状态向量为
Figure BDA0003655390910000061
整车运动学模型表达式为：
Figure BDA0003655390910000062
其中，x(t)表示t时刻车辆后轴中点横坐标，y(t)表示t时刻车辆后轴中点纵坐标，
Figure BDA0003655390910000063
表示t时刻车辆航向角，δ(t)表示t时刻前轮转向角，L表示轴距，v(t)表示t时刻后轴车速。
整车的控制输入向量为U＝[v,δ]T，由上式可得：
Figure BDA0003655390910000064
其中，Υ(·)表示速度方向。
S3：根据整车运动学模型和全局泊车路径，以泊车所耗时间最短为优化目标，以自动泊车起始点位姿、目标点位姿、行驶车速、整车运动学以及避障安全距离为基本约束条件，构建行驶轨迹优化模型。
根据整车的结构特点，其转向半径满足R＝L/tan(δ)。与最大转向角δmax对应的转向半径便是最小转向半径Rmin。用Sk,k＝1,2,...n表示满足控制输入的一系列离散位姿点，整车直线运动和转向运动之间过渡过程的两个位姿Sk和Sk+1需满足同一个恒定曲率的圆弧，进而避免整车行驶过程中曲率不连续现象的出现。根据整车运动学模型和全局泊车路径，可得到整车的运动学位姿迁移过程，如图3所示，在k时刻，整车位姿Sk与向量dk＝[xk+1-xk,yk+1-yk,0]T之间的夹角用θk,k表示。在k+1时刻，整车位姿Sk+1与向量dk＝[xk+1-xk,yk+1-yk,0]T之间的夹角用θk,k+1表示。当且仅当θk,k与θk,k+1相等时方可满足整车位姿的连续迁移过程，即符合整车的运动学特点，其必须满足：
Figure BDA0003655390910000071
车辆的转向半径约束为：
Figure BDA0003655390910000072
其中，hk(Sk+1,Sk)表示从位姿点Sk到位姿点Sk+1的连续迁移指数，
Figure BDA0003655390910000073
表示第k个位姿点的车辆航向角，/>
Figure BDA0003655390910000074
表示第k+1个位姿点的车辆航向角，dk表示车辆在位姿点Sk指向位姿点Sk+1的方向向量，Rk表示车辆在第k个位姿点的转向半径，/>
Figure BDA0003655390910000075
表示车辆在第k个位姿点的与前一位姿点的航向角变化量，Rmin表示车辆的最小转向半径。
在泊车场景下，整车运动学约束为连续迁移指数约束和转向半径约束。
行驶车速表示为：
Figure BDA0003655390910000076
构建方向向量
Figure BDA0003655390910000077
在距离向量dk上的投影：
Figure BDA0003655390910000078
其中，vk表示车辆在第k个位姿点的行驶车速，Rk表示车辆在第k个位姿点的转向半径，
Figure BDA0003655390910000079
表示车辆在第k个位姿点的与前一位姿点的航向角变化量，ΔTk表示车辆在第k个位姿点的与前一位姿点的时间间隔，Υ(Sk,Sk+1)表示车辆从位姿点Sk到位姿点Sk+1的速度方向，dk表示车辆在位姿点Sk指向位姿点Sk+1的方向向量，/>
Figure BDA00036553909100000710
表示比例因子。
行驶车速约束为：
Figure BDA00036553909100000711
进一步的，角速度
Figure BDA0003655390910000081
但是其必须满足|ωk|≤ωmax，其中ωmax＝vmax/Rmin，因此行驶车速约束表达式为：vk(Sk+1,Sk,ΔTk)＝[vmax-|vk|,ωmax-|ωk|]T。ak(Sk+2,Sk+1,Sk,ΔTk+1,ΔTk)表示加速度约束，其中，加速度表达式为：
Figure BDA0003655390910000082
其中，ak表示车辆在第k个位姿点的加速度。
k时刻的车速由k和k+1时刻共同决定，所以k时刻的加速度由k、k+1和k+2时刻共同决定，进而加速度约束的表达式可表示为：
ak(Sk+2,Sk+1,Sk,ΔTk+1,ΔTk)＝amax-|ak|
加速度约束为：
ak(Sk+2,Sk+1,Sk,ΔTk+1,ΔT)≥0,(k＝2,3,…,n-2)
a1(S2,S1,ΔT1)≥0,an(Sn,Sn-1,ΔTn-1)≥0
避障安全距离约束表示为：
ok(Sk)＝[σ(Sk,O1),σ(Sk,O2),…,σ(Sk,Om)]T-[σmin,σmin,…,σmin]T≥0
其中，ok(Sk)表示车辆在位姿点Sk的避障安全距离，σ(Sk,Om)表示车辆在位姿点Sk与障碍物Om间的避障安全距离，σmin表示最小避障安全距离。
采用Timed-Elastic-Band(TEB)思想即局部路径规划算法思想，构建行驶轨迹优化模型，其表达式为：
Figure BDA0003655390910000083
s.t.S1＝Sc,Sn＝Sf,0≤ΔTk≤ΔTmax
Figure BDA0003655390910000084
ok(Sk)≥0
vk(Sk+1,Sk,ΔTk)≥0,(k＝1,2,…,n-1)
ak(Sk+2,Sk+1,Sk,ΔTk+1,ΔT)≥0,(k＝2,3,…,n-2)
a1(S2,S1,ΔT1)≥0,an(Sn,Sn-1,ΔTn-1)≥0
其中，ΔTk表示车辆在第k个位姿点的与前一位姿点的时间间隔，ΔTmax表示位姿点间最大时间间隔，n表示位姿点数量，S1表示初始位姿，Sc表示车辆初始状态，Sn表示目标位姿，Sf表示车辆最终状态，hk(Sk+1,Sk)表示从位姿点Sk到位姿点Sk+1的连续迁移指数，
Figure BDA0003655390910000091
表示从位姿点Sk到位姿点Sk+1的允许转向半径，ok(Sk)表示车辆在位姿点Sk的避障安全距离，vk(Sk+1,Sk,ΔTk)表示车辆在第k个位姿点的行驶车速，ak(Sk+2,Sk+1,Sk,ΔTk+1,ΔT)表示车辆在第k个位姿点的加速度，a1(S2,S1,ΔT1)表示初始位姿加速度，an(Sn,Sn-1,ΔTn-1)表示目标位姿加速度。
S4：采用LM方法对行驶轨迹优化模型进行求解，得到优化的局部行驶轨迹；根据优化的局部行驶轨迹实现泊车场景下的完整车辆行驶轨迹优化。
引入权重因子，对行驶轨迹优化模型中的约束进行转化，得到行驶轨迹优化目标函数。
将精确的非线性规划问题转化为近似非线性平方优化问题，并针对行驶轨迹优化模型中的等式和不等式约束，引入与之对应的权重因子。
对于等式约束，例如，连续迁移指数约束，其转化约束为：
Figure BDA0003655390910000092
对于不等式约束，例如，行驶速度约束，其转化约束为：
Figure BDA0003655390910000093
/>
对于其他约束，同样采用上述方法，此处不再赘述。
最终，将非线性规划问题表达为具有目标函数的优化问题，得到行驶轨迹优化目标函数即：
Figure BDA0003655390910000094
Figure BDA0003655390910000095
其中，B*表示行驶轨迹最优解向量，
Figure BDA0003655390910000096
表示中间函数，φ(hk,ξh)表示连续迁移指数约束，/>
Figure BDA0003655390910000097
表示转向半径约束，χ(vk,ξv)表示行驶车速约束，χ(ok,ξo)表示避障安全距离约束，χ(ak,ξa)表示第k个位姿点的加速度约束，χ(an,ξa)表示目标位姿加速度约束；hk表示车辆在第k个位姿点的连续迁移指数，/>
Figure BDA0003655390910000098
表示车辆在第k个位姿点的允许转向半径，vk表示车辆在第k个位姿点的行驶车速，ok表示车辆在第k个位姿点的避障安全距离，ak表示车辆在第k个位姿点的加速度，an表示车辆在目标位姿点的加速度；ξh表示连续迁移指数约束权重，ξr表示转向半径约束权重，ξv表示行驶车速约束权重，ξo表示避障安全距离约束权重，ξa表示加速度约束权重。
在图优化理论g2o框架下，采用LM方法对行驶轨迹优化目标函数进行求解，得到最优解向量；根据最优解向量，得到优化的局部行驶轨迹。
具体的，利用Levenberg-Marquardt(LM)方法即列文伯格-马夸尔特法具有鲁棒性和高效计算的特点，求解行驶轨迹优化目标函数。进一步，在图优化理论g2o框架下，求解获取的解向量B，进而得到最优解向量B*；分析最优解向量，得到优化的局部行驶轨迹。
根据优化的局部行驶轨迹实现泊车场景下的完整车辆行驶轨迹优化；在无人驾驶情况下，车辆根据优化的完整车辆行驶轨迹进行自动泊车。
本发明还提供一种面向泊车场景的无人驾驶车辆行驶轨迹优化***，该***用于执行本发明设计的一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法；包括：信息获取模块、行驶轨迹优化模型构建模块、解算模块和控制模块；
所述信息获取模块用于获取全局泊车路径；
所述行驶轨迹优化模型构建模块用于根据全局泊车路径和车辆状态参数构建行驶轨迹优化模型；
所述解算模块用于对行驶轨迹优化模型进行解算，得到优化的车辆行驶轨迹；
所述控制模块用于根据优化的车辆行驶轨迹进行泊车操作。
如图4所示，从图中可以看出的，优化后车辆行驶轨迹不再以路径的长度为单一维度考虑，而是进一步考虑了行驶轨迹与障碍物之间的安全距离，换言之，在同一车速约束条件下更符合实际行驶安全的需要。
本发明设计的面向泊车场景的无人驾驶车辆行驶轨迹优化方法基于局部路径规划算法思想，以泊车所耗时间最短为优化目标，以自动泊车起始点位姿、目标点位姿、行驶车速、整车运动学以及避障安全距离为基本约束条件，构建行驶轨迹优化模型，通过对行驶轨迹优化模型进行求解，得到优化的局部行驶轨迹，进而实现完整的车辆行驶轨迹优化；与采用现有的全局路径规划算法及其优化算法对车辆轨迹进行优化相比，本发明优化的车辆行驶轨迹，考虑了实际泊车过程中符合整车行驶特点的约束条件，满足分层导航体系机构中的局部优化需求，提高了在泊车场景下，无人驾驶车辆行驶的安全性，实用性高。
以上所举实施例，对本发明的目的、技术方案和优点进行了进一步的详细说明，所应理解的是，以上所举实施例仅为本发明的优选实施方式而已，并不用以限制本发明，凡在本发明的精神和原则之内对本发明所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法，其特征在于，包括：
S1：获取全局泊车路径；
S2：根据车辆状态参数构建整车运动学模型；
S3：根据整车运动学模型和全局泊车路径，以泊车所耗时间最短为优化目标，以自动泊车起始点位姿、目标点位姿、行驶车速、整车运动学以及避障安全距离为基本约束参数，构建行驶轨迹优化模型；
S4：引入权重因子，对行驶轨迹优化模型中的约束进行转化，得到行驶轨迹优化目标函数；在图优化理论g2o框架下，采用LM方法对行驶轨迹优化目标函数进行求解，得到最优解向量；根据最优解向量，得到优化的局部行驶轨迹；根据优化的局部行驶轨迹实现泊车场景下的完整车辆行驶轨迹优化。
2.根据权利要求1所述的一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法，其特征在于，整车运动学模型为：
Figure FDA0004148375270000011
其中，x(t)表示t时刻车辆后轴中点横坐标，y(t)表示t时刻车辆后轴中点纵坐标，
Figure FDA0004148375270000012
表示t时刻车辆航向角，δ(t)表示t时刻前轮转向角，L表示轴距，v(t)表示t时刻后轴车速。
3.根据权利要求1所述的一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法，其特征在于，行驶车速约束为：
Figure FDA0004148375270000013
其中，vk表示车辆在第k个位姿点的行驶车速，Rk表示车辆在第k个位姿点的转向半径，
Figure FDA0004148375270000014
表示车辆在第k个位姿点的与前一位姿点的航向角变化量，ΔTk表示车辆在第k个位姿点的与前一位姿点的时间间隔，Υ(Sk,Sk+1)表示车辆从位姿点Sk到位姿点Sk+1的速度方向，dk表示车辆在位姿点Sk指向位姿点Sk+1的方向向量。
4.根据权利要求1所述的一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法，其特征在于，整车运动学约束为：
Figure FDA0004148375270000021
Figure FDA0004148375270000022
其中，hk(Sk+1,Sk)表示从位姿点Sk到位姿点Sk+1的连续迁移指数，
Figure FDA0004148375270000024
表示第k个位姿点的车辆航向角，/>
Figure FDA0004148375270000023
表示第k+1个位姿点的车辆航向角，dk表示车辆在位姿点Sk指向位姿点Sk+1的方向向量，Rk表示车辆在第k个位姿点的转向半径，/>
Figure FDA0004148375270000025
表示车辆在第k个位姿点的与前一位姿点的航向角变化量，Rmin表示车辆的最小转向半径。
5.根据权利要求1所述的一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法，其特征在于，避障安全距离约束为：
ok(Sk)＝[σ(Sk,O1),σ(Sk,O2),…,σ(Sk,Om)]T-[σmin,σmin,…,σmin]T≥0
其中，T表示矩阵的转置，ok(Sk)表示车辆在位姿点Sk的避障安全距离，σ(Sk,Om)表示车辆在位姿点Sk与第m个障碍物Om间的避障安全距离，σmin表示最小避障安全距离。
6.根据权利要求1所述的一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法，其特征在于，行驶轨迹优化模型为：
Figure FDA0004148375270000036
s.t.S1＝Sc，Sn＝Sf，0≤ΔTk≤ΔTmax
Figure FDA0004148375270000037
ok(Sk)≥0
vk(Sk+1，Sk，ΔTk)≥0，(k＝1，2，…，n-1)
ak(Sk+2，Sk+1，Sk，ΔTk+1，ΔTk)≥0，(k＝2，3，…，n-2)
a1(S2，S1，ΔT1)≥0，an(Sn，Sn-1，ΔTn-1)≥0
其中，ΔTk表示车辆在第k个位姿点的与前一位姿点的时间间隔，ΔTmax表示位姿点间最大时间间隔，n表示位姿点数量，S1表示初始位姿，Sc表示车辆初始状态，Sn表示目标位姿，Sf表示车辆最终状态，hk(Sk+1，Sk)表示从位姿点Sk到位姿点Sk+1的连续迁移指数，
Figure FDA0004148375270000031
表示从位姿点Sk到位姿点Sk+1的允许转向半径，ok(Sk)表示车辆在位姿点Sk的避障安全距离，vk(Sk+1，Sk，ΔTk)表示车辆在第k个位姿点的行驶车速，ak(Sk+2，Sk+1，Sk，ΔTk+1，ΔTk)表示车辆在第k个位姿点的加速度，a1(S2，S1，ΔT1)表示初始位姿加速度，an(Sn，Sn-1，ΔTn-1)表示目标位姿加速度；B表示需要优化的参数集；ΔTk+1表示车辆从位姿Sk+1到Sk+2所需的时间，ΔT1表示从位姿S1到S2所需的时间，ΔTn-1表示从位姿Sn-1到Sn所需的时间。
7.根据权利要求1所述的一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法，其特征在于，行驶轨迹优化目标函数为：
Figure FDA0004148375270000032
Figure FDA0004148375270000033
其中，B*表示行驶轨迹最优解向量，B表示需要优化的参数集，
Figure FDA0004148375270000034
表示中间函数，S1表示初始位姿，Sn表示目标位姿，n表示位姿点数量，ΔTk表示车辆在第k个位姿点的与前一位姿点的时间间隔，φ(hk，ξh)表示连续迁移指数约束，/>
Figure FDA0004148375270000035
表示转向半径约束，χ(vk，ξv)表示行驶车速约束，χ(ok，ξo)表示避障安全距离约束，χ(ak，ξa)表示第k个位姿点的加速度约束，χ(an，ξa)表示目标位姿加速度约束；hk表示车辆在第k个位姿点的连续迁移指数，/>
Figure FDA0004148375270000041
表示车辆在第k个位姿点的允许转向半径，vk表示车辆在第k个位姿点的行驶车速，ok表示车辆在第k个位姿点的避障安全距离，ak表示车辆在第k个位姿点的加速度，an表示车辆在目标位姿点的加速度；ξh表示连续迁移指数约束权重，ξr表示转向半径约束权重，ξv表示行驶车速约束权重，ξo表示避障安全距离约束权重，ξa表示加速度约束权重。
8.一种面向泊车场景的无人驾驶车辆行驶轨迹优化***，该***用于执行权利要求1-7所述的任意一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法，其特征在于，包括：信息获取模块、行驶轨迹优化模型构建模块、解算模块和控制模块；
所述信息获取模块用于获取全局泊车路径；
所述行驶轨迹优化模型构建模块用于根据全局泊车路径和车辆状态参数构建行驶轨迹优化模型；
所述解算模块用于对行驶轨迹优化模型进行解算，得到优化的车辆行驶轨迹；
所述控制模块用于根据优化的车辆行驶轨迹进行泊车操作。
CN202210552336.3A 2022-05-20 2022-05-20 一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及*** Active CN114940164B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210552336.3A CN114940164B (zh) 2022-05-20 2022-05-20 一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210552336.3A CN114940164B (zh) 2022-05-20 2022-05-20 一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及***

Publications (2)

Publication Number Publication Date
CN114940164A CN114940164A (zh) 2022-08-26
CN114940164B true CN114940164B (zh) 2023-05-23

Family

ID=82909706

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210552336.3A Active CN114940164B (zh) 2022-05-20 2022-05-20 一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及***

Country Status (1)

Country Link
CN (1) CN114940164B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115909790A (zh) * 2022-11-07 2023-04-04 重庆邮电大学 一种增强型自主代客泊车停车场外区域的行车***及其应用方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109606354B (zh) * 2018-10-18 2020-07-28 同济大学 一种基于分层规划的自动泊车方法及辅助***
CN113361152A (zh) * 2020-03-05 2021-09-07 北京京东乾石科技有限公司 轨迹规划方法、轨迹规划装置、存储介质与电子设备
CN111674390B (zh) * 2020-05-09 2021-07-02 惠州市德赛西威汽车电子股份有限公司 一种自动泊车路径规划的避障方法及泊车路径规划***
CN112572416A (zh) * 2020-11-20 2021-03-30 芜湖格陆博智能科技有限公司 一种矿区无人驾驶车辆的泊车方法及***
CN112937555A (zh) * 2021-03-08 2021-06-11 华南理工大学 一种基于车辆运动学模型的平行泊车轨迹规划方法

Also Published As

Publication number Publication date
CN114940164A (zh) 2022-08-26

Similar Documents

Publication Publication Date Title
CN111845774B (zh) 一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法
Tang et al. An improved kinematic model predictive control for high-speed path tracking of autonomous vehicles
CN108227491B (zh) 一种基于滑模神经网络的智能车轨迹跟踪控制方法
Guo et al. Design of automatic steering controller for trajectory tracking of unmanned vehicles using genetic algorithms
CN110568758B (zh) 一种自动驾驶汽车的参数自适应横向运动lqr控制方法
CN113320542B (zh) 一种自动驾驶车辆的跟踪控制方法
CN110244735B (zh) 移动机器人跟踪预定轨迹的启发式动态规划控制方法
CN110162046B (zh) 基于事件触发型模型预测控制的无人车路径跟随方法
CN111522351B (zh) 水下机器人三维编队及避障方法
CN114379583B (zh) 一种基于神经网络动力学模型的自动驾驶车辆轨迹跟踪***及方法
CN111506099B (zh) 一种无人机高度智能控制***及方法
CN113220021B (zh) 基于虚拟领导者的飞行编队协同自适应跟踪控制方法
CN111538241A (zh) 一种平流层飞艇水平轨迹智能控制方法
CN113050651B (zh) 智能车自主行驶路径跟踪的时滞控制方法及***
CN114940164B (zh) 一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及***
CN109947112B (zh) 两轮自平衡车直线定点运动的最优时间轨迹规划方法
CN114138022B (zh) 一种基于精英鸽群智能的无人机集群分布式编队控制方法
CN112947073B (zh) 一种基于模型预测控制的舰载机滑行轨迹跟踪方法
Nurmaini Differential drive mobile robot control using variable fuzzy universe of discourse
Beji et al. Motion generation and adaptive control method of automated guided vehicles in road following
Fukushima et al. An optimal control method based on the energy flow equation
Guo et al. Research on path-tracking control method of intelligent vehicle based on adaptive two-point preview
Kanjanawanishkul Coordinated path following for mobile robots using a virtual structure strategy with model predictive control
CN114995426A (zh) 一种基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制方法、***及车载控制设备
CN112161626B (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