双轮差速移动小车的运动状态完全取决于双轮的转速，因此，可以建立如图1所示的小车运动模型。在图1中，p_x与p_y分别为小车在坐标轴x和坐标轴y方向上的位置量，θ为小车的航向角，l为小车的轮间距，r为车轮半径，w_l与w_r分别为小车左右轮的转速。由图1可知，在某t时刻，当小车的航向角为 θ ( t ) ，双轮转速分别为 w l ( t ) 与 w r ( t ) 时，小车在坐标轴方向的速度分量与分别为

p ˙ x ( t ) = r cos θ ( t ) 2 ⋅ w l ( t ) + r cos θ ( t ) 2 ⋅ w r ( t )

p ˙ y ( t ) = r sin θ ( t ) 2 ⋅ w l ( t ) + r sin θ ( t ) 2 ⋅ w r ( t )

航向角的变化率为

θ ˙ ( t ) = − r l ⋅ w l ( t ) + r l ⋅ w r ( t )

此时，令状态量 x = [ p x , p y , θ ] ，控制量 u = [ w l , w r ] ，并假设双轮与地面的接触运动均为纯滚动而无滑动，则可建立如下所示的小车运动控制模型：

[ p ˙ x p ˙ y θ ˙ ] = [ r cos θ 2 r cos θ 2 r sin θ 2 r sin θ 2 − r l r l ] ⋅ [ w l w r ] (1)

图1. 小车运动模型

由式(1)可知，该系统为非线性动态系统，无法使用状态矩阵与输入矩阵的形式进行表达，因而需要使用非线性控制方法进行求解，而非线性控制往往会面临过高的计算量，不利于实时控制。现有的对于车辆的研究通常基于小航向角假设，由此可以将车辆的运动模型转化为线性模型进行求解 [ 11 ]，然而在本文中小车的航向角是未知量，无法假设为小航向角，该方法自然不成立；另一种处理非线性系统的方法是利用泰勒展开将系统线性化，然而这种方法会导致模型精度降低，不利于控制。观察到在式(1)中，当 w l = − w r 时，有

p ˙ x | w l = − w r = p ˙ y | w l = − w r = 0

因此，航向角θ是一个独立可控的变量。基于这一点，将状态量x与控制量u重构为

x = [ p x , p y , p ˙ x , p ˙ y ] ,     u = [ p ¨ x , p ¨ y ]

则新的小车运动控制模型可以构建为

[ p ˙ x p ˙ y p ¨ x p ¨ y ] = [ 0 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 ] ⋅ [ p x p y p ˙ x p ˙ y ] + [ 0 0 0 0 1 0 0 1 ] ⋅ [ p ¨ x p ¨ y ] (2)

式(2)所示的状态方程具有可用状态矩阵与输入矩阵进行表达标准形式，为线性动态系统。此外，将系统重构为式(2)的形式的另一个优点是使用小车在坐标轴方向上的加速度作为控制量可以使得小车的运动更为平滑而不易发生抖动。

对于式(2)所表示的具有标准形式的时不变动态系统，可以使用前向欧拉法进行离散化，设采样时间为T_s，则其离散状态方程为

x ( k + 1 ) = A ⋅ x ( k ) + B ⋅ u ( k ) (3)

其中

A = [ 1 0 T s 0 0 1 0 T s 0 0 1 0 0 0 0 1 ] ,     B = [ 0 0 0 0 T s 0 0 T s ]

对于任意第k个控制时刻，令预测步长与控制步长分别为N_p与N_c，可以对离散状态方程(3)递推步长N_p可得预测时域内的离散状态方程组

x ( k + 1 ) = A ⋅ x ( k ) + B ⋅ u ( k ) x ( k + 2 ) = A ⋅ x ( k + 1 ) + B ⋅ u ( k + 1 ) = A 2 ⋅ x ( k ) + A ⋅ B ⋅ u ( k ) + B ⋅ u ( k + 1 )                                                               ⋮ x ( k + N p ) = A N p ⋅ x ( k ) + A N p − 1 ⋅ B ⋅ u ( k ) + ⋯ + B ⋅ u ( k + N c )

