CoTiMo represents Collision-Free Smooth Path Generation, Time-Optimal Path Parameterization, and Model Predictive Control Planner. It is a comprehensive solution for robotic motion planning.
This report provides a detailed explanation of the planner's core algorithmic implementations, covering key modules such as path generation, time parameterization, and model predictive control.
Collision-Free Smooth Path Generation
Cubic Spline
For a group of certain points \((x_{0,1},x_{0,2},\cdots,x_{0,m}),(x_{1,1},x_{1,2},\cdots,x_{1,m}), (x_{2,1},x_{2,2},\cdots,x_{2,m}), \cdots, (x_{n,1},x_{n,2},\cdots,x_{n,m})\), we want to find a smooth enough curve that passes all these points to achieve the interpolation goal. To do so, we can use piecewise cubic functions to fitting the points.
Let's talk about a specific dimention \(k\) first. To simplify the denotation, we let \(x_i=x_{i,k}\). For a set of \(n+1\) points \((x_{0,k},x_{1,k},x_{2,k},\cdots,x_{n,k})\) and for the \(i\)-th piece of the spline, we can represent it by
We want to avoid our robot get crashed into the obstacle. So that we also need to define a potential function.
First, we need to calculate the distance between the robot and the obstacle. Which is find a point \(o\) in obstacle (\(Ao\leq b\)), to minimize the distance from robot \(x\) to point \(o\)
Additional to \(q(s)\), \(\frac{{\rm d}q}{{\rm d}s}\) and \(\frac{{\rm d}^2q}{{\rm d}s^2}\) are also known. Because the cubic spline is second order differentiable.
Try to obtain the discretized form \(a\in{\mathbb R}^K,s,b\in{\mathbb R}^{K+1}\). \(K\) is different from the number of curves or control points \(N\) and is the result of resampling the curve. Usually \(K\) is greater than \(N\).
To rewrite the problem to a second-order conic programming form, we first try to bound nonlinear term \(\sqrt{b^k}\) with \(c^k\), such that \(\sqrt{b^k}\ge c^k,k\in[1,K+1]\). Going a step further, we introduce one more slack variable \(d^k\), which satisfies \(\frac1{c^{k+1}+c^k}\le d^k,k\in[1,K]\).
\(\gamma\) is the growth rate of \(\rho\) and \(\beta\) is the upper bound of \(\rho\), which is typically 1e3
Model Predictive Control
Swerve Kinematic Model
A simple omnidirectional platform is mecanum wheel chassis. But for a FRC player, a swerve chassis is what we need.
Consider only the translation case. Let's simply denote \((x,y)\) for position, \(v\) for velocity, \(a\) for acceleration, \(\theta\) for angle, \(\omega\) for angular velocity, \(\alpha\) for angular acceleration.
\[
\begin{bmatrix}
\dot x \\
\dot y \\
\dot v \\
\dot \theta \\
\dot \omega \\
\end{bmatrix} = \begin{bmatrix}
v\cos\theta \\
v\sin\theta \\
a \\
\omega \\
\alpha \\
\end{bmatrix}
\]
So that, we can use the following equation to update the states in discrete time domian.
\[
\begin{cases}
v_{k+1} = v_k +a\cdot\Delta t \\
\omega_{k+1} = \omega_k + \alpha\cdot\Delta t \\
\theta_{k+1} = \theta_k + \frac12(\omega_k+\omega_{k+1})\cdot\Delta t \\
x_{k+1} = x_k+\frac12(v_k\cos\theta_k+v_{k+1}\cos\theta_{k+1})\cdot\Delta t \\
y_{k+1} = y_k+\frac12(v_k\sin\theta_k+v_{k+1}\sin\theta_{k+1})\cdot\Delta t \\
\end{cases},\ \ u_k=\begin{bmatrix}a\\\alpha\end{bmatrix}
\]
Objective Function
Denote the number of control points of optimal control is \(M\), and \(\bar u_k=\begin{bmatrix}u_{k}^T&u_{k+1}^T&\cdots u_{k+M-1}^T\end{bmatrix}^T\in\mathbb R^{2M}\)
The method to solve this objective function is similar to the PHR ALM method mentioned above, except that the terms related to the symmetric cone are removed.
