MPC_ORCA

男娘i 2022-10-27 12:29 554阅读 0赞

### 文章目录 ###

*  2 问题描述
 *   *  2.1 设置移动机器人
     *   *  2.1.1 差分移动机器人的电影约束
         *  2.1.2 应用输入命令
         *  2.1.3 线性化
     *  2.2 状态变量中的模型
     *  2.3系统模型
 *  3 基于模型的预测控制
 *   *  3.1 确定MPC问题
     *  3.2 凸优化
     *   *  3.2.1二次规划
     *  3.3 将MPC问题转换为QP
 *  4 机器人导航
 *   *  4.1路径规划
     *  4.2 本地控制以防止机器人之间发生碰撞
     *   *  4.2.1 速度障碍物
         *  4.2.2 互惠速度障碍物
         *  4.2.3 最佳互惠避碰
         *  4.3.1 将ORCA应用于QP问题
         *  4.3.2 解决不可行问题的策略
 *  5 实验结果
 *   *  5.1 实施
     *  5.2 单个机器人的跟踪控制
     *   *  5.2.1 场景1
         *  5.2.2 场景2
         *  5.3.1 场景3
         *  5.3.2 场景4
     *  5.4 讨论
 *  6 结束语
 *  参考文献：

# 2 问题描述 #

## 2.1 设置移动机器人 ##

在这项工作中，机器人由在平面 \{ x 0 ， y 0 \} \\\{x\_0，y\_0\\\} \{ x0，y0\}中移动的半径为 λ ∈ R \\lambda \\in \\mathbb\{R\} λ∈R的圆形实体表示，并在原点的全局坐标系 F 0 = \{ O 0 ， x 0 ， y 0 ， z 0 \} \\mathcal\{F\}\_0 = \\\{O\_0，x\_0，y\_0，z\_0\\\} F0=\{ O0，x0，y0，z0\}中定义。设 F R = \{ O R ， x R ， y R \} \\mathcal\{F\}\_R = \\\{O\_R，x\_R，y\_R\\\} FR=\{ OR，xR，yR\}是机器人的坐标系，起源于 O R O\_R OR，在全局坐标中可表示为 O R = ( p x 0 ， p y 0 ) O\_R =(p\_\{x0\}，p\_\{y0\}) OR=(px0，py0)，即 p x 0 ， p y 0 ∈ R p\_\{x0\}，p\_\{y0\} \\in \\mathbb\{R\} px0，py0∈R，如图2.1所示。 全局坐标系 F 0 \\mathcal\{F\}\_0 F0和机器人坐标系 F R F\_R FR之间的旋转的角度 θ z 0 ∈ S 1 θ\_\{z0\}∈\\mathbb\{S\}^1 θz0∈S1。 因此在全局坐标系下可以将机器人的结构描述为 q = \[ p x 0 p y 0 θ z 0 \] T ∈ Q ⊂ R 2 × S 1 q = \[p\_\{x0\} \\quad p\_\{y0\} \\quad θ\_\{z0\}\]^T\\in \\mathbb\{Q\} \\subset \\mathbb\{R\}^2 \\times \\mathbb\{S\}^1 q=\[px0py0θz0\]T∈Q⊂R2×S1。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70]  
  旋转矩阵 R R 0 R^0\_R RR0给出了机器人在世界坐标系 F 0 F\_0 F0和机器人坐标系 F R F\_R FR之间的方向关系，从而可以在全局坐标系中表示机器人坐标系中描述的点（或矢量）。 例如，机器人在自身坐标系下的速度 v R v^R vR到全局坐标系下速度的转换为  
![在这里插入图片描述][20210223104525692.png]

其中 v 0 = \[ v x 0 v y 0 \] T ∈ R 2 v^0 = \[v\_\{x0\} \\quad v\_\{y\_0\}\]^T \\in \\mathbb\{R\}^2 v0=\[vx0vy0\]T∈R2表示全局惯性系中机器人的速度，其中 v x 0 = p ˙ x 0 ∈ R v\_\{x0\} =\\dot p\_\{x0\} \\in \\mathbb\{R\} vx0=p˙x0∈R,  v y 0 = p ˙ y 0 ∈ R v\_\{y0\} =ṗ\_\{y0\}\\in \\mathbb\{R\} vy0=p˙y0∈R分别代表机器人在 x 0 x\_0 x0轴和 y 0 y\_0 y0轴上的速度。

### 2.1.1 差分移动机器人的电影约束 ###

差动移动机器人会受到非完整的限制（或循环限制），从而限制了瞬时运动，也即限制了允许选择的速度空间。 带有差速器轮的机器人的运动具有两个非完整条件:

*  无法横向移动，即 v y R = 0 v\_\{y\_R\}=0 vyR=0
 *  轮子与地面紧密接触，在 x R x\_R xR和 y R y\_R yR上均不会打滑。  
    在上述的假设下，可以得到线速度 v 0 = v x R x R v\_0 = v\_\{x\_R\}x\_R v0=vxRxR，角速度 ω 0 = ω z R z R ω\_0=ω\_\{z\_R\}z\_R ω0=ωzRzR， ω z R = θ ˙ z R = θ ˙ z 0 ω\_\{z\_R\}=\\dot\{θ\}\_\{z\_R\}=\\dot\{θ\}\_\{z\_0\} ωzR=θ˙zR=θ˙z0。

通过应用方程式（2.1）中的这些非完整条件，可以得到差速机器人的运动方程式。 考虑二阶分量 a x R = v ˙ x R a\_\{x\_R\}=\\dot\{v\}\_\{x\_R\} axR=v˙xR和 α z R = ω ˙ z R \\alpha\_\{z\_R\}=\\dot\{ω\}\_\{z\_R\} αzR=ω˙zR，分别与力和转矩有关的线性和旋转加速度输入，因此可以得到：  
![在这里插入图片描述][20210223110138922.png]

### 2.1.2 应用输入命令 ###

在接收速度命令时，一些移动机器人界面以极坐标表示而不是笛卡尔坐标表示。 因此，可以利用笛卡尔坐标系下的速度矢量  v 0 = \[ v x 0 v y 0 \] v^0=\[v\_\{x\_0\}\\quad v\_\{y\_0\}\] v0=\[vx0vy0\]或加速度矢量 a 0 = \[ a x 0 a y 0 \] a^0=\[a\_\{x\_0\}\\quad a\_\{y\_0\}\] a0=\[ax0ay0\]来表示极坐标下对应的指令。  
![在这里插入图片描述][20210223110935871.png]  
  根据计算出的角速度，定义负线速度ν<0。 另一种计算 ω ω ω的方法是使用速度矢量定义的方向的当前值:  
![在这里插入图片描述][20210223111024122.png]  
函数atant2 : R × R → S : \\mathbb\{R\} \\times \\mathbb\{R\} \\to \\mathbb\{S\} :R×R→S，其取值范围为 \[ − π π \] \[-\\pi \\quad \\pi\] \[−ππ\]。

