CN114593742A - 一种智能驾驶车辆的局部路径规划方法、装置及设备 - Google Patents

一种智能驾驶车辆的局部路径规划方法、装置及设备 Download PDF

Info

Publication number
CN114593742A
CN114593742A CN202011406294.XA CN202011406294A CN114593742A CN 114593742 A CN114593742 A CN 114593742A CN 202011406294 A CN202011406294 A CN 202011406294A CN 114593742 A CN114593742 A CN 114593742A
Authority
CN
China
Prior art keywords
pose
curve
optimized
parameter
determining
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
Application number
CN202011406294.XA
Other languages
English (en)
Other versions
CN114593742B (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.)
SAIC Motor Corp Ltd
Shanghai Automotive Industry Corp Group
Original Assignee
SAIC Motor Corp Ltd
Shanghai Automotive Industry Corp Group
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 SAIC Motor Corp Ltd, Shanghai Automotive Industry Corp Group filed Critical SAIC Motor Corp Ltd
Priority to CN202011406294.XA priority Critical patent/CN114593742B/zh
Publication of CN114593742A publication Critical patent/CN114593742A/zh
Application granted granted Critical
Publication of CN114593742B publication Critical patent/CN114593742B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3453—Special cost functions, i.e. other than distance or default speed limit of road segments
    • 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

Abstract

本申请公开了一种智能驾驶车辆的局部路径规划方法、装置及设备，该方法包括：首先根据获取到的智能驾驶车辆在全局坐标系下轨迹规划中起始点的第一位姿和目标点的第一位姿，确定在以起始点为坐标原点的局部坐标系下起始点的第二位姿和目标点的第二位姿，然后，根据起始点的第二位姿和目标点的第二位姿，将5阶Bezier曲线转化为待优化曲线，并利用基于SQP的目标函数，对待优化曲线进行优化，得到优化参数的最优解，进一步可以根据优化参数的最优解，确定智能驾驶车辆的最佳局部路径。从而能够实现基于5阶Bezier曲线，通过SQP的参数优化，确定出各个控制点的准确坐标，进而快速、准确的规划出智能驾驶车辆的局部路径。

Description

