0. 微分平坦

定义

对于非线性系统的一般形式如下：
$$\dot x = f(x,u,t)$$
如果系统这个系统通过化简或者变换后能够找到一组变量$y$，使得系统的状态量$x$和控制量$u$可以由$y$及$y$的多阶导数表示，则系统是微分平坦的。即：
$$
\begin{matrix}
x = t_1(y,y^{‘},…,y^{n})\\
u = t_2(y,y^{’},…,y^{n})
\end{matrix}
$$

例子

例如一个常见的二自由度自行车模型，它的运动学模型和微分平坦模型的一般形式如下：
$$
\begin{cases}
\dot x = vcos\theta \\
\dot y = vsin\theta \\
\dot \theta = \frac{vtan(\delta)}{L}\\
\dot v = a_t \\
k = \frac{tan(\delta)}{L} \\
a_n = v^2\cdot k
\end{cases}
$$

  • 其中$(x,y,\theta)$是状态量，代表车体的位置和航向角
  • $(a_t,\delta)$是控制量，代表车的纵向加速度和前轮转角
  • $(k,a_n)$是曲率和向心加速度

它的微分平坦模型如下：
$$
\begin{cases}
{l}v=\eta\sqrt{\dot{x}^{2}+\dot{y}^{2}} \\
\psi=arctan2(\eta\dot{y},\eta\dot{x}) \\
a_{t}=\eta\frac{(\dot{x}\dot{x}+\dot{y}\dot{y})}{\sqrt{\dot{x}^{2}+\dot{y}^{2}}} \\
\theta=arctan\mathrm{~}\eta L\frac{\dot{x}\ddot{y}-\dot{y}\ddot{x}}{(\dot{x}^{2}+\dot{y}^{2})^{\frac{3}{2}}} \\
k=\eta\frac{\dot{x}\dot{y}-\dot{y}\ddot{x}}{(\dot{x}^{2}+y^{2})^{\frac{3}{2}}} \\
a_{n}=\eta\frac{\dot{x}\dot{y}-\dot{y}\ddot{x}}{\sqrt{\dot{x}^{2}+\dot{y}^{2}}} \\
\dot{\theta}=\eta L\frac{(\dot{x}\ddot{y}-\dot{y}\ddot{x})(\dot{x}^{2}+\dot{y}^{2})^{\frac{3}{2}}-3(\dot{x}\ddot{y}-\dot{y}\ddot{x})(\dot{x}^{2}+\dot{y}^{2})^{\frac{1}{2}}(\dot{x}\ddot{x}+\dot{y}\dot{y})}{(\dot{x}^{2}+\dot{y}^{2})^{3}+L^{2}(\dot{x}\ddot{y}-\dot{y}\dot{x})^{2}}
\end{cases}$$

  • 其中$\eta \in {-1,1}$，-1表示车辆倒车运动，1表示前进行驶。

曲率的定义
自行车微分平坦模型

1. BVP（Boundary Value Problem）

定义：最优控制中的边值问题（BVP）是指在给定初始和终止条件下，求解最优控制问题的控制策略和状态轨迹的一种方法。

BVP： 一个两点（起点、终点）状态已知，求中间最优序列的问题。

1.1 最优性条件

$$
\begin{aligned}
&min\int_0^T{u}(t) ^{\mathrm{T}}\bm{W}u(t) dt\\
s.t. \ \ \ &z^s(t) =u(t) ,\forall t\in \left[ 0,T \right] \\
&z^{\left[ s-1 \right]}(0) =\bar{z}_s,z^{\left[ s-1 \right]}(T) =\bar{z}_e
\end{aligned}
$$

对于以上最小控制量$u(t)$的BVP问题，若系统是微分平坦的，且$u(t)$是平坦变量的 $s$ 阶次导数，则BVP问题的最优解是一个 $2s-1$ 次多项式。

1.2 BVP

对于一个 $minJerk$ 问题，它的最优解是一个5次多项式，通过起点、终点状态只有唯一解。
$$\beta (t) = \mathbf{\lambda}_t^T\cdot\mathbf{c} = c_0+c_1t+c_2t^2+c_3t^3+c_4t^4+c_5t^5=[1\ t\ \ t^2\ t^3\ t^4\ t^5]\cdot[c_0\ c_1\ c_2\ c_3\ c_4\ c_5]^T$$