### 2.1.3 线性化 ###

机器人的非完整约束没有线性关系，因此无法根据方程式（2.2）建立时不变线性系统模型（LTI，线性时不变）。 为了避免由这些限制引起的问题，如图2.2所示，中心轴 x R x\_R xR的距离很小，因此可以将机器人视为质点。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 1]  根据图（2.2），机器人中心与线性化参考之间的关系如下公式所示：  
![在这里插入图片描述][20210223112303343.png]  
取方程（2.5）的一阶导数，我们有

![在这里插入图片描述][20210223130441788.png]  
然后二阶导数变成  
![在这里插入图片描述][20210223130506184.png]  
从方程（2.2）可以得出  
![在这里插入图片描述][20210223130732294.png]  
在（2.7）中替换（2.8）：  
![在这里插入图片描述][20210223130806861.png]状态变量模型  
![在这里插入图片描述][20210223130854605.png]![在这里插入图片描述][20210223130910266.png]  
其中 a x 0 a\_\{x\_0\} ax0和 a y 0 a\_\{y\_0\} ay0是惯性坐标系中速度 p ˙ x 0 = p ′ ˙ x 0 \\dot\{p\}\_\{x\_0\} = \\dot\{p'\}\_\{x\_0\} p˙x0=p′˙x0和 p ˙ y 0 = p ′ ˙ y 0 \\dot\{p\}\_\{y\_0\} = \\dot\{p'\}\_\{y\_0\} p˙y0=p′˙y0的加速度输入，应用速度命令或加速度命令时，无需考虑运动学限制，从而表现出一个完整的机器人  
![在这里插入图片描述][2021022313175712.png]

## 2.2 状态变量中的模型 ##

从一组一阶微分方程（或离散时间系统的差分方程）中，系统状态由状态变量 x x x（通常作为时间t的函数）的向量表示，该向量提供了系统运动过程中内部的描述。随着系统的发展，状态变化由状态 x x x和输入 u u u的线性组合描述,例如  
![在这里插入图片描述][20210223133448678.png]  
上面公式中， A A A和 B B B是确定系统内部动态的矩阵，而 C C C和 D D D显示了如何在输出中测量或观察变量。 图2.3以框图显示了公式（2.13）的实现。 在二阶或更高阶方程的情况下，可以将一些高阶项视为状态变量，并将方程组扩展为仅包含一阶的形式。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 2]  
  在数字应用中，均使用离散模型。 在离散情况下，状态向量通过与采样时间 T s ∈ R T\_s \\in \\mathbb\{R\} Ts∈R相关的 x x x步迭代地演化。状态向量的下一次迭代 x ( k + 1 ) x(k +1) x(k\+1)是根据状态电流 x ( k ) x(k) x(k)和当前输入 u ( k ) u(k) u(k)计算的来。  
![在这里插入图片描述][20210223141109347.png]  
其中参数 F F F和 G G G的计算如下：

![在这里插入图片描述][20210223141152281.png]  
请注意，可以从泰勒级数展开式计算矩阵指数  
![在这里插入图片描述][2021022314122480.png]

## 2.3系统模型 ##

使用线性化，有可能以与完整机器人相同的方式描述带有差动轮的机器人的运动，而无需考虑完整的限制。 从公式2.12开始，表示连续模型

![在这里插入图片描述][20210223142710488.png]  
状态向量为 x ( t ) = \[ p ( t ) T v ( t ) T \] T x(t)= \[p(t)^T \\quad v(t)^T\]^T x(t)=\[p(t)Tv(t)T\]T，输入 u ( t ) = a ( t ) u(t)=a(t) u(t)=a(t)。

将方程式（2.17）与方程式（2.13a）进行比较可得，离散模型中矩阵 A = \[ 0 1 0 0 \] A=\\left\[ \\begin\{matrix\} 0 & 1 \\\\ 0 & 0 \\\\ \\end\{matrix\} \\right\] A=\[0010\]和 B = \[ 0 1 \] B=\\left\[ \\begin\{matrix\} 0 \\\\ 1 \\\\ \\end\{matrix\} \\right\] B=\[01\]，应用方程式（2.15a）和（2.15b），省略了矩阵指数的第二或更高项，使得  
![在这里插入图片描述][20210223150605634.png]  
取向量的二维分量，得到二阶离散模型：  
![在这里插入图片描述][20210223151040252.png]

# 3 基于模型的预测控制 #

模型预测控制（Model Predictive Control， MPC），或简称为预测控制，是通过基于显式系统模型的预测和优化策略的联合，用来实现系统自动化的控制器设计方法，同时系统变量必须满足限制条件。通常，最优化问题考虑的是对未来有限范围内状态和控制措施的预测，仅在实际系统中使用第一个措施，然后丢弃其他措施。在下一次迭代中，优化问题将通过获取的新测量值进行更新并再次解决，新测量值称为回溯期。

图3.1在SISO应用程序中（Single Input Single Output，单输入单输出）可以举例说明使用预测控制的系统的行为。从初始时刻开始，假设采样周期为 T s ∈ R T\_s \\in \\mathbb\{R\} Ts∈R，则在大小为 N ∈ N N \\in \\mathbb\{N\} N∈N的预测范围内定义参考值或轨迹 r ( k ) ， r ( k + 1 ) ， . . . ， r ( k + N ) r(k)，r(k + 1)，...，r(k + N) r(k)，r(k\+1)，...，r(k\+N)。根据过程模型和以往的读数，设计一种机制来估计输出 y ( k ) ， y ( k + 1 ) ， . . . ， y ( k + N ) y(k)，y(k + 1)，...，y(k + N) y(k)，y(k\+1)，...，y(k\+N)和输入信号 u ( k ) ， u ( k + 1 ) ， . . . ， u ( k + N ) u(k)，u( k + 1)，...，u(k + N) u(k)，u(k\+1)，...，u(k\+N)。基于准则或目标函数构造优化问题，以选择输入信号集，以保持系统的行为接近参考轨迹，并且还可以考虑诸如输入变化（控制误差）之类的因素。通常，优化问题在处理控制案例或任务时需要更多的时间，但必须要在少于 T s T\_s Ts的时间内解决。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 3]  MPC的概念自1960年代中期以来就在文献中出现，主要在化学工业中非常流行，用于显示动力学变化较慢的系统。自1980年代以来，它已用于具有多个变量且受限制的应用程序中。随着计算能力的提高，MPC不再局限于动态缓慢的过程，而是使用更快、更有效的优化技术，这使得在越来越多的应用程序中使用预测控制成为可能。

由于它是一种方法，因此存在预测控制的几种实现方式，它们具有建模、预测策略和优化算法的不同组合。 多年来，一些实现已发展并已广泛传播。 一个例子是QDMC算法（Quadratic Dynamic Matrix Control），该算法对前代DMC（Dynamic Matrix Control）进行了改进，具有较低的计算成本。 QDMC的主要功能是：

