一种基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制方法、系统及车载控制设备

1.本发明涉及无人驾驶车辆控制技术领域，更具体地说，是涉及一种基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制方法、系统及车载控制设备。


背景技术：

2.无人驾驶技术可以显著提升交通效率、降低交通事故发生率并提高能源使用效率，已成为近年来汽车行业的研究热点。随着无人驾驶技术的快速发展，其在众多领域的应用场景变得越来越广泛。无人小巴车、无人公交车、无人运输卡车等基本实现了特定场景的试运行，且在特定道路和园区等场景实现了初步的商业化落地。无人驾驶技术主要包括感知、决策、控制三个部分，而轨迹跟踪控制可以实现车辆从当前位置稳定、快速地到达目标位置，是无人驾驶技术中尤为重要的一环。
3.当前，无人驾驶车辆轨迹跟踪控制方法可以分为基于模型的控制方法和无模型的控制方法。基于模型的控制方法被成功地用于控制复杂动态系统。在无模型控制方法中，代理必须从大量样本中为每个任务学习新的策略。传统无人驾驶车辆轨迹跟踪控制器的设计依赖于使用系统的动态模型来计算当前任务的控制律。与无模型的控制方法相比，由于模型的不精确性及外界干扰的不确定性，基于模型的控制方法通常在渐近性能上略有不足。在高度动态的环境中运行的真实车辆上采集样本可能是极其危险的，并且对这类动态系统来说，使用无模型的控制方法所产生的效果通常无法达到预期。在实际的动态系统上，基于模型的控制方法需要具备一定的专业知识，以及无人驾驶车辆中执行器、轮胎力、侧滑等复杂动力学的建模。这些约束在一定程度上限制了基于模型的控制方法在真实车辆系统上的应用。


技术实现要素：

