本发明涉及无人驾驶汽车控制领域，尤其是涉及一种并行设计的量子粒子群算法优化求解非线性模型预测控制在无人驾驶汽车轨迹跟踪时的代价函数的方法。
背景技术：
模型预测控制是一种先进的控制算法，由于其独特的预测能力，在许多领域具有良好的控制效果，并在许多生产过程中得到了广泛的应用。然而模型预测控制需要在每一采样时刻，根据当前的测量信息，在线求解一个有限时域开环优化问题，这个过程会消耗很多时间。为了提高算法的快速性，人们引入了控制时域的概念。当预测时域大于控制时域时，将不再进行优化求解，而是把最后一个控制时刻求解出的控制量，作为今后时刻的控制输出。但是这样的处理方式在一定程度上降低了优化的自由度。为了提高模型预测控制的优化求解速度，许多研究者尝试各种快速优化求解的方法，主要分为以下两个方面：
1)算法性能优化，包括引入自适应时域和先进算法加速优化求解过程。为了将模型预测控制更好地应用于快速响应系统，基于二叉树的小波变换和多尺度过程模型在自适应预测时域得到应用。在优化算法方面，序列二次规划算法是求解非线性优化问题的常用有效方法。但由于其是一种基于梯度的优化算法，只有在非线性规划是凸的时候，才能保证解的存在，并收敛到全局最优。随着复杂性和规模的不断增加，随机优化方法和智能优化算法开始进入人们的视野，如遗传算法，粒子群优化算法。其中，遗传算法具有较强的全局搜索能力，不依赖于初始值的选取。然而，随着时域的增大，在线计算成本开始成倍增加，使得求解过程变得缓慢。粒子群算法是一种基于种群的模拟鸟类捕食过程的搜索算法，通过个体间信息交互，种群向最可能的问题解决点聚集。
2)算法硬件实现。现场可编程门阵列(fpga)的并行特性经常用来加快算法的求解速度。而想利用fpga的这种并行特性实现硬件加速，基本的要求是算法可以并行展开，并且算法比较容易在fpga实现。文献表明，粒子群算法满足上述要求，并取得了良好的控制效果。但是传统的粒子群算法，其粒子的运动状态是用位置和速度来描述的，随着时间演化，粒子的运动轨迹是既定的。同时，粒子的移动速度受到一定的限制，使得粒子的搜索空间是一个有限的，并逐渐减小的区域，从而无法覆盖整个解空间。针对粒子群算法的不足，量子力学的相关理论被引入。其中，量子态的叠加效应，使得量子系统具有很高的不确定性，这种强大的不确定性使算法具有更强的全局搜索能力，保证了全局收敛性。而在实际工程应用中，量子粒子群算法也有其优势，因为只有收缩—扩张系数的最大值和最小值需要在实际应用中进行调整。
无人驾驶汽车作为一种快速的动态响应系统，其轨迹跟踪性能主要取决于转向和速度的控制。而模型预测控制由于其独特的预见性，已被应用于无人驾驶汽车的轨迹跟踪，包括理论基础的分析、matlab与carsim联合仿真和实车测试。这些都表明了模型预测控制在无人驾驶汽车轨迹跟踪上有着良好的表现。
技术实现要素：
为克服现有技术的不足，快速优化求解非线性模型预测控制的代价函数，本发明旨在提出采用量子粒子群算法并行设计的方案，将约束问题转化为无约束问题，并确保控制输出满足车辆物理限制和人体舒适度要求。此外，在不同的驾驶环境下，根据当前行驶环境和车辆姿态，实时调整评价函数的权重因子，使车辆更好地适应当前路况。为此，本发明采取的技术方案是，基于量子粒子群算法的非线性模型预测控制方法，首先建立基于无人驾驶汽车的运动学模型，并将其进行离散化；在此基础上采用广义拉格朗日乘子法构造具有惩罚项和鼓励项的广义代价函数，从而将约束问题转化为无约束问题；接下来，进行量子粒子群算法的并行设计，并用其对模型预测控制的代价函数进行优化求解，得到一系列的控制量，最后将控制量的第一个分量作用到汽车上。
具体地，基于车辆运动学模型的广义代价函数构造：
a.无人驾驶汽车运动学模型：
将其进行离散化：
其中,(x,y)表示车辆在坐标系中的位置，
b.广义代价函数
其中，hp与hc分别是预测时域和控制时域，xr|j、yr|j和
其中，ωv0是权重因子，condition1由三部分组成：车辆速度在增加并且加速度大于设定值、车辆偏离给定参考轨迹0.3米和航向角偏离给定参考航向角0.09弧度，其中只要有一部分成立，condition1就成立；
代价函数(2)中的第一项代表着预测值与参考值之间的偏差；第二项表示对控制输入变化率的约束，作用是为了防止突然的转向和加速；第三项是惩罚项，作用是为了防止输入超出车辆物理因素和环境因素所决定的约束；第四项是强调一种激励，确保车辆在正常情况下以最大速度行驶。
量子粒子群算法的并行设计具体地：并行设计量子粒子群算法，并用其进行无人驾驶汽车轨迹跟踪代价函数的优化求解：
第一步是随机初始化粒子位置，由于各个粒子在初始条件时是没有任何关联的，所以所有粒子能够并行初始化；第二步通过代价函数对每个粒子当前状态进行评估，在无人驾驶汽车轨迹跟踪时，每个粒子都可以获得当前参考点信息，从而保证代价函数的计算能够同时进行；第三步是更新局部最优值，将同一个粒子的两个适应度值进行比较，故也进行并行设计；第四步计算全局最优值和平均最佳位置，由于全局最优值是通过对比所有粒子的适应度进行实现的，因此必须将所有粒子聚集在一起进行，同样地，平均最佳位置的计算也需要所有粒子的局部最优位置，不过全局最优值和平均最佳位置能够同时计算；第五步选取平均最佳位置和全局最优值之间的随机值；第六步通过平均最佳位置和随机点更新所有粒子位置，在更新的过程中，因为平均最佳位置和全局最优值已经得到，每个粒子的随机点仍能够进行并行计算；最后，确定更新得到的的控制输出是否满足终止条件，如果满足，则输出控制结果，否则，算法将重复进行，这里的停止条件是满足控制效果或达到最大的迭代次数。
本发明的特点及有益效果是：
本发明所提出的基于量子粒子群算法的快速非线性模型预测控制在无人驾驶汽车轨迹跟踪上的并行设计，其优化求解器的设计具有更强的全局搜索能力，更快的计算能力，可以更好的满足无人驾驶汽车在高速运行时对控制器的要求。为了满足车辆本身的物理限制和保证乘客的舒适度，采用广义拉格朗日乘子法构造了具有惩罚项和鼓励项的广义代价函数，并将有约束问题转化为无约束问题，方便优化求解。此外，在不同的驾驶环境下，根据当前行驶环境和车辆姿态，随时调整评价函数的权重因子，使车辆更好地适应当前路况。
附图说明：
图1、车辆运动学模型。
图2、量子粒子群算法的并行设计流程图。
具体实施方式
为了快速优化求解非线性模型预测控制的代价函数。本发明采用量子粒子群算法并行设计的方案来解决上述问题。而对于无人驾驶汽车这种自身带有物理约束，同时又要满足人体舒适度要求的系统，本发明利用广义拉格朗日乘子法构造了具有惩罚项和鼓励项的广义代价函数，将约束问题转化为无约束问题，并确保控制输出满足车辆物理限制和人体舒适度要求。此外，在不同的驾驶环境下，根据当前行驶环境和车辆姿态，实时调整评价函数的权重因子，使车辆更好地适应当前路况。为此，本发明采取的技术方案是，基于量子粒子群算法的快速非线性模型预测控制在无人驾驶汽车轨迹跟踪上的并行设计，具体包括：
1、基于车辆运动学模型的广义代价函数构造：
a.车辆运动学模型(如图1所示)：
将其进行离散化：
其中,(x,y)表示车辆在坐标系中的位置，
b.广义代价函数
其中，hp与hc分别是预测时域和控制时域，xr|j、yr|j和
其中，ωv0是权重因子。condition1由三部分组成：车辆速度在增加并且加速度大于设定值、车辆偏离给定参考轨迹0.3米和航向角偏离给定参考航向角0.09弧度，其中只要有一部分成立，condition1就成立。
代价函数(5)中的第一项代表着预测值与参考值之间的偏差；第二项表示对控制输入变化率的约束，作用是为了防止突然的转向和加速；第三项是惩罚项，作用是为了防止输入超出车辆物理因素和环境因素所决定的约束；第四项是强调一种激励，确保车辆在正常情况下以最大速度行驶。当然，vmax在不同的驾驶条件下将会有所不同。
2.量子粒子群算法的并行设计
并行设计量子粒子群算法，并用其进行无人驾驶汽车轨迹跟踪代价函数的优化求解。由车辆运动学表达式(4)可知，车的速度和方向盘转向是相互耦合，相互关联的。因此，表征粒子的内部各个分量不能单独求解。但由于分量个数少，即使顺序执行，也不会消耗太多时间。对于每一个粒子来说，它们之间是相互独立的，所以非常适合并行设计。此外，粒子数目通常情况下数量较多，所以是并行设计的关键。图2为量子粒子群算法的并行设计流程图。第一步是随机初始化粒子位置，由于各个粒子在初始条件时是没有任何关联的，所以所有粒子可以并行初始化。第二步通过代价函数对每个粒子当前状态进行评估。在无人驾驶汽车轨迹跟踪时，每个粒子都可以获得当前参考点信息，从而保证代价函数的计算能够同时进行。第三步是更新局部最优值。将同一个粒子的两个适应度值进行比较，故也可以进行并行设计。第四步计算全局最优值和平均最佳位置。由于全局最优值是通过对比所有粒子的适应度进行实现的，因此必须将所有粒子聚集在一起进行。同样地，平均最佳位置的计算也需要所有粒子的局部最优位置。不过全局最优值和平均最佳位置能够同时计算。第五步选取平均最佳位置和全局最优值之间的随机值。第六步通过平均最佳位置和随机点更新所有粒子位置。在更新的过程中，因为平均最佳位置和全局最优值已经得到，每个粒子的随机点仍能够进行并行计算。最后，确定更新得到的的控制输出是否满足终止条件。如果满足，则输出控制结果。否则，算法将重复进行。这里的停止条件是满足控制效果或达到最大的迭代次数。此外，量子粒子群算法扩展所有可以并行执行的步骤，将大大提高计算速度，与此同时也消耗更多的fpga资源。有时，为了寻求速度与资源之间的平衡，可将粒子进行分组，然后组与组之间并行执行，组内顺序执行。
本发明是基于量子粒子群算法的快速非线性模型预测控制在无人驾驶汽车轨迹跟踪上的并行设计，具有求解速度快、搜素能力强的特点。具体实现方式为：首先建立基于无人驾驶汽车的运动学模型，并将其进行离散化；在此基础上采用广义拉格朗日乘子法构造具有惩罚项和鼓励项的广义代价函数，从而将约束问题转化为无约束问题，方便优化求解；接下来，进行量子粒子群算法的并行设计，并用其对模型预测控制的代价函数进行优化求解，得到一系列的控制量，最后将控制量的第一个分量作用到汽车上。
为了更清楚地说明本发明的目的、技术方案及优点，以下从带约束的非线性模型预测控制的基本框架、量子粒子群优化算法、基于无人驾驶汽车轨迹跟踪的量子粒子群算法并行设计等几个方面来对本发明作进一步解释说明。应当理解，此处所描述的具体设计方法仅仅用以解释本发明，并不用于限定本发明。
一、具有约束的非线性模型预测控制的基本框架
假设非线性控制系统可以由以下离散状态模型描述：
在上式中，k为计数变量，
umin为最小控制输入量，umax为最大控制输入量，△umin为允许的最小控制输入变化量，△umax为允许的最大控制输入变化量。控制量u(k)的约束和控制增量△u(k)的约束一般来自执行器的饱和。在通常情况下，非线性模型预测控制的控制量u(k)的求解被转换为求取目标函数的最优值。基于带约束的离散时间状态模型,非线性最小优化问题可以按照如下形式给出：
在上述表达式中，j为需要优化求解的代价函数，x(k)是系统状态向量，u(k)为一系列的控制输入变量。此外，目标函数满足以下关系：
其中hp是预测时域，hc是控制时域，并且满足hp≥hc。
到目前为止，尚没有十分有效的方法对带有约束的代价函数直接进行优化优解。因此通常采用间接方法来解决该问题，即利用目标函数和约束条件构成增广目标函数，将约束优化问题转化为无约束优化问题。本发明利用广义拉格朗日乘子法构造具有惩罚项和鼓励项的广义代价函数。在惩罚因子的作用下，广义代价函数的最优解，即为带有约束的代价函数最优解。
这里j(u)是目标函数，
其中mt是一个正的惩罚值，随着迭代次数的增加逐渐增大，但不需要趋于无穷。gl(u)代表各种约束条件，
在上u(t)式中是在第t次迭代的最优解。此时，带约束的非线性优化问题的求解转化为无约束问题：
二、量子粒子群优化算法
量子粒子群算法是一种在传统粒子群优化算法基础上，结合量子理论提出的改进算法。在量子粒子群算法中，建立一个量子化的吸引势场来束缚粒子，从而使得群体具有聚集态。在量子束缚态中，粒子可以以一定的概率密度出现在空间上的任意一点上。和传统粒子群优化算法相比，量子粒子群优化算法在全局优化性能上有了很大提高。量子粒子群的演化公式为：
其中，m表示总粒子数，d表示粒子维数。xij(t)＝(xi1(t),xi2(t),xid(t))表示粒子的位置。pi(t)＝(pi1(t),pi2(t),,pid(t))表示第i个粒子在第t次迭代时的最佳位置。pg(t)＝(pg1(t),pg2(t),,pgd(t))表示在第t次迭代时的全局最佳位置。mbest(t+1)是所有粒子最佳位置的平均值。ppij(t+1)是粒子个体最优值pi(t)和群全局最优值pg(t)之间的随机点。fij(t+1)和uij(t+1)是[0,1]之间的随机数。r(t+1)以一定概率分别取1或-1，通常的做法是：
三、基于无人驾驶汽车轨迹跟踪的量子粒子群算法并行设计
车辆运动学模型(如图1所示)为：
将其进行离散化：
其中,(x,y)表示车辆在坐标系中的位置，
广义代价函数
其中，hp与hc分别是预测时域和控制时域，xr|j、yr|j和
其中，ωv0是权重因子。condition1由三部分组成：车辆速度在增加并且加速度大于设定值、车辆偏离给定参考轨迹0.3米和航向角偏离给定参考航向角0.09弧度，其中只要有一部分成立，condition1就成立。
代价函数(17)中的第一项代表着预测值与参考值之间的偏差；第二项表示对控制输入变化率的约束，作用是为了防止突然的转向和加速；第三项是惩罚项，作用是为了防止输入超出车辆物理因素和环境因素所决定的约束；第四项是强调一种激励，确保车辆在正常情况下以最大速度行驶。当然，vmax在不同的驾驶条件下将会有所不同。
并行设计量子粒子群算法，并用其进行无人驾驶汽车轨迹跟踪代价函数的优化求解。由车辆运动学表达式(16)可知，车的速度和方向盘转向是相互耦合，相互关联的。因此，表征粒子的内部各个分量不能单独求解。但由于分量个数少，即使顺序执行，也不会消耗太多时间。对于每一个粒子来说，它们之间是相互独立的，所以非常适合并行设计。此外，粒子数目通常情况下数量较多，所以是并行设计的关键。图2为量子粒子群算法的并行设计流程图。第一步是随机初始化粒子位置，由于各个粒子在初始条件时是没有任何关联的，所以所有粒子可以并行初始化。第二步通过代价函数对每个粒子当前状态进行评估。在无人驾驶汽车轨迹跟踪时，每个粒子都可以获得当前参考点信息，从而保证代价函数的计算能够同时进行。第三步是更新局部最优值。将同一个粒子的两个适应度值进行比较，故也可以进行并行设计。第四步计算全局最优值和平均最佳位置。由于全局最优值是通过对比所有粒子的适应度进行实现的，因此必须将所有粒子聚集在一起进行。同样地，平均最佳位置的计算也需要所有粒子的局部最优位置。不过全局最优值和平均最佳位置能够同时计算。第五步选取平均最佳位置和全局最优值之间的随机值。第六步通过平均最佳位置和随机点更新所有粒子位置。在更新的过程中，因为平均最佳位置和全局最优值已经得到，每个粒子的随机点仍能够进行并行计算。最后，确定更新得到的的控制输出是否满足终止条件。如果满足，则输出控制结果。否则，算法将重复进行。这里的停止条件是满足控制效果或达到最大的迭代次数。此外，量子粒子群算法扩展所有可以并行执行的步骤，将大大提高计算速度，与此同时也消耗更多的fpga资源。有时，为了寻求速度与资源之间的平衡，可将粒子进行分组，然后组与组之间并行执行，组内顺序执行。