使用多重打靶与配点法的移动机器人自动驾驶

摘要

提出了一种用于操控自主移动机器人的非线性模型预测控制（NMPC）方法。带有反向转向系统的车辆动力学采用非线性自行车模型进行描述。在考虑将障碍物描述为不等式约束的基础上构建NMPC问题，并在每次采样时间根据激光扫描仪检测结果更新这些约束。该非线性最优控制问题（NOCP）通过组合多点打靶与配点法高效求解。实验结果表明了该方法在主动自主转向中避让突发障碍物的可行性。

关键词

自主移动机器人，自行车模型，障碍物检测与避让，非线性模型预测控制。

1. 引言

近年来，障碍物检测与避让以及车道保持已成为自动驾驶汽车研究中的主要问题。模型预测控制（MPC）已被证明是一种具有多种理想特性的有前景的控制策略。与任务前规划和标称参考的离线设计不同，MPC能够基于实际且动态变化的交通状况提供自适应控制策略，从而提高系统的自主性和鲁棒性。目前存在多种用于自动驾驶车辆转向的MPC方法，这些方法在所采用的模型、应用目的、构建的优化问题、数值求解技术、应用软件等方面各不相同。

Keviczky等人(2006)报道了一种预测控制方法，该方法采用具有恒定轮胎力的非线性自行车模型以及考虑联合制动与转向过程中牵引力和侧向力相互作用的轮胎模型。采用欧拉方法对最优控制问题进行离散化，并将转向角作为控制变量。该问题通过商业软件在不同的仿真场景中求解。

金等人（2014）提出了一种包含转向执行器动力学的基于模型预测控制的路径跟踪算法。该问题通过二次规划方法求解。文中报告了关于预测和控制时域长度、转向系统的模型阶数以及速度的各种仿真场景结果。

Gao等人(2014)提出了一种基于管的鲁棒非线性模型预测控制（NMPC）方法，用于车道保持避障。该方法基于一种控制律，其中标称状态和控制量通过标称NMPC和离线计算的鲁棒不变集获得。针对避障问题给出了仿真和实验结果。

崔和崔（2014）采用具有滞后轮胎力的扩展自行车模型以提高预测精度，研究了基于模型预测控制的电子稳定性控制。基于非线性模型生成参考轨迹以保持车辆偏航稳定性。为避免在满足状态不等式时产生较大的计算负担，采用可容易获得闭式解的线性模型预测控制来跟踪参考策略。

有关自动驾驶车辆地面交通以及使用模型预测控制技术的更多参考文献，例如Kim和Kumar(2014)、希尔德巴赫和博雷利（2015）。

NMPC问题的基本思想是利用离散化方法将最优控制问题转化为非线性规划（NLP）。众所周知，应用NMPC的一个重要限制在于每次采样时间求解该非线性规划所需较长的计算时间。通常情况下，计算时间应远小于NMPC方案的采样间隔。因此，迫切需要提高求解非线性规划的计算效率。为此，所采用的离散化方法起着至关重要的作用。然而，在几乎所有关于基于模型预测控制的自主转向车辆的先前研究中，均采用了最简单的欧拉方法。但已知该方法在对动态系统进行离散化时效率较低，Aktas和Stetter（1977），阿舍等人（1979）。

因此，本研究旨在实现一种高效的数值方法，用于在线求解NMPC问题，并在移动机器人上进行实验验证。该移动机器人采用非线性自行车模型进行建模。从机器人的视角来看，障碍物是随机出现的。利用激光扫描仪检测障碍物，并根据实时情况在每个采样时间更新不等式，从而形成对障碍物的描述。建立包含这些不等式约束的NMPC问题，并通过组合多点打靶与配点法进行求解。最后将展示在不同场景下的实时实验结果。

2. 建模

2.1 系统描述

