前言
把路径从相对低维的空间去转换到全维度的状态空间,就是轨迹生成/优化,生成一条动力学可行,光滑的路径是路径规划的终极目标。
为什么要生成光滑的路径原因:
微分平坦
平坦性:可以类比为线性系统当中的能控性,平坦性的定义,核心就是,能取平坦输出及其各阶导数
y
f
,
y
˙
f
,
y
¨
f
,
…
,
y
f
(
k
)
y_{f}, \dot{y}_{f}, \ddot{y}_{f}, \dots, y_{f}^{(k)}
yf,y˙f,y¨f,…,yf(k)为微分参数,作为基向量,来代数地表示原有的状态变量x以及输入变量u,要满足这个条件,意味着平坦输出的各阶导数线性无关,也就是说不存在这样一个线性微分方程。
具体可以参考善道大佬的这篇解释
举例:无人机系统
可以把机器人系统的全维度状态空间的Planning问题转化为Flat outputs(平坦的输出空间)里的问题,来进行轨迹规划。
一些要求:
无人机的动力学描述:
牛顿方程反应的是力的关系,欧拉方程反应的是转动的关系。
Tips:无人机永远只能产生垂直于自己机体桨平面的力,且没有机头这个概念。
需求:根据无人机的基本公式,去推把无人机的12维状态空间,给压缩到(x,y,z,Yaw)的平坦输出的微分空间里。无人机做Planning,要不要对姿态做Planning?其姿态肯定是与加速度紧紧绑在一起的,就是因为无人机和的推力永远只能产生垂直于自己机体桨平面。所以对于无人机的路径规划,姿态和加速度两者只能去其一,所以选择了加速度Planning,这样就把欧拉角干掉了(因为欧拉角还有旋转矩阵的因素)(除了Yaw角,Pitch和Roll都被干掉了)( 一个姿态产生一个推力,现在沿着Yaw角进行旋转,但产生的力还是垂直桨平面的力,是独立的,所以还是要单独规划Yaw角),所以就在加速度的空间上进行Planning。
因为位置的组合,以及其导数的组合已经在平坦空间当中了,所以无需证明。所以x,y,z及其导数就搞定了。
t向量:无人机体坐标系的Z轴,其方向是t这个向量的方向,就表示世界坐标系下三个轴的加速度,表示和加速方向,与姿态紧紧绑定的。
现在body系下的Z轴方向(
z
B
=
t
∥
t
∥
,
t
=
[
σ
¨
1
,
σ
¨
2
,
σ
¨
3
+
g
]
T
\mathbf{z}_{B}=\frac{\mathbf{t}}{\|\mathbf{t}\|}, \mathbf{t}=\left[\ddot{\boldsymbol{\sigma}}_{1}, \ddot{\boldsymbol{\sigma}}_{2}, \ddot{\boldsymbol{\sigma}}_{3}+g\right]^{T}
zB=∥t∥t,t=[σ¨1,σ¨2,σ¨3+g]T)就已经表示成了Flatness Outputs(
σ
=
[
x
,
y
,
z
,
ψ
]
T
\boldsymbol{\sigma}=[x, y, z, \psi]^{T}
σ=[x,y,z,ψ]T)的代数组合的形式。
无人机的姿态实际上就是这个ody系下三个轴的方向,所以说搞定了
x
B
,
y
B
,
z
B
\mathbf{x}_{B},\mathbf{y}_{B},\mathbf{z}_{B}
xB,yB,zB其实就已经搞定了无人机自己旋转的三个角了(Pitch,Roll,Yaw),这个是一一对应的关系,组合起来就是一个旋转矩阵。为了方便看
x
B
,
y
B
\mathbf{x}_{B},\mathbf{y}_{B}
xB,yB,引入了一个中间的坐标系C,C系与W系(World系)只差了一个偏航角,所以说
x
C
\mathbf{x}_{C}
xC肯定可以只用Yaw角来表示的,
x
C
=
[
cos
σ
4
,
sin
σ
4
,
0
]
T
\mathbf{x}_{C}=\left[\cos \boldsymbol{\sigma}_{4}, \sin \boldsymbol{\sigma}_{4}, 0\right]^{T}
xC=[cosσ4,sinσ4,0]T。利用C系
x
C
\mathbf{x}_{C}
xC,以及B系(Body系)的
z
B
\mathbf{z}_{B}
zB,运用叉乘的关系,就可以算出
y
B
\mathbf{y}_{B}
yB,以及
x
B
\mathbf{x}_{B}
xB,
R
B
\boldsymbol{R}_{B}
RB。
y
B
=
z
B
×
x
C
∥
z
B
×
x
C
∥
,
x
B
=
y
B
×
z
B
R
B
=
[
x
B
y
B
z
B
]
\mathbf{y}_{B}=\frac{\mathbf{z}_{B} \times \mathbf{x}_{C}}{\left\|\mathbf{z}_{B} \times \mathbf{x}_{C}\right\|}, \quad \mathbf{x}_{B}=\mathbf{y}_{B} \times \mathbf{z}_{B} \quad \boldsymbol{R}_{B}=\left[\begin{array}{lll} \mathbf{x}_{B} & \mathbf{y}_{B} & \mathbf{z}_{B} \end{array}\right]
yB=∥zB×xC∥zB×xC,xB=yB×zBRB=[xByBzB]
现在,如何把角速度表示成(x,y,z,Yaw)的线性组合?
p
¨
\ddot{p}
p¨的就是加速度
a
{a}
a,之后对方程两边求导,就得出了上式
a
˙
\dot{a}
a˙。
ω
B
W
\boldsymbol{\omega}_{B W}
ωBW就是在世界系下看机体的角速度。
ω
B
W
=
ω
x
x
B
+
ω
y
y
B
+
ω
z
z
B
\boldsymbol{\omega}_{B W}=\omega_{x} \mathbf{x}_{B}+\omega_{y} \mathbf{y}_{B}+\omega_{z} \mathbf{z}_{B}
ωBW=ωxxB+ωyyB+ωzzB
就是在三个轴上三个角速度的分量。
u
˙
1
\dot{u}_{1}
u˙1就是在垂直于无人机桨平面的推力合力在Body系方向上的分量后的求导项。
h
ω
=
ω
B
W
×
z
B
=
m
u
1
(
a
˙
−
(
z
B
⋅
a
˙
)
z
B
)
\mathbf{h}_{\omega}=\boldsymbol{\omega}_{B W} \times \mathbf{z}_{B}=\frac{m}{u_{1}}\left(\dot{\boldsymbol{a}}-\left(\mathbf{z}_{B} \cdot \dot{\boldsymbol{a}}\right) \mathbf{z}_{B}\right)
hω=ωBW×zB=u1m(a˙−(zB⋅a˙)zB)
ω
B
W
×
z
B
\boldsymbol{\omega}_{B W} \times \mathbf{z}_{B}
ωBW×zB这个叉乘就用上面的
ω
B
W
\boldsymbol{\omega}_{B W}
ωBW表达式代入,再一个个用右手叉乘展开就行,例如
x
B
×
z
B
{x}_{B} \times \mathbf{z}_{B}
xB×zB,右手叉乘,得出的就是一个反向的
y
B
{y}_{B}
yB,其余的以此类推,最后
h
ω
\mathbf{h}_{\omega}
hω是所有量都满足我们的要求的,就可以得出:
ω
x
=
−
h
ω
⋅
y
B
,
ω
y
=
h
ω
⋅
x
B
\omega_{x}=-\mathbf{h}_{\omega} \cdot \mathbf{y}_{B}, \quad \omega_{y}=\mathbf{h}_{\omega} \cdot \mathbf{x}_{B}
ωx=−hω⋅yB,ωy=hω⋅xB
Z轴上的分量要特殊处理,如下所示
所以,综上,所有的12个变量,位置速度角速度欧拉角都搞定了,成功的使用Flatness Outputs中的(x,y,z,Yaw)进行了表示。
综上,对无人机的运动规划只需要在Flatness Outputs中的(x,y,z,Yaw)进行。
Planning-Control Loop
位置控制器解算出 u 1 {u}_{1} u1,姿态控制器解算出 u 2 , u 3 , u 4 {u}_{2},{u}_{3},{u}_{4} u2,u3,u4三个方向的力矩。规划就只是规划了红框的这一部分。
轨迹表示形式
Minimum-Snap轨迹生成
一段的情况,一维的生成光滑的轨迹,最简单情况只有一段的轨迹,就是一个典型的BVP问题。
具体可看上一篇OBVP的blog。
多段的情况,
对每一段假设都具备一个多项式函数,不能只假设每个位置的速度以及加速度为0了,还得首尾给个两点的速度值。那么速度与加速度是超参数,怎么指定最好?
现在用无人机系统举例,如何求解最佳的
v
{v}
v和
a
{a}
a。
反复强调其加速度和姿态是绑定在一起的,角速度和推力是绑定在一起的。
所以说最小化jerk(加速度的导数)就能最小化角速度,就能最小化推力,就能最小化视差率。
最小化snap,就能使得推力的变化变的更加平坦。
现在目标已经明确了,就是要计算每一段轨迹的多项式的系数,使得总体的轨迹满足首末约束,经过中间点,在中间点,首末段的高阶导数都是一样的。使得整体轨迹可以光滑且最小化其jerk和snap。
每段轨迹都是多项式。
若每段轨迹的参数化系数都一样的,比如都初始化最高幂次一样,则会使问题简化。
想要轨迹几阶连续与几阶光滑是不一样的,k阶光滑意味着k+1阶连续。
保证光滑与连续性与想最小化轨迹的几阶导数也是彼此独立的,没有一一对应的联系。
每段轨迹的多项式系数就可以用OBVP来求解。
总结
经过以上对Minimum Snap的推导就转化成了凸优化问题,凸优化问题先不肝了哈哈哈,今天就到这吧23333.