智能飞行器的PID控制技术

智能飞行器的研究
一、序言（问题的提出、研究的动机）。

二、理论依据。

三、研究目标。

四、研究方法（采用哪些教育科研方法）。

五、研究的主要内容。

1。

PID 算法在四轴飞行器中的应用
摘要：本文讨论了如何将PID 算法应用到四轴飞行器上，使之保持机身平衡与稳定。

关键词：PID 算法、四轴飞行器
（1） 问题的提出：
PID （比例、积分、微分）控制器作为最早实用化的控制器已有 50多年的历史，现在仍然是应用最广泛的工业控制器。

PID 控制器简单易懂，使用中不需精确的系统模型等先决条件，因而成为应用最广泛的控制器。

如何将PID 算法应用到PID 控制器中，使其在陀螺仪的配合下调整电机转速而达到飞行器的平稳飞行。

（2） 问题的分析：
我们都知道PID 算法可以表示为：
])()(1)([)(⎰++=dt
t de TI dt t e TI t e kp t u
其中u(t)为输出，e(t)为输入，kp为比例系数，四轴的飞行模式有两种，一种是“+”模式，另一种是"
"⨯模式，它
"⨯模式，我们打算用"
更加灵活。

要将PID算法运用到四轴飞行器中，就要搞清楚四轴飞行器中的什么数据作为PID算法的输入，什么作为PID算法的输出。

因为我们是为了稳定飞行器，所以将角速度的数值作为输入是比较合适的，所以我们要装一个陀螺仪芯片，实时监测角速度的变化。

PID算法中有三个参数：kp,TI,TD。

这三个参数应该根据飞行器具体情况来设定。

得到输出u(t)后，怎样处理u(t)是关键。

四轴有四个电机，对应4个转速，每一个转速棵对应1个PPM值。

电机的转速直接影响陀螺仪的值。

我们的目的就是要通过PID的反馈确定PPM 的合适的增量让陀螺仪示数稳定到基准值。

怎样处理u(t)呢？由于我们用的是"
"⨯模式，如下图
1和4是逆时针旋转，2和3是顺时针旋转。

陀螺仪输出X、Y、Z、三轴的角速度W1、W2、W3。

当将W1输入到PID中，得到一个结果。

如果它大于基准值，我们应将2和4的转速调大，1和3的转速
调小，使X轴方向的角速度趋近于基准值从而达到稳定。

同样的，Y 轴的结果如果大于基准值，我们应将1和2的速度调大，3和4的速度调小，使Y轴方向的角速度趋近于它的基准值。

Z轴的结果如果大于基准值，就要将1和4的速度调大，2和3的速度调小，靠扭转力使它达到稳定状态。

这就是我们设想的使飞行器达到稳定的原理，要具体实现它，我们要建立具体的模型。

（3）模型假设及说明
1.电调PWM分辨率是8位的。

2.PPM信号从0-499。

3.X轴正方向为飞行器前进方向，Z轴方向为飞行器垂直向上的方向。

4.轴的角速度的方向正负判定基于右手螺旋法则。

说明：
1.假设1是限定电机转速级数即256级，简化模型的建立。

2.假设2是确定PPM脉宽的级数，简化模型的建立。

（4）文中用到的符号及说明
Xi(i) 第i次采集时X轴角速度对时间的积分
Yi(i) 第i次采集时Y轴角速度对时间的积分
Zi(i) 第i次采集时Z轴角速度对时间的积分
Xd(i) 第i次采集时X轴角速度对时间的导数
Yd(i) 第i次采集时Y轴角速度对时间的导数
Zd(i) 第i次采集时Z轴角速度对时间的导数
X(i) 第i次采集后，通过PID算法得到的X轴上的PID增量Y(i) 第i次采集后，通过PID算法得到的Y轴上的PID增量Z(i) 第i次采集后，通过PID算法得到的Z轴上的PID增量P(x) 关于X轴的比例参数
I(x) 关于X轴的积分参数
D(x) 关于X轴的微分参数
P(y) 关于Y轴的比例参数
I(y) 关于Y轴的积分参数
D(y) 关于Y轴的微分参数
P(z) 关于Z轴的比例参数
I(z) 关于Z轴的积分参数
D(z) 关于Z轴的微分参数
T 采集的时间间隔
thr(i) 第i次采集时的油门命令信号
rol(i) 第i次采集时的滚转命令信号
pit(i) 第i次采集时的俯仰命令信号
yaw(i) 第i次采集时的航向命令信号
Xrol(i) 第i次采集后的滚转输出信号
Ypit(i) 第i次采集后的俯仰输出信号
Zyaw(i) 第i次采集后的航向输出信号
（5）模型的建立及求解
1）积分模型建立
由于对陀螺仪的采样是离散的，所以角速度对时间的积分不能用传统的方式求解，必须找到一种合适的近似算法。

数值积分我们可以用到“梯形法”：将[a,b]划分为若干小区间b x x x a n =<<<=...10,则 ⎰∑⎰=-==b a n i x x i i dx x f dx x f I 11)()(
在每一小区间],[1i i x x -上f(x)近似为一直线，用弦代替，有
))()((2
)(111i i x x i i x f x f x x dx x f i
i +-=--⎰- 从而 ))()((211
1i i n i i i x f x f x x I +-=-=-∑ 将梯形法用于我们这个模型，则
)()1((2
1)1()(i Xp i Xp i Xi i Xi +-+-= 规定Xp(0)=Xi(0)=0,Y 轴、Z 轴同X 轴。

2)微分模型建立
我们用后向差分：
1
1')
()()(----=k k k k k x x x f x f x f 将它用于我们的模型，则
t
i Xp i Xp i Xd )1()()(--=。

Y 轴、Z 轴同X 轴。

根据X(i)=P(x)*Xp(i)+I(x)*Xi(i)+D(x)*Xd(i)可得到PID 增量X(i)。

Y(i)、Z(i)由同样方法得出。

3)PID 增量转化为PPM 信号
由于我们用的是 ""⨯模式，那么每个轴的角速度和角加速度是由四个电机转速共同决定的。

根据每个电机转速对飞行的影响不同，我们可以列出这样的式子：
]499,499[,,],499,0[)()
()()()()(4)
()()()()(3)
()()()()(2)
()()()()(1)
()()()
()()()
()()(+-∈∈++-=+++=---=+-+=-=-=-=Zyaw Ypit Xrol i thr i Zyaw i Ypit i Xrol i thr i m i Zyaw i Ypit i Xrol i thr i m i Zyaw i Ypit i Xrol i thr i m i Zyaw i Ypit i Xrol i thr i m i Z i yaw i Zyaw i Y i pit i Ypit i X i rol i Xrol 其中，
这得到的4个数值要经过限幅和取整才能成为PPM 信号。

我们选择用四舍五入的方法将它变换成整数。

⎪⎭
⎪⎬⎫⎪⎩⎪⎨⎧>≤≤+<=4991,49949910],5.01[01,01m m m m m m2,m3,m4的计算方法同m1。

2。

四轴飞行器的捷联惯性导航
摘要：本文讨论了在四轴飞行器上利用惯性导航的方法确定飞行
器的位置，陀螺仪输入角速度数据，加速度计输入加速度数据，利用这两个数据就能求得飞行器的速度、位置及姿态。

关键词：惯性导航、地球坐标系
（1）问题的提出：
加速度的两次积分就是位移，所以如果知道加速度随时间的函数，初始速度和初始位移，就能通过两次积分得到位移随时间的函数。

而捷联式惯性导航是在计算机内形成一个数学解析平台来代替平台式惯性导航里的稳定平台。

那么如何在计算机内建立数学模型来构建这个平台呢？如何通过加速度计和陀螺仪对飞行器进行定位？如何对障碍物进行定位？
（2）问题的分析：
捷联惯性导航需要知道5种坐标系，分别是地心坐标系、地球坐标系、地理坐标系、导航坐标系、运载体坐标系。

地心坐标系可以近似地认为是惯性坐标系；地球坐标系纵轴通过南北极，与地球固连；运载体坐标系与飞行器固连。

我们所建立的运载体坐标系是：
b x 轴沿运载体横轴指右
b y 轴沿运载体纵轴指前
b z 轴与b x 、b y 构成右手直角坐标系
航向角Heading(ψ)
俯仰角Pitch(ϑ)
滚转角Roll(γ)
地理坐标系：原点在运载体重心，t x 指东，t y 指北
t z 指天。

（3）模型假设及说明
1.地心坐标系近似看做惯性坐标系
2.将地球看做旋转椭球体，R=6367.65km,短半轴6356.863km ，长半轴6378.245km 。

（4） 模型的建立及求解
现在来求运载体坐标系与地理坐标系的转换，及运载体坐
标系转地理坐标系的矩阵o r c 和地理坐标系转运载体坐标系矩阵r o c 。

⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡----+-+⎥⎥⎥⎦
⎤⎢⎢⎢⎣⎡-∙⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡-∙⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡-==ϑγψϑγψγψϑγψγϑψϑψϑϑγψϑγψγψϑγψγψψψψϑϑϑϑγγγγcos cos cos sin cos sin sin sin sin cos cos sin sin cos cos sin cos cos sin cos sin sin sin cos sin sin sin cos cos 1000cos sin 0sin cos cos sin 0sin cos 0001cos 0sin 010sin 0cos a o b a r b r o c c c c
⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡-++-+-+=γϑϑγϑγϑψγψϑψγϑψγψγϑψγψϑψγϑψγψcos cos sin sin cos cos sin cos sin sin cos cos sin sin cos cos sin cos sin sin sin cos cos sin sin sin sin cos cos o r c
地理坐标系相对惯性参考系转动角速度应包括：一是地理坐标系
相对地球坐标系的转动角速度；另一个是地球坐标系相对惯性坐标系的转动角速度。

以水平航行情况讨论：设运载体高度h,维度ϕ,速度v,航向角ψ。

ψψ
sin cos v v v v E N ==
N v 引起地理坐标系绕东西方向地心轴相对地球坐标系转动
h R v h R v N +=+=∙
ψϕcos E v 引起绕极轴转动
ϕϕϕλcos )(sin cos )(h R v h R v E +=+=∙
地球坐标系相对惯性参考系转动是地球自转引起 ϕϕ
sin cos 0
ie t iez ie t iey t iex w w w w w ===
综合考虑自转和航行速度
ϕψϕψϕϕtg h
R v w w h
R v w w h R v w ie t itz ie t
ity t
itx
++=++=+-=sin sin sin cos cos
→
→
→
→
→
→→
→
→→⨯+=+=+=r
w dt
r d dt
r d dt
r d dt R d dt R d r
R R ie e
i
i
i
i
00
运载体绝对速度：
→→→
→
→
⨯++=r w dt r
d dt R d dt R d i
e e
i
i
i
dt
R d →
0地球公转引起的地心相对惯性空间的速度，运载体牵连速度的一部
分。

→
→⨯r
w ie 自转引起的牵连点相对惯性空间的速度，牵连速度又一部分。

对前式求一阶导数:
→
→
→
→→
→→
→
→
⨯+⨯+⨯++=r dt
w d dt r d w dt r d w dt r d dt R d dt R d i
ie i
ie e
ie e
i
i
222220222 而
0)
(=⨯⨯+⨯
=⨯→
→
→→→
→→→
i
ie
ie ie e
ie i
ie dt
w d r w w dt
r d w dt r d w
则
)(222
2022
2
→→→→
→→
→
→
⨯⨯+⨯++=r w w dt
r
d w dt r d dt R d dt R d i
e ie e
ie e i
i
e
ie dt
r d w →
→
⨯
2哥氏加速度
现在讨论加速度计测量加速度：测量满足比例方程ma kx A =，由于引力
的影响
mG kx G =,延同一轴向的a 矢量与G 矢量引起质量块位移方向相反。

)
(G a k m
x -=，
G a f -=为比力，比力大小与弹簧变形量成正比，加速
度计输出电压大小与弹簧变形量成正比。

∑-=+++=31
n i i
s m e G G G G G
)
()(231
22202∑-=→→→→
→
→
+++-⨯⨯+⨯++=n i i s m e ie ie e
ie e
i
G G G G r w w dt r d w dt r d dt R d f
一般公转引起的向心加速度20
2dt R d 与s G 大致相等，故
02
2≈-s G dt R d
对于一般精度惯性系统，月球及其他星球引力可忽略。

e
ie ie e
ie e
G r w w dt
r
d w dt r d f -⨯⨯+⨯+=→→→→
→)(222
e ie ie e dt
r d v r w w G g →
→
→
→
→
=
⨯⨯-=),(
g
v w dt
v d f ie e -⨯+=→→→
2
安装加速度计的测量坐标系为p 系，相对地球坐标系转动角度为
→
ep
w ，则
→→→
→⨯+=v w dt
v
d dt v d ep p
e
g
v w v w dt
v d f ie ep p
-⨯+⨯+=→→→→→
2
令
g
v w w a ie ep B -⨯+=→
→
→
)2(，则
→∙
→
→
=-v a f B ，→
B a 即有害加速度
捷联式惯性导航的程序见附录一。

3。

基于节能原则的最佳飞行轨迹 4。

按轨迹飞行的控制策略
六、研究过程概述。

七、研究成果（概括性描述、列出图表、研究假设的检验结果）、结论与建议。

八、存在的问题或研究的局限性。

附录一：%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 指北方位捷联式惯性导航系统 %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%初始化
clear
%定义常量
weix=zeros(1,4800);
weiy=zeros(1,4800);
Re=6378245; e=1/298.3;
wie_scalar=7.292115147e-5; %自转角速度 RAD/S
g0=9.7803267714;
gk1=0.00193185138639;
gk2=0.00669437999013;
%变量赋初值
longitude=[116.344695283*pi/180 zeros(1,47999)];
latitude=[39.975172*pi/180 zeros(1,47999)];
h=30;
f=zeros(3,48000);
wnb=zeros(3,48000);
theta=[0.120992605 zeros(1,47999)]*pi/180;
gamma=[0.010445947 zeros(1,47999)]*pi/180;
psi=[360-91.637207 zeros(1,47999)]*pi/180;
Q=[cos(psi(1)/2)*cos(theta(1)/2)*cos(gamma(1)/2)+sin(psi(1)/2)*sin(th eta(1)/2)*sin(gamma(1)/2);
cos(psi(1)/2)*sin(theta(1)/2)*cos(gamma(1)/2)+sin(psi(1)/2)*cos(theta (1)/2)*sin(gamma(1)/2);
cos(psi(1)/2)*cos(theta(1)/2)*sin(gamma(1)/2)-sin(psi(1)/2)*sin(theta (1)/2)*cos(gamma(1)/2);
cos(psi(1)/2)*sin(theta(1)/2)*sin(gamma(1)/2)-sin(psi(1)/2)*cos(theta (1)/2)*cos(gamma(1)/2)];
velocity=[0.000048637 0.000206947 0.007106781];
v=[zeros(1,48000)];
%%%%%%%%%%%%%%
jiao=double(zeros(3,1));
ser_port1=serial('COM2') ; %创建一个对象
ser_port1.InputBufferSize=4096; %输入缓冲大小
ser_port1.OutputBufferSize=4096; %输出缓冲大小
ser_port1.Timeout=30; %限时S
ser_port1.BaudRate=9600; %波特率
ser_port1.Parity='even'; %偶校验
ser_port1.StopBits=1; %一个停止位
fopen(ser_port1);
%%%%%%%%%%%%%%
%%%%%%%%%%%%%%
jia=double(zeros(3,1));
ser_port2=serial('COM4') ; %创建一个对象
ser_port2.InputBufferSize=4096; %输入缓冲大小
ser_port2.OutputBufferSize=4096; %输出缓冲大小
ser_port2.Timeout=30; %限时S
ser_port2.BaudRate=9600; %波特率
ser_port2.Parity='even'; %偶校验
ser_port2.StopBits=1; %一个停止位
fopen(ser_port2);
%%%%%%%%%%%%%%。