赞
踩
首先建立移动机器人运动学方程:
[
x
˙
y
˙
θ
˙
]
=
[
c
o
s
θ
0
s
i
n
θ
0
0
1
]
[
v
ω
]
其中,
P
=
[
x
y
θ
]
P=
移动机器人跟踪问题中输入为参考轨迹 P r P_r Pr和参考速度 u r u_r ur。
这里MPC模型的状态量有多种选择:1)全局坐标系位姿;2)全局坐标系误差;3)局部坐标系误差(即全局误差投影到局部坐标系)。下面来一一建模分析。
首先以局部误差为状态量进行分析,对于移动机器人的非完整性,存在以下假设:机器人不发生侧向滑动,即在全局坐标系下
y
˙
L
=
0
\dot{y}_L=0
y˙L=0,即有:
x
˙
s
i
n
θ
−
y
˙
c
o
s
θ
=
0
x
˙
c
o
s
θ
+
y
˙
s
i
n
θ
=
v
此公式对于参考位姿和参考速度依然成立。
定义机器人局部坐标系下位姿误差为:
[
x
e
y
e
θ
e
]
=
[
c
o
s
θ
s
i
n
θ
0
−
s
i
n
θ
c
o
s
θ
0
0
0
1
]
[
x
r
−
x
y
r
−
y
θ
r
−
θ
]
则有:
x
˙
e
=
−
ω
s
i
n
θ
(
x
r
−
x
)
+
ω
c
o
s
θ
(
y
r
−
y
)
+
c
o
s
θ
(
x
˙
r
−
x
˙
)
+
s
i
n
θ
(
y
˙
r
−
y
˙
)
=
ω
y
e
−
v
+
x
˙
r
c
o
s
θ
+
y
˙
r
s
i
n
θ
(
由
于
θ
≠
θ
r
,
后
两
项
不
能
合
并
为
v
r
)
=
ω
y
e
−
v
+
x
˙
r
c
o
s
(
θ
r
−
θ
e
)
+
y
˙
r
s
i
n
(
θ
r
−
θ
e
)
=
ω
y
e
−
v
+
x
˙
r
(
c
o
s
θ
r
c
o
s
θ
e
+
s
i
n
θ
r
s
i
n
θ
e
)
+
y
˙
r
(
s
i
n
θ
r
c
o
s
θ
e
−
c
o
s
θ
r
s
i
n
θ
e
)
=
ω
y
e
−
v
+
(
x
˙
r
c
o
s
θ
r
+
y
˙
r
s
i
n
θ
r
)
c
o
s
θ
e
+
(
x
˙
r
s
i
n
θ
r
−
y
˙
r
c
o
s
θ
r
)
s
i
n
θ
e
=
ω
y
e
−
v
+
v
r
c
o
s
θ
e
y
˙
e
=
−
ω
c
o
s
θ
(
x
r
−
x
)
−
ω
s
i
n
θ
(
y
r
−
y
)
−
s
i
n
θ
(
x
˙
r
−
x
˙
)
+
c
o
s
θ
(
y
˙
r
−
y
˙
)
=
−
ω
x
e
−
x
˙
r
s
i
n
θ
+
y
˙
r
c
o
s
θ
=
−
ω
x
e
−
x
˙
r
s
i
n
(
θ
r
−
θ
e
)
+
y
˙
r
c
o
s
(
θ
r
−
θ
e
)
=
−
ω
x
e
−
x
˙
r
(
s
i
n
θ
r
c
o
s
θ
e
−
c
o
s
θ
r
s
i
n
θ
e
)
+
y
˙
r
(
c
o
s
θ
r
c
o
s
θ
e
+
s
i
n
θ
r
s
i
n
θ
e
)
=
−
ω
x
e
+
(
−
x
˙
r
s
i
n
θ
r
+
y
˙
r
c
o
s
θ
r
)
c
o
s
θ
e
+
(
x
˙
r
c
o
s
θ
r
+
y
˙
r
s
i
n
θ
r
)
s
i
n
θ
e
=
−
ω
x
e
+
v
r
s
i
n
θ
e
θ
˙
e
=
θ
˙
r
−
θ
˙
=
ω
r
−
ω
写成矩阵形式为:
[
x
˙
e
y
˙
e
θ
˙
e
]
=
[
ω
y
e
−
v
+
v
r
c
o
s
θ
e
−
ω
x
e
+
v
r
s
i
n
θ
e
ω
r
−
ω
]
(1)
很明显上式是一个非线性系统,需要进行线性化:
[
x
˙
e
y
˙
e
θ
˙
e
]
=
[
0
ω
0
−
ω
0
0
0
0
0
]
[
x
e
y
e
θ
e
]
+
[
−
v
+
v
r
c
o
s
θ
e
v
r
s
i
n
θ
e
ω
r
−
ω
]
由于
lim
θ
e
→
0
c
o
s
θ
e
=
1
,
lim
θ
e
→
0
s
i
n
θ
e
=
θ
e
\displaystyle\lim_{\theta_e \to 0}cos\theta_e=1,\ \lim_{\theta_e \to 0}sin\theta_e=\theta_e
θe→0limcosθe=1, θe→0limsinθe=θe,上式可写成:
[
x
˙
e
y
˙
e
θ
˙
e
]
=
[
0
ω
0
−
ω
0
v
r
0
0
0
]
[
x
e
y
e
θ
e
]
+
[
v
r
−
v
0
ω
r
−
ω
]
=
[
0
ω
0
−
ω
0
v
r
0
0
0
]
[
x
e
y
e
θ
e
]
+
[
1
0
0
0
0
1
]
[
v
r
−
v
ω
r
−
ω
]
此时定义MPC系统状态量为
X
=
[
x
e
y
e
θ
e
]
X=
令
A
=
[
0
ω
0
−
ω
0
v
r
0
0
0
]
,
B
=
[
1
0
0
0
0
1
]
A=
X
˙
=
A
X
+
B
u
~
\dot X=AX+B\widetilde u
X˙=AX+Bu
离散化:
X
(
k
+
1
)
−
X
(
k
)
Δ
T
=
A
X
(
k
)
+
B
u
~
(
k
)
其中
Δ
T
\Delta T
ΔT为离散时间间隔,即:
X
(
k
+
1
)
=
(
I
+
Δ
T
)
A
X
(
k
)
+
Δ
T
B
u
~
(
k
)
令
A
~
=
(
I
+
Δ
T
)
A
,
B
~
=
Δ
T
B
\widetilde A=(I+\Delta T)A,\ \widetilde B=\Delta TB
A
=(I+ΔT)A, B
=ΔTB,同时设定预测时域为
N
N
N,则有:
X
(
k
+
1
)
=
A
~
X
(
k
)
+
B
~
u
~
(
k
)
X
(
k
+
2
)
=
A
~
X
(
k
+
1
)
+
B
~
u
~
(
k
+
1
)
=
A
~
2
X
(
k
)
+
A
~
B
~
u
~
(
k
)
+
B
~
u
~
(
k
+
1
)
X
(
k
+
3
)
=
A
~
3
X
(
k
)
+
A
~
2
B
~
u
~
(
k
)
+
A
~
B
~
u
~
(
k
+
1
)
+
B
~
u
~
(
k
+
2
)
.
.
.
X
(
k
+
N
)
=
A
~
N
X
(
k
)
+
∑
i
=
0
N
−
1
A
~
N
−
i
−
1
B
~
u
~
(
k
+
i
)
这里其实做了简化,根据
A
A
A的表达式,
A
~
(
k
)
\widetilde A(k)
A
(k)和
A
~
(
k
+
1
)
\widetilde A(k+1)
A
(k+1)不完全等价。
令:
Y
(
k
)
=
[
X
(
k
+
1
)
X
(
k
+
2
)
.
.
.
X
(
k
+
N
)
]
,
Ψ
=
[
A
~
A
~
2
.
.
.
A
~
N
]
,
Δ
U
=
[
u
~
(
k
)
u
~
(
k
+
1
)
.
.
.
u
~
(
k
+
N
−
1
)
]
Θ
=
[
B
~
0
.
.
.
0
A
~
B
~
B
~
.
.
.
0
.
.
.
A
~
N
−
1
B
~
A
~
N
−
2
B
~
.
.
.
B
~
]
Y(k)=
则有:
Y
(
k
)
=
Ψ
X
(
k
)
+
Θ
Δ
U
Y(k)=\Psi X(k) + \Theta \Delta U
Y(k)=ΨX(k)+ΘΔU
其中
Y
(
k
)
Y(k)
Y(k)为
k
k
k时刻预测时域内的误差,定义代价函数:
J
=
Y
T
(
k
)
Q
Y
(
k
)
+
Δ
U
T
R
Δ
U
其中
Q
Q
Q和
R
R
R为权重。该代价函数可以使预测时域内误差最小,且对控制量也有一定约束。对
J
J
J作展开:
J
=
(
Ψ
X
(
k
)
+
Θ
Δ
U
)
T
Q
(
Ψ
X
(
k
)
+
Θ
Δ
U
)
+
Δ
U
T
R
Δ
U
=
X
T
(
k
)
Ψ
T
Q
Ψ
X
(
k
)
+
2
X
T
(
k
)
Ψ
T
Q
Θ
Δ
U
+
Δ
U
T
Θ
T
Q
Θ
Δ
U
+
Δ
U
T
R
Δ
U
=
Δ
U
T
(
Θ
T
Q
Θ
+
R
)
Δ
U
+
2
X
T
(
k
)
Ψ
T
Q
Θ
Δ
U
+
X
T
(
k
)
Ψ
T
Q
Ψ
X
(
k
)
由于在
k
k
k时刻,
X
(
k
)
,
Ψ
,
Θ
X(k),\ \Psi,\ \Theta
X(k), Ψ, Θ均为确定值,所以上式即为关于
Δ
U
\Delta U
ΔU的二次型。
令
H
=
2
(
Θ
T
Q
Θ
+
R
)
,
f
T
=
2
X
T
(
k
)
Ψ
T
Q
Θ
H=2(\Theta ^TQ\Theta+R),\ f^T=2X^T(k)\Psi ^TQ\Theta
H=2(ΘTQΘ+R), fT=2XT(k)ΨTQΘ
即可利用MATLAB中quadprog函数求解。
在得到
Δ
U
∗
\Delta U^*
ΔU∗后,取其中第一个元素
u
~
∗
(
k
)
\widetilde u^*(k)
u
∗(k)作为
k
k
k时刻的MPC系统控制量,则实际速度控制量为:
u
∗
(
k
)
=
u
r
(
k
)
−
u
~
∗
(
k
)
u^*(k)=u_r(k)-\widetilde u^*(k)
u∗(k)=ur(k)−u
∗(k)
clear;clc; close all; %% 仿真参数设定 dt = 0.01; % 离散时间间隔 num_step = 2000; % 仿真步数 t = (0:num_step-1)*dt; % 生成仿真时间序列 %% 运动学参数设定(本实例给定参考速度) u_r = [1;0.5]*ones(1,num_step); % 参考速度(圆轨迹) % u_r = [3;0]*ones(1,num_step); % 参考速度(直线轨迹) % 利用积分法生成参考轨迹 P_r = zeros(3,num_step); for k = 2:num_step last_theta = P_r(3,k-1); P_r(:,k) = P_r(:,k-1)+[cos(last_theta),0;sin(last_theta),0;0,1]*u_r(:,k-1)*dt; end % 设定初始位置和初始速度 P = [1;-1;0]; % 初始位置 u = [0;0]; % 初始速度 X = zeros(3,num_step); % 存储误差向量 %% MPC控制器参数设定 N = 10; % 预测时序长度 Q = 5*diag([4,10,0.1]); % 状态量权重 R = 1*diag([1,0.5]); % 控制量权重 Q_ = kron(eye(N),Q); % 利用K积将权重矩阵扩充到与Y相同维度 R_ = kron(eye(N),R); B = [1,0;0,0;0,1]; % B 矩阵 B_ = dt*B; % B~ 矩阵 %% 二次型求解器设置(使用 interior-point-convex 算法,不显示迭代过程) options = optimoptions('quadprog',... 'Algorithm','interior-point-convex','Display','off'); %% 滚动优化 (为了不处理最后N个时刻的特殊情况,循环只进行到 num_step-N ) tic; figure(1); hold on; xlabel('x(m)'); ylabel('y(m)'); for k = 1:num_step-N % 刻画每个时刻的参考位置和实际位置 plot(P(1,k),P(2,k),'b.'); plot(P_r(1,k),P_r(2,k),'r.'); drawnow; % 计算当前时刻的 X Tx = [cos(P(3,k)),sin(P(3,k)),0; -sin(P(3,k)),cos(P(3,k)),0; 0,0,1]; X(:,k) = Tx*(P_r(:,k)-P(:,k)); % 计算当前时刻的 \Psi 和 \Theta A = [0,u(2,k),0;-u(2,k),0,u_r(1,k);0,0,0]; % A 矩阵 A_ = eye(size(P,1))+dt*A; % A_ 矩阵 Psi = zeros(3*N,3); % 初始化 \Psi Theta = zeros(3*N,2*N); % 初始化 \Theta for j = 1:N Psi(3*j-2:3*j,1:3) = A_^j; % \Psi 的第j块 for i = 1:j Theta(3*j-2:3*j,2*i-1:2*i) = A_^(j-i)*B_; % \Theta 的第(j,i)块 end end % 计算 H 和 f H = 2*(Theta'*Q_*Theta+R_); f = (2*X(:,k)'*Psi'*Q_*Theta)'; % 调用 quadprog 求解器 [U_star,fval,exitflag,output,lambda] = quadprog(H,f,[],[],[],[],[],[],[],options); % 输入控制量利用积分计算下一个位姿和误差 u_star = U_star(1:2); % 取出 U 的第一部分(前两项) u =[u, u_r(:,k) - u_star]; % 计算实际线速度和角速度控制量 last_theta = P(3,k); P =[P, P(:,k)+[cos(last_theta),0;sin(last_theta),0;0,1]*u(:,k+1)*dt]; end toc; %% 误差收敛速度 mse = sqrt(sum((X.^2),1)); figure; plot(t(1:num_step-N),mse(1:num_step-N),'b-','LineWidth',1) xlabel('t(s)'); ylabel('mse');
直线跟踪效果:
圆轨迹跟踪效果:
可以看到收敛速度较慢,有可能是权重影响的,可以自行修改参数进行仿真。另外还可以在quadprog求解器中加入控制量的约束,使其不要产生过大的加速度。
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。