CN115456297B - 自动泊车路径优化方法及装置 - Google Patents

自动泊车路径优化方法及装置 Download PDF

Info

Publication number
CN115456297B
CN115456297B CN202211190937.0A CN202211190937A CN115456297B CN 115456297 B CN115456297 B CN 115456297B CN 202211190937 A CN202211190937 A CN 202211190937A CN 115456297 B CN115456297 B CN 115456297B
Authority
CN
China
Prior art keywords
path
optimization
track
ref
initial
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
CN202211190937.0A
Other languages
English (en)
Other versions
CN115456297A (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.)
Yuanfeng Technology Co Ltd
Original Assignee
Yuanfeng Technology Co Ltd
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 Yuanfeng Technology Co Ltd filed Critical Yuanfeng Technology Co Ltd
Priority to CN202211190937.0A priority Critical patent/CN115456297B/zh
Publication of CN115456297A publication Critical patent/CN115456297A/zh
Application granted granted Critical
Publication of CN115456297B publication Critical patent/CN115456297B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q10/00—Administration; Management
    • G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
    • G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem
    • 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

  • Business, Economics & Management (AREA)
  • Human Resources & Organizations (AREA)
  • Engineering & Computer Science (AREA)
  • Strategic Management (AREA)
  • Economics (AREA)
  • Marketing (AREA)
  • Game Theory and Decision Science (AREA)
  • Entrepreneurship & Innovation (AREA)
  • Development Economics (AREA)
  • Operations Research (AREA)
  • Quality & Reliability (AREA)
  • Tourism & Hospitality (AREA)
  • Physics & Mathematics (AREA)
  • General Business, Economics & Management (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种自动泊车路径优化方法及装置，该方法包括：获取障碍物信息，并生成初始路径规划轨迹，将初始路径规划轨迹的每一段路径转换为若干个离散的初始轨迹点Pi ref(xi ref,yi ref)；以路径规划轨迹中每一段路径的各个轨迹点坐标Pi(xi，yi)为优化变量，以限制最大曲率、起始地与目的地车辆位姿固定、各个轨迹点不与障碍物碰撞为优化约束条件，以路径的平滑度、路径长度和偏离初始轨迹点距离为优化目标，构建序列二次规划问题，进行迭代求解以生成轨迹点信息；依据轨迹点信息生成优化后的规划路径；其中，优化目标的数值为各个优化目标的权重求和，且路径平滑度中，起始地坐标和目的地坐标的权重均大于起始地坐标和目的地坐标之间其他轨迹点坐标的权重。

Description

自动泊车路径优化方法及装置
技术领域
本发明涉及一种自动驾驶领域，尤其涉及自动泊车的路径优化。
背景技术
在自动泊车路径规划之后，需要对路径进行平滑处理，使曲率无突变。目前主要有曲线过渡法以及优化方法，曲线过渡一般只对曲率凸变处进行光滑处理，而无梯度优化算法无法保证求解效率，并且目前大部分平滑算法可能存在局部轨迹与目标点偏差较大，过度平滑，导致车辆与障碍物存在碰撞的风险。由于路径的曲率约束为非线性，二次规划无法添加曲率约束，故现有优化后的规划路径往往具有曲率突变、车辆过度平滑导致车辆与障碍物碰撞的问题。
为此，在专利公开文件CN112277932A中，公开了一种自动驾驶的泊车路径规划方法，将路径曲率约束引入优化函数的约束条件和优化目标中，使得优化后的泊车轨迹和曲率连续，车辆沿轨迹行驶时方向盘变化连续，不需要停车。然而，这种优化方法只是对初始路径的少量点进行优化，不能保证整条路径满足约束条件和只局限于车辆初始位置限制一定范围的平行泊车规划问题，同时只能针对该专利中的路径规划方法(几何法)再进行优化，不具有通用性，难以在实际泊车路径规划中使用，且优化时，并不能解决车辆静止时大幅度打方向盘的问题。
故，急需一种可解决上述问题的自动泊车路径规划方法及装置。
发明内容
本发明的目的是提供一种自动泊车路径优化方法及装置，可有效减小优化后的规划路径过度平滑导致车辆与障碍物存在碰撞的风险。
为了实现上述目的，本发明公开了一种自动泊车路径优化方法，包括：获取障碍物信息，并生成初始路径规划轨迹，将初始路径规划轨迹的每一段路径转换为若干个离散的初始轨迹点Pi ref(xi ref, yi ref)，i=1、2、3…n；以路径规划轨迹的每一段路径中各个轨迹点坐标Pi（xi，yi）为优化变量，以限制最大曲率、起始地与目的地车辆位姿固定、各个轨迹点不与障碍物碰撞为优化约束条件，以路径的平滑度、路径长度和偏离初始轨迹点距离为优化目标，构建序列二次规划问题，对序列二次规划问题进行迭代求解以生成轨迹点信息；依据所述轨迹点信息生成优化后的规划路径；其中，所述优化目标的数值为路径平滑度、路径长度以及偏离初始轨迹点距离的权重求和，且路径平滑度中，起始地坐标P1（x1，y1）和目的地坐标Pn（xn，yn）的权重均大于起始地坐标和目的地坐标之间轨迹点坐标P2（x2，y2）到Pn-1（xn-1，yn-1）的权重。
较佳地，限制最大曲率的优化约束条件包括：
；
其中，向量，Cmax为曲率限制值，/>为相邻轨迹点之间间距的均值。
较佳地，对序列二次规划问题的迭代求解时，将限制最大曲率的优化约束函数进行线性展开为：
，/>，/>为序列二次规划上一步优化迭代得到的解；对序列二次规划问题的迭代求解时，还限制所述优化变量的运动极限。
较佳地，各个轨迹点不与障碍物碰撞的约束条件为：起始地坐标P1（x1，y1）和目的地坐标Pn（xn，yn）不变，轨迹点坐标P2（x2，y2）到Pn-1（xn-1，yn-1）与初始轨迹点Pi ref(xi ref,yi ref)之间X坐标和Y坐标的偏差值均小于等于，di为路径规划轨迹的每一段路径中各个初始轨迹点处对应车辆与障碍物最近的距离。
较佳地，航向角约束条件为：
，/>和/>分别为依据起始地车辆位姿和目的地车辆位姿设定的起始地和目的地处的航向角给定值，P0（x0，y0）为约束车辆起始地航向角不变添加的辅助点，Pn+1（xn+1，yn+1）为约束车辆目的地航向角不变添加的辅助点。
较佳地，路径平滑度代价为：
；
路径长度代价为：
；
偏离初始轨迹点距离代价为：
；
Psmooth、Plength和Pref分别为平滑度、长度和相对初始轨迹点距离的惩罚因子，为轨迹点Pi平滑度的权重，/>中为轨迹点Pi偏离初始轨迹点距离的权重；
优化目标为：
；
H是半正定对称矩阵，g为梯度向量，。
较佳地，对序列二次规划问题进行迭代求解具体包括：给定所述优化变量一个运动极限并进行优化近似问题构造，对所述优化近似问题进行序列二次规划求解以获得新的优化变量，依据所述新的优化变量更新优化变量，计算优化目标的数值和优化约束条件的约束值，依据所述优化目标的数值判断是否收敛，依据所述优化约束条件的约束值判断优化约束条件合格时是否合格，并在收敛且优化约束条件合格时输出优化变量，在未收敛且优化约束条件合格时进行下一迭代序列二次规划问题求解，在优化约束条件未合格时缩减运动极限并进行下一迭代序列二次规划问题求解。
具体地，缩减运动极限具体为：对违犯约束的优化约束条件进行预设倍率的缩减。
更佳地，在依据所述优化约束条件的约束值判断优化约束条件合格时，还判断多次迭代中优化变量是否均处于优化变量运动区间边缘，若是则扩大运动极限增大，以提高求解效率。
具体地，扩大运动极限具体为：对多次迭代优化中优化变量处于边缘时对预设倍率的扩大。
具体地，判断所述优化目标和优化约束条件是否合格时，如果违犯限制最大曲率条件，则对优化目标中，增加路径平滑度代价的权重。
具体地，采用Hybrid A*算法进行路径规划以生成所述初始路径规划轨迹，并将初始路径规划轨迹的每一段路径分解为n+2个离散的初始轨迹点Pi ref(xi ref, yi ref)，i=0、1、2、3…n、n+1，P0 ref(x0 ref, y0 ref)为路径规划轨迹的每一段路径中约束车辆起始地航向角不变添加的辅助点，Pn+1 ref(xn+1 ref, yn+1 ref)为路径规划轨迹的每一段路径中约束车辆目的地航向角不变添加的辅助点。使用Hybrid A*算法进行初始路径规划，可适合于任何车位的泊车，适用性广。当然，本发明还可以采用其他路径规划方法获得初始路径规划轨迹，以进行后续的路径光滑处理。
本发明还公开了一种自动泊车路径优化装置，包括：检测模块，获取障碍物信息；初始路径规划模块，生成初始路径规划轨迹，将初始路径规划轨迹的每一段路径转换为若干个初始轨迹点Pi ref(xi ref, yi ref)；问题构件模块，以路径规划轨迹的每一段路径中各个轨迹点坐标Pi（xi，yi）为优化变量，以限制最大曲率、起始地与目的地车辆位姿固定、各个轨迹点不与障碍物碰撞为优化约束条件，以路径的平滑度、路径长度和偏离初始轨迹点距离为优化目标，构建序列二次规划问题，i=1、2、3…n；问题求解模块，对序列二次规划问题进行迭代求解以生成轨迹点信息；路径优化模块，依据所述轨迹点信息生成优化后的规划路径；其中，所述优化目标的数值为路径平滑度、路径长度以及偏离初始轨迹点距离的权重求和，且路径平滑度中，起始地坐标P1（x1，y1）和目的地坐标Pn（xn，yn）的权重均大于起始地坐标和目的地坐标之间轨迹点坐标P2（x2，y2）到Pn-1（xn-1，yn-1）的权重。
本发明还公开了一种自动泊车路径优化装置，包括：一个或多个处理器；存储器；以及一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被配置成由一个或多个处理器执行以实现如上所述的自动泊车路径优化方法。
本发明还公开了一种计算机可读存储介质，包括与具有存储器的电子设备结合使用的计算机程序，所述计算机程序可被处理器执行以实现如上所述的自动泊车路径优化方法。
与现有技术相比，本发明不但以曲率约束作为路径优化时序列二次规划条件之一，还在将优化目标设置为路径平滑度等多个目标的权重求和，使得优化好的路径更适应于不同环境，且将路径平滑度的权重起始地和目的地设置的更大，有效减小优化后的规划路径过度平滑导致车辆与障碍物存在碰撞的风险。再者，本发明对初始路径规划轨迹的每一段路径进行离散处理，获得全部路径规划轨迹的离散点作为轨迹点，以对整个路径规划轨迹进行全方位优化，确保优化好的整条线路均符合约束条件，同时，还使得本发明适用于多种路径规划方法获得的初始规划轨迹后的路径光滑处理，适用性强。
附图说明
图1是本发明自动泊车路径优化方法的流程图。
图2是本发明初始路径规划轨迹中一段路径的坐标图。
图3是本发明序列二次规划求解获得的路径规划轨迹中一段路径的坐标图。
图4是本发明序列二次规划求解的流程图。
图5是本发明自动泊车路径优化装置的结构图。
具体实施方式
为详细说明本发明的技术内容、构造特征、所实现目的及效果，以下结合实施方式并配合附图详予说明。
参考图1，本发明公开了一种自动泊车路径优化方法，包括步骤S11-S14。
S11获取障碍物信息，并生成初始路径规划轨迹，将初始路径规划轨迹的每一段路径转换为若干个离散的初始轨迹点Pi ref(xi ref, yi ref)。i=1、2、3…n。具体获取障碍物信息方法，可以通过相关的传感器检测获得，也可以通过外部设备输入，也可以直接获取，例如依据地图信息获取。
其中，生成的初始路径规划轨迹可以有一段也可以由多段，具体将整个初始路径规划轨迹按照行驶方向分为相应段路径。
其中，本发明将初始路径规划轨迹的每一段路径转换为若干个离散的初始轨迹点Pi ref(xi ref, yi ref)。初始轨迹点具有若干个，为路径规划轨迹整条路径上的离散点。其中，每一段路径中轨迹点的数目可以相等也可以不等，轨迹点的具体数目由设置的相邻轨迹点间距范围决定或者依据路径的实际复杂情况决定。
参考图2，本发明采用Hybrid A*算法进行路径规划以生成所述初始路径规划轨迹，并将初始路径规划轨迹的每一段路径分解为n+2个离散的初始轨迹点Pi ref(xi ref,yi ref)，i=0、1、2、3…n、n+1。
本发明将Hybrid A*算法与序列二次规划算法结合，对Hybrid A*算法获取的路径规划轨迹进行全方位优化，可以确保优化好的整条线路均符合约束条件，且适合于任何车位的泊车，适用性广。其中，初始轨迹点P0 ref(x0 ref, y0 ref)为初始路径规划轨迹的每一段路径中，约束车辆的起始地航向角不变添加的辅助点，P0 ref(x0 ref, y0 ref)与P2 ref(x2 ref, y2 ref)连线的斜率与车辆初始的航向角对应的斜率相等，P0 ref(x0 ref, y0 ref)与P1 ref(x1 ref, y1 ref)的间距与P1 ref(x1 ref, y1 ref)与P2 ref(x2 ref, y2 ref)的间距相等。
其中，初始轨迹点Pn+1 ref(xn+1 ref, yn+1 ref)为初始路径规划轨迹的每一段路径中，约束车辆200目的地航向角不变添加的辅助点，Pn-1 ref(xn-1 ref, yn-1 ref)与Pn+1 ref(xn+1 ref,yn+1 ref)连线的斜率与车辆目的地的航向角对应的斜率相等，Pn+1 ref(xn+1 ref, yn+1 ref)与Pn ref(xn ref, yn ref)的间距与P1 ref(x1 ref, y1 ref)与P2 ref(x2 ref, y2 ref)的间距相等。
S12以路径规划轨迹的每一段路径中各个轨迹点坐标Pi（xi，yi）为优化变量，以限制最大曲率、起始地与目的地车辆位姿固定（起始地和目的地坐标点固定，航向角固定）、各个轨迹点不与障碍物碰撞为优化约束条件，以路径的平滑度、路径长度和偏离初始轨迹点距离（路径规划轨迹的每一段路径中轨迹点坐标与初始路径规划轨迹的对应段路径中初始轨迹点坐标的偏差值）为优化目标，构建序列二次规划问题。其中，i=0、1、2、3…n、n+1。
参考图3，本发明通过序列二次规划算法对初始路径规划轨迹的每一段路径进行优化，以获得优化后的路径规划轨迹对应段路径轨迹点。路径规划轨迹的每一段路径中，车辆起始地坐标为P1（x1，y1），车辆目的地坐标为Pn（xn，yn），轨迹点P0（x0，y0）为约束车辆起始地航向角不变添加的辅助点，轨迹点Pn+1（xn+1，yn+1）为约束车辆目的地航向角不变添加的辅助点。
其中，所述优化目标的数值为路径平滑度、路径长度以及偏离初始轨迹点距离的权重求和。不同优化目标（包括路径平滑度、路径长度以及偏离初始轨迹点距离）的权重可依据实际泊车环境进行调节，并非固定不变的数值，例如在障碍物与各个初始轨迹点处对应车辆200与障碍物最近的距离di的数值小于预设值时，在初始权重的基础上增加偏离初始轨迹点距离的权重和/或减小路径长度的权重。
步骤S12中还包括依据初始轨迹点与障碍物之间距离计算距离di。其中，求解车辆200四条边线段分别与障碍物线段的距离，取其中的最小值，作为车辆200与障碍物的最短距离di。
同一优化目标中，不同轨迹点的权重可以相同也可以不同，依据实际需要设置。例如，本实施例在路径平滑度中，起始地坐标P1（x1，y1）和目的地坐标Pn（xn，yn）的权重均大于起始地坐标和目的地坐标之间轨迹点坐标P2（x2，y2）到Pn-1（xn-1，yn-1）的权重。
本实施例路径平滑度中，轨迹点坐标P2（x2，y2）到Pn-1（xn-1，yn-1）之间的权重可以相同，也可以将路径平滑度中，前m个轨迹点的权重逐渐减小，后m个轨迹点的权重逐渐增加，例如将轨迹点P1（x1，y1）到轨迹点P3（x3，y3）的权重逐渐减小，轨迹点Pn-2（xn-2，yn-2）到轨迹点Pn（xn，yn）的权重逐渐增加，m为大于等于1的整数，可以为1、2、3等，其数值小于等于n/2。
S13对序列二次规划问题进行迭代求解以生成轨迹点信息。
S14依据所有段路径中所述轨迹点信息中的轨迹点P1（x1，y1）到 Pn（xn，yn）生成优化后的规划路径。
当初始路径规划轨迹具有多段路径时，步骤S14中，依据所有段的路径中的轨迹点P1（x1，y1）到 Pn（xn，yn）生成最后优化后的总的规划路径，其中，可以直接依据所有段的始路径规划轨迹中的轨迹点P1（x1，y1）到 Pn（xn，yn）生成最后优化后的规划路径，也可以先依据每一段的始路径规划轨迹中的轨迹点P1（x1，y1）到 Pn（xn，yn）生成每一段优化后的规划路径，再将所有段优化后的规划路径合并成总的规划路径。
步骤S12中，优化约束条件的具体内容和公式如下：
限制最大曲率的优化约束条件为：
；
其中，向量，Cmax为曲率限制值，/>为相邻轨迹点之间间距的均值。/>具体为优化迭代上一步求解得到的/>中，轨迹点P1(x1, y1)到Pn(xn, yn)之间相邻轨迹点之间的间距均值。当本次为第一代优化迭代时，/>具体为初始轨迹点P1 ref(x1 ref, y1 ref)到Pn ref(xn ref, yn ref)之间相邻轨迹点之间的间距均值。
各个轨迹点不与障碍物碰撞的约束条件为：起始地坐标P1（x1，y1）和目的地坐标Pn（xn，yn）不变，轨迹点坐标P2（x2，y2）到Pn-1（xn-1，yn-1）与初始轨迹点Pi ref(xi ref, yi ref)之间X坐标和Y坐标的偏差值均小于等于，di为路径规划轨迹的每一段路径中各个轨迹点处对应车辆与障碍物最近的距离。轨迹点坐标P0（x0，y0）到初始轨迹点坐标P0 ref(x0 ref, y0 ref)之间X坐标和Y坐标的偏差值小于等于/>，/>为预设的坐标位置偏移量给定上限值，/>大于等于/>。
具体地，约束条件公式为：
式中，，其中：
。
航向角约束条件为：
，/>和/>分别为依据起始地车辆位姿和目的地车辆位姿设定的起始地和目的地处的航向角给定值。
步骤S12中，优化目标的具体内容和公式如下：
路径平滑度为：。
路径平滑度代价为：
。
路径长度为：。
路径长度代价为：
。
偏离初始轨迹点距离为：。
偏离初始轨迹点距离代价为：
。
Psmooth、Plength和Pref分别为平滑度、长度和相对初始轨迹点距离的惩罚因子，是预设的常量。为轨迹点Pi平滑度的权重，/>中为轨迹点Pi偏离初始轨迹点距离的权重，是序列二次规划时设置的数值，初始值为预设值。本实施例中，各个优化目标路径平滑度、路径长度和偏离初始轨迹点距离的权重比为：/>：1：/>。当然，各个优化目标的权重并限制在上述公式中，路径长度的权重也可以为一个设置数值，而非固定在数字1上。
优化目标为：
；
H是半正定对称矩阵，g为2n+4维梯度向量，是通过上述公式计算得到的常量。/>，是轨迹点坐标P0- Pn+1矩阵的倒置，是序列二次规划的变量。
步骤S12中，依据上述约束条件和优化目标，可建立优化问题：
在步骤S13中，进行序列二次规划问题的迭代求解时，
将限制最大曲率的优化约束函数进行线性展开为：
，/>。
为序列二次规划上次优化迭代得到的解；对序列二次规划问题的迭代求解时，还限制所述优化变量的运动极限。
步骤S13中，对序列二次规划问题进行迭代求解具体包括步骤S31-步骤S39：
S31给定优化变量Pi（xi，yi）初始的运动极限。该步骤中，还进行其他各个参数（包括/>、/>、Cmax等参数）的初始化。
S32进行优化近似问题构造（QP）。具体为：
为序列二次规划上次优化迭代得到的解。
S33对所述优化近似问题进行序列二次规划求解以获得新的优化变量。
S34，依据新的优化变量更新优化变量。
S35计算优化目标的数值和优化约束条件的约束值。
S36依据所述优化目标的数值判断是否收敛。其中，在优化目标数值比前一次迭代优化目标数值小且差值小于预设值时判断收敛，若优化目标数值与前一迭代优化目标数值差值大于预设值或优化目标比前一迭代优化目标数值大时，判断未收敛。
S37依据所述优化约束条件的约束值判断优化约束条件合格时是否合格。具体为依据约束值是否符合优化约束条件。
在收敛且优化约束条件合格时S38输出优化变量。
在未收敛且优化约束条件合格时进行下一迭代序列二次规划问题求解，在优化约束条件未合格时S39缩减运动极限以完成运动极限更新，并依据新的运动极限返回步骤S32以进行下一迭代序列二次规划问题求解。
具体地，缩减运动极限具体为：对违犯约束的优化约束条件进行预设倍率的缩减。当然，也可以按照其他规则缩减优化约束条件，例如以一个常数缩减或者以预设曲线缩减等等。
较佳者，判断所述优化目标和优化约束条件是否合格时，如果违犯限制最大曲率条件，则对优化目标中，增加路径平滑度代价的权重，有效防止曲率突变，确保优化好的路径对车辆的舒适性更好。
在步骤S36和步骤S37的同时，还判断从本次迭代向前其预设次迭代中优化变量是否均处于运动区间边缘，若是则扩大运动极限增大。例如，判断从当代迭代算起两次迭代中优化变量是否均处于运动区间边缘，具体判断当前迭代和前一代的优化变量Pi是否均处于区间边缘。其中，判断优化变量Pi是否处于区间边缘具体为：判断优化变量/>接近区间边缘的相差百分比/>或/>是否小于预设值，在二者其一小于预设值时，判断优化变量/>处于区间边缘。当前，也可以判断从当前迭代起向前三、四次等数目的迭代中优化变量的区间范围，并不限制在两次。
具体地，扩大运动极限具体为：对多次迭代优化中优化变量处于边缘时对预设倍率的扩大。当然，也可以使用其他方式扩大运动极限，例如以一个常数扩大或者以预设曲线扩大等等。
参考图5，本发明还公开了一种自动泊车路径优化装置100，包括检测模块10、初始路径规划模块20、问题构件模块30、问题求解模块40和路径优化模块50。检测模块10获取障碍物信息；初始路径规划模块20生成初始路径规划轨迹，将初始路径规划轨迹的每一段路径转换为若干初始轨迹点Pi ref(xi ref, yi ref) ，i=1、2、3…n；问题构件模块30以路径规划轨迹的每一段路径中各个轨迹点坐标Pi（xi，yi）为优化变量，以限制最大曲率、起始地与目的地车辆位姿固定、各个轨迹点不与障碍物碰撞为优化约束条件，以路径的平滑度、路径长度和偏离初始轨迹点距离为优化目标，构建序列二次规划问题，i=1、2、3…n，每一段路径，车辆起始地坐标为P1（x1，y1），车辆目的地坐标为Pn（xn，yn）,P0（x0，y0）为约束车辆起始地航向角不变添加的辅助点，Pn+1（xn+1，yn+1）为约束车辆目的地航向角不变添加的辅助点。其中，所述优化目标的数值为路径平滑度、路径长度以及偏离初始轨迹点距离的权重求和，且路径平滑度中，起始地坐标P1（x1，y1）和目的地坐标Pn（xn，yn）的权重均大于起始地坐标和目的地坐标之间轨迹点坐标P2（x2，y2）到Pn-1（xn-1，yn-1）的权重；问题求解模块40对序列二次规划问题进行迭代求解以生成轨迹点信息；路径优化模块50依据所述轨迹点信息生成优化后的规划路径。
本发明还公开了一种自动泊车路径优化装置，包括：一个或多个处理器；存储器；以及一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被配置成由一个或多个处理器执行以实现如上所述的自动泊车路径优化方法。
本发明还公开了一种计算机可读存储介质，包括与具有存储器的电子设备结合使用的计算机程序，所述计算机程序可被处理器执行以实现如上所述的自动泊车路径优化方法。
本发明不但以曲率约束作为路径优化时二次规划条件之一，还在将优化目标设置为路径平滑度等多个目标的权重求和，可依据实际需要（例如不同的泊车场景、不同车辆类型和/或初始规划路径轨迹不同求解方式）设置不同目标的权重，使得优化好的路径更适应于不同环境，且将路径平滑度的权重起始地和目的地设置的更大，有效减小优化后的规划路径过度平滑导致车辆与障碍物存在碰撞的风险。再者，本发明对初始路径规划轨迹进行离散处理，获得全部路径规划轨迹的离散点作为轨迹点，以对整个路径规划轨迹进行全方位优化，确保优化好的整条线路均符合约束条件。
以上所揭露的仅为本发明的优选实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明申请专利范围所作的等同变化，仍属本发明所涵盖的范围。

Claims (15)

1.一种自动泊车路径优化方法，其特征在于：包括：
获取障碍物信息，并生成初始路径规划轨迹，将初始路径规划轨迹的每一段路径转换为若干个离散的初始轨迹点Pi ref(xi ref, yi ref)，i=1、2、3…n；
以路径规划轨迹的每一段路径中各个轨迹点坐标Pi（xi，yi）为优化变量，以限制最大曲率、起始地与目的地车辆位姿固定、各个轨迹点不与障碍物碰撞为优化约束条件，以路径的平滑度、路径长度和偏离初始轨迹点距离为优化目标，构建序列二次规划问题，对序列二次规划问题进行迭代求解以生成轨迹点信息；其中，路径平滑度代价为：，Psmooth为平滑度的惩罚因子，/>为轨迹点Pi平滑度的权重；
依据所述轨迹点信息生成优化后的规划路径；
其中，所述优化目标的数值为路径平滑度、路径长度以及偏离初始轨迹点距离的权重求和，且路径平滑度中，起始地坐标P1（x1，y1）和目的地坐标Pn（xn，yn）的权重均大于起始地坐标和目的地坐标之间轨迹点坐标P2（x2，y2）到Pn-1（x n-1，y n-1）的权重，而且轨迹点坐标P2（x2，y2）到Pn-1（xn-1，yn-1）中，前m个轨迹点的权重逐渐减小，后m个轨迹点的权重逐渐增加，m为大于等于1且小于等于n/2的整数。
2.如权利要求1所述的自动泊车路径优化方法，其特征在于：
限制最大曲率的优化约束条件包括：
；
其中，向量，C max为曲率限制值，/>为相邻轨迹点之间间距的均值。
3.如权利要求1所述的自动泊车路径优化方法，其特征在于：对序列二次规划问题的迭代求解时，将限制最大曲率的优化约束函数进行线性展开为：
，/>，为序列二次规划上一步优化迭代得到的解；
对序列二次规划问题的迭代求解时，还限制所述优化变量的运动极限。
4.如权利要求1所述的自动泊车路径优化方法，其特征在于：
各个轨迹点不与障碍物碰撞的约束条件为：起始地坐标P1（x1，y1）和目的地坐标Pn（xn，yn）不变，轨迹点坐标P2（x2，y2）到Pn-1（x n-1，y n-1）与对应初始轨迹点Pi ref(xi ref, yi ref)之间X坐标和Y坐标的偏差值均小于等于，di为路径规划轨迹的每一段路径中各个初始轨迹点处对应车辆与障碍物最近的距离。
5.如权利要求1所述的自动泊车路径优化方法，其特征在于：
航向角约束条件为：
，/>和/>分别为依据起始地车辆位姿和目的地车辆位姿设定的起始地和目的地处的航向角给定值；P0（x0，y0）为路径规划轨迹的每一段路径中约束车辆起始地航向角不变添加的辅助点，Pn+1（x n+1，y n+1）为路径规划轨迹的每一段路径中约束车辆目的地航向角不变添加的辅助点。
6.如权利要求1所述的自动泊车路径优化方法，其特征在于：路径长度代价为：
；
偏离初始轨迹点距离代价为：
；
Plength和Pref分别为长度和相对初始轨迹点距离的惩罚因子， 为中为轨迹点Pi偏离初始轨迹点距离的权重；
优化目标为：
；
H是半正定对称矩阵，g为梯度向量，。
7.如权利要求1所述的自动泊车路径优化方法，其特征在于：对序列二次规划问题进行迭代求解具体包括：给定所述优化变量一个运动极限并进行优化近似问题构造，对所述优化近似问题进行序列二次规划求解以获得新的优化变量，依据所述新的优化变量更新优化变量，计算优化目标的数值和优化约束条件的约束值，依据所述优化目标的数值判断是否收敛，依据所述优化约束条件的约束值判断优化约束条件是否合格，并在收敛且优化约束条件合格时输出优化变量，在未收敛且优化约束条件合格时进行下一迭代二次规划问题求解，在优化约束条件未合格时缩减运动极限并进行下一迭代二次规划问题求解。
8.如权利要求7所述的自动泊车路径优化方法，其特征在于：缩减运动极限具体为：对违犯约束的优化约束条件进行预设倍率的缩减。
9.如权利要求7所述的自动泊车路径优化方法，其特征在于：在依据所述优化约束条件的约束值判断优化约束条件合格时，还判断多次迭代中优化变量是否均处于优化变量运动区间边缘，若是则扩大运动极限增大。
10.如权利要求9所述的自动泊车路径优化方法，其特征在于：扩大运动极限具体为：对多次迭代优化中优化变量处于边缘时对预设倍率的扩大。
11.如权利要求7所述的自动泊车路径优化方法，其特征在于：判断所述优化目标和优化约束条件是否合格时，如果违犯限制最大曲率条件，则对优化目标中，增加路径平滑度代价的权重。
12.如权利要求1所述的自动泊车路径优化方法，其特征在于：采用Hybrid A*算法进行路径规划以生成所述初始路径规划轨迹，并将初始路径规划轨迹的每一段路径分解为n+2个离散的初始轨迹点Pi ref(xi ref, yi ref)，i=0、1、2、3…n、n+1，P0 ref(x0 ref, y0 ref)为路径规划轨迹的每一段路径中约束车辆起始地航向角不变添加的辅助点，Pn+1 ref(xn+1 ref, yn+1 ref)为路径规划轨迹的每一段路径中约束车辆目的地航向角不变添加的辅助点。
13.一种自动泊车路径优化装置，其特征在于：包括：
障碍物信息获取模块，获取障碍物信息；
初始路径规划模块，生成初始路径规划轨迹，将初始路径规划轨迹的每一段路径转换为若干个离散的初始轨迹点Pi ref(xi ref, yi ref)，i=1、2、3…n；
问题构造模块，以路径规划轨迹的每一段路径中各个轨迹点坐标Pi（xi，yi）为优化变量，以限制最大曲率、起始地与目的地车辆位姿固定、各个轨迹点不与障碍物碰撞为优化约束条件，以路径的平滑度、路径长度和偏离初始轨迹点距离为优化目标，构建序列二次规划问题，i=1、2、3…n，所述优化目标的数值为路径平滑度、路径长度以及偏离初始轨迹点距离的权重求和，其中，路径平滑度代价为：，Psmooth为平滑度的惩罚因子，/>为轨迹点Pi平滑度的权重；且路径平滑度中，起始地坐标P1（x1，y1）和目的地坐标Pn（xn，yn）的权重均大于起始地坐标和目的地坐标之间轨迹点坐标P2（x2，y2）到Pn-1（x n-1，y n-1）的权重；而且轨迹点坐标P2（x2，y2）到Pn-1（xn-1，yn-1）中，前m个轨迹点的权重逐渐减小，后m个轨迹点的权重逐渐增加，m为大于等于1且小于等于n/2的整数；
问题求解模块，对序列二次规划问题进行迭代求解以生成轨迹点信息；
路径优化模块，依据所述轨迹点信息生成优化后的规划路径。
14.一种自动泊车路径优化装置，其特征在于：包括：
一个或多个处理器；
存储器；以及
一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被配置成由一个或多个处理器执行以实现如权利要求1-12中任一项所述的自动泊车路径优化方法。
15.一种计算机可读存储介质，包括与具有存储器的电子设备结合使用的计算机程序，其特征在于：所述计算机程序可被处理器执行以实现如权利要求1-12中任一项所述的自动泊车路径优化方法。
CN202211190937.0A 2022-09-28 2022-09-28 自动泊车路径优化方法及装置 Active CN115456297B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211190937.0A CN115456297B (zh) 2022-09-28 2022-09-28 自动泊车路径优化方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211190937.0A CN115456297B (zh) 2022-09-28 2022-09-28 自动泊车路径优化方法及装置

Publications (2)

Publication Number Publication Date
CN115456297A CN115456297A (zh) 2022-12-09
CN115456297B true CN115456297B (zh) 2024-05-21

Family

ID=84306388

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211190937.0A Active CN115456297B (zh) 2022-09-28 2022-09-28 自动泊车路径优化方法及装置

Country Status (1)

Country Link
CN (1) CN115456297B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117533354B (zh) * 2023-12-28 2024-04-02 安徽蔚来智驾科技有限公司 轨迹生成方法、行驶控制方法、存储介质及智能设备

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104050390A (zh) * 2014-06-30 2014-09-17 西南交通大学 一种基于可变维粒子群膜算法的移动机器人路径规划方法
EP3685968A1 (en) * 2019-01-22 2020-07-29 Bayerische Motoren Werke Aktiengesellschaft Robot motion planning in manufacturing
CN111591307A (zh) * 2020-04-15 2020-08-28 毫末智行科技有限公司 一种避障轨迹规划方法、***及车辆
CN112000088A (zh) * 2019-05-08 2020-11-27 北京京东乾石科技有限公司 路径规划方法与装置
CN112277932A (zh) * 2020-10-21 2021-01-29 深圳市德航智能技术有限公司 基于北斗定位的农机自动驾驶***关键技术研发
CN112862204A (zh) * 2021-02-23 2021-05-28 国汽(北京)智能网联汽车研究院有限公司 一种路径规划方法、***、计算机设备及可读存储介质
CN113050652A (zh) * 2021-03-25 2021-06-29 上海海事大学 一种用于智能船舶自动靠泊的轨迹规划方法
CN113306549A (zh) * 2021-06-17 2021-08-27 英博超算(南京)科技有限公司 一种自动泊车轨迹规划算法
CN114987492A (zh) * 2021-03-01 2022-09-02 武汉智行者科技有限公司 一种考虑曲率约束的自动驾驶车辆轨迹平滑优化方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10571921B2 (en) * 2017-09-18 2020-02-25 Baidu Usa Llc Path optimization based on constrained smoothing spline for autonomous driving vehicles
US11186276B2 (en) * 2018-07-27 2021-11-30 Baidu Usa Llc Adjusting speeds along a path for autonomous driving vehicles

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104050390A (zh) * 2014-06-30 2014-09-17 西南交通大学 一种基于可变维粒子群膜算法的移动机器人路径规划方法
EP3685968A1 (en) * 2019-01-22 2020-07-29 Bayerische Motoren Werke Aktiengesellschaft Robot motion planning in manufacturing
CN112000088A (zh) * 2019-05-08 2020-11-27 北京京东乾石科技有限公司 路径规划方法与装置
CN111591307A (zh) * 2020-04-15 2020-08-28 毫末智行科技有限公司 一种避障轨迹规划方法、***及车辆
CN112277932A (zh) * 2020-10-21 2021-01-29 深圳市德航智能技术有限公司 基于北斗定位的农机自动驾驶***关键技术研发
CN112862204A (zh) * 2021-02-23 2021-05-28 国汽(北京)智能网联汽车研究院有限公司 一种路径规划方法、***、计算机设备及可读存储介质
CN114987492A (zh) * 2021-03-01 2022-09-02 武汉智行者科技有限公司 一种考虑曲率约束的自动驾驶车辆轨迹平滑优化方法
CN113050652A (zh) * 2021-03-25 2021-06-29 上海海事大学 一种用于智能船舶自动靠泊的轨迹规划方法
CN113306549A (zh) * 2021-06-17 2021-08-27 英博超算(南京)科技有限公司 一种自动泊车轨迹规划算法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
全自动平行泊车路径规划方法研究;赵海兰;高松;孙宾宾;武哲;唐鹏;;科学技术与工程;20170308(07) *
海洋平台结构动力响应优化设计与灵敏度分析;顾元宪, 马红艳, 姜成, 亢战, 张洪武;海洋工程;20010430(01) *
海洋平台结构动力响应优化设计与灵敏度分析;顾元宪等;《海洋工程》;第19卷(第1期);第7-9页 *

Also Published As

Publication number Publication date
CN115456297A (zh) 2022-12-09

Similar Documents

Publication Publication Date Title
CN107063280B (zh) 一种基于控制采样的智能车辆路径规划***及方法
CN112378408B (zh) 一种实现轮式移动机器人实时避障的路径规划方法
Wang et al. Collision-free navigation of autonomous vehicles using convex quadratic programming-based model predictive control
CN115456297B (zh) 自动泊车路径优化方法及装置
Muller et al. Continuous curvature trajectory design and feedforward control for parking a car
CN112622932B (zh) 一种基于势能场启发式搜索的自动驾驶换道轨迹规划算法
JP5212753B2 (ja) レーダ装置、及び当該レーダ装置において用いられる測定方法
CN111650929B (zh) 一种自适应滑模控制方法、***及移动机器人控制器
Xiong et al. Application improvement of A* algorithm in intelligent vehicle trajectory planning
JP6708535B2 (ja) 車両軌道用グラフ生成装置
CN115454062A (zh) 一种基于贝兹曲线的机器人动态路径规划方法及***
JP7019885B2 (ja) 車両制御値決定装置、車両制御値決定方法、およびプログラム
CN114371709A (zh) 路径规划方法、设备、存储介质及程序产品
CN111857148B (zh) 一种非结构化道路车辆路径规划方法
CN117141520B (zh) 一种实时轨迹规划方法、装置和设备
CN115534998A (zh) 自动驾驶集成决策方法、装置、车辆及存储介质
Jiang et al. Multi-target tracking algorithm of vehicle-mounted millimeter-wave radar based on IMM-JPDA-STUKF
CN111781920A (zh) 自动驾驶方法、装置和存储介质
CN117170355A (zh) Agv小车避障路径规划方法、装置及设备
CN113961016B (zh) 基于a*算法的无人机动态目标航迹规划方法及***
CN115891982A (zh) 一种基于二次规划与并行迭代的自动泊车路径规划方法
Daniel et al. Energy constrained trajectory generation for ADAS
CN114879665A (zh) 用于车辆远程召唤***的安全节能运动规划方法及***
Devaragudi et al. MPC-based control of autonomous vehicles with localized path planning for obstacle avoidance under uncertainties
CN113320544A (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