*  线性模型；
 *  在预测时间范围内是二次目标函数；
 *  感兴趣的变量在未来时刻的行为必须遵循参考且受惩罚项的约束。
 *  通过解决二次问题获得最优输入。

## 3.1 确定MPC问题 ##

基于方程式（2.19）中所示的模型并采用QDMC实现，用于控制移动机器人的MPC问题可以定义为  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 4]由于所有状态变量都是可观察到的，因此状态矢量被视为系统自身的输出。 要最小化的表达式由二次分量的加权和组成，其中 x ( k ) − r ( k ) x(k)-r(k) x(k)−r(k)表示移动机器人的轨迹误差。

要最小化的目标函数由二次分量的加权和组成，这样矩阵 Q Q Q和 R R R是半正定的，并且可以在预测范围内变化。矩阵 Q Q Q和 R R R是控制器调整的基本参数，分别用于将权重与参考距离标准和控制动作的应用相关联。问题约束限制了优化区域，其不仅考虑系统的动力学，还考虑了系统相对于位置和速度的物理限制（以 x m i n x\_\{min\} xmin， x m a x x\_\{max\} xmax和 u m i n u\_\{min\} umin和 u m a x u\_\{max\} umax表示），以及由当前位置和速度给出的初始条件 x 0 x\_0 x0。

## 3.2 凸优化 ##

凸优化问题通过公认的理论形成一类数学优化问题，可以实现非常有效和可靠的解决方案。 此外，它们在全局优化和变量组合的应用中具有很大的优势。 由于这些原因，它已用于控制系统，估计和信号处理，通信和网络，电子电路设计，数据分析，经济学等应用中。

通常，可以将凸优化问题定义如下：  
![在这里插入图片描述][20210223162419478.png]  
其中 z ∈ R n z \\in \\mathbb\{R\}^n z∈Rn是根据成本函数 J ( z ) : R n → R J(z):\\mathbb\{R\}^n \\to \\mathbb\{R\} J(z):Rn→R优化的变量（通常是个向量），且满足约束函数 f i ( z ) : R n → R f\_i(z):\\mathbb\{R\}^n \\to \\mathbb\{R\} fi(z):Rn→R和常数 b i ∈ R b\_i \\in \\mathbb\{R\} bi∈R。此外， J ( z ) J(z) J(z)和 f i ( z ) f\_i(z) fi(z)满足凸性，即对于 z 1 , z 2 ∈ R n z\_1,z\_2 \\in \\mathbb\{R\}^n z1,z2∈Rn和 α , β ∈ R \\alpha,\\beta \\in \\mathbb\{R\} α,β∈R存在下列关系：  
![在这里插入图片描述][20210223163624885.png]  
  由上面的公式可以看出线性函数也是凸的。 最优化的一些已知应用（例如最小二乘法，线性规划，半定义规划和二次规划）是凸优化的特殊子类。 总而言之，最小二乘优化的问题是平方和无约束作为目标函数，线性规划假设目标函数和凸约束，半定规划根据正负矩阵锥的交点优化线性成本函数，二次规划使用二次目标函数和凸约束。

每个限制表示一个半空间 a i T z ≤ b i a\_i^Tz \\le b\_i aiTz≤bi，由法线向量 a i a\_i ai和点 z 0 i z\_\{0i\} z0i决定，如图3.2所示。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 5]

线性约束所界定的区域描述了一个多边形 A z ≤ b Az \\le b Az≤b，其中 b = \[ b 1 b 2 . . . b i . . . b m \] b=\[b\_1 \\quad b\_2 \\quad ... b\_i ... \\quad b\_m\] b=\[b1b2...bi...bm\]， A A A为m行由向量 a a a组成的矩阵。多面体的尺寸基于约束的数量和优化变量的数量，因此为 z ∈ R z \\in \\mathbb\{R\} z∈R，并且在考虑某些约束的情况下为 A ∈ R m × n A \\in R^\{m \\times n\} A∈Rm×n。 图3.3显示了一个示例，其中包含由五个约束条件构建的二维多面体。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 6]

### 3.2.1二次规划 ###

可以通过以下表达式来定义二次规划（QP，Quadratic Programming）优化问题  
![在这里插入图片描述][2021022317065743.png]  
其中，半正定矩阵 P P P和 c ∈ R n c \\in \\mathbb\{R\}^n c∈Rn分别表示二次。在限制条件下， z m i n z\_\{min\} zmin和 z m a x z\_\{max\} zmax决定了操作 A z Az Az的最小限制和最大限制。假设 z m a x = b z\_\{max\} = b zmax=b且每个 z m i n = − ∞ z\_\{min\}=-∞ zmin=−∞，则可以用来表示图3.3中多面体围成的区域。注：通常不考虑 z m i n = z m a x z\_\{min\} = z\_\{max\} zmin=zmax的情况。

这种方法的优点之一是，一旦问题可行，就可以保证唯一的全局最小值。相反，在线性规划中，解决方案没有保证的唯一性，但是可以在策略的顶点（超平面之间的交点）中找到。

为了解决二次规划问题，可以使用例如活动集(Active Set)，内部点(Interior Point)和一阶方法(First Order)。具体来说，一阶方法仅使用来自目标函数的一阶信息来迭代计算最优解。一类特殊的一阶技术是运算符拆分，它将优化问题建模为寻找单调运算符之和的根的问题。

在这项工作中，使用了OSQP（Operator Splitting Quadratic Program），它是解决涉及二次项优化问题的通用求解器，它应用了一种分隔运算符的方法，能够基于ADMM算法（Alternating Direction Method of Multipliers）高精度计算解决方案。

## 3.3 将MPC问题转换为QP ##

为了在OSQP或某些其他编程解决方案中进行求解，有必要将公式（3.1）中的MPC问题传递到公式3.4中给出的QP问题。 将所有状态和预测范围 N N N输入组合成向量 z z z，可以写成：  
![在这里插入图片描述][20210225133034476.png]  
  目标函数的二次矩阵分量 P P P和线性 c c c考虑了预测范围。 为此，使用Kronecker乘积，在其中应用权重矩阵 Q Q Q和 R R R，并以MPC问题的形式表示参考路径 r r r，  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 7]  
  对于本文中开发的应用程序， n x = 4 n\_x = 4 nx=4是状态 x x x的向量的维数， n u = 2 n\_u = 2 nu=2是状态 u u u的向量的维数。

