Robust Sampling-based Motion Planning for Autonomous Tracked Vehicles in Deformable High Slip Terrai

preface

首先要点一下motion planning algorithm的作用：
The motion planning algorithm should find the optimal and probabilistically feasible trajectory from the initial state $z_{init}$ to the goal region $X_{goal}$ satisfying the system equation.

• probabilistically feasible trajectory
  This means that, given the obstacle state $X_{obs}$, and the collision free state defined as $X_{free}=X$ \ $X_{obs}$,
  every point in the trajectory given as $\tau :[0,1]\to X_{free}$ must always have smaller probability of collision with obstacles than some threshold value $\delta$, that is $P(\tau (s)\in X_{obs})<\delta ,\forall t,(0\le s\le 1)$
• optimal path
  This means that, the trajectory must be the optimal one with respect to the specified cost function

introduction

这篇文章主要做了啥：
To the author’s knowledge, this paper is the first to demonstrate a practical methodology for robust optimal motion planning of autonomous vehicles operating in rough outdoor scenarios using several existing prior works.

For motion planning, a robust incremental sampling based motion planning algorithm (CC-RRT*) is combined with the LQG-MP algorithm.

• CC-RRT* yields the optimal and probabilistically feasible trajectory by using a chance constrained approach under the RRT* framework.
• LQG-MP provides the capability of considering the role of compensator in the motion planning phase and bounds the degree of uncertainty to appropriate size.

When the two algorithms are combined, the degree of uncertainty in vehicle’s state is bounded, thus offering a more realistic and less conservative planning strategy.

为了增加plan出的trajectory的robustness, use a chance constrained approach and calculating the chance of collision by considering the vehicle state as a random variable rather than a deterministic variable.

1. how to model the influence of slip phenomenon on the vehicle state prior to the planning phase?
2. 如何计算chance of collision?
3. 如何把collision chance限制在一定范围内?

problem statement

vehicle model:
$$\dot{z}=f(z,u)+w$$
where
$z\in X$ is the state vector, $X\subset R^n$ is the state space
$u\in U$ is the input vector, $U\subset R^m$ is the input space
$w$ is the process noise into the system, here, it is assumed to be depend mainly on the slip

The problem can be formulated as follows:
$$\underset{u(t)}{\min }J(z,u)$$ subject to$$\tau (0)=z_{init},\tau (1)=X_{goal}$$ $$\dot{z}=f(z,u)+w_k,differentkforeachterraintype$$ $$P(\tau (s)\in X_{obs})<\delta ,\forall t,(0\le s\le 1)$$