$$\begin{bmatrix}
\beta(0)\\
\beta^{‘}(0)\\
\beta^{’‘}(0)\\
\beta(T)\\
\beta^{’}(T)\\
\beta^{‘’}(T)
\end{bmatrix}=
\begin{bmatrix}
\lambda_0^T\\
\lambda_0^{(1)T}\\
\lambda_0^{(2)T}\\
\lambda_T^T\\
\lambda_T^{(1)T}\\
\lambda_T^{(2)T}
\end{bmatrix}\cdot \mathbf{c}=
\begin{bmatrix}
1 &0 &0 &0 &0 &0\\
0 &1 &0 &0 &0 &0\\
0 &0 &2 &0 &0 &0\\
1 &T &T^2 &T^3 &T^4 &T^5\\
0 &1 &2T &3T^2 &4T^3 &5T^4\\
0 &0 &2 &6T &12T^2 &20T^3
\end{bmatrix}\cdot \mathbf{c}=\begin{bmatrix}
x_0\\
v_0\\
a_0\\
x_T\\
v_T\\
a_T\\
\end{bmatrix} \Longrightarrow A(0,T)\mathbf{c}=\mathbf{b}(0,T)$$
$$\mathbf c = A^{-1}\cdot \mathbf{b}(0,T)$$

2. BIVP（Boundary Intermediate Value Problem）

BIVP： 一个两点（起点、终点）状态已知，中间 waypoints 已知，求中间最优轨线的问题。

2.1 最优性条件

$$ min\int_{t_0}^{t_M}v(t)^TWv(t)dt $$
$$ s.t. \ \
\begin{aligned}
z^{s}(t)=v(t), \forall t \in [t_0,t_M], \\
z^{[s-1]}(t_0)=\bar z_{o},z^{[s-1]}(t_M)=\bar z_{f},\\
z^{[d_i-1]}(t_i)=\bar z_{i},\ 1 \le i \le M,\\
t_{i-1} \le t_i, \ 1 \le i \le M.
\end{aligned}
$$

对于以上 $minmum \ control$ 问题，若系统是微分平坦的，且最小控制量是平坦变量的 $s$ 阶次导数，则 $BIVP$问题的最优解的每一段都是一个 $2s-1$ 次多项式,且具有大于 $s$ 阶的连续性。$minJerk$ 具有 $snap(4)$ 阶的连续性，$minSnap$具有 $pop(6)$ 阶的连续性

2.2 BIVP

对于 $minJerk$ 的BIVP问题。假设每段轨迹的时间 $T_i$ 已知，所有的waypoints状态已知，则waypoints和连续性约束可以表示为：

$$\beta (t) = \mathbf{\lambda}_t^T\cdot\mathbf{c} = c_0+c_1t+c_2t^2+c_3t^3+c_4t^4+c_5t^5=[1\ t\ \ t^2\ t^3\ t^4\ t^5]\cdot[c_0\ c_1\ c_2\ c_3\ c_4\ c_5]^T$$

$$
\begin{bmatrix}
\beta(t)\\
\beta^{‘}(t)\\
\beta^{’‘}(t)\\
\beta^{’‘’}(t)\\
\beta^{‘’‘’}(t)
\end{bmatrix}=
\begin{bmatrix}
\lambda_t^T\\
\lambda_t^{(1)T}\\
\lambda_t^{(2)T}\\
\lambda_t^{(3)T}\\
\lambda_t^{(4)T}
\end{bmatrix}\cdot \mathbf{c}=
\begin{bmatrix}
1 &t &t^2 &t^3 &t^4 &t^5\\
0 &1 &2t &3t^2 &4t^3 &5t^4\\
0 &0 &2 &6t &12t^2 &20t^3\\
0 &0 &0 &6 &24t &60t^2\\
0 &0 &0 &0 &24 &120t
\end{bmatrix}\cdot \mathbf{c} = \begin{bmatrix}
x(t)\\
v(t)\\
a(t)\\
jerk(t)\\
snap(t)\\
\end{bmatrix}\Longrightarrow G(t)\mathbf{c}=\mathbf{b}(t)$$