MPC问题由方程和不等式形式的凸函数确定。 因此，可以根据等式（下标eq）和不等式（下标ineq）图的子矩阵计算QP问题的分量 z m i n z\_\{min\} zmin， z m a x z\_\{max\} zmax和 A A A，如  
![在这里插入图片描述][20210225134446252.png]  
  因此 A e q A\_\{eq\} Aeq， z m i n e q z\_\{min\_\{eq\}\} zmineq和 z m a x e q z\_\{max\_\{eq\}\} zmaxeq是从代表系统动态的矩阵 F F F和 G G G推导到预测范围 N N N上得出的。由于这些因素代表了优化问题的相等条件，因此 z m i n e q = z m a x e q z\_\{min\_\{eq\}\} = z\_\{max\_\{eq\}\} zmineq=zmaxeq，生成一组超平面 A e q z = z m i n e q = z m a x e q A\_\{eq\}z = z\_\{min\_\{eq\}\} = z\_\{max\_\{eq\}\} Aeqz=zmineq=zmaxeq。 另外， z m i n e q z\_\{min\_\{eq\}\} zmineq和 z m a x e q z\_\{max\_\{eq\}\} zmaxeq包含系统所在的初始条件 x ( 0 ) x(0) x(0)。 所以，你可以写  
![在这里插入图片描述][20210225135128512.png]  
其中矩阵 I N + 1 k = − 1 I\_\{N+1\}^\{k =-1\} IN\+1k=−1的参数 k k k表示从对角线的偏移， k > 0 k>0 k>0表示向右偏移， k < 0 k<0 k<0表示向左偏移。

不等式 A i n e q A\_\{ineq\} Aineq， z m i n i n e q z\_\{min\_\{ineq\}\} zminineq和 z m a x i n e q z\_\{max\_\{ineq\}\} zmaxineq的矩阵图甚至都不是一组半空间，只是形成一个凸区域，该凸区域对状态（ x m i n / x m a x x\_\{min\} / x\_\{max\} xmin/xmax）和控制输入（ u m i n / u m a x u\_\{min\} / u\_\{max\} umin/umax）施加最大限制和最小限制。 因此，通常 z m i n i n e q ≠ z m a x i n e q z\_\{min\_\{ineq\}\} \\neq z\_\{max\_\{ineq\}\} zminineq=zmaxineq。 最后，你可以写  
![在这里插入图片描述][20210225140143534.png]

# 4 机器人导航 #

机器人导航可分为三个主要任务，即定位，路径规划和运动控制。在自主控制系统中，这些任务是相互关联的，因此一个步骤会使用另一步骤计算的结果，例如定位，地图的表示形式和要采用的路线。

定位负责映射和建模机器人所在的环境，并通过预定的世界模型，电子传感器或数据处理来确定机器人的状态。该领域中的一些工作涉及如何处理传感器读数中的误差和不确定性。

知道机器人所处的环境或地图后，轨迹规划为机器人建立从初始位置移动到目标位置的路径。通常可以分布全局路径规划和局部路径规划两部分。前者依赖于环境地图规划出一条从起始位置到目标未知的路径，后者用于在移动过程中动态的规划局部路径来躲避障碍物。常用的算法有PRM（概率路线图），图搜索算法A \*和势场法等。

运动控制系统试图确保机器人可以跟踪由全局路径规划出的位置、速度或加速度。 在这种情况下，系统必须能够响应某些类型的干扰，例如障碍物，地形倾斜和其他移动机器人的干扰。 与大多数全局路径规划算法不同，运动控制算法应在环境中实时、局部、动态、迅速响应。

## 4.1路径规划 ##

大多研究通常假设障碍物是静态的，因此机器人在移动时需要对轨迹进行连续的、动态的、局部重新规划，以避免碰撞。 在这项工作中，轨迹的规划阶段不考虑移动障碍物或活动物，这些由控制系统在本地进行处理。

在这项研究中，路径由函数 r ( t ) = \[ p r e f ( t ) T v r e f ( t ) T \] T r(t)= \[p\_\{ref\}(t)^T \\quad v\_\{ref\}(t)^T\]^T r(t)=\[pref(t)Tvref(t)T\]T定义，其中 p r e f ( t ) p\_\{ref\}(t) pref(t)是位置的参考向量，而 v r e f ( t ) v\_\{ref\}(t) vref(t)是速度参考向量。 为了确保平稳和逼真的运动，需要满足关系 v r e f ( t ) = p ˙ r e f ( t ) v\_\{ref\}(t)= \\dot p\_\{ref\}(t) vref(t)=p˙ref(t)。  
![在这里插入图片描述][20210225221505500.png]  
是具有这些特征的S型函数，由于它以导数形式表示，因此在多个领域中都得到了应用  
![在这里插入图片描述][20210225221727119.png]  
其中， t m a x t\_\{max\} tmax是函数的导数达到最大值的时间， K ∈ R K \\in \\mathbb\{R\} K∈R是斜率系数。 使用 p r e f ( t ) pref(t) pref(t)中的逻辑函数，当运动接近目标时，将获得加速度减小，即最初为正，最后为负。 考机器人从起点开始位置到目标位置，通过凸组合及其派生形式将n个组件分为  
![在这里插入图片描述][20210225223118226.png]  图4.1显示了使用具有不同斜率系数（但即使是 t m a x t\_\{max\} tmax）的对数函数的轨迹的三个示例，也就是说，所有轨迹同时显示出峰值速度。 可以看出， K K K与从 p s t a r t p\_\{start\} pstart到 p g o a l p\_\{goal\} pgoal所需的时间成反比。 因此，根据 K K K的值，可以在不损害 p r e f ( t ) pref(t) pref(t)连续性的情况下提高 t m a x t\_\{max\} tmax，但是必须选择 K K K的方式，以使生成的路径仍然遵守速度限制和机器人的最大加速度的限制 。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 8]

## 4.2 本地控制以防止机器人之间发生碰撞 ##

以下是文献中存在的一阶方法，这些方法专用于避免离散的移动机器人之间的冲突。这种方法需要了解环境的某些特征，例如其他机器人的当前位置和速度。，而无需在机器人之间进行明确的通信，并且可以通过每个机器人中的传感器来估计位置和速度。

图4.2显示了将应用这些方法的场景，其中考虑了圆形机器人A和B，并通过速度 v A v\_A vA和 v B v\_B vB、中心 p A p\_A pA和 p B p\_B pB， r A r\_A rA和 r B r\_B rB进行了描述。 从机器人A的角度提出了这些技术， v B v\_B vB被视为固定向量， v A v\_A vA为感兴趣的速度向量，并且还可以考虑由全局计划程序 v r e f A v\_\{refA\} vrefA确定的参考速度。

### 4.2.1 速度障碍物 ###