一种智能驾驶车辆的局部路径规划方法、装置及设备
技术领域
本申请涉及车辆技术领域，尤其涉及一种智能驾驶车辆的局部路径规划方法、装置及设备。
背景技术
随着智能驾驶技术的快速发展，路径规划也成为了重要的突破领域。现有的路径规划方法主要分为两类：一类是采用多项式、回旋曲线、贝塞尔(Bezier)曲线等具有解析解的直接规划法，另一类是采用快速扩展随机树(rapidly exploring random tree，简称RRT)、人工试场等通过采样拟合生成路径的间接规划法。目前智能驾驶车辆的局部路径规划大多采用第一类的直接规划方法，在该类方法中，相比于其他类型的曲线，Bezier曲线具有良好的端点性质和切向矢量性，可以在满足端点约束、曲率连续约束和车辆运动学约束的同时，在起始点与目标点之间生成无数条路径以供选择，极大地减少了无可行解的情况，设计时可以根据自己的目标要求从无数条路径中求解出期望的最优路径，目标要求可以是平滑性、安全性、横向偏移程度等一种或几种的组合。
但目前采用Bezier曲线进行局部路径规划的方法中大多数没有完全考虑两个端点处的曲率约束，或者考虑端点曲率约束后求解期望的最优路径比较困难。若要满足端点处的位姿及曲率约束，Bezier曲线的阶次至少应为5阶，同时会导致曲线约束的个数小于求解量的个数，从而无法完全确定各个控制点准确坐标。因此，如何快速、准确的规划出智能驾驶车辆的局部路径，已成为亟待解决的问题。
发明内容
本申请实施例的主要目的在于提供一种智能驾驶车辆的局部路径规划方法、装置及设备，能够快速、准确的规划出智能驾驶车辆的局部路径。
在本申请第一方面提出了一种智能驾驶车辆的局部路径规划方法，包括：
获取智能驾驶车辆在全局坐标系下轨迹规划中起始点的第一位姿和目标点的第一位姿；
根据所述起始点的第一位姿和所述目标点的第一位姿，确定在以起始点为坐标原点的局部坐标系下所述起始点的第二位姿和所述目标点的第二位姿；
根据所述起始点的第二位姿和所述目标点的第二位姿，将5阶贝塞尔Bezier曲线转化为待优化曲线；
利用基于序列二次规划SQP的目标函数，对所述待优化曲线进行优化，得到优化参数的最优解；
根据所述优化参数的最优解，确定所述智能驾驶车辆的最佳局部路径。
在一种可选的实现方式中，所述根据所述起始点的第二位姿和所述目标点的第二位姿，将5阶贝塞尔Bezier曲线转化为待优化曲线，包括：
将5阶贝塞尔Bezier曲线的曲线方程转换为矩阵乘积形式的待优化曲线；
根据所述起始点的第二位姿和所述目标点的第二位姿，确定所述矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵。
在一种可选的实现方式中，所述利用基于序列二次规划SQP的目标函数，对所述待优化曲线进行优化，得到优化参数的最优解，包括：
确定所述基于序列二次规划SQP的目标函数的参数初始值；
计算所述矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵的一阶偏导数；
计算所述矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵的二阶偏导数；
确定所述一阶偏导数和所述二阶偏导数在所述目标函数的参数初始值的取值，并通过循环调用QP解算器逐步求解横坐标的增量，当所述增量满足预设的条件时，退出循环，得到优化参数的最优解。
在一种可选的实现方式中，在所述确定所述基于序列二次规划SQP的目标函数的参数初始值之后，所述方法还包括：
确定所述目标函数在所述参数初始值位置的表达形式。
对应于上述智能驾驶车辆的局部路径规划方法，本申请提出了一种智能驾驶车辆的局部路径规划装置，包括：
获取单元，用于获取智能驾驶车辆在全局坐标系下轨迹规划中起始点的第一位姿和目标点的第一位姿；
第一确定单元，用于根据所述起始点的第一位姿和所述目标点的第一位姿，确定在以起始点为坐标原点的局部坐标系下所述起始点的第二位姿和所述目标点的第二位姿；
转化单元，用于根据所述起始点的第二位姿和所述目标点的第二位姿，将5阶贝塞尔Bezier曲线转化为待优化曲线；
获得单元，用于利用基于序列二次规划SQP的目标函数，对所述待优化曲线进行优化，得到优化参数的最优解；
第二确定单元，用于根据所述优化参数的最优解，确定所述智能驾驶车辆的最佳局部路径。
在一种可选的实现方式中，所述转化单元包括：
转换子单元，用于将5阶贝塞尔Bezier曲线的曲线方程转换为矩阵乘积形式的待优化曲线；
第一确定子单元，用于根据所述起始点的第二位姿和所述目标点的第二位姿，确定所述矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵。
在一种可选的实现方式中，所述获得单元包括：
第二确定子单元，用于确定所述基于序列二次规划SQP的目标函数的参数初始值；
第一计算子单元，用于计算所述矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵的一阶偏导数；
第二计算子单元，用于计算所述矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵的二阶偏导数；
获得子单元，用于确定所述一阶偏导数和所述二阶偏导数在所述目标函数的参数初始值的取值，并通过循环调用QP解算器逐步求解横坐标的增量，当所述增量满足预设的条件时，退出循环，得到优化参数的最优解。
在一种可选的实现方式中，所述装置还包括：
第三确定单元，用于确定所述目标函数在所述参数初始值位置的表达形式。
本申请实施例还提供了一种智能驾驶车辆的局部路径规划设备，包括：处理器、存储器、系统总线；
所述处理器以及所述存储器通过所述系统总线相连；
所述存储器用于存储一个或多个程序，所述一个或多个程序包括指令，所述指令当被所述处理器执行时使所述处理器执行上述智能驾驶车辆的局部路径规划方法中的任意一种实现方式。
本申请实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有指令，当所述指令在终端设备上运行时，使得所述终端设备执行上述智能驾驶车辆的局部路径规划方法中的任意一种实现方式。
由此可见，本申请实施例具有如下有益效果：
本申请实施例提供一种智能驾驶车辆的局部路径规划方法、装置及设备，首先获取智能驾驶车辆在全局坐标系下轨迹规划中起始点的第一位姿和目标点的第一位姿。然后，根据起始点的第一位姿和目标点的第一位姿，确定在以起始点为坐标原点的局部坐标系下起始点的第二位姿和目标点的第二位姿，接着，根据起始点的第二位姿和目标点的第二位姿，将5阶Bezier曲线转化为待优化曲线，并利用基于SQP的目标函数，对待优化曲线进行优化，得到优化参数的最优解，进一步可以根据优化参数的最优解，确定智能驾驶车辆的最佳局部路径。从而能够实现基于5阶Bezier曲线，通过SQP的参数优化，确定出各个控制点的准确坐标，进而快速、准确的规划出智能驾驶车辆的局部路径。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本申请实施例提供的一种智能驾驶车辆的局部路径规划方法的流程图；
图2为本申请实施例提供的利用Bezier曲线进行规划的效果示意图；
图3为本申请实施例提供的三类优化参数生成Bezier曲线的对比示意图；
图4为本申请实施例提供的三类优化参数生成Bezier曲线的曲率对比示意图；
图5为本申请实施例提供的一种智能驾驶车辆的局部路径规划装置的示意图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
众所周知，随着智能驾驶技术的飞速发展，路径规划已成为了重要的突破领域。目前智能驾驶车辆的局部路径规划大多采用多项式、回旋曲线、Bezier曲线等具有解析解的直接规划法。其中，相比于其他类型的曲线，Bezier曲线具有良好的端点性质和切向矢量性，可以在满足端点约束、曲率连续约束和车辆运动学约束的同时，在起始点与目标点之间生成无数条路径以供选择，极大地减少了无可行解的情况，设计时可以根据自己的目标要求从无数条路径中求解出期望的最优路径，目标要求可以是平滑性、安全性、横向偏移程度等一种或几种的组合。
但目前采用Bezier曲线进行局部路径规划的方法中大多数没有完全考虑两个端点处的曲率约束，或者考虑端点曲率约束后求解期望的最优路径比较困难。若要满足端点处的位姿及曲率约束，Bezier曲线的阶次至少应为5阶，同时会导致曲线约束的个数小于求解量的个数，从而无法完全确定各个控制点准确坐标。因此，如何准确确定出各个控制点的坐标，并快速、准确的规划出智能驾驶车辆的局部路径，已成为亟待解决的问题。
基于此，本申请提出了一种智能驾驶车辆的局部路径规划方法、装置及设备，能够快速、准确的规划出智能驾驶车辆的局部路径。
以下将结合附图对本申请实施例提供的智能驾驶车辆的局部路径规划方法进行详细说明。参见图1所示，其示出了本申请实施例提供的一种智能驾驶车辆的局部路径规划方法实施例的流程图，本实施例可以包括以下步骤：
S101：获取智能驾驶车辆在全局坐标系下轨迹规划中起始点的第一位姿和目标点的第一位姿。
在本实施例中，为了能够快速、准确的规划出智能驾驶车辆的局部路径，首先需要从决策层获取全局坐标系下车辆轨迹规划起始点的位姿(此处将其定义为起始点的第一位姿)和目标点的位姿(此次将其定义为目标点的第一位姿)，具体表达式如下公式(1)：
Figure BDA0002818597820000061
其中，A(x0,y0,θ0,k0)表示全局坐标系下车辆轨迹规划起始点的第一位姿；B(x1,y1,θ1,k1)表示全局坐标系下车辆轨迹规划目标点的第一位姿。
S102：根据起始点的第一位姿和目标点的第一位姿，确定在以起始点为坐标原点的局部坐标系下起始点的第二位姿和目标点的第二位姿。
在本实施例中，通过步骤S101获取到智能驾驶车辆在全局坐标系下轨迹规划中起始点的第一位姿和目标点的第一位姿后，为了降低Bezier曲线优化参数的个数，首先建立以A点为坐标原点的局部坐标系oxy，其x轴指向起始点的航向角，则经过坐标变换可以得到局部坐标系中的规划起始点位姿(此处将其定义为起始点的第二位姿)和目标点位姿(此次将其定义为目标点的第二位姿)，具体表达式如下公式(2)：
Figure BDA0002818597820000062
其中，A’(x0,y0,θ0,k0)表示局部坐标系下起始点的第二位姿；B’(x1,y1,θ1,k1)表示局部坐标系下目标点的第二位姿。
通过局部坐标变换后可以得到如下基本信息公式(3)：
Figure BDA0002818597820000063
S103：根据起始点的第二位姿和目标点的第二位姿，将5阶贝塞尔Bezier曲线转化为待优化曲线。
需要说明的是，由于5阶及以上阶次的Bezier曲线可以同时满足包括曲率在内的起始点和目标点位姿约束，因此，为了降低算法的复杂程度，本申请采用采用5阶Bezier数学模型进行路径规划，生成从起始点到目标点满足平滑性要求的最优路径。然后，再通过分析Bezier曲线的端点性质和切矢量性，并结合规划起始点和目标点的位姿要求，将由6个控制点共12个未知数确定的5阶Bezier曲线转化为由4个优化参数x＝(d0,d1,x2,x3)决定的待后续的优化曲线。
在本申请实施例的一种可能的实现方式中，本步骤S103的实现过程具体可以包括下述步骤A1-A2：
步骤A1：将5阶贝塞尔Bezier曲线的曲线方程转换为矩阵乘积形式的待优化曲线。
步骤A2：根据起始点的第二位姿和目标点的第二位姿，确定矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵。
具体来讲，在本实现方式中，首先，可以利用6个控制点坐标(P0,P1,P2,P3,P4,P5)可以确定唯一一条点对点的5阶Bezier曲线，其参数方程如下公式(4)：
Figure BDA0002818597820000071
其中，Bi,5(t)表示为伯恩斯坦多项式；t表示曲线参数，t∈[0,1]。
由于Bezier曲线在路径规划中应用的主要优势在于端点的切向矢量性，Bezier曲线的起点和终点处与其特征多边形的起点和终点重合，起点和终点处的切线方向与其特征多边形的第一条边及最后一条边的走向一致，因此，5阶Bezier的端点性质和切矢量性可以用如下公式(5)表示：
Figure BDA0002818597820000072
然后，根据曲率公式的一般参数方程
Figure BDA0002818597820000081
结合公式(5)，可得出曲线在起始点和目标点的曲率表达式分别如下公式(6)所示：
Figure BDA0002818597820000082
由上述公式(5)和(6)可以看出，Bezier曲线起始点位姿由P0、P1、P2三个控制点所确定，目标点位姿由P3、P4、P5三个控制点确定。设d0＝|P0P1|，d1＝|P4P5|，则可以由公式(5)和(6)得出P0、P1、P4、P5控制点坐标如下：
Figure BDA0002818597820000083
Figure BDA0002818597820000086
Figure BDA0002818597820000087
Figure BDA0002818597820000088
起始点和目标点处的曲率与控制点的关系可以由公式(6)解得如下公式(11)所示：
Figure BDA0002818597820000084
其中，Δx0＝x2-xs；Δy0＝y2-ys；Δx1＝xe-x3；Δy1＝ye-y3。
进一步地，将上述公式(7)、(8)、(9)、(10)带入上述公式(11)可以得到控制点P2和P3分别为：
Figure BDA0002818597820000085
Figure BDA0002818597820000091
需要说明的是，本申请考虑的是cosθe≠0时的工况。
进而，为了后续通过步骤S104进行参数优化，还需要将Bezier曲线参数方程式(即上述公式(4))转换为矩阵乘积形式的待优化曲线，并将曲线横坐标和纵坐标参数方程分别表示，如下公式(14)、(15)、(16)所示：
P(t)＝T(t)TAP (14)
X(t)＝T(t)TAX (15)
Y(t)＝T(t)TAY (16)
其中，A表示系数矩阵；T(t)表示参数矩阵；P表示控制点矩阵，矩阵表示具体如下公式(17)所示：
Figure BDA0002818597820000092
进而，结合上述公式(7)、(8)、(9)、(10)、(12)、(13)卷皮，最终落确定参数优化所用的横坐标矩阵X和纵坐标矩阵Y如下公式(18)所示：
Figure BDA0002818597820000093
S104：利用基于序列二次规划SQP的目标函数，对待优化曲线进行优化，得到优化参数的最优解。
在本实施中，通过步骤S103将5阶贝塞尔Bezier曲线转化为待优化曲线后，进一步可以利用基于序列二次规划SQP的目标函数，对待优化曲线进行优化，得到优化参数的最优解。
在本申请实施例的一种可能的实现方式中，本步骤S104的实现过程具体可以包括下述步骤B1-B4：
步骤B1：确定基于序列二次规划SQP的目标函数的参数初始值。
此时，可以确定出目标函数在参数初始值位置的表达形式。
步骤B2：计算矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵的一阶偏导数。
步骤B3：计算矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵的二阶偏导数。
步骤B4：确定一阶偏导数和二阶偏导数在目标函数的参数初始值的取值，并通过循环调用QP解算器逐步求解横坐标的增量，当增量满足预设的条件时，退出循环，得到优化参数的最优解。
具体来讲，在本实现方式中，首先，由上述公式(18)可以看出，由6个控制点共12个未知数确定的5阶Bezier曲线转化为由4个优化参数x＝(d0,d1,x2,x3)确定的满足运动学约束、初始位姿约束、目标位姿约束以及曲率连续约束的曲线。仅根据起始点和目标点位姿就可以获得无数条Bezier曲线，因此需要确定4个优化参数的确定值。由于其中存在非优化参数t，为了消除参数t引起的无局部最优解的情况，保证目标函数为凸优化问题，本申请预先设定了基于序列二次规划SQP的目标函数如下公式(19)所示：
Figure BDA0002818597820000101
其中，该目标函数的物理意义为：只考虑一个规划周期时，路径的曲率变化越小说明路径变化缓慢，路径的平滑性越好，利用路径在离散采样点上曲率的平方和最小得到较为平滑的轨迹。
然后，为了使路径满足曲率边界约束，本申请采用了如下公式(20)所示的约束条件：
kmax(tkmax)≤Kmax
kmin(tkmin)≥Kmin (20)
d0＞0
d1＞0
x2＞d0
x3＞x2
xe-d1sinθe-x3＞0
其中，tkmax和tkmin可以先通过求解曲率方程的导函数，再通过数值方法求解导函数的零点得到极值点，再通过比较极值与端点值的曲率获得，本申请规定的最大曲率Kmax和最小曲率Kmin分别为0.004m-1和-0.004m-1。
需要说明的是，目标函数的参数初始值(此处将其定义为xinit)可定义为：
Figure BDA0002818597820000111
则进一步可以确定出目标函数J(x)在xk位置的表达形式如下公式(21)所示：
Figure BDA0002818597820000112
其中，d＝x-xk，gk(ti)和Hk(ti)分别是f＝k2(ti)的一阶偏导数矩阵▽f(xk)和Hessian矩阵▽2f(xk)，矩阵表达式分别如下公式(22)和(23)所示：
Figure BDA0002818597820000114
Figure BDA0002818597820000113
并且，为了求得上述矩阵表达式的元素，首先需要计算非优化参数T矩阵的一阶和二阶导数矩阵分别如下公式(24)所示：
Figure BDA0002818597820000121
其中，需要说明的是，为了便于后续计算，此处将
Figure BDA0002818597820000122
简化为dT，将
Figure BDA0002818597820000123
简化为ddT。
接着，对于4个优化参数x＝(d0,d1,x2,x3)，由上述公式(18)可以计算得出控制点矩阵X和Y的一阶偏导数矩阵分别如下公式(25)、(26)所示：
Figure BDA0002818597820000124
进而，对于4个优化参数x＝(d0,d1,x2,x3)，将一阶偏导数的参数变量记为m，将二阶偏导数的参数变量记为n，则根据上述公式(25)和(26)可以计算得到控制点矩阵X和Y的二阶偏导数矩阵分别为如下公式(27)所示：
Figure BDA0002818597820000131
进一步的，可以将参数初始值xinit带入上述公式(25)、(26)和(27)，以便得到t＝ti时函数f的4个一阶偏导数和16个二阶偏导数如下公式(28)和(29)所示：
Figure BDA0002818597820000136
Figure BDA0002818597820000132
对于上述公式(28)，其中参数的取值如下公式(30)所示：
Figure BDA0002818597820000133
对于上述公式(29)，其中参数的取值如下公式(31)和(32)所示：
Figure BDA0002818597820000134
Figure BDA0002818597820000135
对于上述公式(31)和(32)，其中参数的取值如下公式(33)所示：
Figure BDA0002818597820000141
在此基础上，通过循环调用QP解算器逐步求解横坐标x的增量d，当增量d满足预设的条件时，退出循环，得到优化参数的最优解。其中，预设的条件可以是如下公式(34)所示：
Figure BDA0002818597820000142
S105：根据优化参数的最优解，确定智能驾驶车辆的最佳局部路径。
在本实施例中，首先通过5阶Bezier曲线和SQP参数优化可以确定出给定起点和终点位姿时的较为平滑的路径轨迹，然后通过修改目标点横向偏移量可以得到目标点集，接着，再用以上路径生成的方法即可生成得到一组轨迹簇，进而可以通过设计代价函数选取最优轨迹，作为智能驾驶车辆的最佳局部路径。
举例说明：以道路宽度为3.75m，规划长度为80m为例，在此情况下，可以生成如图2所示的Bezier曲线规划效果图。参见图3，对比三种方法优化参数x生成的三类曲线，如图中箭头指示，分别为采用本申请提出的SQP优化参数的方法、在规划横坐标区间上均为分布控制点的方法、以及先设置|P0P1|＝|P1P2|＝d0，|P4P5|＝|P3P4|＝d1，然后通过循环遍历方法，以f＝kmax-kmin最小为目标获取优化参数的方法，得到了如图3所示的三种曲线规划的结果，从图3中可以看出采用SQP优化的曲线横向偏移量更小一些。参见图4，对比三种方法优化参数x生成的三类曲线的曲率变化情况，可知三种规划方法均达到曲率边界约束要求，进一步地，下表1展示出了三种参数优化方法的平滑性对比结果，由表1中的数据可以看出采用SQP优化参数的方法可以明显的提高Bezier曲线的平滑性，进一步表明了本申请提供的路径规划方法的有效性更高。下表2列出了采用采用SQP优化参数的方法优化参数时的收敛情况，由表2中的数据可以看出采用SQP优化参数的方法可以很快收敛至最优参数。
Figure BDA0002818597820000151
表1三种方法优化参数生成曲线的平滑性对比
收敛次数 优化参数x的变化 QP目标函数值
初始参数x<sub>init</sub> [16 16 32 48] -
1 [5.1484 5.1484 39.5 40.5] -0.000132948
2 [6.1866 6.1866 38.5914 41.4086] -4.12423e-06
3 [6.55738 6.55738 38.0797 41.9203] -2.75909e-07
4 [6.58406 6.58406 38.0446 41.9554] -1.27736e-09
5 [6.58418 6.58418 38.0445 41.9555] -2.64726e-14
6 [6.58418 6.58418 38.0445 41.9555] -1.1342e-23
表2采用SQP优化参数的方法优化参数过程中的参数收敛变化
综上，本申请实施例提供的一种智能驾驶车辆的局部路径规划方法，首先获取智能驾驶车辆在全局坐标系下轨迹规划中起始点的第一位姿和目标点的第一位姿。然后，根据起始点的第一位姿和目标点的第一位姿，确定在以起始点为坐标原点的局部坐标系下起始点的第二位姿和目标点的第二位姿，接着，根据起始点的第二位姿和目标点的第二位姿，将5阶Bezier曲线转化为待优化曲线，并利用基于SQP的目标函数，对待优化曲线进行优化，得到优化参数的最优解，进一步可以根据优化参数的最优解，确定智能驾驶车辆的最佳局部路径。从而能够实现基于5阶Bezier曲线，通过SQP的参数优化，确定出各个控制点的准确坐标，进而快速、准确的规划出智能驾驶车辆的局部路径。
参见图5所示，本申请还提供一种智能驾驶车辆的局部路径规划装置实施例，可以包括：
获取单元501，用于获取智能驾驶车辆在全局坐标系下轨迹规划中起始点的第一位姿和目标点的第一位姿；
第一确定单元502，用于根据所述起始点的第一位姿和所述目标点的第一位姿，确定在以起始点为坐标原点的局部坐标系下所述起始点的第二位姿和所述目标点的第二位姿；
转化单元503，用于根据所述起始点的第二位姿和所述目标点的第二位姿，将5阶贝塞尔Bezier曲线转化为待优化曲线；
获得单元504，用于利用基于序列二次规划SQP的目标函数，对所述待优化曲线进行优化，得到优化参数的最优解；
第二确定单元505，用于根据所述优化参数的最优解，确定所述智能驾驶车辆的最佳局部路径。
在本申请一些可能的实现方式中，所述转化单元503包括：
转换子单元，用于将5阶贝塞尔Bezier曲线的曲线方程转换为矩阵乘积形式的待优化曲线；
第一确定子单元，用于根据所述起始点的第二位姿和所述目标点的第二位姿，确定所述矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵。
在本申请一些可能的实现方式中，所述获得单元504包括：
第二确定子单元，用于确定所述基于序列二次规划SQP的目标函数的参数初始值；
第一计算子单元，用于计算所述矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵的一阶偏导数；
第二计算子单元，用于计算所述矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵的二阶偏导数；
获得子单元，用于确定所述一阶偏导数和所述二阶偏导数在所述目标函数的参数初始值的取值，并通过循环调用QP解算器逐步求解横坐标的增量，当所述增量满足预设的条件时，退出循环，得到优化参数的最优解。
在本申请一些可能的实现方式中，所述装置还包括：
第三确定单元，用于确定所述目标函数在所述参数初始值位置的表达形式。
由上述实施例可以看出，本申请实施例提供的智能驾驶车辆的局部路径规划装置，首先获取智能驾驶车辆在全局坐标系下轨迹规划中起始点的第一位姿和目标点的第一位姿。然后，根据起始点的第一位姿和目标点的第一位姿，确定在以起始点为坐标原点的局部坐标系下起始点的第二位姿和目标点的第二位姿，接着，根据起始点的第二位姿和目标点的第二位姿，将5阶Bezier曲线转化为待优化曲线，并利用基于SQP的目标函数，对待优化曲线进行优化，得到优化参数的最优解，进一步可以根据优化参数的最优解，确定智能驾驶车辆的最佳局部路径。从而能够实现基于5阶Bezier曲线，通过SQP的参数优化，确定出各个控制点的准确坐标，进而快速、准确的规划出智能驾驶车辆的局部路径。
进一步地，本申请实施例还提供了一种智能驾驶车辆的局部路径规划设备，包括：处理器、存储器、系统总线；
所述处理器以及所述存储器通过所述系统总线相连；
所述存储器用于存储一个或多个程序，所述一个或多个程序包括指令，所述指令当被所述处理器执行时使所述处理器执行上述智能驾驶车辆的局部路径规划方法的任一种实现方法。
进一步地，本申请实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有指令，当所述指令在终端设备上运行时，使得所述终端设备执行上述智能驾驶车辆的局部路径规划方法的任一种实现方法。
通过以上的实施方式的描述可知，本领域的技术人员可以清楚地了解到上述实施例方法中的全部或部分步骤可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者诸如媒体网关等网络通信设备，等等)执行本申请各个实施例或者实施例的某些部分所述的方法。
需要说明的是，本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本申请。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本申请的精神或范围的情况下，在其它实施例中实现。因此，本申请将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (10)