上式中， N c = N p − 1 ，以保证最高精度的求解效果。将上述预测方程组结合起来，并以矩阵的形式表达可得如下预测模型：

X p ( k ) = A p ⋅ x ( k ) + B p ⋅ U p ( k ) (4)

其中

X p ( k ) = [ x ( k + 1 ) x ( k + 2 ) ⋮ x ( k + N p ) ] ,     U p ( k ) = [ u ( k ) u ( k + 1 ) ⋮ u ( k + N c ) ]

A p = [ A A 2 ⋮ A N p ] ,     B p = [ B A B B ⋮ ⋱ ⋱ A N c B A N c − 1 B ⋯ B ]

对于预测模型(4)，为了优化预测状态量 X p ( k ) 并获得最优控制序列 U p ( k ) ，对于任意第k个控制时刻的状态量 x ( k ) 可以建立如下代价函数：

J ( x ( k ) , U p ( k ) ) = 1 2 ‖ X p ( k ) − X g ‖ Q g 2 + 1 2 ‖ U p ( k ) ‖ Q u 2 (5)

其中X_g为全局目标状态量，Q_g、Q_u分别为相应的权重矩阵。优化目标即是以最小的控制代价使当前状态渐进收敛到目标状态，并将最优控制序列 U p ∗ ( k ) 的首个时刻的控制量作用于系统。

人工智能(AI)的目标是开发具有在复杂的不确定环境中完成富含挑战性的特定任务能力的智能体(Agents)。近年来，解决这类问题较为流行的方法是基于马尔科夫决策过程(MDP-Based)的强化学习(RL)算法，例如Q-Learning、策略梯度法(PG)等等。这类方法在游玩Atari游戏 [ 12 ]、机械臂规划 [ 13 ]、乒乓球机器人 [ 14 ] 等领域都取得了巨大的成功。

解决这类问题的另一种方法是采用以进化策略(ES)为代表的黑箱优化算法(Black-Box Optimization)。在RL中，ES常常被用于直接策略搜索。但一直以来，研究者们认为与Q-Learning、PG等算法相比，ES无法解决硬RL问题，或者在解决硬RL问题时效率较低。Salimans等人 [ 8 ] 和Chrabaszcz等人 [ 9 ] 的结果证明了ES可以可靠地训练神经网络，并具备与RL抗衡甚至取代RL的优异性能。与RL相比，ES具有以下优点：

1) ES对于奖励函数的分布并不敏感，因为ES通常会采用一整个episode的奖励总和，同时ES也无需折扣因子；

2) ES无需进行梯度反向传播，从而使得每一个episode的计算量大幅减少；

3) ES在参数空间中进行扰动，致使搜索空间增大，在训练过程中更容易收敛到可行解。

本文所使用的经典进化策略算法隶属于(μ, λ)-ES算法。(μ, λ)-ES算法是ES算法大家族中最为杰出的一类，它会强迫上一代种群中的所有个体全部离开种群，只留下少数优异的个体存活至下一代，从而规避某些适应度较好但并非最好的个体因变异率选取不当而进化失败，但却强行存活到下一代甚至几代从而浪费种群位置的问题 [ 15 ]。经典进化策略算法的步骤为：

Step 1：初始化种群数目λ，优异种群数目μ，变异率σ，初代个体θ₀，适应度函数 F ( ⋅ ) ；

Step 2：利用标准正态分布函数产生扰动 ε i ，令子代个体 θ i = θ 0 + σ ⋅ ε i (其中 i = 1 , 2 , ⋯ , λ )；

Step 3：计算适应度函数值 f i = F ( θ i ) ，并对所有的 f i 进行排序，根据排序结果选取对应的最优的μ个个体 θ j (其中 j = 1 , 2 , ⋯ , μ )；

Step 4：将μ个个体 θ j 进行加权求和得到新个体θ_n，并将θ_n作为新一代的θ₀；

Step 5：重复上述步骤Step 2~Step 4直到满足终止条件。