速度障碍（VO）的概念基于机器人速度空间中环境的可变行为的映射。 定义了碰撞锥，它确定了速度 v A , B v\_\{A,B\} vA,B，其中A的速度是相对于B的速度，如果速度保持恒定，则在将来的某个时间会导致智能体之间的碰撞，例如  
![在这里插入图片描述][20210301112408453.png]

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 9]  
 λ ( p A ; v A − v B ) \\lambda(p\_A; v\_A-v\_B) λ(pA;vA−vB)是沿 v A − v B v\_A-v\_B vA−vB方向通过点 p A p\_A pA的线，并由与切线B相切的直线 λ 1 \\lambda\_1 λ1和 λ 2 \\lambda\_2 λ2界定，如图4.3所示。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 10]  考虑到具有多个对撞机的系统，必须根据绝对速度或相对于固定惯性系来考虑区域。 使用Minkowski的加法，并将 V O A ∣ B ( v B ) VO\_\{A|B\}(v\_B) VOA∣B(vB)区域定义为  
![在这里插入图片描述][20210301120624365.png]图4.4显示了由该总和得出的区域。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 11]  v定义障碍物集合，可以根据每个障碍物从A建立速度障碍物区域。 所有这些区域的联合为  
![在这里插入图片描述][20210301120717927.png] 该联合区域定义A速度的组合区域，该区域会在将来的某个时间与某些智能体发生碰撞。

因此，由于代理A的期望速度是由全局规划计算的，因此 v A ∗ ∉ V O A v\_A^\* \\notin VO\_A vA∗∈/VOA，因此 v A v\_A vA接近 v r e f A v\_\{refA\} vrefA。

根据另一种说明，可以在考虑时间窗口的情况下获得 V O VO VO区域，如图4.5所示，该区域是有限的，因此忽略了导致稍后碰撞的速度。 由于 B ( p , r ) B(p,r) B(p,r)是一个半径为 r r r且以 p p p为中心的空心球，因此定义速度空间中的碰撞锥和VO区域：  
![在这里插入图片描述][20210301122109600.png]![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 12]  尽管在这种情况下 v B v\_B vB只是一个矢量，但此定义不允许将Minkowski和应用到一组允许的速度 V B V\_B VB上，这不仅会导致碰撞锥的位移，还会导致碰撞锥的扩大，减小安全区域的面积。

### 4.2.2 互惠速度障碍物 ###

使用VO时，如果每个智能体都没有考虑到环境中的其他代理，则所产生的运动很容易振动。 在这种情况下，互惠速度障碍（RVO）方法提供了VO的扩展，导致无碰撞运动的振荡。

使用RVO，而不是选择VO区域之外的速度，而是选择当前速度 v A v\_A vA和紧邻该区域之外的速度的平均值，从而将防止碰撞的责任划分为两个主体之间的责任。 两个智能体A和B之间的RVO区域，其时间和定义窗口为  
![在这里插入图片描述][20210301123037809.png]在几何上该区域可以解释为 V O A ∣ B τ ( v B ) VO\_\{A|B\}^\{\\tau\}(vB) VOA∣Bτ(vB)的位移，乘以(v\_A-v\_B)/ 2。

在被动障碍的情况下，即对环境没有感知和反应的情况下，将保持“速度障碍”策略。 假设设置了一组被动障碍物 O \\mathbb\{O\} O，则将智能体A放置在与其他物体发生碰撞的路线上的速度组合区域定义为  
![在这里插入图片描述][20210301123457186.png]

### 4.2.3 最佳互惠避碰 ###

最佳互惠碰撞避免（Optimal Reciprocal Collision Avoidance，ORCA）提议包括重新设计VO和RVO中使用的技术，以便为多个机器人进行无碰撞导航创造足够的条件。 这种方法有助于使用凸优化算法解决问题，尤其是在机器人密度较高的情况下。

首先，考虑当前速度 v A v\_A vA是 V O A ∣ B τ ( v B ) VO\_\{A|B\}^\{\\tau\}(v\_B) VOA∣Bτ(vB)中包含的速度，然后计算向量 u u u，它表示将机器人A的速度带到该区域外的最小变化。  
![在这里插入图片描述][20210301123949131.png]其中 ∂ V O A ∣ B τ ( v B ) \\partial VO\_\{A|B\}^\{\\tau\}(v\_B) ∂VOA∣Bτ(vB)是 V O A ∣ B τ ( v B ) VO\_\{A|B\}^\{\\tau\}(v\_B) VOA∣Bτ(vB)区域的极限点。

向量 n n n在指向VO区域内部的点 v A + u v\_A + u vA\+u处也被视为VO轮廓 V O A ∣ B τ ( v B ) VO\_\{A|B\}^\{\\tau\}(v\_B) VOA∣Bτ(vB)的法线向量, 图4.6描述了示例场景中的向量。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 13] O R C A A ∣ B τ ORCA\_\{A|B\}^\{\\tau\} ORCAA∣Bτ区域被定义为A的速度集合，它基于两个智能体之间的分力原理，类似于RVO，包含比vA更大的，更接近vA的速度，可以防止机器人之间发生碰撞。方法。 因此，将由点 v 0 = v A + 1 2 u v\_0 = v\_A + \\frac\{1\}\{2\}u v0=vA\+21u和法向矢量 n n n确定的 O R C A A ∣ B τ ORCA\_\{A|B\}^\{\\tau\} ORCAA∣Bτ区域和半平面定义为  
![在这里插入图片描述][20210301132548141.png]![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 14]\#\# 4.3 MPC-ORCA  
  在具有多个代理的情况下，在每个机器人中使用ORCA所产生的半平面是凸区域，因此这些区域的交点确定了一个多面体，如图3.3所示。 因此，可以构造凸优化问题以确定在给定时间接近 v r e f A v\_\{refA\} vrefA参考速度的无碰撞速度命令。

与仅由ORCA构建的优化算法的应用相比，使用MPC和ORCA的组合策略具有多个优势。 MPC中后退时域的特征提供了更安全的轨迹，可以对可能的碰撞情况做出预先反应，并减少优化问题变得不可行（无解）的情况。 另外，通过合并公式（2.19）中提出的二阶模型的动力学，计算出的轨迹更加平滑，因为它们假设速度没有发生突变。