1.一种智能驾驶车辆的局部路径规划方法，其特征在于，包括：
获取智能驾驶车辆在全局坐标系下轨迹规划中起始点的第一位姿和目标点的第一位姿；
根据所述起始点的第一位姿和所述目标点的第一位姿，确定在以起始点为坐标原点的局部坐标系下所述起始点的第二位姿和所述目标点的第二位姿；
根据所述起始点的第二位姿和所述目标点的第二位姿，将5阶贝塞尔Bezier曲线转化为待优化曲线；
利用基于序列二次规划SQP的目标函数，对所述待优化曲线进行优化，得到优化参数的最优解；
根据所述优化参数的最优解，确定所述智能驾驶车辆的最佳局部路径。
2.根据权利要求1所述的方法，其特征在于，所述根据所述起始点的第二位姿和所述目标点的第二位姿，将5阶贝塞尔Bezier曲线转化为待优化曲线，包括：
将5阶贝塞尔Bezier曲线的曲线方程转换为矩阵乘积形式的待优化曲线；
根据所述起始点的第二位姿和所述目标点的第二位姿，确定所述矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵。
3.根据权利要求2所述的方法，其特征在于，所述利用基于序列二次规划SQP的目标函数，对所述待优化曲线进行优化，得到优化参数的最优解，包括：
确定所述基于序列二次规划SQP的目标函数的参数初始值；
计算所述矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵的一阶偏导数；
计算所述矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵的二阶偏导数；
确定所述一阶偏导数和所述二阶偏导数在所述目标函数的参数初始值的取值，并通过循环调用QP解算器逐步求解横坐标的增量，当所述增量满足预设的条件时，退出循环，得到优化参数的最优解。
4.根据权利要求3所述的方法，其特征在于，在所述确定所述基于序列二次规划SQP的目标函数的参数初始值之后，所述方法还包括：
确定所述目标函数在所述参数初始值位置的表达形式。
5.一种智能驾驶车辆的局部路径规划装置，其特征在于，包括：
获取单元，用于获取智能驾驶车辆在全局坐标系下轨迹规划中起始点的第一位姿和目标点的第一位姿；
第一确定单元，用于根据所述起始点的第一位姿和所述目标点的第一位姿，确定在以起始点为坐标原点的局部坐标系下所述起始点的第二位姿和所述目标点的第二位姿；
转化单元，用于根据所述起始点的第二位姿和所述目标点的第二位姿，将5阶贝塞尔Bezier曲线转化为待优化曲线；
获得单元，用于利用基于序列二次规划SQP的目标函数，对所述待优化曲线进行优化，得到优化参数的最优解；
第二确定单元，用于根据所述优化参数的最优解，确定所述智能驾驶车辆的最佳局部路径。
6.根据权利要求5所述的装置，其特征在于，所述转化单元包括：
转换子单元，用于将5阶贝塞尔Bezier曲线的曲线方程转换为矩阵乘积形式的待优化曲线；
第一确定子单元，用于根据所述起始点的第二位姿和所述目标点的第二位姿，确定所述矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵。
7.根据权利要求6所述的装置，其特征在于，所述获得单元包括：
第二确定子单元，用于确定所述基于序列二次规划SQP的目标函数的参数初始值；
第一计算子单元，用于计算所述矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵的一阶偏导数；
第二计算子单元，用于计算所述矩阵乘积形式的待优化曲线中参数优化所用的横坐标矩阵和纵坐标矩阵的二阶偏导数；
获得子单元，用于确定所述一阶偏导数和所述二阶偏导数在所述目标函数的参数初始值的取值，并通过循环调用QP解算器逐步求解横坐标的增量，当所述增量满足预设的条件时，退出循环，得到优化参数的最优解。
8.根据权利要求7所述的装置，其特征在于，所述装置还包括：
第三确定单元，用于确定所述目标函数在所述参数初始值位置的表达形式。
9.一种智能驾驶车辆的局部路径规划设备，其特征在于，包括：处理器、存储器、系统总线；
所述处理器以及所述存储器通过所述系统总线相连；
所述存储器用于存储一个或多个程序，所述一个或多个程序包括指令，所述指令当被所述处理器执行时使所述处理器执行权利要求1-4任一项所述的方法。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质中存储有指令，当所述指令在终端设备上运行时，使得所述终端设备执行权利要求1-4任一项所述的方法。
CN202011406294.XA 2020-12-04 2020-12-04 一种智能驾驶车辆的局部路径规划方法、装置及设备 Active CN114593742B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011406294.XA CN114593742B (zh) 2020-12-04 2020-12-04 一种智能驾驶车辆的局部路径规划方法、装置及设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011406294.XA CN114593742B (zh) 2020-12-04 2020-12-04 一种智能驾驶车辆的局部路径规划方法、装置及设备

