CoTiMo Planner

Collision-free Smooth Path Generation
Time Optimal Path Parameterization
Model Predictive Control

@ZhangzrJerry

Path Planning

Swipe down to see more

Heuristic Path

Original A* path

Improved A* path

Obstacle Representation

Polytope represented by linear inequality:

\[ \begin{aligned} Ax \le b \end{aligned} \]

With a finite set of polytopes,
we can represent the feasible environment.

Distance to Obstacle

Distance to obstacle can be solved by Quadratic Programming

\[ \begin{aligned} \min_{x} & ||x-p||^2 \\ \text{s.t.} & Ax\le b \end{aligned} \]

\(x\) is the point on the obstacle boundary,
\(p\) is the point to be evaluated.

Keep away from Obstacles

Equivalent to maximize the distance to obstacles

\[ \min_{x}\sum\exp(-d(x_i)) \quad \text{s.t.} \ d(x_i) = \min(||x_i-p_j||_2),\forall j \]

Trajectory Planning

Swipe down to see more

Time Optimal Equation

\[ T=\int_0^T1{\rm d}t = \int_0^L\frac1{\frac{{\rm d}s}{{\rm d}t}}{\rm d}s \]

Constraints on voltage and current:

\[ \begin{aligned} -V_{x,\max} & \le V_x \le V_{x,\max} \\ -I_{x,\max} & \le I_x \le I_{x,\max} \\ \end{aligned} \]

DC Motor Model

Elevator model: \[ V_E = K_g + K_S \cdot \frac{\dot d}{|\dot d|} + \frac{K_v}{I_e} \cdot \dot d + \frac{K_a}{I_e} \cdot \ddot d \] Arm model: \[ V_A = K_g \cdot \cos(\theta) + K_S \cdot \frac{\dot{\theta}}{|\dot{\theta}|} + \frac{K_v}{I_a} \cdot \dot{\theta} + \frac{K_a}{I_a} \cdot \ddot{\theta} \]

Discrete Kinematics

Based on the DC motor model,
voltage and current constraints are represented by:

discrete accelerations and discrete velocities

Second-Order Cone Programming

Rewriting the time optimal trajectory planning equations,
we can formulate it as a second-order cone programming problem: \[ \begin{aligned} \min_x\ & c^Tx \\ \text {s.t.}\ & A_ix+b_i\in \mathcal K_i \\ & x^TJx-r^Tx = 0 \\ & Gx=h \\ & Px\le q \\ \end{aligned} \]

Augmented Lagrangian Method

\[ \begin{aligned} & {\mathcal L}_\rho(x,\mu,\nu,\lambda,\eta) \\ =& \frac\rho2\sum_{i=1}^m ||P_{\mathcal K_i}(\frac{\mu_i}\rho-A_ix-b_i)||^2 + \\ & \frac\rho2\sum_{j=1}^q ||x^TJ_jx-r_j^Tx+\frac{\nu_j}\rho||^2 + \\ & \frac\rho2||\max[Px-q+\frac\eta\rho,0]||^2 + \\ & \frac\rho2||Gx-h+\frac\lambda\rho||^2 + c^Tx \end{aligned} \]

L-BFGS Optimization

\[ \begin{aligned} x &\leftarrow \arg\min_x {\mathcal L}_\rho(x,\mu,\nu,\lambda,\eta) \\ \mu_i &\leftarrow P_{\mathcal{K}_i}[\mu_i-\rho(A_ix+b_i)] \\ \nu_j &\leftarrow \nu_j + \rho[x^TJ_jx-r_j^Tx] \\ \lambda &\leftarrow \lambda + \rho[Gx-h] \\ \eta &\leftarrow \rho[\max[\eta +\rho(Px-q),0]] \\ \rho &\leftarrow \min[(1+\gamma)\rho, \beta] \end{aligned} \]

Implementation Results

Desktop application showing the planner in action

Conclusion

Developed collision-free path planning with continuous potential fields
Implemented time-optimal trajectory planning with physical constraints
Solved optimization problem with Augmented Lagrangian + L-BFGS

Thank You

@ZhangzrJerry