【无人机控制】基于线性二次型调节器LQR实现无人机飞行控制附matlab代码
【无人机控制】基于线性二次型调节器LQR实现无人机飞行控制附matlab代码
TT_Matlab
博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，完整matlab代码或者程序定制加qq1575304183。
✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
个人主页： Matlab科研工作室
个人信条：格物致知。
更多Matlab仿真内容点击
智能优化算法 神经网络预测 雷达通信 无线传感器 电力系统
信号处理 图像处理 路径规划 元胞自动机 无人机
⛄ 内容介绍
LQR（Linear Quadratic Regulator）是一种常用的线性二次型调节器，可以用于无人机的飞行控制。LQR方法通过设计一个状态反馈控制器来实现无人机的稳定飞行。下面是基于LQR的实现无人机飞行控制的步骤：
-
系统建模：首先需要建立无人机的动力学模型，包括位置、速度和姿态等状态变量，以及控制输入变量如推力和地形等。该模型通常是一个非线性系统，可以使用线性化技术将其近似为一个线性系统。
-
设计状态反馈控制器：使用LQR方法设计一个状态反馈控制器，该控制器根据当前状态变量的测量值来计算控制输入。LQR方法通过最小化一个成本函数来求解最优的控制器增益矩阵，该成本函数通常包括状态偏差和控制输入的加权和。
-
线性二次型调节器设计：根据系统模型和控制器增益矩阵，可以构造一个线性二次型调节器，该调节器根据当前状态变量的测量值和目标状态变量的参考值来计算控制输入。线性二次型调节器将当前状态与目标状态之间的差异通过控制器增益矩阵进行线性组合，得到最终的控制输入。
-
实施控制策略：将线性二次型调节器应用于无人机的飞行控制系统中，并将其与传感器和执行器进行集成。通过实时测量无人机的状态变量，并计算相应的控制输入，可以实现无人机的稳定飞行。
⛄ 部分代码
%%
declarations
cT
=
1.3529e-06 ;
cQ
=
2.1113*1e-08 ;
l
=
110*1e-3 ;
lp
=
sqrt(2)/2*l ;
m
=
0.536 ;
g
=
9.81 ;
rho_ground
=
1.4 ;
rotor_radius
=
6.0*1e-2 ;
hauteur_rotor
=
29.3*1e-3 ;
Ix
=
0.029125 ;
Iy
=
0.029125 ;
Iz
=
0.055225 ;
Kv_motor
=
1750 ;
Kv_motor_rad
=
Kv_motor*2*pi/60 ;
km
=
1/Kv_motor_rad ;
Jr
=
1.2991*1e-05 ;
R
=
0.07 ;
U_sat
=
12 ;
Mx
=
[ cT cT cT cT ;
-cT*lp
cT*lp cT*lp -cT*lp ;
-cT*lp
-cT*lp cT*lp cT*lp ;
cQ
-cQ cQ -cQ ] ;
%%
Dynamic system
A
=
[0 0 0 1 0 0 0 0 0 0 0 0; %1
0
0 0 0 1 0 0 0 0 0 0 0; %2
0
0 0 0 0 1 0 0 0 0 0 0; %3
0
0 0 0 0 0 0 g 0 0 0 0; %4
0
0 0 0 0 0 -g 0 0 0 0 0; %5
0
0 0 0 0 0 0 0 0 0 0 0; %6
0
0 0 0 0 0 0 0 0 1 0 1; %7
0
0 0 0 0 0 0 0 0 0 1 0; %8
0
0 0 0 0 0 0 0 0 0 1 1; %9
0
0 0 0 0 0 0 0 0 0 (Iz-Iy)/Ix (Iz-Iy)/Ix; %10
0
0 0 0 0 0 0 0 0 (Ix-Iz)/Iy 0 (Ix-Iz)/Iy; %11
0
0 0 0 0 0 0 0 0 (Iy-Ix)/Iz (Iy-Ix)/Iz 0; %12
];
B
=
[ 0 0 0 0;
0
0 0 0;
0
0 0 0;
0
0 0 0;
0
0 0 0;
0
0 0 1/m;
0
0 0 0;
0
0 0 0;
0
0 0 0;
1/Ix
0 0 0;
0
1/Iy 0 0;
0
0 1/Iz 0;
];
helix
=
[];
Xeq
=
[100 100 100 0 0 0 0 0 0 0 0 0];
Xref
=
[100 100 100 0 0 0 ];
%%
Position velocity
Apv
=
[ 0 0 0 1 0 0;
0
0 0 0 1 0;
0
0 0 0 0 1;
0
0 0 0 0 0;
0
0 0 0 0 0;
0
0 0 0 0 0;
];
Bpv
=
[ 0 0 0 0;
0
0 0 0;
0
0 0 0;
0
g 0 0;
-g
0 0 0;
0
0 0 1/m;
];
Qpv
=
[1 0 0 0 0 0;
0
1 0 0 0 0;
0
0 1 0 0 0;
0
0 0 50 0 0;
0
0 0 0 50 0;
0
0 0 0 0 50];
Rpv
=
[20 0 0 0;
0
20 0 0;
0
0 1 0;
0
0 0 1];
[Kpv,S,E]
=
lqr(Apv,Bpv,Qpv,Rpv);
%%
Angle velocity
Aang
=
zeros(3,3);
Bang
=
[1/Ix 0 0;
0
1/Iy 0;
0
0 1/Iz;
];
Qang
=
[1 0 0;
0
1 0;
0
0 1];
Rang
=
[0.05 0 0;
0
0.05 0;
0
0 0.05];
[Kang,S,E]
=
lqr(Aang,Bang,Qang,Rang);
%%
Euler angles
Aeu
=
zeros(3,3);
Beu
=
eye(3);
Qeu
=
[1 0 0;
0
1 0;
0
0 1];
Reu
=
[1 0 0;
0
1 0;
0
0 1];
[Keu,S,E]
=
lqr(Aeu,Beu,Qeu,Reu);
⛄ 运行结果