Publications (2)

Publication Number Publication Date
CN114593742A true CN114593742A (zh) 2022-06-07
CN114593742B CN114593742B (zh) 2024-04-19

Family

ID=81813572

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011406294.XA Active CN114593742B (zh) 2020-12-04 2020-12-04 一种智能驾驶车辆的局部路径规划方法、装置及设备

Country Status (1)

Country Link
CN (1) CN114593742B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117495989A (zh) * 2023-12-29 2024-02-02 腾讯科技（深圳）有限公司 数据处理方法、装置、设备及可读存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107490965A (zh) * 2017-08-21 2017-12-19 西北工业大学 一种空间自由漂浮机械臂的多约束轨迹规划方法
WO2018119745A1 (zh) * 2016-12-28 2018-07-05 深圳配天智能技术研究院有限公司 机器人运动控制方法及相关装置
CN109375632A (zh) * 2018-12-17 2019-02-22 清华大学 自动驾驶车辆实时轨迹规划方法
CN109933057A (zh) * 2017-12-18 2019-06-25 中国农业机械化科学研究院 拖拉机自动驾驶系统的局部引导轨迹规划方法及装置
CN110262488A (zh) * 2019-06-18 2019-09-20 重庆长安汽车股份有限公司 自动驾驶的局部路径规划方法、系统及计算机可读存储介质

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018119745A1 (zh) * 2016-12-28 2018-07-05 深圳配天智能技术研究院有限公司 机器人运动控制方法及相关装置
CN107490965A (zh) * 2017-08-21 2017-12-19 西北工业大学 一种空间自由漂浮机械臂的多约束轨迹规划方法
CN109933057A (zh) * 2017-12-18 2019-06-25 中国农业机械化科学研究院 拖拉机自动驾驶系统的局部引导轨迹规划方法及装置
CN109375632A (zh) * 2018-12-17 2019-02-22 清华大学 自动驾驶车辆实时轨迹规划方法
CN110262488A (zh) * 2019-06-18 2019-09-20 重庆长安汽车股份有限公司 自动驾驶的局部路径规划方法、系统及计算机可读存储介质

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
YIQUN ZHANG 等: "Deployment trajectory design of hoop truss deployable space antennas", 《2010 IEEE INTERNATIONAL CONFERENCE ON MECHATRONICS AND AUTOMATION》, pages 1 - 5 *
陈成;何玉庆;卜春光;韩建达;: "基于四阶贝塞尔曲线的无人车可行轨迹规划", 自动化学报, no. 03, pages 486 - 496 *
高嵩 等: "基于贝塞尔曲线的无人车局部避障应用", 《现代电子技术》, vol. 42, no. 9, pages 163 - 166 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117495989A (zh) * 2023-12-29 2024-02-02 腾讯科技（深圳）有限公司 数据处理方法、装置、设备及可读存储介质
CN117495989B (zh) * 2023-12-29 2024-04-19 腾讯科技（深圳）有限公司 数据处理方法、装置、设备及可读存储介质

