Robust Constrained Learning-based NMPC enabling reliable mobile robot path tracking

Introduction

这篇文章的目的就是achieving robust constrained, high performance path-tracking in spite of unknown disturbances.

这篇文章的思路：simple process model, high model uncertainty ${\to }^{learn}$accurate, low-uncertainty model

这篇文章用VO做Localization.

这篇文章和传统contrained NMPC有如下两方面的不同：

• 传统方法中process model是预先设计好并且不变的，在这篇文章中learn到disturbance model来加强process model，使得process model可以predict the mean and uncertainty of effects.
• 传统contrained NMPC没有考虑模型的不确定性，这篇文章apply robust constraints in real time considering the learned uncertainty. We provide robust constraint satisfaction when uncertainty is high and increased performance as uncertainty is reduced through learning.

这篇文章的主要创新点就是：

1. use learned models
2. account for model uncertainty

上面就是本文整体控制框图。RC-LB-NMPC主要包含两个主要的部分：

• the robust constrained, path-tracking NMPC algorithm based on an a priori process
• the GP-based disturbance model

Mathematical Formulation

先大概介绍一下NMPC吧：
At a given sample time, NMPC finds a sequence of control inputs that optimizes the plant behavior over a prediction horizon based on current state. The first input in the optimal sequence is then applied to the system. The entire process is repeated at the next sample time for the new system state.

Robust Constrained NMPC

• 首先肯定是要讲一下状态转移model

The true system is approximate by the sum of an a priori model and an experienced-based, learned model:
$$x_{k+1}=f(x_k,u_k)+g(a_k)$$
where:
$f(\cdot )$——a known nonlinear process model representing our knowledge of $f_{true}(\cdot )$
$g(\cdot )$—— an (initially unknown) disturbance model representing discrepancies between the a priori model and the actual system behavior. $g(\cdot )$ is modeled as GP. For simplicity, $a_k=(\overline{x_k},u_k)$

• 再来讲一下cost function

定义the cost function to be minimized over the next $K$ time-steps as:
$$J(\bar{x},u)=(x_d-\bar{x}{)}^{T}Q(x_d-\bar{x})+(u_d-u{)}^{T}R(u_d-u)$$
其中：
$Q$是半正定矩阵，$R$是正定矩阵
$x_d=(x_{d,k+1},...,x_{d,k+K})$——a sequence of desired states
$x=(x_{k+1},...,x_{k+K})$——a sequence of uncertain predicted states, $\bar{x}$ is the sequence of mean values based on $x$
$u_d=(u_{d,k},...,u_{d,k+K-1})$——a sequence of desired inputs
$u=(u_k,...,u_{k+K-1})$——a sequence of inputs

• 接下来就是要定义robust constraint了
  从state和input两个角度定义

基于以上基础，我们就可以formulate the following constrained optimization problem:

$$x_{opt},u_{opt}=\underset{x,u}{\mathrm{argmin}}J(\bar{x},u)$$ $$subjuctto{\bar{x}}_{k+i+1}=f({\bar{x}}_{k+i},u_{k+i})+g(a_{k+i}),i=0,...,K-1$$ $$c_i(\bar{x},u)>0$$

整个算法的流程：

在算法收敛之后，we apply the first element of the resulting optimal control input sequence for one time-step, and start all over at the next time-step.

Predicting uncertain trajectories

state都是正态分布的，所以使用Sigma-Point Transform来iteratively predict state sequences.

定义state$z_i=({\bar{x}}_{k+i},\mu (a_{k+i}))\in R^{2n}$ representing the mean state and disturbance at time $k+i$ with uncertainty $P_i=diag({\sum }_{k+i},{\sum }_{gp}(a_{k+i}))$

这个过程循环K次就可以生成完整的$x$序列。在这种方式下，$3\sigma$置信区间accouts for uncertainty arising from both localization and modeling

Gaussian Process Disturbance Model

The learned model depends on disturbance observations collection during previous trials.