通过合并ORCA限制，可以扩展表达式（3.1）中存在的MPC问题的描述。 为此，将 g ( x ( x ( k ) , \{ x n e i g h ( k ) \} ) ≤ 0 g(x(x(k),\\\{x\_\{neigh(k)\}\\\}) \\leq 0 g(x(x(k),\{ xneigh(k)\})≤0定义为速度限制，该速度限制是由应用ORCA计算所得，与其他机器人速度相关的速度限制。 此外，考虑到代理将保持恒定的速度，可以通过在预测范围 N N N上应用算法来生成针对未来状态的ORCA限制。 因此，MPC-ORCA问题可以写成  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 15]  组合策略MPC-ORCA的另一个特殊之处是可以实现以伺服或调节器模式运行的控制器。 在伺服模式下，从全局规划器接收参考路径 r ( k ) ， r ( k + 1 ) ， . . . ， r ( k + N ) r(k)，r(k + 1)，...，r(k + N) r(k)，r(k\+1)，...，r(k\+N)，并且控制器会输出是的机器人保持在参考路径附近的速度命令，或是或进行小的偏离以防止与其他代理冲突。 在监管模式下，当满足 r ( k ) = r ( k + 1 ) = . . . = r ( k + N ) = x g o a l r(k)= r(k + 1)= ... = r(k + N)= x\_\{goal\} r(k)=r(k\+1)=...=r(k\+N)=xgoal时说明机器人已经到达目标点，其中通常用 x g o a l = \[ p g o a l 0 \] T x\_\{goal\} = \[p\_\{goal\} \\quad 0\]^T xgoal=\[pgoal0\]T描述目标。 因此，控制器必须将机器人带到目标位置，避免与其他机器人发生碰撞。 在这项工作中，MPC-ORCA以伺服模式使用，因此它允许机器人以恒定或可变速度执行通用和复杂的轨迹，例如圆周，正弦运动。

### 4.3.1 将ORCA应用于QP问题 ###

按照第3.3节中使用的方法，必须将方程式（4.12）的MPC问题转换为方程式（3.4）的公式QP，才能使用OSQP之类的二次程序求解器。 为此，将行添加到 A A A， z m i n z\_\{min\} zmin和 z m a x z\_\{max\} zmax，重做表达式（3.7），如下所示：  
![在这里插入图片描述][20210301135046171.png]  
  考虑到机器人附近的许多代理， A O R C A A\_\{ORCA\} AORCA， z m i n , O R C A z\_\{min,ORCA\} zmin,ORCA， z m a x , O R C A z\_\{max,ORCA\} zmax,ORCA来描述ORCA限制的 N × m N \\times m N×m行。 每条线都是使用 v 0 v\_0 v0的值构造的，而不是由算法的应用得出的，描述了状态 x ( k ) x(k) x(k)允许的速度值的半平面，其中 k = 0 , 1 , . . . , N k = 0,1, ..., N k=0,1,...,N。

可以将仅对一个状态 x ( k ) x(k) x(k)施加限制的半平面定义为  
![在这里插入图片描述][2021030113545672.png]  
  为了将这些限制应用于 z z z优化变量（公式（3.5））， A O R C A A\_\{ORCA\} AORCA矩阵以稀疏结构确定。 A O R C A A\_\{ORCA\} AORCA， z m i n , O R C A z\_\{min,ORCA\} zmin,ORCA和 z m a x , O R C A z\_\{max,ORCA\} zmax,ORCA的值由下式给出  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 16]  
在每次迭代中，将具有ORCA限制的MPC问题转换为QP问题，仅保留矩阵P，该矩阵不会随时间变化。

### 4.3.2 解决不可行问题的策略 ###

在密集的场景中或具有多个相互交错轨迹时，可能会出现优化问题无解的情况。 考虑到移动机器人的上下文，在这些情况下可以使用一些替代方法：

1.  考虑零加速度，以便代理可以保持当前速度；
2.  强制零速，使代理完全停止；
3.  以当前速度执行阻尼，即施加与当前速度相反的信号加速度，使其速度接近零。

所有这些策略都有可能解决由于动态环境导致的优化问题无解的情况，注意不是静态环境而导致优化问题无解。但是，如果不能很快解决问题，第一种策略则更容易发生冲突。在这方面，第二种策略更安全，但存在完全停滞系统的风险。另外，由于机器人的最大和最小加速度限制，并非总是能够执行。第三种策略是一种混合策略，可保持第一种策略的动态性和第二种特性的安全性，因为MPC的预测功能可以预测不可行的情况，从而使机器人能够保持平滑的变化和可实现的速度变化。在这项工作中使用了后一种策略。

# 5 实验结果 #

在本章中，将介绍实现细节，控制器的调整参数以及仿真。 开发了两个解决方案，一个只有一个机器人的方案中的MPC控制器（用于评估其操作），一个基于在多系统代理的每个机器人中实例化的组合策略MPC-ORCA的控制器。 尽管控制器使用的模型是二阶模型，但是机器人会接收速度命令，因此，速度分量（线性和角度）和位置分量（在平面中）被视为输入（ x 0 , y 0 x\_0, y\_0 x0,y0）。

## 5.1 实施 ##

算法的实现是在ROS操作系统之上使用Python语言编写的。 ROS（Robot Operating System）是一个用于机器人软件开发的框架，具有一系列库，工具以及与仿真器（如Gazebo，Stage和V-Rep）接口。Gazebo被用于这项工作，因为它的模拟具有ODE物理引擎（Open Dynamics Engine），被广泛用于诸如DARPA机器人挑战赛和NASA太空机器人挑战赛之类的挑战中。使用Gazebo功能，以URDF描述语言（Uni ed Robot Description Format）设置了带有差速器轮的机器人，并且可以设置作用在车辆上的干扰力和扭矩，例如如图5.1所示。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 17]  
  可以将例如控制系统，模拟器等在ROS中定义为单独的节点。节点之间的通信可以使用称为话题（topic）的通信通道来执行。图5.2显示了其中一种仿真中使用的节点结构和话题。在仿真中，通过在每个代理的topic / cmd级别中发布的速度命令来控制机器人。可以看出，控制器之间没有直接的通信，但是可以执行环境信息的读取（通过topic /gazebo/mdeLstates）。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 18]  
  用于解决QP问题的求解器是OSQP，它运行ADMM算法。 在优化问题中，矩阵 P P P（公式（3.6））和 A A A（公式 4.15）呈现大量等于0的值，这可能占用大量存储空间。 为了解决这个问题，这些矩阵被分配到与OSQP兼容的稀疏数据的CSC结构中（Compressed Sparse Column），并且可以在科学计算机库SciPy中获得。

## 5.2 单个机器人的跟踪控制 ##

最初模拟旨在使用第2.3节中介绍的线性化模型研究差动机器人中MPC控制器的行为。 表5.1显示了控制器中使用的参数。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 19]  只要遵守机器人的速度限制， Q Q Q的选择只会惩罚位置误差，而不管速度变量如何。  Q ( 1 ) Q(1) Q(1)对于控制器而言具有较高的值，以此寻求一种解决方案来尝试在下一次交互中立即减小位置误差，但仍要考虑其他预测犬的误差。

R R R假定的值在加速度上施加较小的损失，目的是使速度表现得更平稳。 在采样时间 T s = 0.1 T\_s = 0.1 Ts=0.1秒的情况下，预测范围 N = 10 N = 10 N=10表示会进行1秒的预测，并且以这样的方式定义：该值的增加不会对结果有很大的改变。

### 5.2.1 场景1 ###

