写在前面
本周主要介绍了四旋翼的控制理论和路径规划算法。
课后作业是四旋翼的二维pid控制,需要沿着规定的路线对四旋翼的控制进行仿真,并达到一定的精度。
四旋翼控制
2维
y-z平面上的四旋翼运动和控制。
运动方程
惯性坐标系为
A
\mathcal{A}
A,机体坐标系为
B
\mathcal{B}
B,由机体坐标系到惯性坐标系的旋转矩阵为:
A
[
R
]
B
=
[
c
o
s
(
ϕ
)
−
s
i
n
(
ϕ
)
s
i
n
(
ϕ
)
c
o
s
(
ϕ
)
]
^{\mathcal{A}}[R]_{\mathcal{B}}=\begin{bmatrix}cos(\phi)&-sin(\phi)\\sin(\phi)&cos(\phi)\end{bmatrix}
A[R]B=[cos(ϕ)sin(ϕ)−sin(ϕ)cos(ϕ)]
设
r
=
[
y
,
z
]
T
r=[y,z]^T
r=[y,z]T,作用在四旋翼上的有
b
3
⃗
\vec{b_3}
b3方向的升力和
a
3
⃗
\vec{a_3}
a3方向的重力,进行坐标系转换,可以写出四旋翼的力平衡方程:
m
r
¨
=
[
0
−
m
g
]
+
A
[
R
]
B
[
0
u
1
]
u
1
=
∑
i
=
1
4
F
i
m\ddot{r}=\begin{bmatrix}0\\-mg\end{bmatrix}+^{\mathcal{A}}[R]_{\mathcal{B}}\begin{bmatrix}0\\u_1\end{bmatrix}\\ u_1=\sum^4_{i=1}F_i
mr¨=[0−mg]+A[R]B[0u1]u1=i=1∑4Fi
力矩平衡方程为,L是旋翼臂长:
I
x
x
ϕ
¨
=
L
(
F
1
−
F
2
)
=
u
2
I_{xx}\ddot{\phi}=L(F_1-F_2)=u_2
Ixxϕ¨=L(F1−F2)=u2
则假设俯仰和偏航角均为零,Y-Z平面上的四旋翼的运动方程如下:
[
y
¨
z
¨
ϕ
¨
]
=
[
0
−
g
0
]
+
[
−
1
m
s
i
n
(
ϕ
)
0
1
m
c
o
s
(
ϕ
)
0
0
1
I
x
x
]
[
u
1
u
2
]
\begin{bmatrix}\ddot{y}\\\ddot{z}\\\ddot{\phi}\end{bmatrix}= \begin{bmatrix}0\\-g\\0\end{bmatrix}+ \begin{bmatrix}-\frac{1}{m}sin(\phi)&0\\\frac{1}{m}cos(\phi)&0\\0&\frac{1}{I_{xx}}\end{bmatrix} \begin{bmatrix}u_1\\u_2\end{bmatrix}\\
⎣⎡y¨z¨ϕ¨⎦⎤=⎣⎡0−g0⎦⎤+⎣⎡−m1sin(ϕ)m1cos(ϕ)000Ixx1⎦⎤[u1u2]
状态变量表示
定义状态变量。
x
=
[
y
,
z
,
ϕ
,
y
˙
,
z
˙
,
ϕ
˙
]
T
x=[y,z,\phi,\dot{y},\dot{z},\dot{\phi}]^T
x=[y,z,ϕ,y˙,z˙,ϕ˙]T
则可以将该方程写为状态空间的形式:
x
˙
=
[
y
˙
z
˙
ϕ
˙
0
−
g
0
]
+
[
0
0
0
0
0
0
−
1
m
s
i
n
(
ϕ
)
0
1
m
c
o
s
(
ϕ
)
0
0
1
I
x
x
]
[
u
1
u
2
]
\dot{x}=\begin{bmatrix}\dot{y}\\\dot{z}\\\dot{\phi}\\0\\-g\\0\end{bmatrix}+ \begin{bmatrix}0&0\\0&0\\0&0\\-\frac{1}{m}sin(\phi)&0\\\frac{1}{m}cos(\phi)&0\\0&\frac{1}{I_{xx}}\end{bmatrix} \begin{bmatrix}u_1\\u_2\end{bmatrix}
x˙=⎣⎢⎢⎢⎢⎢⎢⎡y˙z˙ϕ˙0−g0⎦⎥⎥⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎢⎢⎡000−m1sin(ϕ)m1cos(ϕ)000000Ixx1⎦⎥⎥⎥⎥⎥⎥⎤[u1u2]
线性化
由于pd控制器只适用于线性系统,因此在使用之前需要将系统线性化。当四旋翼悬停时,可以将运动线性化,引入小扰动假设,这时有:
s
i
n
(
ϕ
)
≈
ϕ
,
c
o
s
(
ϕ
)
≈
1
sin(\phi)\approx \phi,cos(\phi)\approx1
sin(ϕ)≈ϕ,cos(ϕ)≈1,可以将四旋翼的运动方程线性简化为。
y
¨
=
−
g
ϕ
z
¨
=
u
1
m
−
g
ϕ
¨
=
u
2
I
x
x
\ddot{y}=-g\phi\\ \ddot{z}=\frac{u_1}{m}-g \\ \ddot{\phi}=\frac{u_2}{I_{xx}}
y¨=−gϕz¨=mu1−gϕ¨=Ixxu2
控制方程
已知
r
T
(
t
)
,
r
˙
T
(
t
)
,
r
¨
T
(
t
)
r_T(t),\dot{r}_T(t),\ddot{r}_T(t)
rT(t),r˙T(t),r¨T(t),
r
¨
C
\ddot{r}_C
r¨C为控制器计算出的指令加速度,则控制方程如下:
e
p
=
r
T
(
t
)
−
r
e
v
=
r
˙
T
(
t
)
−
r
˙
w
a
n
t
:
(
r
¨
T
(
t
)
−
r
¨
c
)
+
k
d
e
v
+
k
p
e
p
=
0
e_p=r_T(t)-r\\ e_v=\dot{r}_T(t)-\dot{r}\\ want:(\ddot{r}_T(t)-\ddot{r}_c)+k_de_v+k_pe_p=0
ep=rT(t)−rev=r˙T(t)−r˙want:(r¨T(t)−r¨c)+kdev+kpep=0
则
r
¨
c
=
r
¨
T
(
t
)
+
k
d
e
v
+
k
p
e
p
\ddot{r}_c=\ddot{r}_T(t)+k_de_v+k_pe_p
r¨c=r¨T(t)+kdev+kpep
由控制方程可以计算得到该四旋翼所应输出的
u
1
,
u
2
u_1,u_2
u1,u2 。
u
1
=
m
g
+
m
z
c
¨
=
m
(
g
+
z
¨
d
e
s
+
k
d
,
z
(
z
˙
d
e
s
−
z
˙
)
+
k
p
,
z
(
z
d
e
s
−
z
)
)
u
2
=
I
x
x
ϕ
¨
T
(
t
)
=
I
x
x
(
ϕ
c
+
k
p
,
ϕ
(
ϕ
c
−
ϕ
)
+
k
d
,
ϕ
(
ϕ
˙
c
−
ϕ
˙
)
¨
)
,
ϕ
c
¨
=
−
y
c
¨
g
=
1
g
(
y
T
¨
(
t
)
+
k
v
,
y
(
y
˙
T
(
t
)
−
y
˙
)
+
k
p
,
y
(
y
T
(
t
)
−
y
)
)
u_1=mg+m\ddot{z_c}=m(g+\ddot{z}_{des}+k_{d,z}(\dot{z}_{des}-\dot{z})+k_{p,z}(z_{des}-z))\\ u_2=I_{xx}\ddot{\phi}_T(t)=I_{xx}(\ddot{\phi_c+k_{p,\phi}(\phi_c-\phi)+k_{d,\phi}(\dot\phi_c-\dot\phi)}),\\ \ddot{\phi_c}=-\frac{\ddot{y_c}}{g}=\frac{1}{g}(\ddot{y_T}(t)+k_{v,y}(\dot{y}_T(t)-\dot{y})+k_{p,y}(y_T(t)-y))
u1=mg+mzc¨=m(g+z¨des+kd,z(z˙des−z˙)+kp,z(zdes−z))u2=Ixxϕ¨T(t)=Ixx(ϕc+kp,ϕ(ϕc−ϕ)+kd,ϕ(ϕ˙c−ϕ˙)¨),ϕc¨=−gyc¨=g1(yT¨(t)+kv,y(y˙T(t)−y˙)+kp,y(yT(t)−y))
在期望轨迹为悬停的特殊情况下(期望位置为常数,期望滚转角为零),有
R
T
(
t
)
=
[
y
0
,
z
0
]
T
,
r
˙
T
(
t
)
=
r
¨
T
(
t
)
=
0
R_T(t)=[y_0,z_0]^T,\dot{r}_T(t)=\ddot{r}_T(t)=0
RT(t)=[y0,z0]T,r˙T(t)=r¨T(t)=0。
作业
本周作业为二维控制,目标是控制四旋翼走一个直线和一个sin形曲线,需达到一定精度要求方为作业合格。
本周的作业编程本身其实并不难,比较耗时间的是调参,如果四旋翼的飞行不理想,应该首先观察自己四旋翼的飞行过程,看看是哪里出了问题,在这周的课程论坛里,还有一篇文章讲了调参的一些技巧。
你在调参的时候会发现,滚转角太大的时候,四旋翼容易失稳,这是因为我们用的是简化过的线性控制器,角度太大的时候会引入非线性因素,控制器容易失稳。
如果实在调不出来了,可以参考我调的参数。
my code
function [ u1, u2 ] = controller(~, state, des_state, params)
%CONTROLLER Controller for the planar quadrotor
%
% state: The current state of the robot with the following fields:
% state.pos = [y; z], state.vel = [y_dot; z_dot], state.rot = [phi],
% state.omega = [phi_dot]
%
% des_state: The desired states are:
% des_state.pos = [y; z], des_state.vel = [y_dot; z_dot], des_state.acc =
% [y_ddot; z_ddot]
%
% params: robot parameters
% Using these current and desired states, you have to compute the desired
% controls
% u1 = 0;
% u2 = 0;
kp_z = 30;%达到des_z的时间
kv_z = 10;%减慢z轴的晃动
kp_phi = 200;%将该参数放得更大,可以使飞行时的phi更接近0,这时飞行更加稳定
kv_phi = 20;%使四旋翼晃动更慢
kp_y = 1;%达到des_y的时间
kv_y = 10;%减小y轴的晃动
phi_c = -1/params.gravity*(des_state.acc(1) + ...
kv_y*(des_state.vel(1)-state.vel(1)) + ...
kp_y*(des_state.pos(1)-state.pos(1)));
phi_dc = 0;
phi_ddc = 0;
u1 =params.mass*(params.gravity + des_state.acc(2) + ...
kv_z*(des_state.vel(2) - state.vel(2)) + ...
kp_z*(des_state.pos(2) - state.pos(2)));
u2 =params.Ixx*(phi_ddc + ...
kv_phi*(phi_dc - state.omega) + ...
kp_phi*(phi_c - state.rot));
% debug code
% for i = 1:500
% x = trajhandle(i);
% H(i,:) = [x.pos(1),x.pos(2),x.vel(1),x.vel(2),x.acc(1),x.acc(2)];
% end
if(u1 > params.maxF)
u1=params.maxF;
end
M_max = params.maxF*params.arm_length;
if(u2 > M_max)
u2 = M_max;
end
% FILL IN YOUR CODE HERE
end
仿真结果
可以看出,最后的结果还是比较令人满意的。
3维
第四周的编程作业中有3维控制的内容,对其编程细节感兴趣的读者可以看后续第四周的笔记。
运动方程
惯性坐标系为
A
\mathcal{A}
A,机体坐标系为
B
\mathcal{B}
B,坐标系表示如下所示:
由机体坐标系到惯性坐标系的旋转矩阵为:
R
=
[
C
ψ
C
θ
−
S
ϕ
S
ψ
S
θ
−
C
ϕ
S
ψ
C
ψ
S
θ
+
C
θ
S
ϕ
S
ψ
C
θ
S
ψ
+
C
ψ
S
ϕ
S
θ
C
ϕ
C
ψ
S
ψ
S
θ
−
C
θ
S
ϕ
C
ψ
−
C
ϕ
S
θ
S
ϕ
C
ϕ
C
θ
]
R = \begin{bmatrix} C\psi C\theta - S\phi S\psi S\theta&-C\phi S\psi&C\psi S\theta + C\theta S\phi S\psi\\C\theta S\psi + C\psi S\phi S\theta&C\phi C\psi &S\psi S\theta -C\theta S\phi C\psi\\-C\phi S\theta&S\phi &C\phi C\theta \end{bmatrix}
R=⎣⎡CψCθ−SϕSψSθCθSψ+CψSϕSθ−CϕSθ−CϕSψCϕCψSϕCψSθ+CθSϕSψSψSθ−CθSϕCψCϕCθ⎦⎤
姿态角变化率和机体角速度的关系如下(某些教材上与课程课件表述不一致,暂时使用课件上的表述,此处存疑):
[
p
q
r
]
=
[
C
θ
0
−
C
ϕ
S
θ
0
1
S
θ
S
θ
0
C
ϕ
C
θ
]
[
ϕ
˙
θ
˙
ψ
˙
]
\begin{bmatrix}p\\q\\r\end{bmatrix}= \begin{bmatrix}C\theta&0&-C\phi S\theta\\0&1&S\theta\\S\theta&0&C\phi C\theta\end{bmatrix} \begin{bmatrix}\dot\phi\\\dot\theta\\\dot\psi\end{bmatrix}
⎣⎡pqr⎦⎤=⎣⎡Cθ0Sθ010−CϕSθSθCϕCθ⎦⎤⎣⎡ϕ˙θ˙ψ˙⎦⎤
平衡方程
力的平衡方程
m
r
¨
=
[
0
0
−
m
g
]
+
R
[
0
0
F
1
+
F
2
+
F
3
+
F
4
]
m\ddot r=\begin{bmatrix}0\\0\\-mg\end{bmatrix}+ R\begin{bmatrix}0\\0\\F_1+F_2+F_3+F_4\end{bmatrix}
mr¨=⎣⎡00−mg⎦⎤+R⎣⎡00F1+F2+F3+F4⎦⎤
将第一个输出定义为
u
1
=
∑
i
=
1
4
F
i
u_1=\sum_{i=1}^4F_i
u1=∑i=14Fi
力矩平衡方程
I
[
p
˙
q
˙
r
˙
]
=
[
L
(
F
2
−
F
4
)
L
(
F
3
−
F
1
)
M
1
−
M
2
+
M
3
−
M
4
]
−
[
p
q
r
]
×
I
[
p
q
r
]
I\begin{bmatrix}\dot p\\\dot q \\\dot r\end{bmatrix}= \begin{bmatrix}L(F_2-F_4)\\L(F_3-F_1)\\M_1-M_2+M_3-M_4\end{bmatrix}- \begin{bmatrix}p\\q\\r\end{bmatrix}\times I\begin{bmatrix}p\\q\\r\end{bmatrix}
I⎣⎡p˙q˙r˙⎦⎤=⎣⎡L(F2−F4)L(F3−F1)M1−M2+M3−M4⎦⎤−⎣⎡pqr⎦⎤×I⎣⎡pqr⎦⎤
对
u
2
u_2
u2又可以列动量矩定理:
u
2
=
I
[
ϕ
¨
θ
¨
ψ
¨
]
=
[
L
(
F
2
−
F
4
)
L
(
F
3
−
F
1
)
M
1
−
M
2
+
M
3
−
M
4
]
u_2=I\begin{bmatrix}\ddot \phi\\ \ddot \theta \\ \ddot\psi \end{bmatrix}=\begin{bmatrix}L(F_2-F_4)\\L(F_3-F_1)\\M_1-M_2+M_3-M_4\end{bmatrix}
u2=I⎣⎡ϕ¨θ¨ψ¨⎦⎤=⎣⎡L(F2−F4)L(F3−F1)M1−M2+M3−M4⎦⎤
线性化
将力的平衡方程线性化:
r
¨
1
=
g
(
Δ
θ
c
o
s
(
ψ
0
)
+
Δ
ϕ
s
i
n
(
ψ
0
)
)
r
¨
2
=
g
(
Δ
θ
s
i
n
(
ψ
0
)
−
Δ
ϕ
c
o
s
(
ψ
0
)
)
r
¨
3
=
1
m
u
1
−
g
\ddot r_1=g(\Delta\theta cos(\psi_0)+\Delta\phi sin(\psi_0))\\ \ddot r_2=g(\Delta \theta sin(\psi_0)-\Delta\phi cos(\psi_0))\\ \ddot r_3 = \frac{1}{m}u_1-g
r¨1=g(Δθcos(ψ0)+Δϕsin(ψ0))r¨2=g(Δθsin(ψ0)−Δϕcos(ψ0))r¨3=m1u1−g
将力矩平衡方程线性化,
γ
=
K
M
K
F
\gamma=\frac{K_M}{K_F}
γ=KFKM:
p
˙
=
u
2
,
x
I
x
x
(
F
2
−
F
4
)
q
˙
=
u
2
,
y
I
y
y
(
F
3
−
F
1
)
r
˙
=
u
2
,
z
I
z
z
=
γ
I
z
z
(
F
1
−
F
2
+
F
3
−
F
4
)
\dot p = \frac{u_{2,x}}{I_{xx}}(F_2-F_4)\\ \dot q = \frac{u_{2,y}}{I_{yy}}(F_3-F_1)\\ \dot r = \frac{u_{2,z}}{I_{zz}}=\frac{\gamma}{I_{zz}}(F_1-F_2+F_3-F_4)
p˙=Ixxu2,x(F2−F4)q˙=Iyyu2,y(F3−F1)r˙=Izzu2,z=Izzγ(F1−F2+F3−F4)
控制方程
3维四旋翼控制问题可以这样描述:以悬停或循迹
z
d
e
s
z_{des}
zdes为目标,找出四维输入量
{
u
1
,
u
2
⃗
}
\{u_1,\vec{u_2}\}
{u1,u2}。
如上图所示,内部的姿态控制器用板载加速度计和陀螺仪来控制滚转、俯仰和偏航姿态角度,运行频率约为1000 Hz。外部的位置控制环则使用速度和位置估计来控制航迹,运行频率约为100-200 Hz。
######姿态控制
联立下列控制方程和动力学方程:
(
r
¨
T
(
t
)
−
r
¨
c
)
+
k
d
e
v
+
k
p
e
p
=
0
u
2
=
I
[
ϕ
¨
θ
¨
ψ
¨
]
T
(\ddot{r}_T(t)-\ddot{r}_c)+k_de_v+k_pe_p=0\\ u_2=I\begin{bmatrix}\ddot \phi& \ddot \theta & \ddot\psi \end{bmatrix}^T
(r¨T(t)−r¨c)+kdev+kpep=0u2=I[ϕ¨θ¨ψ¨]T
将转动惯量并入增益,可以得到
u
2
u_2
u2的解:
u
2
=
[
k
p
,
ϕ
(
ϕ
d
e
s
−
ϕ
)
+
k
d
,
ϕ
(
p
d
e
s
−
p
)
k
p
,
θ
(
θ
d
e
s
−
θ
)
+
k
d
,
θ
(
q
d
e
s
−
q
)
k
p
,
ψ
(
ψ
d
e
s
−
ψ
)
+
k
d
,
ψ
(
r
d
e
s
−
r
)
]
u_2=\begin{bmatrix} k_{p,\phi}(\phi_{des}-\phi)+k_{d,\phi}(p_{des}-p)\\ k_{p,\theta}(\theta_{des}-\theta)+k_{d,\theta}(q_{des}-q)\\ k_{p,\psi}(\psi_{des}-\psi)+k_{d,\psi}(r_{des}-r) \end{bmatrix}
u2=⎣⎡kp,ϕ(ϕdes−ϕ)+kd,ϕ(pdes−p)kp,θ(θdes−θ)+kd,θ(qdes−q)kp,ψ(ψdes−ψ)+kd,ψ(rdes−r)⎦⎤
又由z方向上的力的平衡方程和控制方程联立,可以得到
u
1
u_1
u1的解:
u
1
=
m
g
+
m
r
¨
3
,
d
e
s
=
m
g
−
m
{
k
d
,
3
(
r
˙
3
−
r
˙
3
,
T
)
+
k
p
,
3
(
r
3
−
r
3
,
T
)
}
u_1=mg+m\ddot r_{3,des}=mg-m\{k_{d,3} (\dot r_3-\dot r_{3,T})+k_{p,3}(r_3-r_{3,T})\}
u1=mg+mr¨3,des=mg−m{kd,3(r˙3−r˙3,T)+kp,3(r3−r3,T)}
由x,y方向上的力的平衡方程可得:
(1)
ϕ
d
e
s
=
1
g
(
r
¨
1
,
d
e
s
s
i
n
(
ψ
T
)
−
r
¨
2
,
d
e
s
c
o
s
(
ψ
T
)
)
\phi_{des}=\frac{1}{g}(\ddot r_{1,des}sin(\psi_T)-\ddot r_{2,des}cos(\psi_T))\tag{1}
ϕdes=g1(r¨1,dessin(ψT)−r¨2,descos(ψT))(1)
(2)
θ
d
e
s
=
1
g
(
r
¨
1
,
d
e
s
c
o
s
(
ψ
T
)
+
r
¨
2
,
d
e
s
s
i
n
(
ψ
T
)
)
\theta_{des}=\frac{1}{g}(\ddot r_{1,des}cos(\psi_T)+\ddot r_{2,des}sin(\psi_T))\tag{2}
θdes=g1(r¨1,descos(ψT)+r¨2,dessin(ψT))(2)
忽略高阶导数项,有:
(3)
p
d
e
s
=
0
p_{des}=0\tag{3}
pdes=0(3)
(4)
q
d
e
s
=
0
q_{des}=0\tag{4}
qdes=0(4)
由于偏航角是独立的,因此有:
(5)
ψ
d
e
s
=
ψ
T
(
t
)
\psi_{des}=\psi_T(t)\tag{5}
ψdes=ψT(t)(5)
(6)
r
d
e
s
=
ψ
˙
T
(
t
)
r_{des}=\dot \psi_T(t)\tag{6}
rdes=ψ˙T(t)(6)
悬停控制和三维循迹控制
悬停控制用于将位置保持在某个位置,而三维循迹控制可以用于跟踪指定的轨迹。
悬停控制
对于悬停控制,位置误差定义如下:
e
i
=
r
i
,
T
−
r
i
e_i=r_{i,T}-r_i
ei=ri,T−ri
联立控制方程如下,即可求得
r
¨
i
,
d
e
s
\ddot r_{i,des}
r¨i,des:
(
r
¨
i
,
T
−
r
¨
i
,
d
e
s
)
+
k
d
,
i
e
˙
i
+
k
p
,
i
e
i
=
0
(\ddot r_{i,T}-\ddot r_{i,des})+k_{d,i}\dot e_i+ k_{p,i}e_i=0
(r¨i,T−r¨i,des)+kd,ie˙i+kp,iei=0
对于悬停状态,有
r
˙
i
,
T
=
r
¨
i
,
T
=
0
\dot r_{i,T}=\ddot r_{i,T}=0
r˙i,T=r¨i,T=0
再联立(1)-(6)式,即可求得
u
1
,
u
2
u_1,u_2
u1,u2
三维循迹控制
三维循迹控制控制的前提是准悬停假设成立(near-hover assumptions),以保证系统仍是线性的,但
r
˙
i
,
T
\dot r_{i,T}
r˙i,T和
r
¨
i
,
T
\ddot r_{i,T}
r¨i,T不再为零,而是由具体的航迹来决定。
r
T
r_T
rT表示目标航迹上距离当前位置
r
r
r最近的点,
r
˙
T
,
r
¨
T
\dot r_T,\ddot r_T
r˙T,r¨T分别是目标速度和目标加速度。轨迹
r
T
r_T
rT的单位切向量为
t
^
\hat t
t^,单位法向量为
n
^
\hat n
n^,单位副法向量为
b
^
=
t
^
×
n
^
\hat b =\hat t \times \hat n
b^=t^×n^。
则误差为:
e
p
=
(
(
r
T
−
r
)
n
^
)
n
^
+
(
(
r
T
−
r
)
b
^
)
b
^
e
v
=
r
˙
T
−
r
˙
e_p=((r_T-r)\hat n)\hat n+ ((r_T-r)\hat b)\hat b\\e_v=\dot r_T -\dot r
ep=((rT−r)n^)n^+((rT−r)b^)b^ev=r˙T−r˙
注意上式中的比例误差忽略了切线方向的误差,并且只考虑了目标航迹上最近的点。
根据新定义的误差,我们联立控制方程,即可求得 r ¨ i , d e s \ddot r_{i,des} r¨i,des,再联立(1)-(6)式,即可求得 u 1 , u 2 u_1,u_2 u1,u2。
航迹规划(planning)
- 起始点位、终止点位
- 航路点位
- smoothness原则:将输入的变化率减到最小
- 系统阶数:系统阶数决定了输入的边界条件
数学补充:变分法
x
∗
(
t
)
=
min
x
(
t
)
∫
0
T
L
(
x
˙
,
x
,
t
)
d
t
x^*(t)=\min_{x(t)} \int_0^T\mathscr{L}(\dot x,x,t)dt
x∗(t)=x(t)min∫0TL(x˙,x,t)dt
给定起始点
x
(
0
)
x(0)
x(0)和终点
x
(
T
)
x(T)
x(T),考虑所有的可微曲线
x
(
t
)
x(t)
x(t)。
对于最优函数
x
(
t
)
x(t)
x(t),它必须遵循欧拉-拉格朗日方程:
d
d
t
(
∂
L
∂
x
˙
)
−
∂
L
∂
x
=
0
\frac{d}{dt}(\frac{\partial \mathscr{L}}{\partial \dot x})-\frac{\partial \mathscr{L}}{\partial x}=0
dtd(∂x˙∂L)−∂x∂L=0
例
- 最短距离 (几何): x ∗ ( t ) = min x ( t ) ∫ 0 T x ˙ 2 d t x^*(t)=\min_{x(t)}\int_0^T\dot x^2 dt x∗(t)=x(t)min∫0Tx˙2dt
- 费马原理(光学): x ∗ ( t ) = min ∫ 0 T 1 d t x^*(t)=\min \int_0^T 1 dt x∗(t)=min∫0T1dt
- 最小作用量原理(机械): x ∗ ( t ) = min x ( t ) ∫ 0 T { T ( x ˙ , x , t ) − V ( x , t ) } d t x^*(t)=\min_{x(t)}\int_0^T\{T(\dot x,x,t)-V(x,t)\}dt x∗(t)=x(t)min∫0T{T(x˙,x,t)−V(x,t)}dt
对一个一阶系统:
x
∗
(
t
)
=
min
x
(
t
)
∫
0
T
x
˙
2
d
t
x^*(t)=\min_{x(t)}\int_0^T\dot x^2dt
x∗(t)=x(t)min∫0Tx˙2dt
代入欧拉-拉格朗日方程得到:
(
x
˙
)
2
→
x
¨
=
0
→
x
=
c
1
t
+
c
0
(\dot x)^2 \rightarrow \ddot {x}=0 \rightarrow x=c_1t+c_0
(x˙)2→x¨=0→x=c1t+c0
由给出的
x
(
0
)
x(0)
x(0)和
x
(
T
)
x(T)
x(T)可以求出
c
1
,
c
0
c_1,c_0
c1,c0
对一个n阶系统:
x
∗
(
t
)
=
min
x
(
t
)
∫
0
T
(
x
(
n
)
)
2
d
t
x^*(t)=\min_{x(t)}\int_0^T (x^{(n)})^2dt
x∗(t)=x(t)min∫0T(x(n))2dt
输入为
x
(
n
)
x^{(n)}
x(n)
∂
L
∂
x
−
d
d
t
(
∂
L
∂
x
˙
)
+
d
2
d
t
2
(
∂
L
∂
x
¨
)
+
.
.
.
+
(
−
1
)
n
d
n
d
t
n
(
∂
L
∂
x
(
n
)
)
=
0
\frac{\partial \mathscr{L}}{\partial x}- \frac{d}{dt}(\frac{\partial \mathscr{L}}{\partial \dot x})+\frac{d^2}{dt^2}(\frac{\partial \mathscr{L}}{\partial \ddot x})+...+(-1)^n\frac{d^n}{dt^n}(\frac{\partial \mathscr{L}}{\partial x^{(n)}})=0
∂x∂L−dtd(∂x˙∂L)+dt2d2(∂x¨∂L)+...+(−1)ndtndn(∂x(n)∂L)=0
在航迹规划问题中有如下叫法:
n=1为最短距离曲线,同时也是最小速度曲线,数学上可以证明,不再赘述。
n=2为最小加速度曲线
n=3称为min jerk(这俩不知道咋翻译)
n=4称为min snap
minimum jerk
KaTeX parse error: Expected 'EOF', got '\dddot' at position 39: …0^T\mathscr{L}(\̲d̲d̲d̲o̲t̲ ̲x,\ddot x,\dot …
使用欧拉-拉格朗日方程可得:
x
(
6
)
=
0
x^{(6)}=0
x(6)=0
积分后可得:
x
=
c
5
t
5
+
c
4
t
4
+
c
3
t
3
+
c
2
t
2
+
c
1
t
+
c
0
x=c_5t^5+c_4t^4+c_3t^3+c_2t^2+c_1t+c_0
x=c5t5+c4t4+c3t3+c2t2+c1t+c0
起始和终止两点的边界条件为:
p
o
s
v
e
l
a
c
c
t
=
0
a
0
0
t
=
T
b
0
0
\begin{array}{r|rrr} & pos & vel & acc\\ \hline t=0 & a &0 &0\\ t=T & b &0&0 \end{array}
t=0t=Tposabvel00acc00
根据边界条件,可以求出方程的六个参数。求出
x
(
t
)
x(t)
x(t) 后,可以通过求导求得加速度和速度。
而对于途经多点的航迹规划问题,在下一周的笔记中有详细的原理和代码实现。