含匹配扰动的多智能体领航跟随一致性Matlab仿真

文章目录

• • @[TOC](文章目录)
• 前言
• 一、问题描述
• 二、基于LQR的观测器和控制器设计
• • 1.观测器设计
  • 2.控制器设计
• 三、数值仿真
• 四、参考文献
• 总结

前言

​本文探讨了带有匹配扰动的多智能体领航跟随一致性控制方法，并提供了相应的Matlab仿真代码。

具体的设计步骤如下：

1. 将匹配扰动看为系统的扩张状态，设计扩张状态观测器，估计扰动的大小；
2. 基于邻居节点间的状态误差设计控制器，并更具扰动估计值，在控制器中补偿扰动；
3. 使用使用线性二次型调节器（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$个含有匹配扰动跟随者组成。跟随者动力学模型为：
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$，$x_i$、$u_i$、$d_i$和$y_i$分别是跟随者的状态、控制输入、匹配扰动和系统输出。

​ 领导者动力学模型为：
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​=Ax0​y0​=Cx0​​
式中，$x_0$和$y_0$分别为领航者的状态和系统输出。

二、基于LQR的观测器和控制器设计

1.观测器设计

将扰动$d_i$看为跟随者的状态，则模型(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​+Epi​yi​=Cˉxˉi​​
式中，${\bar{x}}_i={\left[x_i{d}_i\right]}^{\mathrm{T}}$，$p_i={\dot{d}}_i$，$\bar{A}=\left[\begin{array}{cc}A& B\\ \text{0}& \text{0}\end{array}\right]$，$\bar{B}={\left[B\text{0}\right]}^{\mathrm{T}}$，$\bar{C}=\left[C\text{0}\right]$，其中$\text{0}$是适度维度的零矩阵。
假设模型(3)中$(\bar{A},\bar{C})$是可观的，可设计一个龙伯格扩张状态观测器为：
$${\dot{\hat{\bar{x}}}}_i=\bar{A}{\hat{\bar{x}}}_i+\bar{B}{u}_i+G(y_i-\bar{C}{\hat{\bar{x}}}_i)$$
式中，${\hat{\bar{x}}}_i=$是对${\bar{x}}_i$的估计，$G$是观测器增益矩阵。

​ 增益$G$常采用极点配置法求取，但理想的期望极点很难确定，且该方法无法通过误差和控制输入的优化来选取增益G，因此，可以使用LQR的方法设计$G$，具体设计方法参考文献【1】。

2.控制器设计

​ 第$i$个跟随者节点与其邻居节点之间的状态误差定义为：
$${\xi }_i={\sum }_{j\in N_i}((a_{ij}({\hat{x}}_i-{\hat{x}}_j)+b_i({\hat{x}}_i-x_0))$$
式中，$N_i$表示$i$节点邻居节点的集合；$j$节点对$i$节点有信息传递时，$a_{ij}=1$，否则$a_{ij}=0$；领航者对$i$节点有信息传递时，$b_i=0$，否则$b_i=0$。

​ 假设模型(2)中(A,B)是可控的，根据扩张状态观测器得到的扰动估计值，可设计带扰动补偿的控制器为：
$$u_i=K{\xi }_i-{\hat{d}}_i$$
式中，$K=-\tau K_a$，其中$\tau$是一个与多智能体通讯拓扑结构相关的参数，$K_a$为控制器增益矩阵。

​ 为了提高控制器的性能，同样可以使用LQR方法设计控制器增益$K_a$，具体设计方法参考文献【2】。
关于控制器和观测器的稳定性，感兴趣的读者可以自行证明。

三、数值仿真

考虑一个二阶领航跟随多智能体系统有1个领航者和4个跟随者，状态$x_i={\left[x_{1,i}{x}_{2,i}\right]}^{\mathrm{T}}$，系统矩阵 A = [ 0 1 0 0 ] A=\left[

$$\begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix}$$ \right] A=[00​10​]，$B={\left[01\right]}^{\mathrm{T}}$，$\bar{C}=\left[10\right]$。领航跟随多智能体系统通讯拓扑图为：

设置外部扰动为：
$$d_i={\alpha }_{i}t+{\beta }_i{e}^{{\chi }_{i}t}\sin t+{\gamma }_{i}i=1,2\cdots 8$$
式中，扰动参数$\alpha ={\left[0.001-0.0010.0020.001\right]}^{\mathrm{T}}$，$\beta ={\left[11.522.5\right]}^{\mathrm{T}}$，$\chi ={\left[-0.3-0.3-0.50.3\right]}^{\mathrm{T}}$，$\gamma ={\left[12-15\right]}^{\mathrm{T}}$
设置跟随者初始状态：
$$\begin{array}{rl}{x}_1(0)& ={\left[2040\right]}^{\mathrm{T}}\\ x_2(0)& ={\left[-10-11\right]}^{\mathrm{T}}\end{array}$$
设置领航初始状态与运动轨迹为：
$$\begin{array}{rl}{x}_{1,0}(0)& =2\\ x_{2,0}(0)& =0\\ {\dot{x}}_{2,0}(t)=0.2\cos (0.15t)& \end{array}$$
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，可改变一致性跟踪效果。

上面是扰动的真实值和观测值，修改Matlab中LQR权重矩阵Qa和Ra，可改变状态估计结果。

四、参考文献

【1】费红姿, 刘冰鑫, 柳一林, 等. 基于LQR的高压共轨系统喷油量观测器设计[J]. 内燃机学报, 2023, 41(03): 247-254.

【2】高琳琳, 唐风敏, 郭蓬, 等. 自动驾驶横向运动控制的改进LQR方法研究[J]. 机械科学与技术, 2021, 40(03): 435-441.

总结

我的个人站：JosephWen (rovn.ink)