本文所构建的策略网络如图2所示，该网络以MPC的状态量 x ( k ) 与小车航向角θ为作为输入，以MPC的控制量的调节量 U p ( k ) 为输出，输入层与输出层之间包含两个隐含层。层与层之间通过激活函数进行连接，其中隐含层之间的激活函数为ReLu函数，隐含层与输出层之间的激活函数为输出在−1到1之间的tanh函数，以使得策略网络的输出可以对MPC的控制量进行全覆盖的调节。当网络的输出为0时，控制器将输出原始MPC的最优控制量。

图2. 策略网络

在每一个episode中，以初始网络参数θ₀为基础通过变异产生λ个子网络并分别与原始MPC控制器结合成λ个ES-MPC控制器；在同一系统初始状态同时运行这λ个ES-MPC控制器；运行结束时每个控制器都对应产生一条数据轨迹(即小车的运动轨迹)。通过评估每一条轨迹的好坏程度就可以选出性能最为优异的μ个网络并产生下一个episode的初始网络参数θ₀，由此不断更新网络从而完成网络的训练。

在策略网络的训练过程中，代价评估是最为关键的环节之一(这个环节相当于RL里的奖励函数设置)，代价函数的设置体现了对小车期望动作的要求，因此将直接影响到网络的训练效率以及算法的性能。为了准确评估episode中每一个策略网络的优异程度，本节设置了如下的代价函数用于评估策略网络输出的好坏：

1) 碰撞代价C_c

避免小车在导航的过程中与环境中的障碍物发生碰撞，是ES-MPC算法设计中必须要遵循的重要原则，因此我们引入碰撞代价对ES-MPC运行过程中发生了小车碰撞的轨迹进行惩罚。ES-MPC中碰撞代价的设计也代替了原始MPC算法中障碍约束的设计。

引入状态布尔量 C o l l i S t a t e ( u n ( k ) ) ，它的值代表第k个采样时刻小车执行控制序列 U ( k ) 的前n个时刻的控制量 u n ( k ) 后小车的碰撞状态(其中 n = 0 , 1 , ⋯ , N c )，0代表未碰撞，1代表碰撞， C c ( u n ( k ) ) 代表相应的代价值。当 C o l l i S t a t e ( u n ( k ) ) 为1时，轨迹的全局碰撞ColliState也设为1。状态 C c ( u n ( k ) ) 的具体值有如下定义：

C c ( u n ( k ) ) = { 0 , C o l l i S t a t e ( u n ( k ) ) = 0 ε n a c , C o l l i S t a t e ( u n ( k ) ) = 1

其中a_c为基础代价值，ε为介于0和1之间的衰减因子，则第k时刻 U ( k ) 所对应的碰撞代价为

C c ( U ( k ) ) = ∑ n = 0 N c C c ( u n ( k ) )

2) 路径代价C_p

路径代价C_p反映了小车对目标点导航效果的好坏，其意义与代价函数(3)相似，但是路径代价C_p反映了小车对目标点的全局导航效果，我们自然希望小车以最短路径逼近目标点。 p n ( k ) 代表第k个采样时刻小车执行控制序列 U ( k ) 的前n个时刻的控制量 u n ( k ) 后的位置，并定义 p − 1 ( k ) = p ( k ) 为小车当前时刻的位置，路径代价 C p ( u n ( k ) ) 的具体值有如下定义：

C p ( u n ( k ) ) = k p ⋅ ‖ p n ( k ) − p n − 1 ( k ) ‖ 2 + k g ⋅ ‖ p n ( k ) − p g ‖ 2

其中k_p与k_g为惩罚系数，p_g为目标位置，则第k时刻 U ( k ) 所对应的路径代价为

C p ( U ( k ) ) = ∑ n = 0 N c C p ( u n ( k ) )

3) 搜索代价C_e

以上两个代价函数——C_c与C_p的设置体现了小车运动规划的两个目标：避障，以及快速导航到目标位置；然而在策略网络训练的初期，C_c与C_p很有可能会导致矛盾。例如当种群内的所有策略网络所输出的策略都让小车发生碰撞，而路径代价C_p的存在又限制了种群进化的出路，导致优化陷入死胡同。因此，设置搜索代价C_e从而鼓励种群内个体进行广泛搜索。 C e ( u n ( k ) ) 的具体值有如下定义：