所研究的系统是用于研究应用的移动机器人SUMMIT以及其他设备，Robotnik（2015），见图1。该移动机器人配备了对称双轴反向转向系统。对称性意味着刚体的质心到前轴和后轴的距离相等。通过四个车轮的转向角和行驶速度控制实现平面定位。车轮按轴向进行操控。后轮的角速度由编码器测量。假设由于控制输入相同，前轮的行为与后轮一致。每个轴上由一台无刷直流电机驱动车轮，且不配备差速器。每个轴上各有一个伺服电机，用于调节转向角。该移动机器人未配备制动系统，通过阻塞车轮来停止。使用Hokuyo激光扫描仪作为检测障碍物的传感器。还配备了一个云台变焦相机，未来将用于传感器融合。

通信由外部笔记本电脑、Wi‐Fi TCP/IP网络和内部处理单元（IPU）建立。笔记本电脑通过TCP/IP网络接收传感器数据，利用包含Player/Stage（2010）库和优化求解器的C++ code计算最优控制，并将控制信号发送回IPU。IPU实现对伺服电机和驱动电机的执行以及传感器数据读取。

2.2 状态空间模型

根据车辆动力学、转向系统以及相对较低的行驶速度和转向角，建立了以下非线性动力学模型：Jazar（2009）、M¨uller（2014）、Thieme（2014）、Drozdova（2015）：

$$
\dot{x} =
\begin{bmatrix}
\frac{c_\alpha \cos(u_2)[-2x_1 + u x_{31}(l_h - l_v)]}{m_{rob} u_1 \cos(x_1)} \
- x_3 \
x_3 \
\frac{c_\alpha \cos(u_2)[u_2(l_h + l_v) + x_1(l_h - l_v) - u x_{31}(l^2_h + l^2_v)]}{J_{rob}} \
u_1 \cos(x_1 + x_2) \
u_1 \sin(x_1 + x_2)
\end{bmatrix}
\quad (1)
$$

其中，$x = [x_1\ x_2\ x_3\ x_4\ x_5]^T = [\beta\ \psi\ \dot{\psi}\ s_x\ s_y]^T$ 是由五个状态组成的时间相关状态向量，包括侧滑角$\beta$、偏航角$\psi$、偏航率$\dot{\psi}$、全局坐标系中的$xy$坐标$s_x$和$s_y$。两个控制量汇总在控制向量$u$中，分别为$[u_1\ u_2]^T = [v\ \delta]^T$ ——行驶速度$v$和转向角$\delta$。模型参数包括：$c_\alpha$ ——侧滑系数，$l_h$、$l_v$ ——轮胎接地点到后轴和前轴的距离至刚体平面，$m_{rob}$ ——机器人质量，以及$J_{rob}$ ——机器人的转动惯量。执行器的行为未被建模，因为我们的非线性模型预测控制为底层控制器提供设定点。

2.3 障碍物检测与描述

安装在移动机器人上的激光扫描仪的测量范围为5.60米，扫描角度为240°。但由于前进运动、测量信息中不必要的复杂性的降低以及相应的评估CPU时间，扫描角度被设置为120°。每个采样时刻的传感器数据包括障碍物的角度和距离。扫描区域的图形说明如图2所示。相关边界

障碍物点以红色标出。随后，将利用障碍物的外侧点首先判断检测到的是单个还是多个障碍物；其次，需判断当机器人无法从彼此靠近的障碍物之间通过时，是否将这些障碍物合并为一个障碍物。

从障碍物外侧坐标距离出发，并考虑到技术因素和安全因素，障碍物在此研究中被描述为一个椭圆：

$$
1 \leq \frac{(s_x - s_{x,obs})^2}{a^2_{obs}} + \frac{(s_y - s_{y,obs})^2}{b^2_{obs}}
$$

如图3所示。

3. 非线性模型预测控制

3.1 非线性最优控制问题的表述

一个非线性最优控制问题的一般表述为

$$
\min_{u(t)} \left{ J = E(x(t_f)) + \int_{t_0}^{t_f} L(x(t), u(t), t) dt \right}
$$