4.为了解决现有技术中的不足，本发明提出了一种基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制方法及系统。本发明利用多层神经网络来学习无人驾驶车辆的动力学模型，将该模型用于迭代线性二次型调节器控制算法的设计并推导出控制律，实现无人驾驶车辆的轨迹跟踪控制任务。本发明主要包括基于神经网络的动力学模型的建立，迭代线性二次型调节器的控制算法的设计，轨迹跟踪控制器的建立和性能评估三个部分。
5.在基于神经网络的动力学模型的建立中，使用全连接前馈神经网络设计了神经网络模型，模型有两层隐藏层，每层有64个神经元，选择使用relu激活函数，模型的输入为当前时刻的车辆状态与控制指令，输出为下一时刻的车辆状态，该神经网络通过最小化预测的输出状态和观测的输出状态之间的均方误差来学习动态方程。本发明通过车载设备来收集驾驶行为的数据，即利用搭载有惯性测量单元(imu)等车载设备的实车在公路和非公路环境中人工驾驶一小时，以完成数据的采集。
6.基于所训练的神经网络动力学模型设计ilqr控制器，所述ilqr控制器内包含了迭
代线性二次型调节器的控制算法，通过最小化迭代线性二次型调节器控制算法中的成本函数求得最优控制指令，以实现对参考轨迹的跟踪。
7.进一步，所述神经网络动力学模型具体如下：
8.设n维实向量表示在离散时刻t的系统状态，m维实向量表示在离散时刻t的系统控制指令，系统动态可以由下式给出：
9.x
t 1
＝f(x
t
,u
t
)
ꢀꢀ
(1)
10.车辆系统状态由给出，其中v
t
是在时刻t的线速度，是在时刻t的转向角速度，控制指令由给出，其中p
t
是油门踏板输入，b
t
是制动输入，是在时刻t的指令转向角速度。现在给出车辆的动态方程f如下式：
[0011][0012]
进一步，所述迭代线性二次型调节器的控制算法具体如下：
[0013]
针对设计的神经网络动力学模型这一非线性离散动态系统：
[0014]
x
t 1
＝f(x
t
,u
t
)
ꢀꢀ
(3)
[0015]
其中，是系统状态，是在时刻t的控制输入。
[0016]
将非线性环境和成本函数通过泰勒展开局部线性化，之后通过lqr的向后传递过程解出在线性化环境和成本下的最优解，ilqr算法分为线性化、lqr的反向传递和前向传递三部分；
[0017]
在线性化部分，将ilqr中非线性动态展开成一阶，成本展开成二阶，
[0018][0019][0020]
其中，为在状态时生成的最优控制序列，则下一个状态为时生成的最优控制序列，则下一个状态为为对应于状态x
t
及控制u
t
的函数的导数，c(x
t
,u
t
)为在状态x
t
及控制输入u
t
下的成本函数。
[0021]
将上式(4)进行重排列：
[0022][0023][0024]
将作为新的状态，作为新的控制输入，则下一个状态为q(δx
t
,δu
t
)为在状态δx
t
及控制输入δu
t
下的成本函数。从而得到：
[0025][0026][0027]
其中，其中，二次部分的系数矩阵(其中，代表平均划分矩阵c
t
的左上角部分，代表矩阵c
t
的右上角部分，代表矩阵c
t
的左下角部分，代表矩阵c
t
的右下角部分)，线性部分的系数矩阵(其中，代表平均划分矩阵c
t
的上半部分，代表矩阵c
t
的下半部分)。这样就将ilqr问题转化为常规的lqr问题；
[0028]
在lqr的反向传递中，本发明通过最小化成本函数q(δx
t
,δu
t
)求得最优控制。在时刻t时，通过计算成本函数q(δx
t
,δu
t
)的一阶导数为零时的δu
t
来得到在此时刻的最优控制。即令
[0029][0030]
可得到时刻t的最优控制
[0031][0032]
将式(8)展开并用矩阵k
t
及k
t
来表示，于是可以得到：
[0033][0034]
δu
t
＝k
t
δx
t
k
t
ꢀꢀ
(10)
[0035]
将式(10)带入式(6)的成本函数q(δx
t
,δu
t
)中，将新得到的成本函数记为v(δx
t
)：
[0036][0037]
将式(11)展开可以得到：
[0038][0039]
对展开式进行适当简化，将其看作形如的二次项与一次项的和，其中，矩阵v
t
及v
t
分别为：
[0040]
[0041][0042]
其中，系数矩阵表示矩阵c
t
的下半部分。
[0043]
在时刻t-1时，有式(14)成立：
[0044][0045]
则从时刻t到时刻t-1所产生的成本函数为：
[0046][0047]
再由可得：
[0048][0049]
则成本函数q(δx
t-1
,δu
t-1
)表示为：
[0050][0051]
其中，t-1时刻二次部分的系数矩阵(其中，代表矩阵q
t-1
的左上角部分，代表矩阵q
t-1
的右上角部分，代表矩阵q
t-1
的左下角部分，代表矩阵q
t-1
的右下角部分)，线性部分的系数矩阵(其中，代表矩阵q
t-1
的上半部分，代表矩阵q
t-1
的下半部分)。在时刻t-1时，通过计算成本函数q(δx
t-1
,δu
t-1
)的一阶导数为零时的δu
t-1
来得到在此时刻的最优控制。即令
[0052][0053]
可以得到时刻t-1的最优控制：
[0054]
δu
t-1
＝k
t-1
δx
t-1
k
t-1
ꢀꢀ
(19)
[0055]
其中
[0056]
将式(20)带入式(17)中，对展开式进行简化处理可以得到：
[0057][0058][0059]
其中，系数矩阵表示矩阵q
t-1
的下半部分。
[0060]
在lqr的前向传递中，该过程要和真实的非线性系统进行交互，得到新的真实轨迹；
[0061]
[0062]
其中，为在状态时生成的最优控制序列，则下一个状态为参数α可以通过回溯行搜索进行调整。将时刻n的状态相结合从而得到新的轨迹。
[0063]
进一步，所述轨迹跟踪控制器具体如下：
[0064]
设表示轴距为l的车辆在离散时刻i的状态，其中{xi,yi,θi}是姿态，xi和yi分别表示世界坐标系下车辆的纵向和横向坐标，θi是车辆的航向角，φi是转向角，vi是速度，是离散时刻i的转向角变化率，控制命令由给出，其中pi是油门踏板输入，bi是制动输入，是离散时刻i的指令转向角变化率，用π表示状态转移函数f：
[0065]si 1
＝π(si,ui)
ꢀꢀ
(23)
[0066]
由车辆在典型状态si下的自行车模型，车辆的动态方程π(即状态转移函数f)可由以下公式定义：
[0067]
x
i 1
＝xi v
i cos(θi)δt
[0068]yi 1
＝yi v
i sin(θi)δt
[0069][0070][0071][0072]
给出一组具有速度的m个有序姿态，对它们进行三次样条插值，得到参考轨迹，对于每个状态si，将相对于该参考轨迹的误差状态ψi定义为一个9元元组是在状态si的车辆与参考轨迹的垂向距离，是车辆相对于参考轨迹的航向误差，是对应于参考轨迹上最近点的速度误差(这里v
p
是参考轨迹上最近点的速度)，vi，和φi来自于状态si，将误差状态ψi用于迭代线性二次型调节器的状态，从而对参考轨迹的所有误差进行编码，给定在离散时刻i 的误差状态ψi和控制ui，用γ表示状态转移函数f，下一时刻的误差状态ψ
i 1
如下式：
[0073]
ψ
i 1
＝γ(ψi,ui)
ꢀꢀ
(25)
[0074]
γ可以由以下公式定义：
[0075][0076][0077][0078]
[0079][0080][0081][0082][0083]
在误差状态ψi下执行ui的成本l(ψi,ui)如下：
[0084][0085]
其中，a和b是对角权重矩阵，a的最后3个对角线元素为零，使(14)中的误差项趋近于零，对于状态ψ，其最终成本lf(ψ)如下所示：
[0086]
lf(ψ)＝ψ
t
aψ
ꢀꢀ
(28)
[0087]
将具有给定参考轨迹的车辆轨迹跟踪问题定义为去找到范围为n的最优控制序列 {u0,u1,
…
,u
n-2
}，从而最小化以下成本：
[0088][0089]
加上如下的约束：
[0090][0091]
上述控制系统可集成在车载控制设备内，实现无人驾驶车辆的实时轨迹跟踪。
[0092]
本发明的有益效果：
[0093]
1、本发明提出了通过车载设备来收集驾驶行为数据的方法，即利用搭载有惯性测量单元(imu)等车载设备的实车在公路和非公路环境中人工驾驶一小时，来完成数据的采集。所得数据作为车辆动力学模型的输入。由于数据直接来自于人类驾驶的实车数据，相较于驾驶模拟器所得的数据精度更高，种类更多，而且将驾驶员的驾驶风格也考虑在内。
[0094]
2、本发明提出基于全连接前馈神经网络来设计车辆的神经网络动力学模型，模型有两层隐藏层，与深度神经网络相比不仅提高了计算效率，而且可以准确识别车辆运行过程中各种复杂的动力学行为，并可以学习未建模的车辆动态响应。
[0095]
3、本发明通过将所设计的神经网络动力学模型与迭代线性二次型调节器控制算法相结合，相比于端到端的控制算法，所提出的算法可解释性更强，并且在不同的路况及行驶工况下可以实现参考轨迹的跟踪控制，在保证轨迹跟踪精度的同时，兼顾了横向和纵向稳定性，为无人驾驶车辆开发高性能的运动控制器奠定良好的基础。
附图说明
[0096]
图1为本发明实施例基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制方法的流程图；
[0097]
图2为车辆动力学数据采集模块；
[0098]
图3为车辆的自行车模型和参考轨迹的误差状态图；
[0099]
图4为用于评估轨迹跟踪性能的圆形参考轨迹；
[0100]
图5为用于评估轨迹跟踪性能的椭圆形参考轨迹；
[0101]
图6为用于评估轨迹跟踪性能的蛇形参考轨迹；
[0102]
图7为用于评估轨迹跟踪性能的“8”形参考轨迹；
[0103]
图8为用于评估轨迹跟踪性能的混合参考轨迹。
具体实施方式
[0104]
下面结合附图对本发明作进一步说明。
[0105]
需要说明的是，本发明所有涉及的分割或划分，如矩阵的分割/划分等，都是平均分割或划分。
[0106]
图1为基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制方法的流程图，包括模型训练学习和利用基于神经网络动力学模型的ilqr控制器实现轨迹跟踪，总体过程如下：
[0107]
模型训练学习：通过利用搭载有惯性测量单元(imu)等车载设备的实车在公路和非公路环境中人工驾驶一小时的方式收集车辆的数据。基于全连接前馈神经网络设计车辆动力学模型，并利用所建立的模型和车辆数据对模型进行训练。
[0108]
利用基于神经网络动力学模型的ilqr控制器实现轨迹跟踪：通过训练神经网络来学习车辆的动力学方程，并设计ilqr控制器，通过最小化成本函数求得最优控制，以达到对参考轨迹的跟踪。
[0109]
具体实施过程如下：
[0110]
步骤1：建立基于神经网络的动力学模型
[0111]
设n维实向量表示在离散时刻t的系统状态，m维实向量表示在离散时刻t的系统控制指令。系统动态可以由下式给出：
[0112]
x
t 1
＝f(x
t
,u
t
)
ꢀꢀ
(31)
[0113]
车辆系统状态由给出，其中v
t
是在时刻t的线速度，是在时刻t的转向角速度。控制指令由给出，其中p
t
是油门踏板输入，b
t
是制动输入，是在时刻t的指令转向角速度。现在给出车辆的动态方程f如下式：
[0114][0115]
图2为车辆动力学数据采集模块。本发明通过利用搭载有惯性测量单元(imu)等车载设备的实车在公路和非公路环境中人工驾驶一小时的方式来收集车辆的数据 (x
t 1
,x
t
,u
t
)。通过观察训练过程中的训练损失和验证损失，利用试验和误差来确定驾驶时间。根据车辆的硬件规格确定数据采样频率为30hz。
[0116]
x
t
，u
t
被用作神经网络的输入，x
t 1
被用作神经网络的输出，该神经网络通过最小化预测的输出状态和观测的输出状态x
t 1
之间的均方误差来学习动态方程f。在将数据提
供给神经网络的输入层之前，先对数据进行白化处理，以降低输入数据特征之间的冗余性。本发明的模型架构采用具有两个隐藏层的全连接神经网络。其中，每个隐藏层有64个神经元，每个神经元均使用了relu激活函数。
[0117]
步骤2：设计迭代线性二次型调节器(ilqr)的控制算法
[0118]
考虑一个非线性离散动态系统：
[0119]
x
t 1
＝f(x
t
,u
t
)
ꢀꢀ
(33)
[0120]
其中，是系统状态，是在时刻t的控制输入。
[0121]
迭代线性二次型调节器(ilqr)控制算法可以处理非线性环境和成本，即ilqr 控制算法将非线性环境和成本函数通过泰勒展开局部线性化，之后通过lqr的向后传递过程解出在线性化环境和成本下的最优解。ilqr算法可以分为线性化、lqr的反向传递和前向传递三部分。
[0122]
在线性化部分，将ilqr中非线性动态展开成一阶，成本展开成二阶。
[0123][0124][0125]
其中，为在状态时生成的最优控制序列，则下一个状态为时生成的最优控制序列，则下一个状态为为对应于状态x
t
及控制u
t
的函数的导数，c(x
t
,u
t
)为在状态x
t
及控制输入u
t
下的成本函数。
[0126]
将式(34)进行简单的重排列：
[0127][0128][0129]
将作为新的状态，作为新的控制输入，则下一个状态为q(δx
t
,δu
t
)为在状态δx
t
及控制输入δu
t
下的成本函数。从而得到：
[0130][0131][0132]
其中，其中，二次部分的系数矩阵(其中，代表平均划分矩阵c
t
的左上角部分，
代表矩阵c
t
的右上角部分，代表矩阵c
t
的左下角部分，代表矩阵c
t
的右下角部分)，线性部分的系数矩阵(其中，代表平均划分矩阵c
t
的上半部分，代表矩阵c
t
的下半部分)。这样就将ilqr问题转化为常规的lqr问题了。
[0133]
在lqr的反向传递中，本发明通过最小化成本函数q(δx
t
,δu
t
)求得最优控制。在时刻t时，通过计算成本函数q(δx
t
,δu
t
)的一阶导数为零时的δu
t
来得到在此时刻的最优控制。即令
[0134][0135]
可得到时刻t的最优控制
[0136][0137]
将式(38)展开并用矩阵k
t
及参数k
t
来表示，于是可以得到：
[0138][0139][0140]
δu
t
＝k
t
δx
t
k
t
ꢀꢀ
(40)
[0141]
将式(40)带入式(36)的成本函数q(δx
t
,δu
t
)中，将新得到的成本函数记为v(δx
t
)：
[0142][0143]
将式(41)展开可以得到：
[0144][0145]
对式(42)进行适当简化，将其看作形如的二次项与一次项的和，其中，矩阵v
t
及参数v
t
分别为：
[0146][0147][0148]
其中，系数矩阵表示矩阵c
t
的下半部分。
[0149]
在时刻t-1时，有式(44)成立：
[0150][0151]
则从时刻t到时刻t-1所产生的成本函数为：
[0152][0153]
再由可得：
[0154][0155]
则成本函数q(δx
t-1
,δu
t-1
)表示为：
[0156][0157]
其中，t-1时刻二次部分的系数矩阵(其中，代表矩阵q
t-1
的左上角部分，代表矩阵q
t-1
的右上角部分，代表矩阵q
t-1
的左下角部分，代表矩阵q
t-1
的右下角部分)，线性部分的系数矩阵(其中，代表矩阵q
t-1
的上半部分，代表矩阵q
t-1
的下半部分)。在时刻t-1时，通过计算成本函数q(δx
t-1
,δu
t-1
)的一阶导数为零时的δu
t-1
来得到在此时刻的最优控制。即令
[0158][0159]
可以得到时刻t-1的最优控制：
[0160]
δu
t-1
＝k
t-1
δx
t-1
k
t-1
ꢀꢀ
(49)
[0161]
其中
[0162]
将式(50)代入式(47)中，对展开式进行简化处理可以得到：
[0163][0164][0165]
其中，系数矩阵表示矩阵q
t-1
的下半部分。
[0166]
在lqr的前向传递中，要注意该过程要和真实的非线性系统进行交互，得到新的真实轨迹。
[0167][0168]
其中，为在状态时生成的最优控制序列，则下一个状态为参数α可以通过回溯行搜索进行调整。
[0169]
将时刻n的状态与相结合从而得到新的轨迹。其中，利用 clip函数限制一个向量的上下界，即给定一个范围[min，max]，如果向量中的值不在这个范围内时，会被设定为这个范围的边界。u
min
为所有控制输入的下边界，u
max
为所有控制输入的上边界。
[0170]
步骤3：建立轨迹跟踪控制器并对其性能进行评估
[0171]
这一部分首先介绍了轨迹跟踪控制器的开发。设表示轴距为l 的车辆在离散时刻i的状态，其中{xi,yi,θi}是姿态，xi和yi分别表示世界坐标系下车辆的纵向和横向坐标，θi是车辆的航向角，φi是转向角，vi是速度，是离散时刻i的转向角变化率。控制命令由给出，其中pi是油门踏板输入，bi是制动输入，是离散时刻i的指令转向角变化率。用π表示状态转移函数f：
[0172]si 1
＝π(si,ui)
ꢀꢀ
(53)
[0173]
由车辆在典型状态si下的自行车模型，车辆的动态方程π(即状态转移函数f)可由以下公式定义：
[0174]
x
i 1
＝xi v
i cos(θi)δt
[0175]yi 1
＝yi v
i sin(θi)δt
[0176][0177][0178][0179]
给出一组具有速度的m个有序姿态，对它们进行三次样条插值，得到参考轨迹。
[0180]
图3为车辆在典型状态si下的自行车模型和参考轨迹的误差状态图。对于每个状态si，将相对于该参考轨迹的误差状态ψi定义为一个9元元组如图3所示，是在状态si的车辆与参考轨迹的垂向距离，是车辆相对于参考轨迹的航向误差，是对应于参考轨迹上最近点的速度误差 (这里v
p
是参考轨迹上最近点的速度)，vi，和φi来自于状态si。本发明将误差状态ψi用于迭代线性二次型调节器(ilqr)状态，从而对参考轨迹的所有误差进行编码。给定在离散时刻i的误差状态ψi和控制ui，用γ表示状态转移函数f，下一时刻的误差状态ψ
i 1
如下式：
[0181]
ψ
i 1
＝γ(ψi,ui)
ꢀꢀ
(55)
[0182]
γ可以由以下公式定义：
[0183][0184][0185][0186][0187][0188]
[0189][0190][0191]
在误差状态ψi下执行ui的成本l(ψi,ui)如下：
[0192][0193]
其中，a和b是对角权重矩阵，a的最后3个对角线元素为零，因为本发明只关注使(56)中的误差项趋近于零。对于状态ψi，其最终成本lf(ψi)如下所示：
[0194][0195]
现在可以将具有给定参考轨迹的车辆轨迹跟踪问题定义为去找到范围为n的最优控制序列{u0,u1,
…
,u
n-2
}，从而最小化以下成本：
[0196][0197]
加上如下的约束：
[0198][0199]
公式(59)和(60)便可以将轨迹跟踪问题变换为步骤2中定义的常规ilqr问题。
[0200]
本发明通过利用平均交叉跟踪误差、最大交叉跟踪误差、平均速度误差和最大速度四个指标对轨迹跟踪算法的性能进行了评估。本发明在五种类型的参考轨迹上计算了上述指标，即圆形轨迹(如图4所示)、椭圆形轨迹(如图5所示)、蛇形轨迹(如图6 所示)、“8”形轨迹(如图7所示)和组合轨迹(如图8所示)。
[0201]
表1列出了在各种参考轨迹上的误差结果。考虑到车载设备的精度，在各个参考轨迹上的平均交叉跟踪误差及最大交叉跟踪误差都是可以接受的。最大速度较高的原因是在起始时车辆速度为零，而参考轨迹中的初始点的速度为1m/s-2m/s。
[0202]
表1：各种参考轨迹的误差结果
[0203][0204]
上文所列出的一系列的详细说明仅仅是针对本发明的可行性实施方式的具体说明，它们并非用以限制本发明的保护范围，凡未脱离本发明技术所创的等效方式或变更均
应包含在本发明的保护范围之内。