第一个模拟场景包括一条具有S型函数的路径，从 p s t a r t = \[ 0 0 \] T p\_\{start\} = \[0 \\quad 0\]^T pstart=\[00\]T到 p g o a l = \[ 7 7 \] t p\_\{goal\} = \[7 \\quad 7\]^t pgoal=\[77\]t处的峰值速度 t m a x = 10 t\_\{max\}= 10 tmax=10秒，斜率 K = 0.5 K = 0.5 K=0.5。 图5.3和5.4显示了第一个场景的结果，分别显示了所应用的输出和输入。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 20]  
  在图5.3中，重叠线表示机器人的位置变量遵循参考路径。 结合图5.4中所示的控制动作进行分析，可以观察到控制器的预测特性，因为即使在可变权重为零的情况下，控制器也可以通过平稳地调整机器人的速度来考虑参考信号的变化。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 21]  在大约15秒内，当机器人接近目标时，控制器将以一定速度减速，使机器人的位置遵循每次变化的路径。 在20秒时， r ( k ) ≅ r ( k + 1 ) ≅ . . . ≅ r ( k + N ) r(k)\\cong r(k + 1)\\cong ... \\cong r(k + N) r(k)≅r(k\+1)≅...≅r(k\+N)，此时控制器使机器人完全到达目标点，因此后面将无法预测变化。

### 5.2.2 场景2 ###

设计第二种模拟方案的目的是观察MPC控制器在系统遇到未建模的干扰时的反应，在这种情况下，在机器人或部分地面上施加了意外的力或扭矩、斜坡。 如图5.1所示，扰动的特征是力 F p = \[ − 300 − 300 0 \] T N F\_p = \[-300 \\quad-300 \\quad 0\]^T N Fp=\[−300−3000\]TN且扭矩 τ p = \[ 50 50 − 100 \] T N m \\tau p = \[50 \\quad 50 \\quad -100\]^T Nm τp=\[5050−100\]TNm无法使车辆翻倒，但足以移动机器人从其轨迹到相当远的距离。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 22]  图5.5和5.6显示了第二个场景的结果。使用图5.1所示的界面，在6到7秒之间瞬时施加了力和扭矩，从而导致轨迹偏离。但是，MPC能够根据性能限制将机器人保持在近似位置，并设法恢复其轨迹。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 23]

机器人的行为也可以在图5.7中看到，该图显示了机器人在其上移动的平面的俯视图。参考路径施加了一条可变速度的直线路径，其中标记点以线性间隔表示机器人或其参考的位置。可以在机器人位于点 \[ 1 1 \] T \[1 \\quad 1\]^T \[11\]T的区域中观察到干扰，从而导致其轨迹发生不必要的位移。控制器的反应使机器人有可能返回其路径，避免了由于成本函数的加速度变量的损失而导致的过分突然的运动。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 24]\#\# 5.3 具有多个机器人分布的系统的控制

设计了两种多智能体方案来验证组合策略MPC-ORCA的实施。 这次，最初将四个机器人放置在相对的位置并且彼此等距，参考路径有意引导所有智能体在大约 p = \[ 0 0 \] T p= \[0 \\quad 0\]^T p=\[00\]T处发生碰撞。

控制器使用的参数与表5.1中列出的参数相同，并增加了用于计算ORCA算法的碰撞锥体截断参数 τ = 5 s \\tau= 5s τ=5s。 表5.2总结了用于计算多智能体场景的轨迹的初始设置和最常用的设置。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 25]

### 5.3.1 场景3 ###

图5.8概述了第三次仿真过程中机器人的运动。 尽管参考路径是一条直线，将每个机器人都引导到其各自的最终位置，但是MPC-ORCA施加的限制能够尽早偏离以避免代理之间的冲突，主要是在 p = \[ 0 0 \] T p = \[0\\quad 0\]^T p=\[00\]T，所有机器人同时具有相同的期望位置变量。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 26]

如图5.9所示，机器人1的路径发生了偏差，从而预测了与其他代理的碰撞，其在与Y轴相关的位置变量中，该变量在参考路径中保持不变。 但是，也可以观察到与X轴相关的位置变量的偏差，因为图5.9左侧的图与图5.3左侧的图显示出很小的差异。

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 27]

### 5.3.2 场景4 ###

在第四种情况下，指定了不同的配置以显示所提出解决方案的一般性。 图5.10显示了在此模拟场景中机器人假定的轨迹的平面图，在该场景中也可以观察到无碰撞的轨迹。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 28]  对于机器人1的实验结果如图5.11所示，尽管应用了防撞限制，但机器人所采用的路径与参考轨迹存在很大差异。

## 5.4 讨论 ##

在调整控制器参数的过程中会感觉到一些特性，以及它们如何影响所获得的结果。 在方案1和2中，在每次迭代中允许的速度区域均由所用模型的限制和操作限制（最大速度 v m a x v\_\{max\} vmax和最小速度 v m i n v\_\{min\} vmin）定义。 在第3种情况和第4种情况下，由于ORCA算法在预测范围内的应用，优化问题对该变量的限制变得更加严格，从而导致问题变得不可行。

提前做出反应的可能性以及第4.3.2节中讨论的策略的应用，减少了QP问题无解情况的发生。但是，在ORCA算法导致无法解决的问题之后，非常狭窄的速度限制使控制器很难使机器人返回所需的轨迹，因为优化问题的解必须满足 v m a x v\_\{max\} vmax和 v m i n v\_\{min\} vmin的限制。在这种情况下，可以增加速度变量 \[ v m i n v m a x \] \[v\_\{min\} \\quad v\_\{max\}\] \[vminvmax\]的运行间隔。

还可以观察到，在执行多智能体仿真期间，在代价函数中加速（通过R矩阵）应用惩罚会导致更平稳的避免碰撞偏差，这在以后更容易纠正，无论是从从角度来看，与优化算法一样，系统输入的变化也较小。

在多主体场景的两种模拟中，由于车辆预先定位在无碰撞的配置中，因此它们能够执行快速运动，这表明MPC-ORCA可以被认为是适用于有移动障碍物的环境的技术。

# 6 结束语 #

这项工作提出了一种基于模型的预测控制（MPC）与ORCA防撞算法相结合的方法，该算法应用于具有单个目标点的移动机器人的分布式系统中，而无需进行周期性的路线重新规划或与其他智能体之间的直接通信。 对于预测范围的每次迭代，都会计算速度限制（例如速度限制和加速度），并将其插入到QP优化问题中，这此避免受控机器人与其他智能体之间的碰撞。

使用带差速器轮的机器人存在运动学上的限制，需要线性化模型才能实现已实现的控制器，但是，机器人能够建立路径平滑且无碰撞的方向，与参考轨迹的偏差很小。 使用此策略的机器人可以处理瞬息万变的环境，其他智能体也可以在该环境中高速移动。 与其他研究提出的解决方案相比，该解决方案的优势在于它使用了参考路径而不是目标位置，因为它可以使机器人根据描述的路径移动，而位置不随着时间的流逝而改变。

尽管此研究针对的是在平面上移动的机器人，但可以扩展所使用的技术来控制和防止在小空间中操作的机器人发生碰撞，例如飞机和机械臂等，但有必要调整系统模型并确定ORCA限制。 此外还可以通过施加更基本的例如扭矩命令或车轮加速来使得MPC-ORCA执行工作。

