CoTiMo
Updating...
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
\[
q_i(s)=a_is^3+b_is^2+c_is+d_i, \ \ \ \ s\in[0,1],\ i=0,1,\cdots,n-1
\]
To ensure the smooth of the curve, we write down the constraints,
\[
\begin{cases}
q_{i-1}(1)=q_i(0)=x_{i} \\
q_{i-1}'(1)=q_i'(0) \\
q_{i-1}''(1)=q_i''(0)
\end{cases}
\]
Apply \(a_i,b_i,c_i,d_i\) to the constraints, we get
\[
\begin{cases}
a_{i-1}+b_{i-1}+c_{i-1}+d_{i-1}=d_i \\
3a_{i-1}+2b_{i-1}+c_{i-1}=c_i \\
6a_{i-1}+2b_{i-1}=2b_i \\
\end{cases}
\]
So we can get the following expressions
\[
\begin{cases}
a_i=x_{i+2}-2x_{i+1}+x_{i} \\
b_i=-x_{i+2}+2x_{i+1,k}-x_{i} \\
c_{i}=x_{i+1}-x_{i} \\
d_i=x_{i}
\end{cases}\ ,\ i=1,2,\cdots,n-1
\]
In open loop case, assume stationary boundary \(q_0'(0)=0,q_{n-1}'(1)=0\), which is \(c_0=0, 3a_{n-1}+2b_{n-1}+c_{n-1}=0\).
You can also regard \(x_{n+2}\) as \(x_{n+1}\), so that \(x_{n+2}-2x_{n+1}+x_n=-x_{n+1}+x_n\)
Convert the expressions to vector form, we have
\[
\left[\begin{matrix}
a_0\\a_1\\a_2\\a_3\\\vdots\\a_{n-2}\\a_{n-1}\\
\end{matrix}\right]=
\left[\begin{matrix}
2&-3&1&0&\cdots&0&0&0\\
0&1&-2&1&\cdots&0&0&0\\
0&0&1&-2&\cdots&0&0&0\\
0&0&0&1&\cdots&0&0&0\\
\vdots&\vdots&\vdots&\vdots&\ddots&\vdots&\vdots&\vdots\\
0&0&0&0&\cdots&1&-2&1\\
0&0&0&0&\cdots&0&1&-1\\
\end{matrix}\right]
\left[\begin{matrix}
x_0\\x_1\\x_2\\x_3\\\vdots\\x_{n-2}\\x_{n-1}\\x_{n}
\end{matrix}\right]
\]
\[
\left[\begin{matrix}
b_0\\b_1\\b_2\\b_3\\\vdots\\b_{n-2}\\b_{n-1}\\
\end{matrix}\right]=
\left[\begin{matrix}
-3&4&-1&0&\cdots&0&0&0\\
0&-1&2&-1&\cdots&0&0&0\\
0&0&-1&2&\cdots&0&0&0\\
0&0&0&-1&\cdots&0&0&0\\
\vdots&\vdots&\vdots&\vdots&\ddots&\vdots&\vdots&\vdots\\
0&0&0&0&\cdots&-1&2&-1\\
0&0&0&0&\cdots&0&-1&1\\
\end{matrix}\right]
\left[\begin{matrix}
x_0\\x_1\\x_2\\x_3\\\vdots\\x_{n-2}\\x_{n-1}\\x_{n}
\end{matrix}\right]
\]
\[
\left[\begin{matrix}
c_0\\c_1\\c_2\\c_3\\\vdots\\c_{n-2}\\c_{n-1}\\
\end{matrix}\right]=
\left[\begin{matrix}
0&0&0&0&\cdots&0&0&0\\
0&-1&1&0&\cdots&0&0&0\\
0&0&-1&1&\cdots&0&0&0\\
0&0&0&-1&\cdots&0&0&0\\
\vdots&\vdots&\vdots&\vdots&\ddots&\vdots&\vdots&\vdots\\
0&0&0&0&\cdots&-1&1&0\\
0&0&0&0&\cdots&0&-1&1\\
\end{matrix}\right]
\left[\begin{matrix}
x_0\\x_1\\x_2\\x_3\\\vdots\\x_{n-2}\\x_{n-1}\\x_{n}
\end{matrix}\right]
\]
\[
\left[\begin{matrix}
d_0\\d_1\\d_2\\d_3\\\vdots\\d_{n-2}\\d_{n-1}\\
\end{matrix}\right]=
\left[\begin{matrix}
1&0&0&0&\cdots&0&0&0\\
0&1&0&0&\cdots&0&0&0\\
0&0&1&0&\cdots&0&0&0\\
0&0&0&1&\cdots&0&0&0\\
\vdots&\vdots&\vdots&\vdots&\ddots&\vdots&\vdots&\vdots\\
0&0&0&0&\cdots&1&0&0\\
0&0&0&0&\cdots&0&1&0\\
\end{matrix}\right]
\left[\begin{matrix}
x_0\\x_1\\x_2\\x_3\\\vdots\\x_{n-2}\\x_{n-1}\\x_{n}
\end{matrix}\right]
\]
For every dimension in \(x\), we would obtain four vector \(a, b, c, d\)
Minimize Stretch Energy
Then, we can define energy function by acceleration
\[
Energy(x_0,x_1,\cdots,x_n)=\sum_{i=1}^{n-1}\int_0^1||q_i''(s)||{\rm d}s
\]
But in practice, I use the following formula
\[
Energy(x_0,x_1,\cdots,x_n)=\sum_{i=1}^{n-1}||x_{i-1}-2x_i+x_{i+1}||^2
\]
Collision Free
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\)
\[\begin{aligned}
\min\ & ||o-x||^2 \\
\text{s.t.}\ &Ao\leq b
\end{aligned}\]
\[\begin{aligned}
\min\ & o^To-2x^To+x^Tx \\
\text{s.t.}\ &Ao\leq \text{b}
\end{aligned}\]
By SOCP, we can obtain the distance vector
\[
d(x)=\left[\begin{matrix}
d(x,o_1),d(x,o_2),\cdots,d(x,o_m)
\end{matrix}\right]^T
\]
And the potential function would be defined as below
\[
Potential(x_0,x_1,\cdots,x_n) = \sum_{i=0}^{n}\exp(||d(x_i)||_\infty)
\]
Loss Function
\[
Loss(x_0,x_1,\cdots,x_n)=\eta\cdot Energy(x_0,x_1,\cdots,x_n)+(1-\eta)\cdot Potential(x_0,x_1,\cdots,x_n)
\]
Finally, we convert the problem to an optimization problem, we can use Line Search, Quasi Newton, LBFGS, or Newton-CG to solve it.
\[
\min_{x_0,x_1,\cdots,x_n}Loss(x_0,x_1,\cdots,x_n)
\]
In practice, I add some trick to make the optimizer more stable. Additional to five different \(\eta\), I introduce sigmoid function in update step.
\[
\sigma(x)=\frac1{1+e^{-x}}=\frac{e^x}{1+e^x}=1-\sigma(-x)
\]
\[
x^{k+1}=x^{k}-\eta_k\left[2\sigma(-\nabla Loss(x))-1\right]
\]
Time Optimal Path Parameterization
After optimizing the control points \(x_0, x_1, \cdots, x_n\), we get a spline \(q\) uniquely determined by arc-length \(s\).
Then the optimal time \(T\) is integrated by
\[
T = \int_0^T 1\ {\rm d}t = \int_0^L\frac1{\frac{{\rm d}s}{{\rm d}t}}{\rm d}s
\]
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.
We also have velocity constraint,
\[
-v_{max}\leq v\leq v_{max}
\]
acceleration constraint,
\[
-a_{max}\leq a\leq a_{max}
\]
and always move forward
\[
\frac{{\rm d}s}{{\rm d}t}>0
\]
The true velocity is
\[
\frac{{\rm d}q}{{\rm d}t} = \frac{{\rm d}q}{{\rm d}s}\cdot\frac{{\rm d}s}{{\rm d}t}
\]
The true acceleration is
\[
\frac{{\rm d}^2q}{{\rm d}t^2}=\frac{{\rm d}^2q}{{\rm d}s^2}\cdot\frac{{\rm d}s}{{\rm d}t}+\frac{{\rm d}q}{{\rm d}s}\cdot\frac{{\rm d}^2s}{{\rm d}t^2}
\]
Denote
\[
a(s) = \frac{{\rm d}^2s}{{\rm d}t^2},\ b(s)=\bigg(\frac{{\rm d}s}{{\rm d}t}\bigg)^2
\]
Then the problem is described by
\[
\begin{aligned}
\min_{a(s),b(s)}\ & \int_0^L\frac1{\sqrt{b(s)}}{\rm d}s \\
\text{s.t.}\ & b'(s)=2a(s) \\
& b(s) > 0\ \ \ (\geq) \\
& ||
\end{aligned}
\]
