CN111665853B - 一种面向规划控制联合优化的无人车辆运动规划方法 - Google Patents
一种面向规划控制联合优化的无人车辆运动规划方法 Download PDFInfo
- Publication number
- CN111665853B CN111665853B CN202010645446.5A CN202010645446A CN111665853B CN 111665853 B CN111665853 B CN 111665853B CN 202010645446 A CN202010645446 A CN 202010645446A CN 111665853 B CN111665853 B CN 111665853B
- Authority
- CN
- China
- Prior art keywords
- matrix
- unmanned vehicle
- convex
- model
- planning
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 70
- 238000005457 optimization Methods 0.000 title claims abstract description 30
- 238000005096 rolling process Methods 0.000 claims abstract description 32
- 239000011159 matrix material Substances 0.000 claims description 97
- 230000006870 function Effects 0.000 claims description 59
- 230000004888 barrier function Effects 0.000 claims description 29
- 230000008569 process Effects 0.000 claims description 21
- 239000013598 vector Substances 0.000 claims description 18
- 238000013507 mapping Methods 0.000 claims description 17
- 238000012545 processing Methods 0.000 claims description 10
- 230000009467 reduction Effects 0.000 claims description 10
- 230000001133 acceleration Effects 0.000 claims description 9
- 230000004913 activation Effects 0.000 claims description 8
- 230000007704 transition Effects 0.000 claims description 8
- 206010048669 Terminal state Diseases 0.000 claims description 7
- 230000000295 complement effect Effects 0.000 claims description 6
- 241000764238 Isis Species 0.000 claims description 4
- 230000007613 environmental effect Effects 0.000 claims description 3
- 238000011478 gradient descent method Methods 0.000 claims description 3
- 230000002441 reversible effect Effects 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 238000011156 evaluation Methods 0.000 claims description 2
- 230000006872 improvement Effects 0.000 description 8
- 238000000354 decomposition reaction Methods 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000002787 reinforcement Effects 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000008094 contradictory effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 238000005312 nonlinear dynamic Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- 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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开一种面向规划控制联合优化的无人车辆运动规划方法,通过对车辆原始动力学模型进行辨识获得全局线性化模型,然后利用对全局线性化模型进行降阶得到简化模型;规划时,在考虑简化动力学模型的同时,也将非凸障碍约束进行凸化,来考虑存在的障碍物约束,规划模块结合简化模型和滚动时域ADP方法进行轨迹和速度等的规划,提高规划效率,保证规划与控制的一致性,使得车辆的运动结果有效地满足预想要求。
Description
技术领域
本发明涉及运动规划技术领域,具体是一种面向规划控制联合优化的无人车辆运动规划方法。
背景技术
近年来,无人驾驶作为人工智能的一个重要应用,受到了科研人员的广泛关注和研究。无人车的任务结构主要被分为三种:感知、规划以及控制。一般来说规划分为全局规划跟局部规划,全局规划通常是基于GPS等高精度定位地图,规划出一条从起点出发到终点的可行路径。而局部规划通常是在短距离范围内的规划,规划出当前点到前方一段距离的速度与轨迹曲线,当前点跟预瞄点之间可能存在障碍、车辆等外部干扰,因此局部路径需要优化求解出适合车辆驾驶的方案。
在无人车辆的驾驶过程中,需要规划模块处理由感知模块获取的外部环境,规划出一条全局(局部)路径,然后再将规划出来的路径信息传输给控制模块,由控制模块处理并进行车辆底层的控制。这种分层式的规划-控制方法已经被广泛应用在无人车辆驾驶当中,相关的专利与应用有很多。目前很多的路径规划算法都是基于感知模块传输过来的信息进行路径规划,这种的路径规划会产生出速度跟轨迹曲线。然而,在很多问题当中如果单纯分开考虑规划与控制的话,有时候效果不一定能够达到预期,甚至两个模块之间会发生冲突(如规划出来的路径信息是不符合车辆实际动力学特性),导致实际车辆的运动结果不满足预想的要求。在极限工况或者说是如要求高机动性的特殊情况下,不考虑车辆动力学模型的规划结果会无法满足性能要求。
发明内容
针对上述现有技术中无人车辆的路径规划导致实际车辆的运动结果不满足预想的要求的问题,本发明提供一种面向规划控制联合优化的无人车辆运动规划方法,提高规划效率,保证规划与控制的一致性,使得车辆的运动结果有效地满足预想要求。
为实现上述目的,本发明提供一种面向规划控制联合优化的无人车辆运动规划方法,包括如下步骤:
步骤1,基于模型辨识的方法得到无人车辆的高维全局线性化模型;
步骤2,对高维全局线性化模型进行降维,得到与无人车辆控制层动力学特性具有一致性的简化模型;
步骤3,对无人车辆运动路径上的非凸障碍物进行凸化处理,得到无人车辆运动规划中的凸约束形式;
步骤4,基于简化模型与无人车辆运动规划的凸约束进行不断地滚动优化,学习得到无人车辆在当前时刻的近似最优速度与参考轨迹曲线;
步骤5,将无人车辆在当前时刻的近似最优速度与参考轨迹曲线输出至无人车辆的控制层,在更新环境障碍信息及简化模型的状态变量后重复步骤3-5。
作为上述技术方案的进一步改进,步骤1中,所述高维全局线性化模型为:
式中,∑表示高维全局线性化模型;是高维的抽象状态变量,Nξ表示高维全局线性化模型的状态变量维数,Nξ>nξ,其中nξ表示***原始状态变量维数;/>是线性状态转移矩阵;/>是***输入映射矩阵,其中m表示控制输入矩阵的维数;是***输出矩阵,其中n表示***输出的维数,/>m是控制变量的维数。
作为上述技术方案的进一步改进,步骤2中,所述对高维全局线性化模型进行降维的过程为:
步骤2.1,获取满足且/>的子空间/>其中,式中,G=(I-A)-1B为高维全局线性化模型的增益函数,Im G表示矩阵G的值域,Ker C表示矩阵C的核,/>表示低维抽象状态变量的维数,表示降价的增益函数,β为映射函数;
步骤2.6,得到与无人车辆控制层的动力学特性的具有一致性的简化模型:
式中,表示简化模型,/>是简化模型的状态变量,其中/>表示简化模型的状态维数;/>是线性状态转移矩阵;/>是***输入映射矩阵,其中/>表示控制输入矩阵的维数;/>是***输出矩阵,其中/>表示***输出的维数,/>是控制变量的维数。
作为上述技术方案的进一步改进,步骤2中,所述对高维全局线性化模型进行降维的过程为:
在步骤1中,设置小的状态变量维数Nξ以及控制变量维数m,从而可以直接获得与无人车辆控制层动力学特性具有一致性的简化模型。
作为上述技术方案的进一步改进,步骤2中,简化模型中输入输出的范围约束与高维全局线性化模型输入输出的范围约束保持一致。
作为上述技术方案的进一步改进,步骤3中,所述对无人车辆运动路径上的非凸障碍物进行凸化处理,得到无人车辆运动规划中的凸约束形式,具体过程为:
将任意避障约束分解成若干个子集合的交集/>每个子集表示为其中,φi(x)表示关于x的平滑凸函数;因此存在正定矩阵Hi,使得/>成立,其中/>为n维空间内的方向向量且||z||≤c,c为一很小的定值;定义凸化得到的约束为/>对所有的障碍形式进行凸化处理:
所述凸约束能够通过椭圆型障碍函数进行表示。
作为上述技术方案的进一步改进,步骤4的具体过程为:
步骤4.1,根据状态量得到无人车辆在τ时刻速度v(τ)以及加速度a(τ):
式中,Δt表示预测时域的采样间隔,τ∈[k,k+N-1]表示预测时域内第τ时刻,其中l(τ)表示在全局坐标系下无人车在第τ时刻的横坐标,d(τ)表示在全局坐标系下无人车在第τ时刻的纵坐标,v(τ)表示在第τ时刻的速度;
步骤4.2,将无人车辆在当前时刻的近似最优速度与参考轨迹曲线的求解转换为代价最小化形式:
式中s(τ+1)表示简化模型在第τ+1时刻的状态变量,V(s(τ+1))表示第τ+1时刻的值函数;s(τ)表示简化模型在第τ时刻的状态变量,s(τ)=[l(τ),d(τ),υ(τ)];γ表示折扣因子;u(τ)表示τ时刻的动作;r(τ)表示τ时刻的回报函数,表示为:
式中,分别表示回报函数中与无人车辆的加速度、位置、速度以及车辆动力学约束相关的权值系数,Ja,τ,Jx,τ,Jv,τ,Je,τ分别表示回报函数中在τ时刻与加速度、位置、速度以及车辆动力学约束相关的代价;
步骤4.3,将状态量约束、控制量约束以及凸约束转换成包含在代价函数中的对数障碍函数的软约束形式,具体表示为连续可微的障碍函数乘以标量加权矩阵,因此当前时刻的近似最优速度与参考轨迹曲线的代价最小化形式就可以转换成只含模型等式约束问题:
式中B(s),B(u),Bf(s),B(Mz≤N)分别表示含有***状态、控制量、终端状态以及凸约束的对数障碍函数;式中μ表示标量系数;式中其中Q表示对状态的惩罚矩阵;式中R表示对控制量的惩罚矩阵;/>为终端值函数,终端状态不变集可表示为式中Z为对称正定矩阵,可表示为/>矩阵P是通过计算李雅普诺夫方程得到的:
式中K是一个反馈增益矩阵,使得F=A+BK是舒尔稳定的;
步骤4.4,采用滚动时域ADP方法对步骤4.3中只含模型等式约束问题进行求解,得到无人车辆在当前时刻的近似最优速度与参考轨迹曲线。
作为上述技术方案的进一步改进,步骤4.4中,所述采用滚动时域ADP方法具体为采用滚动时域增量式Actor-Critic结构进行滚动优化,所述滚动时域增量式Actor-Critic结构包括Actor网络和Critic网络,其中,Actor网络为执行网络,Critic网络为评价网络,滚动优化的具体过程为:
步骤4.4.1,初始化
设定预测时域为τ∈[k,k+N-1];
步骤4.4.2,令τ=k;
式中,R为正定的加权矩阵;
得到简化模型在预测时刻τ的近似最优速度与参考轨迹曲线σ*(s(τ));
步骤4.4.6,令τ=τ+1,更新Actor权值矩阵与Critic权值矩阵后重复步骤4.4.3-步骤4.4.6直至τ=k+N,得到无人车辆在当前时刻的近似最优速度与参考轨迹曲线。
作为上述技术方案的进一步改进,步骤4.4.6中,采用梯度下降法更新Actor权值矩阵与Critic权值矩阵:
式中,Wa(s(τ+1))表示Actor网络在τ+1预测时刻的权值矩阵,Wa(s(τ))表示Actor网络在τ预测时刻的权值矩阵,γτ表示Actor网络的学习步长,σ*(s(τ))表示近似最优速度与参考轨迹曲线;Wc(s(τ+1))表示Critic网络在τ+1预测时刻的权值矩阵,Wc(s(τ))表示Critic网络在τ预测时刻的权值矩阵,βτ表示Critic网络的学习步长,δc(s(τ))表示时域差分(Temporal Difference,TD)误差。
本发明提供的一种面向规划控制联合优化的无人车辆运动规划方法,通过对车辆原始动力学模型进行辨识获得全局线性化模型,然后利用对全局线性化模型进行降阶得到简化模型;规划时,在考虑简化动力学模型的同时,也将非凸障碍约束进行凸化,来考虑存在的障碍物约束,规划模块结合简化模型和滚动时域ADP方法进行轨迹和速度等的规划,提高规划效率,保证规划与控制的一致性,使得车辆的运动结果有效地满足预想要求。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图示出的结构获得其他的附图。
图1为本发明实施例中面向规划控制联合优化的无人车辆运动规划方法的流程示意图;
图2为本发明实施例中面向规划控制联合优化的无人车辆运动规划方法中学习预测控制框架的结构示意图;
图3为本发明实施例中模型降阶示意图。
本发明目的的实现、功能特点及优点将结合实施例,参照附图做进一步说明。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明的一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
需要说明,本发明实施例中所有方向性指示(诸如上、下、左、右、前、后……)仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等,如果该特定姿态发生改变时,则该方向性指示也相应地随之改变。
另外,在本发明中如涉及“第一”、“第二”等的描述仅用于描述目的,而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中,“多个”的含义是至少两个,例如两个,三个等,除非另有明确具体的限定。
在本发明中,除非另有明确的规定和限定,术语“连接”、“固定”等应做广义理解,例如,“固定”可以是固定连接,也可以是可拆卸连接,或成一体;可以是机械连接,也可以是电连接,还可以是物理连接或无线通信连接;可以是直接相连,也可以通过中间媒介间接相连,可以是两个元件内部的连通或两个元件的相互作用关系,除非另有明确的限定。对于本领域的普通技术人员而言,可以根据具体情况理解上述术语在本发明中的具体含义。
另外,本发明各个实施例之间的技术方案可以相互结合,但是必须是以本领域普通技术人员能够实现为基础,当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在,也不在本发明要求的保护范围之内。
如图1-2所示的一种面向规划控制联合优化的无人车辆运动规划方法,包括如下步骤:
步骤1,基于模型辨识的方法得到无人车辆的高维全局线性化模型;
步骤2,对高维全局线性化模型进行降维,得到与无人车辆控制层动力学特性具有一致性的简化模型;
步骤3,对无人车辆运动路径上的非凸障碍物进行凸化处理,得到无人车辆运动规划中的凸约束形式;
步骤4,基于简化模型与无人车辆运动规划的凸约束进行不断地滚动优化,学习得到无人车辆在当前时刻的近似最优速度与参考轨迹曲线;
步骤5,将无人车辆在当前时刻的近似最优速度与参考轨迹曲线输出至无人车辆的控制层,在更新环境障碍信息及简化模型的状态变量后重复步骤3-5。
步骤1中,模型辨识的方法可以是传统的辨识方法、也可以是机器学习辨识、或者基于Koopman算子及其近似的方法,进而使获得的线性化模型与原始模型的输入和输出的物理意义以及维数保持一致。
本实施例中模型辨识的方法具体采用基于Koopman算子及其近似的方法。Koopman算子及其近似由两部分组成,一个是Koopman算子本身,当观测值的样本维数足够大的时候就可以线性地表示非线性未知***。在实际使用过程中为了方便,通常使用Koopman算子的近似,即扩展动态模态分解(EDMD)。在样本空间内和逼近误差允许的条件下构造有限维Koopman算子的近似逼近,获得高维线性模型。
Koopman算子及其近似是为无人车的规划与控制提供精确的线性模型信息,其中控制部分使用该精确的线性模型,为保证无人驾驶车辆的高机动性以及规划和控制模块的一致性,本实施例中规划模块使用具备该动力学特性的简化模型。精确的线性模型信息是由有限维Koopman近似逼近的扩展动态模态分解(EDMD)实现,通过利用一组原始状态的可观测标量来定义一个新的高维状态或特征空间,并利用线性变换矩阵估计其演化。在所选观测对象的维数足够大的情况下,扩展动态模态分解可以理想地表示原始非线性动力学。在本实施例的实际实现过程中,为了操作方便,在样本空间内和逼近误差允许的条件下构造有限维Koopman算子的近似逼近,为规划模块和控制模块提供相应的动力学线性模型信息。
令基于数据辨识而获得的全局线性化模型表示如下:
式中,∑表示高维全局线性化模型;是高维的抽象状态变量,Nξ表示高维全局线性化模型的状态变量维数,Nξ>nξ,其中nξ表示***原始状态变量维数;/>是线性状态转移矩阵;/>是***输入映射矩阵,其中m表示控制输入矩阵的维数;是***输出矩阵,其中n表示***输出的维数,/>m是控制变量的维数。
步骤2中,对高维全局线性化模型进行降维即为模型降阶,其实施方式为:首先通过定义一个高阶的动力学模型的(稳态)增益函数,再构造一个降阶的(稳态)增益函数,然后希望找到一个满秩的映射函数矩阵来表达两个增益函数之间的映射关系,实现模型降阶,即如图3所示。简化模型的输入和输出与高维线性模型的维数保持一致,且当给定相同的输入时,输出也是相似的,其具体实施过程为:
步骤2.1,获取满足且/>的子空间/>其中,式中,G=(I-A)-1B为高维全局线性化模型的增益函数,Im G表示矩阵G的值域,Ker C表示矩阵C的核,/>表示低维抽象状态变量的维数,表示降价的增益函数,β为映射函数;
步骤2.6,得到与无人车辆控制层的动力学特性的具有一致性的简化模型:
式中,表示简化模型,/>是简化模型的状态变量,其中/>表示简化模型的状态维数;/>是线性状态转移矩阵;/>是***输入映射矩阵,其中/>表示控制输入矩阵的维数;/>是***输出矩阵,其中/>表示***输出的维数,/> 是控制变量的维数。
需要注意的是,简化模型也可以通过在步骤1中得到,具体实施过程为:设置小的状态变量维数Nξ以及控制变量维数n,从而可以直接获得与无人车辆控制层动力学特性具有一致性的简化模型。
在车辆的实际规划过程中,车辆与障碍的距离通常由笛卡尔坐标下的欧几里德距离表示,即
其中dmin>0是避障阈值。该约束是一个典型的非凸约束。如果应用滚动时域的ADP进行运动避障规划,由于非凸避障约束的存在,求解出的运动规划结果是一个局部近似最优解,甚至可能会导致问题发散造成求解的不可靠性。因此需要对避障约束进行凸逼近处理。因此在本实施例的步骤3中,对无人车辆运动路径上的非凸障碍物进行凸化处理,得到无人车辆运动规划中的凸约束形式,参考论文“Changliu L,Chung-Yen L,MasayoshiT.The Convex Feasible Set Algorithm for Real Time Optimization in MotionPlanning[J].Siam Journal on Control&Optimization,2018,56(4):2712-2733.”,其具体实施过程为:
将任意避障约束分解成若干个子集合的交集/>每个子集表示为其中,φi(x)表示关于x的平滑凸函数;因此存在正定矩阵Hi,使得/>成立,其中/>为n维空间内的方向向量且||z||≤c,c为一很小的定值;定义凸化得到的约束为/>对所有的障碍形式进行凸化处理:
所述凸约束能够通过椭圆型障碍函数进行表示。
综上,对于本身是凸的障碍物,自然将凸约束加入到运动规划的优化问题当中,采用滚动时域ADP进行求解。
对于本身是非凸的障碍物,采用上述凸化的方法来对非凸的障碍进行凸化,同样的,运动规划问题可以自然地采用滚动时域ADP进行求解。
步骤4中,滚动优化具体为滚动时域ADP方法,滚动时域ADP方法,是一种在预测时域内滚动优化学习的方法。包括Actor-Critic结构,以及滚动时域优化的思想。其中Actor表示执行器,Critic表示评价器。根据实际规划任务要求,先定义一个代价函数,在模型预测控制框架下,滚动优化学习得到近似最优速度与参考轨迹曲线。在预测时域k时刻到预测时域k+N-1时刻,每个时刻进行近似最优速度与轨迹规划曲线学习,最终学习得到近似最优速度与轨迹规划序列。将序列中的第一个含有近似最优速度与参考轨迹曲线元素输出给控制模块。下面介绍滚动时域Actor-Critic结构下的近似最优速度与参考轨迹曲线的求解过程:
根据强化学习理论,优化问题形式化表示如下:
Vπ(s(τ))=r(s(τ),u(τ))+γEπV(s(τ+1))
优化目标是最小化上述状态值函数,最优策略π*满足如下Bellman最优方程:
式中,π*表示近似最优速度与参考轨迹曲线;表示最优状态值函数;τ∈[k,k+N-1]表示预测时域内第τ时刻,s(τ)表示状态变量,s(τ)=[l(τ),d(τ),v(τ)],s(τ+1)表示下一个状态变量;u(τ)表示τ时刻的动作;r(·)表示回报函数,可以表示为:
式中,分别表示回报函数中与加速度、位置、速度以及车辆动力学约束相关的权值系数,在实际问题中可以根据车辆的舒适性、安全性等性能要求进行调整。Ja,τ,Jx,t,Jv,t,Je,τ分别表示回报函数中,在τ时刻与加速度、位置、速度以及车辆动力学约束相关的代价。其中车辆动力学约束的模型信息是基于上述求解的简化车辆模型/>
利用前向差分可以根据状态量求得车辆在τ时刻速度v(τ)以及加速度a(τ):
式中Δt表示预测时域的采样间隔。
Vπ(s(τ))=r(s(τ),u(τ))+γEπV(s(τ+1))
求解最优策略的方法是求解离散时间哈密顿-雅可比-贝尔曼(Hamilton-Jacobi-Bellman equation,HJB)方程,最小化Vπ(s(τ)):
将状态量约束、控制量约束以及凸约束转换成包含在代价函数中的对数障碍函数的软约束形式,具体表示为连续可微的障碍函数乘以标量加权矩阵,因此当前时刻的近似最优速度与参考轨迹曲线的代价最小化形式就可以转换成只含模型等式约束问题:
式中B(s),B(u),Bf(s),B(Pz≤M)分别表示含有***状态、控制量、终端状态以及凸约束的对数障碍函数;式中其中Q表示对状态的惩罚矩阵;式中R表示对控制量的惩罚矩阵;/>为终端值函数,终端状态不变集可表示为/>式中Z为对称正定矩阵,可表示为/>矩阵P是通过计算李雅普诺夫方程得到的:
式中K是一个反馈增益矩阵,使得F=A+BK是舒尔稳定的;
障碍函数的特性是在约束不满足的情况下,障碍函数的输出值会很大,趋于正无穷,在满足约束条件的情况下,障碍函数的输出值会很小;同时对数障碍函数梯度中心化处理,保证了在原点附近的代价函数的值为0,本实施例中障碍函数的定义根据机器人的初始状态数据具体分为两种情况:
障碍函数梯度中心化处理。对障碍函数梯度中心化处理,得到新的障碍函数B(z),/>对于任意z,B(z)都是凸且可导的,且有B(0)=0。***状态变量/>和控制量/>的障碍函数,状态和控制量的范围是多面体不变集;终端状态的障碍函数/>终端状态的可行范围是椭圆形不变集。
而由于加入了非线性障碍函数,常规的手段很难解析地求解出哈密顿-雅可比-贝尔曼(HJB)方程,而这时候就使用基于Actor--Critic强化学习算法来实现运动规划。
滚动时域增量式Actor-Critic结构由Actor网络(执行器)和Critic网络(评价器)两部分组成,在滚动时域内进行近似最优策略学习,减少滚动时域内优化求解的计算量。以及包括基于障碍函数的状态量约束和控制量约束处理,主要是在滚动时域内的Actor-Critic设计结构中使用对数障碍函数来对状态量约束和控制量约束进行软处理,收缩约束的限制规模,减少学习过程中的失败次数。
Actor-Critic结构下的近似最优思路是在无法解析求解哈密顿-雅可比-贝尔曼(HJB)方程的前提下使用的,是一种近似最优的方法,其过程为:
Actor网络的输出估计值:
式中,是Actor权值矩阵,ha是基函数向量,τ∈[k+1,k+N-1],ha(s(τ))是一个元素为激活函数的向量,本实施例中,ha(s(τ))表示在τ预测时刻Actor网络的状态输入分别经过激活函数映射后的向量,例如,当激活函数为tanh函数时:
Critic网络的输出估计:
式中,是Critic权值矩阵,hc是基函数向量,τ∈[k+1,k+N-1],本实施例中,hc(s(τ))表示在τ预测时刻Critic网络的状态输入分别经过激活函数映射后的向量,例如,当激活函数为tanh函数时:
式中,R为正定的加权矩阵;
得到简化模型在预测时刻τ的近似最优速度与参考轨迹曲线σ*(s(τ))。
Actor-Critic结构参数更新方式跟所用的ADP方法有关,一般来说有增量式和迭代式两种,下面以基于梯度下降法,一种增量式的更新方式为例。
Actor参数更新:
其中γτ表示执行器的学习步长。
在状态为s(τ)时,TD误差定义为:
Critic网络的目标是最小化上述TD误差,因此Critic参数更新为:
其中,βτ表示评价器的学习步长,δc(s(τ))表示第τ个预测时刻Critic网络的代价函数。
需要注意的是,上述滚动时域增量式Actor-Critic结构主要是采用滚动时域ADP方法中的DHP方法,本实施例中不对该实施方法做出具体限定,也可以采用ADP方法中的HDP方法。
以上所述仅为本发明的优选实施例,并非因此限制本发明的专利范围,凡是在本发明的发明构思下,利用本发明说明书及附图内容所作的等效结构变换,或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。
Claims (8)
1.一种面向规划控制联合优化的无人车辆运动规划方法,其特征在于,包括如下步骤:
步骤1,基于模型辨识的方法得到无人车辆的高维全局线性化模型;
步骤2,对高维全局线性化模型进行降维,得到与无人车辆控制层动力学特性具有一致性的简化模型;
步骤3,对无人车辆运动路径上的非凸障碍物进行凸化处理,得到无人车辆运动规划中的凸约束形式;
步骤4,基于简化模型与无人车辆运动规划的凸约束进行不断地滚动优化,学习得到无人车辆在当前时刻的近似最优速度与参考轨迹曲线;
步骤5,将无人车辆在当前时刻的近似最优速度与参考轨迹曲线输出至无人车辆的控制层,在更新环境障碍信息及简化模型的状态变量后重复步骤3-5;
步骤4的具体过程为:
步骤4.1,根据状态量得到无人车辆在τ时刻速度v(τ)以及加速度a(τ):
式中,Δt表示预测时域的采样间隔,τ∈[k,k+N-1]表示预测时域内第τ时刻,其中l(τ)表示在全局坐标系下无人车在第τ时刻的横坐标,d(τ)表示在全局坐标系下无人车在第τ时刻的纵坐标,v(τ)表示在第τ时刻的速度;
步骤4.2,将无人车辆在当前时刻的近似最优速度与参考轨迹曲线的求解转换为代价最小化形式:
式中s(τ+1)表示简化模型在第τ+1时刻的状态变量,V(s(τ+1))表示第τ+1时刻的值函数;s(τ)表示简化模型在第τ时刻的状态变量,s(τ)=[l(τ),d(τ),v(τ)];表示折扣因子;u(τ)表示τ时刻的动作;r(τ)表示τ时刻的回报函数,表示为:
式中,分别表示回报函数中与无人车辆的加速度、位置、速度以及车辆动力学约束相关的权值系数,Ja,τ,Jx,τ,Jv,τ,Je,τ分别表示回报函数中在τ时刻与加速度、位置、速度以及车辆动力学约束相关的代价;
步骤4.3,将状态量约束、控制量约束以及凸约束转换成包含在代价函数中的对数障碍函数的软约束形式,具体表示为连续可微的障碍函数乘以标量加权矩阵,因此当前时刻的近似最优速度与参考轨迹曲线的代价最小化形式就可以转换成只含模型等式约束问题:
式中B(s),B(u),Bf(s),B(Mz≤N)分别表示含有***状态、控制量、终端状态以及凸约束的对数障碍函数;式中μ表示标量系数;式中其中Q表示对状态的惩罚矩阵;式中R表示对控制量的惩罚矩阵;/>为终端值函数,终端状态不变集可表示为式中Z为对称正定矩阵,可表示为/>矩阵P是通过计算李雅普诺夫方程得到的:
式中K是一个反馈增益矩阵,使得F=A+BK是舒尔稳定的;
步骤4.4,采用滚动时域ADP方法对步骤4.3中只含模型等式约束问题进行求解,得到无人车辆在当前时刻的近似最优速度与参考轨迹曲线。
3.根据权利要求2所述面向规划控制联合优化的无人车辆运动规划方法,其特征在于,步骤2中,所述对高维全局线性化模型进行降维的过程为:
步骤2.1,获取满足且/>的子空间/>其中,式中,G=(I-A)-1B为高维全局线性化模型的增益函数,ImG表示矩阵G的值域,Ker C表示矩阵C的核,/>表示低维抽象状态变量的维数,表示降价的增益函数,β为映射函数;
步骤2.6,得到与无人车辆控制层的动力学特性的具有一致性的简化模型:
4.根据权利要求2所述面向规划控制联合优化的无人车辆运动规划方法,其特征在于,步骤2中,所述对高维全局线性化模型进行降维的过程为:
在步骤1中,设置小的状态变量维数Nξ以及控制变量维数m,从而可以直接获得与无人车辆控制层动力学特性具有一致性的简化模型。
5.根据权利要求1所述面向规划控制联合优化的无人车辆运动规划方法,其特征在于,步骤2中,简化模型中输入输出的范围约束与高维全局线性化模型输入输出的范围约束保持一致。
6.根据权利要求1所述面向规划控制联合优化的无人车辆运动规划方法,其特征在于,步骤3中,所述对无人车辆运动路径上的非凸障碍物进行凸化处理,得到无人车辆运动规划中的凸约束形式,具体过程为:
将任意避障约束分解成若干个子集合的交集/>每个子集表示为其中,φi(x)表示关于x的平滑凸函数;因此存在正定矩阵Hi,使得/>成立,其中/>为n维空间内的方向向量且||z||≤c,c为一很小的定值;定义凸化得到的约束为/>对所有的障碍形式进行凸化处理:
所述凸约束能够通过椭圆型障碍函数进行表示。
7.根据权利要求1所述面向规划控制联合优化的无人车辆运动规划方法,其特征在于,步骤4.4中,所述采用滚动时域ADP方法具体为采用滚动时域增量式Actor-Critic结构进行滚动优化,所述滚动时域增量式Actor-Critic结构包括Actor网络和Critic网络,其中,Actor网络为执行网络,Critic网络为评价网络,滚动优化的具体过程为:
步骤4.4.1,初始化
设定预测时域为τ∈[k,k+N-1];
步骤4.4.2,令τ=k;
式中,R为正定的加权矩阵;
得到简化模型在预测时刻τ的近似最优速度与参考轨迹曲线σ*(s(τ));
步骤4.4.6,令τ=τ+1,更新Actor权值矩阵与Critic权值矩阵后重复步骤4.4.3-步骤4.4.6直至τ=k+N,得到无人车辆在当前时刻的近似最优速度与参考轨迹曲线。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010645446.5A CN111665853B (zh) | 2020-07-07 | 2020-07-07 | 一种面向规划控制联合优化的无人车辆运动规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010645446.5A CN111665853B (zh) | 2020-07-07 | 2020-07-07 | 一种面向规划控制联合优化的无人车辆运动规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111665853A CN111665853A (zh) | 2020-09-15 |
CN111665853B true CN111665853B (zh) | 2023-03-31 |
Family
ID=72391454
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010645446.5A Active CN111665853B (zh) | 2020-07-07 | 2020-07-07 | 一种面向规划控制联合优化的无人车辆运动规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111665853B (zh) |
Families Citing this family (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112193244B (zh) * | 2020-09-30 | 2021-07-20 | 浙江大学 | 基于线性约束的自动驾驶车辆运动规划方法 |
CN112435504B (zh) * | 2020-11-11 | 2022-07-08 | 清华大学 | 一种车路协同环境下的集中式协同轨迹规划方法和装置 |
CN112987577B (zh) * | 2021-04-22 | 2021-07-30 | 中国人民解放军国防科技大学 | 一种无人车数据驱动自适应控制方法和装置 |
CN112989499B (zh) * | 2021-04-22 | 2024-07-02 | 中国人民解放军国防科技大学 | 一种无人车数据驱动控制方法和装置 |
CN113359471B (zh) * | 2021-07-05 | 2022-10-14 | 湖南大学 | 基于协状态辅助的自适应动态规划最优控制方法及*** |
CN113386767B (zh) * | 2021-07-13 | 2024-01-05 | 吉林大学 | 一种基于Koopman算子的四轮转向滚动时域控制方法 |
CN113788021B (zh) * | 2021-09-03 | 2022-08-12 | 东南大学 | 一种结合前车速度预测的自适应跟车巡航控制方法 |
CN113534669B (zh) * | 2021-09-17 | 2021-11-30 | 中国人民解放军国防科技大学 | 基于数据驱动的无人车控制方法、装置和计算机设备 |
CN114578834B (zh) * | 2022-05-09 | 2022-07-26 | 北京大学 | 基于目标分层双感知域的强化学习的无人车路径规划方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8700307B1 (en) * | 2013-03-04 | 2014-04-15 | Mitsubishi Electric Research Laboratories, Inc. | Method for determining trajectories manipulators to avoid obstacles |
WO2017144350A1 (en) * | 2016-02-25 | 2017-08-31 | Nec Europe Ltd. | Method for motion planning for autonomous moving objects |
JP6443905B1 (ja) * | 2017-12-28 | 2018-12-26 | 深セン市優必選科技有限公司 | ロボットの運動経路計画方法、装置、記憶媒体及び端末機器 |
CN110377039A (zh) * | 2019-07-26 | 2019-10-25 | 吉林大学 | 一种车辆避障轨迹规划与跟踪控制方法 |
CN110647042A (zh) * | 2019-11-11 | 2020-01-03 | 中国人民解放军国防科技大学 | 一种基于数据驱动的机器人鲁棒学习预测控制方法 |
CN111258323A (zh) * | 2020-03-30 | 2020-06-09 | 华南理工大学 | 一种智能车辆轨迹规划与跟踪的联合控制方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111381597B (zh) * | 2020-05-14 | 2021-08-03 | 吉林大学 | 一种基于高斯伪谱法的车辆避障轨迹规划方法 |
-
2020
- 2020-07-07 CN CN202010645446.5A patent/CN111665853B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8700307B1 (en) * | 2013-03-04 | 2014-04-15 | Mitsubishi Electric Research Laboratories, Inc. | Method for determining trajectories manipulators to avoid obstacles |
WO2017144350A1 (en) * | 2016-02-25 | 2017-08-31 | Nec Europe Ltd. | Method for motion planning for autonomous moving objects |
JP6443905B1 (ja) * | 2017-12-28 | 2018-12-26 | 深セン市優必選科技有限公司 | ロボットの運動経路計画方法、装置、記憶媒体及び端末機器 |
CN110377039A (zh) * | 2019-07-26 | 2019-10-25 | 吉林大学 | 一种车辆避障轨迹规划与跟踪控制方法 |
CN110647042A (zh) * | 2019-11-11 | 2020-01-03 | 中国人民解放军国防科技大学 | 一种基于数据驱动的机器人鲁棒学习预测控制方法 |
CN111258323A (zh) * | 2020-03-30 | 2020-06-09 | 华南理工大学 | 一种智能车辆轨迹规划与跟踪的联合控制方法 |
Non-Patent Citations (1)
Title |
---|
丁一, 陈婷.基于滚动时域优化策略的多载AGV 充电调度.中国航海.2020,第第43卷卷(第第43卷期),全文. * |
Also Published As
Publication number | Publication date |
---|---|
CN111665853A (zh) | 2020-09-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111665853B (zh) | 一种面向规划控制联合优化的无人车辆运动规划方法 | |
CN110647042B (zh) | 一种基于数据驱动的机器人鲁棒学习预测控制方法 | |
CN113093536B (zh) | 非线性***执行器故障的自适应增量优化容错控制方法 | |
Schoellig et al. | Optimization-based iterative learning for precise quadrocopter trajectory tracking | |
US9156165B2 (en) | Adaptive critic apparatus and methods | |
Yan et al. | Consensus formation tracking for multiple AUV systems using distributed bioinspired sliding mode control | |
CN112987567B (zh) | 非线性***的固定时间自适应神经网络滑模控制方法 | |
CN111273549B (zh) | 一种智能船舶自动舵***的模糊自适应输出反馈容错控制方法及*** | |
Hameduddin et al. | Nonlinear generalised dynamic inversion for aircraft manoeuvring control | |
CN111258218B (zh) | 基于最大相关熵准则的智能车辆路径跟踪方法 | |
CN111007724A (zh) | 一种基于区间ii型模糊神经网络的高超音速飞行器指定性能量化跟踪控制方法 | |
Qiu et al. | Robust path‐following control based on trajectory linearization control for unmanned surface vehicle with uncertainty of model and actuator saturation | |
CN112416021A (zh) | 一种基于学习的旋翼无人机路径跟踪预测控制方法 | |
Xu et al. | Predefined-time hierarchical coordinated neural control for hypersonic reentry vehicle | |
Wang et al. | Path Following Control for Unmanned Surface Vehicles: A Reinforcement Learning-Based Method With Experimental Validation | |
Zhang et al. | A safety planning and control architecture applied to a quadrotor autopilot | |
Zhou et al. | Finite‐time spatial path following control for a robotic underactuated airship | |
CN111176117B (zh) | 一种无人直升机的模糊自适应弹性控制方法 | |
Ruano et al. | Supervised training algorithms for B-Spline neural networks and neuro-fuzzy systems | |
CN116880184A (zh) | 无人船轨迹追踪预测控制方法、***及存储介质 | |
Ma et al. | Adaptive neural network control of a non‐linear two‐degree‐of‐freedom helicopter system with prescribed performance | |
CN115903842A (zh) | 一种多智能体编队控制方法、***及电子设备 | |
Yang et al. | DO-based adaptive consensus control for multiple MUAVs with dynamic constraints | |
Lampton et al. | Morphing airfoils with four morphing parameters | |
Niu et al. | Broad learning control of a two-link flexible manipulator with prescribed performance and actuator faults |
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 |