# 参考文献： #

1.  https://github.com/EASY-SPARC/TCC-Glauber/

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70]: /images/20221024/2ed8e493c06741b3aa5a6deb0a42fc2c.png
[20210223104525692.png]: /images/20221024/c8f3ba18c07e4880a28709a56471d348.png
[20210223110138922.png]: /images/20221024/6b88a1c2df9b45b492e12283f9237965.png
[20210223110935871.png]: /images/20221024/7ad66b327a5741588286ed13bd9416c1.png
[20210223111024122.png]: /images/20221024/5c98c08197e64b52a51bb0c0bf900f5b.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 1]: /images/20221024/0681320b811c4fcc96da615242fcb9e0.png
[20210223112303343.png]: /images/20221024/24b10df4b8324e39b928e2a8dc7a8d05.png
[20210223130441788.png]: /images/20221024/afa18f238d2c44dcb2f6b6b81da28e80.png
[20210223130506184.png]: /images/20221024/9faff0a6e8fd4c4e98c737a9b846e9b2.png
[20210223130732294.png]: /images/20221024/7b6c2aafe9c44494a608bed2dbc5aec9.png
[20210223130806861.png]: /images/20221024/9aab75ec4c6e4f8bbd29aa3d6d761493.png
[20210223130854605.png]: https://img-blog.csdnimg.cn/20210223130854605.png
[20210223130910266.png]: /images/20221024/6419c9f838c44c23a4a3d0eb7a59f361.png
[2021022313175712.png]: /images/20221024/d766ef674d524723be8480abb68a1ebc.png
[20210223133448678.png]: /images/20221024/87af3735b42340f9a437690e27e43f03.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 2]: /images/20221024/acf26c8cf11d4728a0dd779be49670f2.png
[20210223141109347.png]: /images/20221024/4de000ce0d054ae78130135cbb18ca22.png
[20210223141152281.png]: /images/20221024/928544b1a5b04083b6ccf76e2c397c54.png
[2021022314122480.png]: /images/20221024/f1eb58cf98fd4230ae4bfaa40995e3cb.png
[20210223142710488.png]: /images/20221024/a1e0f9e00a494c5ca0a217f26f8f0a5c.png
[20210223150605634.png]: /images/20221024/a2f93b13cc8d49668755eda4e71cfa27.png
[20210223151040252.png]: /images/20221024/e77d00799eab4ccab7c33bbfca0e6277.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 3]: /images/20221024/f087ff31e04a43f697b81370cee2d91f.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 4]: /images/20221024/81d09f24baab4e3ab9e5439dc0ac768b.png
[20210223162419478.png]: /images/20221024/cd3c0f9842914b888b487513f53eaa66.png
[20210223163624885.png]: /images/20221024/2c107a1bd29e4809a549333a648759dc.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 5]: /images/20221024/af37e919b0cd4ff6a4b4cea1b72dc019.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 6]: /images/20221024/6f23ce74e3da4f82b76dd93706ec513f.png
[2021022317065743.png]: /images/20221024/4a2d00a66b584b99a90c11ece97e83fe.png
[20210225133034476.png]: /images/20221024/2113ba359c9449f4926424a9e5373aff.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 7]: /images/20221024/0507be254db4480d8b8b9d145dc6645f.png
[20210225134446252.png]: /images/20221024/f23453c61ddf40339f73edd61e9c642b.png
[20210225135128512.png]: /images/20221024/05b99027ee6c47aa8a92dff3f4c625e6.png
[20210225140143534.png]: /images/20221024/524c291d9ca246bc95e5bf36fc03f57b.png
[20210225221505500.png]: /images/20221024/b9a15aecbde44e9ea4c53128c6cba8d2.png
[20210225221727119.png]: /images/20221024/09847081ed9c46d594a934b9903863e0.png
[20210225223118226.png]: /images/20221024/768322ee0fcc4b4490d94ac507700eaa.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 8]: /images/20221024/cd4ceee5f9e542ed8bf641de9daf344e.png
[20210301112408453.png]: /images/20221024/694ed830f09143f8ade617100dadd418.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 9]: /images/20221024/80e4aaccd47942f38727d21865332c32.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 10]: /images/20221024/5b3f6ea4e4ac41e889b4519363794314.png
[20210301120624365.png]: /images/20221024/e3c27507427d46588d2403af2b42182c.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 11]: /images/20221024/3e016b6a23974b7598df396888077c7b.png
[20210301120717927.png]: /images/20221024/5d300e0ed76f48b58659b858e8bc0110.png
[20210301122109600.png]: https://img-blog.csdnimg.cn/20210301122109600.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 12]: /images/20221024/fffd6e6159f542ea9a6c61ad127bbf82.png
[20210301123037809.png]: /images/20221024/f5e981df16214fe681b6c077951fee5b.png
[20210301123457186.png]: /images/20221024/bb821e4786354966b6a13f86277f36df.png
[20210301123949131.png]: /images/20221024/85102b6877754fbcbb25fe1d2391fe06.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 13]: /images/20221024/81cee15060ee4faba44bb193a86712ff.png
[20210301132548141.png]: https://img-blog.csdnimg.cn/20210301132548141.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 14]: /images/20221024/4021be43c93e4cbe8c2c360f3f2ba140.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 15]: https://img-blog.csdnimg.cn/202103011334192.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz,size_16,color_FFFFFF,t_70
[20210301135046171.png]: /images/20221024/d91475a878224726b59820ba152a0502.png
[2021030113545672.png]: /images/20221024/7c7b508fc48b42d99491c0f200c6cd35.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 16]: /images/20221024/32659d61796743d78c30da8de7a8da5e.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 17]: /images/20221024/9d979db6856a443284e70a42372cbd51.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 18]: /images/20221024/bab0ded994e94984a2e92f8c65d82c73.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 19]: /images/20221024/680f09e3c570414cad282f7d39384879.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 20]: /images/20221024/fba67ca70b334948b2320914ba0a6031.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 21]: /images/20221024/54d200e70b7c47a1ab3fd91ff45a5071.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 22]: /images/20221024/ef828bc9d7d142aba6efa94c868e064e.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 23]: /images/20221024/d33344818f504543a145dff2a209b3a9.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 24]: /images/20221024/02bd7a0a1f8e409b993d09cc3c43bbbe.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 25]: /images/20221024/600cb587c28944289f8a34501dad84ee.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 26]: /images/20221024/e2f97b61dbca4b6aad587b43336541b3.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 27]: /images/20221024/e708f4d5e3fa4486acdf2ea335ab6022.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 28]: /images/20221024/33f7b48fa36b4d27845651d27fd4eb0b.png

MPC_ORCA

发表评论取消回复

还没有评论，来说两句吧...

相关阅读