C e ( u n ( k ) ) = − k e ⋅ ‖ p n ( k ) − p ( 0 ) ‖ 2

其中k_e为搜索因子， p ( 0 ) 为小车初始时刻位置。由于该代价量为增益量，因此取负号。则第k时刻 U ( k ) 所对应的搜索代价为

C e ( U ( k ) ) = ∑ n = 0 N c C e ( u n ( k ) )

则长度为N的轨迹的总代价值为

C = { ∑ k = 1 N C p ( U ( k ) )     , C o l l i S t a t e = 0 ∑ k = 1 N C c ( U ( k ) ) + C e ( U ( k ) )     , C o l l i S t a t e = 1

CoppeliaSim (原V-rep)是一款支持各类机器人运动学及动力学仿真的软件，提供有面向MATLAB的远程API函数接口，通过API函数接口可以从MATLAB脚本中调用相关函数获得机器人信息或控制机器人运动。因此我们在CoppeliaSim平台搭建了复杂障碍环境与小车物理模型并进行小车运动可视化，在MATLAB内搭建了相同的场景与小车动态模型并训练策略网络。图3展示了CoppeliaSim内的场景以及MATLAB内相应的场景，由于小车在仿真训练中被简化为一个质点，因此对障碍作了膨胀化处理。

图3. 仿真实验场景

在ES-MPC策略网络的训练过程中，全局目标X_g总是不变的，初始状态x₀则是随episode的更替而随机生成，以实现在绝大部分初始状态下小车都能成功导航到目标位置。策略网络训练过程中的参数设置情况如表1所示。

表1. 参数设置

图4展示了在两个不同场景下，几类MPC算法所对应的小车运动轨迹。由图4可知在该场景1中，在不添加障碍约束的情况下，原始MPC、HI-MPC的事后预测步长N'为25时无法完成导航任务，而N'为50的HI-MPC与本文的ES-MPC都能提前预知障碍并导航至目标位置；而在场景2中由于存在局部最优陷阱，只有经过策略网络训练的ES-MPC能够寻找到一个好的避障策略并完成导航任务。

图4. 对比实验图

此外，我们还在不同的初始状态或目标状态下进行了多次实验，实验结果如表2所示。根据不同的状态，我们将场景分为“开放”与“封闭”两种类型。由表2可知，在开放场景中，由于任务难度不大，因此3种MPC算法都可以完成导航；然而在封闭场景中，N'为50的HI-MPC可以完成一部分任务，而本文的ES-MPC则可以完成全部任务，这充分体现了ES-MPC的可行性与优越性。

表2. 对比实验结果

在上一节中我们成功训练了策略网络，在这一节中我们将使用策略网络控制小车在CoppeliaSim场景内运动。对模型(1)直接进行采样离散化可得

[ p ˙ x ( k ) p ˙ y ( k ) θ ˙ ( k ) ] = [ r cos θ ( k ) 2 r cos θ ( k ) 2 r sin θ ( k ) 2 r sin θ ( k ) 2 − r l r l ] ⋅ [ w l ( k ) w r ( k ) ] (6)

由此可得小车双轮转速与模型(2)中小车坐标轴方向上的移动速度以及航向角变化率之间的关系。航向角变化率并未显含于模型(2)中，但由几何关系，有

θ ( k + 1 ) + θ ( k ) = 2 × arctan ( p ˙ y ( k ) p ˙ x ( k ) )

因此有

θ ˙ ( k ) = θ ( k + 1 ) − θ ( k ) T s = 2 T s × ( arctan ( p ˙ y ( k ) p ˙ x ( k ) ) − θ ( k ) )

由此可以决定小车在k时刻的双轮转速。

图5展示了CoppeliaSim内小车的运动轨迹，对应于图4中ES-MPC的轨迹，由图5可知基于模型(2)所训练的策略网络可以顺利地控制基于物理模型(1)的小车进行运动，由此验证了本文所提ES-MPC方法的可行性。理论上只要有足够的训练代数，所有可以显式建模的系统都可以使用ES-MPC方法。

图5. CoppeliaSim小车运动轨迹