s.t.
$$
\dot{x}(t) = f(x(t), u(t), t),\ x(t_0) = x_0,\ g(x(t), u(t), t) \leq 0,
$$
$$
x_{min} \leq x(t) \leq x_{max},\ u_{min} \leq u(t) \leq u_{max},
$$

其中 $t \in [t_0, t_f]$ 为时间，$t_0, t_f$ 为初始时间和最终时间，$x(t) \in \mathbb{R}^{N_x}$ 为状态变量向量，$u(t) \in \mathbb{R}^{N_u}$ 为控制变量向量，$N_x, N_u$ 分别为状态和控制的数量，$x_0$ 为初始状态向量，$x_f$ 为最终状态向量，$f \in \mathbb{R}^{N_x} \times \mathbb{R}^{N_u} \times [t_0, t_f] \rightarrow \mathbb{R}^{N_x}$ 为微分方程的右端项，$J$ 为包含梅尔项和拉格朗日项 $E: \mathbb{R}^{N_x} \rightarrow \mathbb{R}$ 和 $L: \mathbb{R}^{N_x} \times \mathbb{R}^{N_u} \times [t_0, t_f] \rightarrow \mathbb{R}$ 的性能指标，分别属于相应的函数空间，$g$ 为附加不等式约束，$x_{min}, x_{max}, u_{min}, u_{max}$ 分别为状态$x(t)$和控制量$u(t)$的下界和上界。

3.2 组合多阶段打靶与配置方法（CMSC）

为了将非线性最优控制问题（2）转化为非线性规划问题，必须采用一种离散化方案。最优控制问题的直接离散化方法中，多阶段打靶法最早由博克和普利特（1984）提出。另一方面，配置方法在状态约束最优控制问题中的离散化已被广泛使用，例如洪等（2006）、巴特尔等（2011）。

多阶段打靶离散化会产生块结构矩阵，并有利于计算的并行化，Lazutkin等（2015）。然而，若将多阶段打靶法与配置法相结合，其精度可得到显著提升。因此，采用组合多阶段打靶与配置（CMSC）方法对最优控制问题进行离散化，被发现能够实现较高的计算效率解决复杂的大型最优控制问题的效率，Tamimi和Li（2010），拉祖特金等（2014）。

在CMSC方法中，整个时间范围$[t_0, t_f]$被划分为打靶区间$[t_i, t_{i+1}]$, $i = 0,1,…, N_s - 1$，其中$N_s$为打靶区间数量，且满足$t_{N_s} = t_f$。每个状态变量在每个打靶区间内通过拉格朗日多项式(3)的线性组合进行近似，使用移位勒让德配置方案

$$
\hat{x}(t) = \sum_{j=0}^{N_c} \left( \prod_{\substack{l=0 \ l \neq j}}^{N_c} \frac{t - t_{i,l}}{t_{i,j} - t_{i,l}} \right) \cdot x^c_{i,j},\ i = 1,…, N_x, \quad (3)
$$

其中，$\hat{x}(t)$ 是连续时间状态变量$x(t)$的多项式逼近，$N_c$ 为配置点数量，$x^c_{i,j}$ 为第$i$个打靶区间内第$j$个配置点处的未知配置系数，作为配置状态，${t_{i,1},…, t_{i,N_c}}$ 为第$i$个打靶区间内的配置点$[t_i, t_{i+1}]$。多项式$\hat{x}(t)$在配置点处与$x(t)$相匹配。在每个打靶区间中，最后一个配置点$t_{N_c}$被定义为下一个区间的初始点，即$t_{i,N_c} = t_{i+1}$，以保证打靶区间之间状态的时间连续性。因此，需要引入额外的等式约束以确保打靶区间边界处状态值的连续性，即$h_{i+1} = \hat{x}_i, i = 0, 1, …, N_s - 1$。图4展示了一个打靶区间内一个状态变量具有$N_c = 3$个配置点的配置法示意图。本研究中对控制量采用分段常数离散化。随后，目标函数、等式约束以及不等式约束(2)被离散化，参见例如Tamimi和Li(2010)。由于所采用并下文提及的优化方法，松弛变量被引入到不等式约束中。在对非线性最优控制问题进行离散化，并对初始状态$h_i, i = 0, 1, …, N_s$和控制量$v_i = const., i = 0, 1, …, N_s - 1$进行参数化之后，建立了如下非线性规划公式：

