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

技术特征：
1.一种基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制方法、系统及车载控制设备，其特征在于，包括：s1、建立基于神经网络的动力学模型；使用全连接前馈网络设计神经网络模型，模型有两层隐藏层，每层有64个神经元，每个神经元使用relu激活函数，模型的输入为当前时刻的车辆状态与控制指令，输出为下一时刻的车辆状态，该神经网络通过最小化预测的输出状态和观测的输出状态之间的均方误差来学习动态方程；s2、设计迭代线性二次型调节器的控制算法；s3、根据s2的控制算法建立轨迹跟踪控制器。2.根据权利要求1所述一种基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制方法，其特征在于，所述s1的具体实现包括：设表示状态，表示在离散时刻t的系统控制指令，系统动态可以由下式给出：x
t 1
＝f(x
t
,u
t
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)车辆系统状态由给出，其中v
t
是在时刻t的线速度，是在时刻t的转向角速度，控制指令由给出，其中p
t
是油门踏板输入，b
t
是制动输入，是在时刻t的指令转向角速度，得出车辆的动态方程f如下式：其中，x
t
，u
t
作为神经网络模型的输入，x
t 1
作为神经网络模型的输出，该神经网络通过最小化预测的输出状态和观测的输出状态x
t 1
之间的均方误差来学习动态方程f。3.根据权利要求1所述一种基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制方法，其特征在于，所述神经网络模型的训练方法：通过利用搭载惯性测量单元(imu)的实车在公路和非公路环境中人工驾驶一小时的方式收集车辆的数据，通过观察训练过程中的训练损失和验证损失，利用试验和误差来确定驾驶时间；其中，在将数据提供给神经网络的输入层之前，先对数据进行白化处理，以降低输入数据特征之间的冗余性。4.根据权利要求2所述一种基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制方法，其特征在于，所述s2的具体实现包括：针对非线性离散动态系统：x
t 1
＝f(x
t
,u
t
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)其中，是系统状态，是在时刻t的控制指令；迭代线性二次型调节器(ilqr)控制算法分为线性化、lqr的反向传递和前向传递三部分：在线性化部分，将ilqr中非线性动态展开成一阶，成本展开成二阶，
其中，为在状态时生成的最优控制序列，则下一个状态为时生成的最优控制序列，则下一个状态为为对应于状态x
t
及控制u
t
的函数的导数，c(x
t
,u
t
)为在状态x
t
及控制输入u
t
下的成本函数；将上式(4)进行重排列：将上式(4)进行重排列：将作为新的状态，作为新的控制输入，则下一个状态为q(δx
t
,δu
t
)为在状态δx
t
及控制输入δu
t
下的成本函数，从而得到：下的成本函数，从而得到：其中，其中，二次部分的系数矩阵其中，代表矩阵c
t
的左上角部分，代表矩阵c
t
的右上角部分，代表矩阵c
t
的左下角部分，代表矩阵c
t
的右下角部分，线性部分的系数矩阵其中，代表矩阵c
t
的上半部分，代表矩阵c
t
的下半部分；在lqr的反向传递部分，通过最小化成本函数q(δx
t
,δu
t
)求得最优控制，在时刻t时，通过计算成本函数q(δx
t
,δu
t
)的一阶导数为零时的δu
t
来得到在此时刻的最优控制；即令可得到时刻t的最优控制将式(8)展开并用矩阵k
t
及k
t
来表示，于是可以得到：
δu
t
＝k
t
δx
t
k
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)将式(10)带入式(6)的成本函数q(δx
t
,δu
t
)中，将新得到的成本函数记为v(δx
t
)：将式(11)展开可以得到：对展开式进行适当简化，将其看作形如的二次项与一次项的和，其中，矩阵v
t
及v
t
分别为：分别为：其中，系数矩阵表示矩阵c
t
的下半部分；在时刻t-1时，有式(14)成立：则从时刻t到时刻t-1所产生的成本函数为：再由可得：则成本函数q(δx
t-1
,δu
t-1
)表示为：其中，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
来得到在此时刻的最优控制，即令
可以得到时刻t-1的最优控制：δu
t-1
＝k
t-1
δx
t-1
k
t-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(19)其中将式(20)带入式(17)中，对展开式进行简化处理可以得到：将式(20)带入式(17)中，对展开式进行简化处理可以得到：其中，系数矩阵表示矩阵q
t-1
的下半部分；在lqr的前向传递部分，该过程要和真实的非线性系统进行交互，得到新的真实轨迹，其中，为在状态时生成的最优控制序列，则下一个状态为参数α可以通过回溯行搜索进行调整，将时刻n的状态与相结合从而得到新的轨迹。5.根据权利要求4所述一种基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制方法，其特征在于，所述s3的具体实现包括：设表示轴距为l的车辆在离散时刻i的状态，其中{x
i
,y
i
,θ
i
}是姿态，x
i
和y
i
分别表示世界坐标系下车辆的纵向和横向坐标，θ
i
是车辆的航向角，φ
i
是转向角，v
i
是速度，是离散时刻i的转向角变化率，控制命令由给出，其中p
i
是油门踏板输入，b
i
是制动输入，是离散时刻i的指令转向角变化率，用π表示状态转移函数f：s
i 1
＝π(s
i
,u
i
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(23)由车辆在典型状态s
i
下的自行车模型，车辆的动态方程π(即状态转移函数f)可由以下公式定义：x
i 1
＝x
i
v
i
cos(θ
i
)δty
i 1
＝y
i
v
i
sin(θ
i
)δt)δt)δt假设一组具有速度的m个有序姿态，对它们进行三次样条插值，得到参考轨迹；对于每个状态s
i
，将相对于该参考轨迹的误差状态ψ
i
定义为一个9元元组定义为一个9元元组是在状态s
i
的车辆与参考轨迹的垂向距离，是车辆相对于参考轨迹的航向误差，是对应于参考轨迹上最近点的速度误差(这里v
p
是参
考轨迹上最近点的速度)，v
i
，和φ
i
来自于状态s
i
，将误差状态ψ
i
用于迭代线性二次型调节器(ilqr)的状态，从而对参考轨迹的所有误差进行编码，给定在离散时刻i的误差状态ψ
i
和控制u
i
，用γ表示状态转移函数f，下一时刻的误差状态ψ
i 1
如下式：ψ
i 1
＝γ(ψ
i
,u
i
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(25)γ可以由以下公式定义：γ可以由以下公式定义：γ可以由以下公式定义：γ可以由以下公式定义：γ可以由以下公式定义：γ可以由以下公式定义：γ可以由以下公式定义：γ可以由以下公式定义：在误差状态ψ
i
下执行u
i
的成本l(ψ
i
,u
i
)如下：其中，a和b是对角权重矩阵，a的最后3个对角线元素为零，使式(26)中的误差项趋近于零，对于状态ψ
i
，其最终成本l
f
(ψ
i
)如下所示：将具有给定参考轨迹的车辆轨迹跟踪问题定义为找到范围为n的最优控制序列{u0,u1,
…
,u
n-2
}，从而最小化以下成本：设计如下的约束：6.一种基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制系统，其特征在于，包括：神经网络动力学模型部分、迭代线性二次型调节器部分以及轨迹跟踪控制器部分；所述神经网络动力学模型部分，使用全连接前馈神经网络设计神经网络动力学模型，模型有两层隐藏层，每层有64个神经元，每个神经元使用relu激活函数，模型的输入为当前时刻的车辆状态与控制指令，输出为下一时刻的车辆状态，该神经网络通过最小化预测的输出状态和观测的输出状态之间的均方误差来学习动态方程；基于神经网络动力学模型设计迭代线性二次型调节器以及轨迹跟踪控制器，由轨迹跟
踪控制器最小化迭代线性二次型调节器的成本函数求得最优控制指令，实现对参考轨迹的跟踪。7.根据权利要求6所述的一种基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制系统，其特征在于，所述神经网络动力学模型具体如下：设n维实向量表示状态，m维实向量表示在离散时刻t的系统控制指令，系统动态可以由下式给出：x
t 1
＝f(x
t
,u
t
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(31)车辆系统状态由给出，其中v
t
是在时刻t的线速度，是在时刻t的转向角速度，控制指令由给出，其中p
t
是油门踏板输入，b
t
是制动输入，是在时刻t的指令转向角速度，现在给出车辆的动态方程f如下式：8.根据权利要求7所述的一种基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制系统，其特征在于，所述迭代线性二次型调节器的控制算法具体如下：针对设计的神经网络动力学模型这一非线性离散动态系统：x
t 1
＝f(x
t
,u
t
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(33)其中，是系统状态，是在时刻t的控制输入；将非线性环境和成本函数通过泰勒展开局部线性化，之后通过lqr的向后传递过程解出在线性化环境和成本下的最优解，ilqr算法分为线性化、lqr的反向传递和前向传递三部分；在线性化部分，将ilqr中非线性动态展开成一阶，成本展开成二阶，化部分，将ilqr中非线性动态展开成一阶，成本展开成二阶，其中，为在状态时生成的最优控制序列，则下一个状态为时生成的最优控制序列，则下一个状态为为对应于状态x
t
及控制u
t
的函数的导数，c(x
t
,u
t
)为在状态x
t
及控制输入u
t
下的成本函数；将式(34)进行重排列：将式(34)进行重排列：将作为新的状态，作为新的控制输入，则下一个状态为q(δx
t
,δu
t
)为在状态δx
t
及控制输入δu
t
下的成本函数，从而得到：
其中，其中，二次部分的系数矩阵(其中，代表矩阵c
t
的左上角部分，代表矩阵c
t
的右上角部分，代表矩阵c
t
的左下角部分，代表矩阵c
t
的右下角部分)，线性部分的系数矩阵其中，代表矩阵c
t
的上半部分，代表矩阵c
t
的下半部分，这样就将ilqr问题转化为常规的lqr问题；在lqr的反向传递中，通过最小化成本函数q(δx
t
,δu
t
)求得最优控制，在时刻t时，通过计算成本函数q(δx
t
,δu
t
)的一阶导数为零时的δu
t
来得到在此时刻的最优控制，即令：可得到时刻t的最优控制将式(38)展开并用矩阵k
t
及k
t
来表示，于是可以得到：来表示，于是可以得到：δu
t
＝k
t
δx
t
k
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(40)将式(40)带入式(36)的成本函数q(δx
t
,δu
t
)中，将新得到的成本函数记为v(δx
t
)：将式(41)展开可以得到：对式(42)进行适当简化，将其看作形如的二次项与一次项的和，其中，矩阵v
t
及v
t
分别为：
其中，系数矩阵表示矩阵c
t
的下半部分；在时刻t-1时，有式(44)成立：则从时刻t到时刻t-1所产生的成本函数为：再由可得：则成本函数q(δx
t-1
,δu
t-1
)表示为：其中，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
来得到在此时刻的最优控制，即令可以得到时刻t-1的最优控制：δu
t-1
＝k
t-1
δx
t-1
k
t-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(49)其中将式(50)代入式(47)中，对展开式进行简化处理可以得到：将式(50)代入式(47)中，对展开式进行简化处理可以得到：其中，系数矩阵表示矩阵q
t-1
的下半部分；在lqr的前向传递中，该过程要和真实的非线性系统进行交互，得到新的真实轨迹；其中，为在状态时生成的最优控制序列，则下一个状态为参数α可以通过回溯行搜索进行调整，将时刻n的状态与相结合从而得到新的轨迹。
9.根据权利要求8所述的一种基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制系统，其特征在于，所述轨迹跟踪控制器的建立具体如下：设表示轴距为l的车辆在离散时刻i的状态，其中{x
i
,y
i
,θ
i
}是姿态，x
i
和y
i
分别表示世界坐标系下车辆的纵向和横向坐标，θ
i
是车辆的航向角，φ
i
是转向角，v
i
是速度，是离散时刻i的转向角变化率，控制命令由给出，其中p
i
是油门踏板输入，b
i
是制动输入，是离散时刻i的指令转向角变化率，用π表示状态转移函数f：s
i 1
＝π(s
i
,u
i
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(53)由车辆在典型状态s
i
下的自行车模型，车辆的动态方程π(即状态转移函数f)可由以下公式定义：x
i 1
＝x
i
v
i
cos(θ
i
)δty
i 1
＝y
i
v
i
sin(θ
i
)δt)δt)δt给出一组具有速度的m个有序姿态，对它们进行三次样条插值，得到参考轨迹，对于每个状态s
i
，将相对于该参考轨迹的误差状态ψ
i
定义为一个9元元组定义为一个9元元组是在状态s
i
的车辆与参考轨迹的垂向距离，是车辆相对于参考轨迹的航向误差，是对应于参考轨迹上最近点的速度误差(这里v
p
是参考轨迹上最近点的速度)，v
i
，和φ
i
来自于状态s
i
，将误差状态ψ
i
用于迭代线性二次型调节器的状态，从而对参考轨迹的所有误差进行编码，给定在离散时刻i的误差状态ψ
i
和控制u
i
，用γ表示状态转移函数f，下一时刻的误差状态ψ
i 1
如下式：ψ
i 1
＝γ(ψ
i
,u
i
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(55)γ可以由以下公式定义：γ可以由以下公式定义：γ可以由以下公式定义：γ可以由以下公式定义：γ可以由以下公式定义：γ可以由以下公式定义：
在误差状态ψ
i
下执行u
i
的成本l(ψ
i
,u
i
)如下：其中，a和b是对角权重矩阵，a的最后3个对角线元素为零，使(56)中的误差项趋近于零，对于状态ψ
i
，其最终成本l
f
(ψ
i
)如下所示：将具有给定参考轨迹的车辆轨迹跟踪问题定义为去找到范围为n的最优控制序列{u0,u1,
…
,u
n-2
}，从而最小化以下成本：加上如下的约束：10.一种车载控制设备，其特征在于，所述车载控制设备内包含权利要求6-9任一项所述的基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制系统。

技术总结
本发明公开了一种基于神经网络动力学模型的无人驾驶车辆轨迹跟踪控制方法、系统及车载控制设备，在基于神经网络的动力学模型中，使用全连接前馈神经网络设计了神经网络模型，模型有两层隐藏层，每层有64个神经元，使用ReLU激活函数，模型的输入为当前时刻的车辆状态与控制指令，输出为下一时刻的车辆状态，该神经网络通过最小化预测的输出状态和观测的输出状态之间的均方误差来学习动态方程。基于神经网络动力学模型设计的ILQR控制器内包含迭代线性二次型调节器的控制算法，通过最小化成本函数求得最优控制指令，实现对参考轨迹的跟踪。本发明相比于端到端的控制，所提出的方案可解释性更强，在保证轨迹跟踪精度的同时，兼顾了横向和纵向稳定性。兼顾了横向和纵向稳定性。兼顾了横向和纵向稳定性。


技术研发人员：蔡英凤 曹秀辰 李祎承 廉玉波 钟益林 孙晓强 陈龙 何友国
受保护的技术使用者：江苏大学
技术研发日：2022.06.06
技术公布日：2022/9/2