$$
\begin{bmatrix}
\beta(T_i) &0\\
G(T_i) &-G(0)
\end{bmatrix}\cdot \begin{bmatrix}
\mathbf{c}(i)\\
\mathbf{c}(i+1)\\
\end{bmatrix}=
\begin{bmatrix}
\mathbf{b}(T_i)\\
0
\end{bmatrix}
\Longrightarrow
\begin{bmatrix}
E_i,F_i
\end{bmatrix}\cdot \begin{bmatrix}
\mathbf{c}(i)\\
\mathbf{c}(i+1)\\
\end{bmatrix}=
\begin{bmatrix}
\mathbf{b}(i)\\
0
\end{bmatrix}
$$
定理：设共有 $M$ 段轨迹 ${1,…,M}$，特别定义 $F_0=A(0)$ 和 $E_M=A(T_M)$ , $b_a,b_M$ 分别为起点、终点状态。BIVP最优解由以下线性方程组唯一确定。
$$
M_{2Ms*2Ms}\mathbf{c}=b \Longrightarrow \left(\begin{array}{ccccc}
\mathbf{F}{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\
\mathbf{E}{1} & \mathbf{F}{1} & \mathbf{0} & \cdots & \mathbf{0} \\
\mathbf{0} & \mathbf{E}{2} & \mathbf{F}{2} & \cdots & \mathbf{0} \\
\vdots & \vdots & \vdots & \ddots & \vdots \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{F}{M-1} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{E}_{M}
\end{array}\right) \cdot \begin{bmatrix}
\mathbf{c}_1\\
\mathbf{c}_2\\
\mathbf{c}3\\
\vdots\\
\mathbf{c}{M-1}\\
\mathbf{c}_M\\
\end{bmatrix}=
\begin{bmatrix}
b_0\\
b(1)\\
0\\
\vdots\\
0\\
b_M\\
\end{bmatrix}
$$
拓展：对于三维或者多维平坦变量的参数化，满足 $M\cdot[\mathbf{c}_x,\mathbf{c}_y,\mathbf{c}_z]=[\mathbf{b}_x,\mathbf{b}_y,\mathbf{b}_z]$

3. 时空形变轨迹规划

3.1 概念

考虑一个更general的轨迹优化问题：
$$
\begin{aligned}
\min {z(t),{T}} \int{0}^{T} v(t)^{\mathrm{T}} \mathbf{W} v(t) \mathrm{d} t+\rho(T) \\
\text { s. t. } z^{(s)}(t)=\bar{v}(t), \forall t \in[0, T] \text {, } \\
\mathcal{G}\left(z(t), \cdots, z^{(s)}(t)\right) \preceq \mathbf{0}, \forall t \in[0, T] \text {, } \\
z(t) \in\mathcal{F}, \forall t \in[0, T] \text {, } \\
z^{[s-1]}(0)=\bar{z}{o}, z^{[s-1]}(T)=\bar{z}{f} \text {, } \\
z^{[s-1]}:=\left(z^{\mathrm{T}}, \dot{z}^{\mathrm{T}}, \cdots, z^{(s-1)^{\mathrm{T}}}\right)^{\mathrm{T}} . \\
\end{aligned}
$$

几个问题：

  • 最优解的参数化形式是未知的。
  • space-time 增大了问题维度
  • 动态可行性约束是非凸的
  • 安全区域是高复杂度的

可能的解决方法：

  • 仍然使用参数化样条，虽然它可能不是最优的
  • 仅仅优化空间，把时间分配固定下来
  • 简化动力学，只考虑速度、加速度，不考虑角速度等底层的约束
  • 尽量简化安全性约束，使得它是局部凸的

3.2 轨迹参数化

每段轨迹的 $T_i$ 未知，waypoints未知或部分已知，例如只知道 $(x,y)$ 。
$$
\begin{aligned}
\min {z(t), T} & \int{0}^{T} v(t)^{\mathrm{T}} \mathbf{W} v(t) \mathrm{d} t+\rho(T), \\
\text { s.t. } & v(t)=z^{(s)}(t), \forall t \in[0, T], \\
& \mathcal{G}\left(z(t), \ldots, z^{(s)}(t)\right) \preceq \mathbf{0}, \forall t \in[0, T], \\
& z(t) \in \mathcal{F}, \forall t \in[0, T], \\
& z^{[s-1]}(0)=\bar{z}{o}, z^{[s-1]}(T)=\bar{z}{f}, \\
& z^{[s-1]}=\left(z^{\mathrm{T}}, \dot{z}^{\mathrm{T}}, \ldots, z^{(s-1)^{\mathrm{T}}}\right)^{\mathrm{T}}
\end{aligned}
$$

Reference

[1] Z. Wang, X. Zhou, C. Xu and F. Gao, “Geometrically Constrained Trajectory Optimization for Multicopters,” in IEEE Transactions on Robotics, vol. 38, no. 5, pp. 3259-3278, Oct. 2022, doi: 10.1109/TRO.2022.3160022.
[2] Z. Han et al., “An Efficient Spatial-Temporal Trajectory Planner for Autonomous Vehicles in Unstructured Environments,” in IEEE Transactions on Intelligent Transportation Systems, vol. 25, no. 2, pp. 1797-1814, Feb. 2024, doi: 10.1109/TITS.2023.3315320.
[3] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C.J. Taylor and V. Kumar. Planning Dynamically Feasible Trajectories for Quadrotors using Safe Flight Corridors in 3-D Complex Environments. ICRA 2017.