3.3 数值实现

本研究的主要目标在于应用一种非线性模型预测控制公式并高效求解。为此，我们推导了一个合适的模型（1）。对控制变量（速度和转向角）以及状态引入了边界约束。特别是，如第2.3节所述，必须考虑由障碍物引起的不等式约束。性能指标包含了反映
- 到达目标位置的精确度，
- y方向位置相对于期望方向的偏差$s_y$，
- 控制能耗，以及
- 控制动作的增量从一个时间区间到下一个时间区间的$v$。

根据目标函数（4）中所列各项的重要性，可以调整权重矩阵。在完成非线性规划公式（4）后，需要选择优化时间域$H_p$以及在此时间域内的采样时间$\Delta t$。在所有研究中，所有初始状态均设为零。每个采样间隔开始时的初始状态由机器人的里程计估计得到。有关模型预测控制理论的更多细节可参见Maciejowski（2002）。

通过内点法优化器求解得到的非线性规划(4)，该优化器实现在软件包Ipopt中，Wächter以及Biegler（2006）。在优化器的每次迭代中，使用牛顿算法结合来自NAG库（数值算法组（2005））的线性代数求解器求解非线性模型方程，以确定牛顿步长，从而计算给定$h_i$和$v_i$值下的状态变量及其在配置点处的灵敏度。灵敏度通过应用隐式微分法对$h_i$和$v_i$进行求导获得，基于离散化的模型方程。所得的方程组也在每个打靶区间内独立地由直接线性代数求解器求解。注意，这些计算占据了大部分CPU时间。灵敏度和方程计算得越快，求解非线性最优控制问题所需的迭代次数越少，新的控制值就能越快地传递到底层控制器。非线性规划(4)的解提供了最优控制序列$v$。其中第一个采样间隔内的值（$v_0$）被应用于移动机器人。在第一个采样间隔结束后，时间域$H_p$向前移动$\Delta t$，然后重复相同的过程。通过施加终端代价$\tilde{x}_f^T M \tilde{x}_f$来保证稳定性。如果容许控制集非空，则该措施也能确保可行性，梅恩(2014)。

利用多核CPU的三个线程进一步加速计算，即：（i）读取和评估激光扫描仪数据，通过不可并行化的Ipopt求解非线性规划（计算量最大）；（ii）将控制量应用于移动机器人并进行修正（见第4节）；（iii）计算位置及其修正，检查是否到达目标位置，并保存数据。

由于CMSC方法中包含了多打靶法，因此在每个打靶区间内状态及其灵敏度可以并行计算，从而提供了尚未开发的进一步并行化潜力，进而加速计算。相关初步研究和实现见Lazutkin等（2015）。

4. 在线实验结果

我们的目标是测试车道保持和避障的性能。图1展示了实验设置的视图。使用一台配备Intel Core i3‐2350 CPU@2.3 GHz x 4和64位Ubuntu 12.04 LTS Linux的笔记本电脑。在所有场景中，预测时域$H_p$设为7秒，采样间隔设为$\Delta t = 700$ ms，即10个打靶区间。选择$H_p$和$\Delta t$是为了能够及时识别障碍物并获取控制量，但通过进一步的并行化和高性能计算设施可显著减少该时间。整个计算框架在一个软件包中实现，并针对移动机器人的自动驾驶在多个场景下进行了测试。