Also Published As

Publication number Publication date
CN114593742B (zh) 2024-04-19

Similar Documents

Publication Publication Date Title
CN109416529B (zh) 数控系统、具有存储功能的装置及nurbs曲线插补方法
Choi et al. Tool path generation and tolerance analysis for free-form surfaces
Hashemian et al. An integrated fitting and fairing approach for object reconstruction using smooth NURBS curves and surfaces
KR101627373B1 (ko) 가이딩 곡선들에 의해 정의된 표면들의 설계의 컴퓨터 구현 방법
US20120206457A1 (en) Methods and Systems for Generating Continuous Surfaces from Polygonal Data
CN114593742A (zh) 一种智能驾驶车辆的局部路径规划方法、装置及设备
CN116894308B (zh) 一种冲压模具压边圈型面多曲面生成方法和系统
US8994724B2 (en) Methods and systems for generating continuous surfaces from polygonal data
Bugeda et al. An integration of a low cost adaptive remeshing strategy in the solution of structural shape optimization problems using evolutionary methods
CN109726406B (zh) 车身接头优化设计方法、装置、终端及存储介质
Strömberg Reliability based design optimization by using a slp approach and radial basis function networks
CN113835397A (zh) 基于b样条曲线和路径积分的线性数控加工路径平滑方法
US7272541B2 (en) Method and system for generating and handling a harmonized network of points
Winter et al. NURBS-based shape and parameter optimization of structural components with an adaptive amount of control points
CN112507410B (zh) 轨道梁图纸的生成方法和生成装置
CN114996950A (zh) 驾驶轨迹规划方法、装置及电子设备
JP7431123B2 (ja) 曲面生成装置および曲面生成用プログラム
KR100344917B1 (ko) 선박용 1차곡면 외판의 가공정보 산출방법
JP4528962B2 (ja) 設計支援方法
CN116227221B (zh) 一种基于最优控制的二维轨迹重构方法
CN116822038B (zh) 基于数据驱动的异型封闭加筋拓扑优化方法
CN110648021B (zh) 一种两级电力负荷预测结果协调方法、装置及设备
CN117232513A (zh) 一种用于矿区场景的规划路径平滑方法及系统
Sun et al. Parametrization of high-speed train streamline shape
Liu et al. Computing point-set surfaces with controlled spatial variation of residuals

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