文章目录
- @[TOC](文章目录)
- 前言
- 一、问题描述
- 二、基于LQR的观测器和控制器设计
- 1.观测器设计
- 2.控制器设计
- 三、数值仿真
- 四、参考文献
- 总结
文章目录
- @[TOC](文章目录)
- 前言
- 一、问题描述
- 二、基于LQR的观测器和控制器设计
- 1.观测器设计
- 2.控制器设计
- 三、数值仿真
- 四、参考文献
- 总结
前言
本文探讨了带有匹配扰动的多智能体领航跟随一致性控制方法,并提供了相应的Matlab仿真代码。
具体的设计步骤如下:
- 将匹配扰动看为系统的扩张状态,设计扩张状态观测器,估计扰动的大小;
- 基于邻居节点间的状态误差设计控制器,并更具扰动估计值,在控制器中补偿扰动;
- 使用使用线性二次型调节器(Linear Quadratic Regulator,LQR)对控制器增益和观测器增益进行了优化设计。
本文所涉及到的关于图论和LQR的基础知识可参考如下博客:
- https://zhuanlan.zhihu.com/p/373435683
- https://zhuanlan.zhihu.com/p/139145957
- https://blog.csdn.net/qq_24649627/article/details/104690279
一、问题描述
多智能体系统由一个领航者和
N
N
N个含有匹配扰动跟随者组成。跟随者动力学模型为:
x
˙
i
=
A
x
i
+
B
(
u
i
+
d
i
)
y
i
=
C
x
i
\begin{array}{l} {{\dot x}_i} = A{x_i} + B({u_i} + {d_i})\\ {y_i} = C{x_i} \end{array}
x˙i=Axi+B(ui+di)yi=Cxi
式中,
i
=
1
,
2
,
.
.
.
,
N
i=1,2,...,N
i=1,2,...,N,
x
i
x_i
xi、
u
i
u_i
ui、
d
i
d_i
di和
y
i
y_i
yi分别是跟随者的状态、控制输入、匹配扰动和系统输出。
领导者动力学模型为:
x
˙
0
=
A
x
0
y
0
=
C
x
0
\begin{array}{c} {{\dot x}_0} = A{x_0}\\ {y_0} = C{x_0} \end{array}
x˙0=Ax0y0=Cx0
式中,
x
0
x_0
x0和
y
0
y_0
y0分别为领航者的状态和系统输出。
二、基于LQR的观测器和控制器设计
1.观测器设计
将扰动
d
i
d_i
di看为跟随者的状态,则模型(1)可重构为:
x
ˉ
˙
i
=
A
ˉ
x
ˉ
i
+
B
ˉ
u
i
+
E
p
i
y
i
=
C
ˉ
x
ˉ
i
\begin{array}{c} {{\dot{\bar x}_i} } = \bar A{{\bar x}_i} + \bar B{u_i} + E{p_i}\\ {y_i} = \bar C{{\bar x}_i} \end{array}
xˉ˙i=Aˉxˉi+Bˉui+Epiyi=Cˉxˉi
式中,
x
ˉ
i
=
[
x
i
d
i
]
T
\bar{x}_i=\left[ x_i \quad d_i \right]^{\rm{T}}
xˉi=[xidi]T,
p
i
=
d
˙
i
p_i=\dot{d}_i
pi=d˙i,
A
ˉ
=
[
A
B
0
0
]
\bar{A}=\left[ \begin{matrix} A & B \\ \textbf{0} & \textbf{0} \end{matrix} \right]
Aˉ=[A0B0],
B
ˉ
=
[
B
0
]
T
\bar{B}=\left[ B \quad \textbf{0} \right]^{\rm{T}}
Bˉ=[B0]T,
C
ˉ
=
[
C
0
]
\bar{C}=\left[ C \quad \textbf{0} \right]
Cˉ=[C0],其中
0
\textbf{0}
0是适度维度的零矩阵。
假设模型(3)中
(
A
ˉ
,
C
ˉ
)
(\bar{A},\bar{C})
(Aˉ,Cˉ)是可观的,可设计一个龙伯格扩张状态观测器为:
x
ˉ
^
˙
i
=
A
ˉ
x
ˉ
^
i
+
B
ˉ
u
i
+
G
(
y
i
−
C
ˉ
x
ˉ
^
i
)
\dot{\hat{\bar{x}}}_i = \bar A{\hat{\bar x} }_i + \bar B{u_i} + G({y_i} - \bar C{\hat{\bar x} }_i)
xˉ^˙i=Aˉxˉ^i+Bˉui+G(yi−Cˉxˉ^i)
式中,
x
ˉ
^
i
=
\hat{\bar{x}}_i=
xˉ^i=是对
x
ˉ
i
\bar{x}_i
xˉi的估计,
G
G
G是观测器增益矩阵。
增益 G G G常采用极点配置法求取,但理想的期望极点很难确定,且该方法无法通过误差和控制输入的优化来选取增益G,因此,可以使用LQR的方法设计 G G G,具体设计方法参考文献【1】。
2.控制器设计
第
i
i
i个跟随者节点与其邻居节点之间的状态误差定义为:
ξ
i
=
∑
j
∈
N
i
(
(
a
i
j
(
x
^
i
−
x
^
j
)
+
b
i
(
x
^
i
−
x
0
)
)
{\xi _i} = \sum\nolimits_{j \in {N_i}} {(({a_{ij}}(\hat{{ x}}_i - {\hat{ x}_j}) + {b_i}({\hat{ x}_i} - {x_0}))}
ξi=∑j∈Ni((aij(x^i−x^j)+bi(x^i−x0))
式中,
N
i
N_i
Ni表示
i
i
i节点邻居节点的集合;
j
j
j节点对
i
i
i节点有信息传递时,
a
i
j
=
1
a_{ij}=1
aij=1,否则
a
i
j
=
0
a_{ij}=0
aij=0;领航者对
i
i
i节点有信息传递时,
b
i
=
0
b_i=0
bi=0,否则
b
i
=
0
b_i=0
bi=0。
假设模型(2)中(A,B)是可控的,根据扩张状态观测器得到的扰动估计值,可设计带扰动补偿的控制器为:
u
i
=
K
ξ
i
−
d
^
i
{u_i} = K{\xi _i} - {\hat d_i}
ui=Kξi−d^i
式中,
K
=
−
τ
K
a
K=-\tau{K_a}
K=−τKa,其中
τ
\tau
τ是一个与多智能体通讯拓扑结构相关的参数,
K
a
K_a
Ka为控制器增益矩阵。
为了提高控制器的性能,同样可以使用LQR方法设计控制器增益
K
a
K_a
Ka,具体设计方法参考文献【2】。
关于控制器和观测器的稳定性,感兴趣的读者可以自行证明。
三、数值仿真
考虑一个二阶领航跟随多智能体系统有1个领航者和4个跟随者,状态
x
i
=
[
x
1
,
i
x
2
,
i
]
T
x_i=\left[ x_{1,i} \quad x_{2,i} \right]^{\rm{T}}
xi=[x1,ix2,i]T,系统矩阵
A
=
[
0
1
0
0
]
A=\left[ \begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix} \right]
A=[0010],
B
=
[
0
1
]
T
B=\left[ 0 \quad 1 \right]^{\rm{T}}
B=[01]T,
C
ˉ
=
[
1
0
]
\bar{C}=\left[ 1 \quad 0 \right]
Cˉ=[10]。领航跟随多智能体系统通讯拓扑图为:
设置外部扰动为:
d
i
=
α
i
t
+
β
i
e
χ
i
t
sin
t
+
γ
i
i
=
1
,
2
⋯
8
{d_i} = {\alpha _i}t + {\beta _i}{e^{{\chi _i}t}}\sin t + {\gamma _i}{\rm{ }}i = 1,2 \cdots 8
di=αit+βieχitsint+γii=1,2⋯8
式中,扰动参数
α
=
[
0.001
−
0.001
0.002
0.001
]
T
\alpha = \left[ 0.001 \quad -0.001 \quad 0.002 \quad 0.001 \right]^{\rm{T}}
α=[0.001−0.0010.0020.001]T,
β
=
[
1
1.5
2
2.5
]
T
\beta= \left[ 1 \quad 1.5 \quad 2 \quad 2.5 \right]^{\rm{T}}
β=[11.522.5]T,
χ
=
[
−
0.3
−
0.3
−
0.5
0.3
]
T
\chi= \left[ -0.3 \quad -0.3 \quad -0.5 \quad 0.3 \right]^{\rm{T}}
χ=[−0.3−0.3−0.50.3]T,
γ
=
[
1
2
−
1
5
]
T
\gamma= \left[ 1 \quad 2 \quad -1 \quad 5 \right]^{\rm{T}}
γ=[12−15]T
设置跟随者初始状态:
x
1
(
0
)
=
[
2
0
4
0
]
T
x
2
(
0
)
=
[
−
1
0
−
1
1
]
T
\begin{align*} x_{1}(0) &= \left[ 2 \quad 0 \quad 4 \quad 0 \right]^{\rm{T}} \\ x_{2}(0) &= \left[ -1 \quad 0 \quad -1 \quad 1 \right]^{\rm{T}} \end{align*}
x1(0)x2(0)=[2040]T=[−10−11]T
设置领航初始状态与运动轨迹为:
x
1
,
0
(
0
)
=
2
x
2
,
0
(
0
)
=
0
x
˙
2
,
0
(
t
)
=
0.2
cos
(
0.15
t
)
\begin{align*} x_{1,0}(0) &= 2 \\ x_{2,0}(0) &= 0 \\ \dot{x}_{2,0}(t)=0.2\cos (0.15t) \end{align*}
x1,0(0)x2,0(0)x˙2,0(t)=0.2cos(0.15t)=2=0
Matlab仿真代码如下所示:
clear
clc
%% 预先设置
global L F K G A B A_bar B_bar D_Mar
%系统矩阵
A = [0 1; 0 0];
B = [0; 1];
C = [1 0];
A_bar = [A B; 0 0 0];
B_bar = [B; 0];
C_bar = [C 0];
%系统通讯拓扑图
L = [2 -1 0 -1
-1 2 -1 0
0 -1 2 -1
-1 0 -1 2];% 拉普拉斯矩阵
F= diag([1 0 0 0]); % 牵引矩阵
%观测器Riccati方程
Qa = diag([15 15 15]);Ra = 0.1;
%控制器Riccati方程
Qb = diag([10 10]);Rb = 0.6;
% LQR方法求解观测器增益
[G,Pa] = lqr(A_bar', C_bar', Qa, Ra);
G = G';
% LQR方法求解控制器增益
[Ka,Pb] = lqr(A, B, Qb, Rb);
tau = 1/(2*min(eig(L+F)));
K = -tau*Ka;
%扰动参数矩阵
D_Mar = [ 0.001 -0.001 0.002 0.001
1 1.5 2 2.5
-0.3 -0.3 -0.5 -0.3
1 2 -1 5];
% 多智能体初始状态设置
X1l = 2;
X2l = 0;
X1f = [2 0 4 0];
X2f = [-1 0 -1 1];
X1hat = X1f; % 跟随者位置状态的初始估计
X2hat = X2f; % 跟随者速度状态的初始估计
Dhat = D_Mar(4,:);% 扰动的初始估计
%时间设置
tBegin = 0;
tFinal = 50;
tspan = [tBegin, tFinal];
%% Calculate ODE Function
In = [X1l X2l X1f X2f X1hat X2hat Dhat]';
[t, X] = ode23(@ctFun, tspan, In);
%% Draw Graphs
% 提取仿真结果
X1l_sim = X(:, 1);
X2l_sim = X(:, 2);
X1f_sim = X(:, 3:6);
X2f_sim = X(:, 7:10);
X1hat_sim = X(:, 11:14);
X2hat_sim = X(:, 15:18);
Dhat_sim = X(:, 19:22);
% 计算真实扰动
% 绘图
% 1-1.绘制领导者与跟随者的状态X1曲线
figure;
hold on
% 定义点画线样式
line_styles = {'--', ':', '-.', '-..'};
% 循环跟随者曲线
for j = 1:4
plot(t, X1f_sim(:, j), line_styles{j}, 'linewidth', 1.5);
end
% 绘制 X1l_sim 曲线
plot(t, X1l_sim, 'r-', 'linewidth', 1);
hold off
% 添加图例
legend('Agent1','Agent2','Agent3','Agent4','Leader', 'FontSize', 16);
xlabel('$t(s)$', 'Interpreter','latex', 'FontSize', 26);
ylabel('$x_{1,i}$','Interpreter','latex', 'FontSize', 26);
% 添加上边框和右边框
box on;
grid on;
% 1-2.绘制领导者与跟随者的状态X2曲线
figure;
hold on
% 定义点画线样式
line_styles = {'--', ':', '-.', '-..'};
% 循环跟随者曲线
for j = 1:4
plot(t, X2f_sim(:, j), line_styles{j}, 'linewidth', 1.5);
end
% 绘制 X2l_sim 曲线
plot(t, X2l_sim, 'r-', 'linewidth', 1);
hold off
set(gca, 'FontSize', 16); % 修改全局图的坐标数字字号为 16
% 添加图例
legend('Agent1','Agent2','Agent3','Agent4','Leader', 'FontSize', 16);
xlabel('$t(s)$', 'Interpreter','latex', 'FontSize', 26);
ylabel('$x_{2,i}$','Interpreter','latex', 'FontSize', 26);
% 添加上边框和右边框
box on;
grid on;
%绘制扰动与扰动观测值
% 计算真实扰动
Disturbance_true = zeros(size(Dhat_sim));
for i = 1:length(t)
for j = 1:4
Disturbance_true(i,j) = D_Mar(1,j)*t(i)+ D_Mar(2,j) * exp(D_Mar(3,j)*t(i))*sin (t(i)) + D_Mar(4,j);
end
end
% 绘制扰动估计对比
for i = 1:4
figure;
plot(t, Disturbance_true(:,i), 'color', 'k', 'linewidth', 1.5);
hold on;
plot(t, Dhat_sim(:,i), '--', 'linewidth', 1.5);
hold off;
% 动态生成图例文本
legend(sprintf('$d_%d$', i), sprintf('$\\hat{d}_%d$', i), 'Interpreter', 'latex', 'Location', 'northeast', 'FontSize', 16);
set(gca, 'FontSize', 16); % 修改全局图的坐标数字字号为 16
xlabel('$t(s)$', 'Interpreter', 'latex', 'FontSize', 26);
ylabel(sprintf('$d_%d(t)$', i), 'Interpreter', 'latex', 'FontSize', 26);
grid on;
end
%% ODE Function
function dX = ctFun(t,In)
global L F K G A B A_bar B_bar D_Mar
% 提取状态
X1l = In(1);
X2l = In(2);
X1f = In(3:6);
X2f = In(7:10);
X1hat = In(11:14);
X2hat = In(15:18);
Dhat = In(19:22);
% 初始化Disturbance为列向量
Disturbance = zeros(length(X1f), 1);
% 设置外部扰动
for j = 1:4
Disturbance(j) = D_Mar(1,j)*t + D_Mar(2,j) * exp(D_Mar(3,j)*t)*sin(t) + D_Mar(4,j);
end
% 领导者动力学
dX1l = X2l;
dX2l = 0.2*cos(0.15*t) ;
% 跟随者动力学
X1_Bar = X1hat - X1l; X2_Bar = X2hat - X2l;
dX1f = A(1,2)*X2f;
u = K(1)*(L+F) * X1_Bar + K(2)*(L+F) * X2_Bar - Dhat;%加了扰动补偿
dX2f = B(2,1)*u + B(2,1)*Disturbance;
% 扩张状态观测器
dX1hat = A_bar(1,2)*X2hat + G(1) * (X1f - X1hat);
dX2hat = A_bar(2,3)*Dhat + B_bar(2,1)*u + G(2)*(X1f - X1hat);
dDhat = G(3) * (X1f - X1hat);
% output
dX = [dX1l; dX2l; dX1f; dX2f; dX1hat; dX2hat; dDhat];
end
仿真结果:
由上图片可知,四个跟随者可以跟踪上领航者的运动状态。修改Matlab中LQR权重矩阵Qb和Rb,可改变一致性跟踪效果。
四、参考文献
【1】费红姿, 刘冰鑫, 柳一林, 等. 基于LQR的高压共轨系统喷油量观测器设计[J]. 内燃机学报, 2023, 41(03): 247-254.
【2】高琳琳, 唐风敏, 郭蓬, 等. 自动驾驶横向运动控制的改进LQR方法研究[J]. 机械科学与技术, 2021, 40(03): 435-441.
总结
我的个人站:JosephWen (rovn.ink)