引入修正系数是为了避免尽管施加了相同的控制信号，前后轴的角速度仍存在微小差异。由于机械车轮安装导致的较大车轮齿隙，需要对行驶速度（$v_{corr} = c_v(\delta)v + d_v$）和转向角（$\delta_{corr} = c_\delta(\delta)\delta + d_\delta$）进行额外的两项修正。对于y方向位置估计，还需应用进一步的修正：$s_{y,corr} = k(s_x)s_x$，（$c_v, d_v, c_\delta, d_\delta, k$ —修正系数）。所有参数通过基于实测试验数据的最小二乘法确定。

本文提出了两种场景。第一种执行的任务称为最终定位，即从起始位置$(s_x(0), s_y(0)) = (0,0)$到达预定义的最终位置$(s_x(t_f), s_y(t_f)) = (20,0)$。所有其他初始状态均为零。在路径上存在三个障碍物，需要由移动机器人自主检测并避开。目标是计算具有典型性能指标的最优控制，该性能指标包括二次加权控制以及偏航角$\psi(t_f)$和位置$s_x(t_f)$、$s_y(t_f)$的终态加权。根据成本函数，相应地调整权重矩阵，$M = \text{diag}([0\ m_2\ 0\ m_4\ m_5])$，$Q = 0$，$R_\Delta = 0$，$R = \text{diag}(R_{ii})$，$R_{ii} = \text{diag}([r_1\ r_2])\ i = 1,2,…, N_s$，$m_i, r_i$——为加权系数。控制量的约束定义为$0 < \varepsilon \leq u_1 \leq 0.85$，$\varepsilon$——阈值，$-0.15 < u_2 \leq 0.15$。

图6显示了实时实验中行驶路径以及对应的速度$u_1$和转向角$u_2$随时间变化的绿色实线。从最右侧障碍物的避障情况可以得出，计算路径依赖于多个条件，此处显然还取决于机器人朝向和控制约束。

第二个场景是车道保持场景。此外，在目标函数中还考虑了在打靶间隔边界处$s_y$‐位置相对于期望y位置的偏差，该期望y位置是从相同的初始位置到与上述相同的最终位置的一条直线。与最终定位场景不同，权重矩阵$Q$被设置为$Q = \text{diag}(Q_{ii})$，$Q_{ii} = \text{diag}([0\ q_2\ 0\ q_4\ q_5])$，$i = 1,2,…, N_s$，$q_i$‐加权系数。在线实验结果由图6中的品红色虚线表示。与第一个场景相反，观察到更接近期望方向的距离，特别是在避障最右侧障碍物方面。

图5展示了在每个采样间隔内最终定位场景（绿色实线）和车道保持场景（品红色虚线）中的中央处理器时间。显然，当存在障碍物时需要计算解，即存在更多的不等式约束，例如在第15个间隔的最终定位场景和第8个间隔的车道保持场景中，可以观察到增加的计算量。第一个场景的平均CPU时间为271毫秒，第二个场景为244毫秒。

在两种场景中，从图6可以看出，速度保持在尽可能高的水平（由于实际位置与最终x位置存在偏差），方向也尽可能保持直线。但是，如果检测到障碍物，机器人必须偏离最大速度和期望方向。

5. 结论

我们提出了一种高效动态优化方法的实现，用于在移动机器人自动驾驶中实现非线性模型预测控制。与大多数先前采用欧拉离散化的研究不同，我们使用多射击法和配点法相结合的方法对最优控制问题进行离散化，从而显著提高了计算效率，具有更高的精度、更小的计算量，并且支持并行化。实验结果表明，我们的方法在车道保持和避障方面具有很高的潜力。本文尚未与其他非线性模型预测控制避障方法进行比较，但将在后续研究中进行。进一步研究将涉及加快计算速度、通过扩展卡尔曼滤波器改进位置估计、鲁棒性分析，并考虑相机数据以实现传感器融合和应对移动障碍物。