第11讲多旋翼底层控制

第十一讲多旋翼的底层飞行全 自动化科学与电航空航天大年 日北航主川省市都江堰市城西，坐落在平原西部的岷江上，始建于秦昭王(约公元前256～前251)，是蜀郡太守父子人鳖灵开。。都江堰 来源 多旋翼底层控制框架和模型位置控姿态控控制分电机控综合仿飞控原理与总作 多旋翼底层控制框架和模型化位置控制：期望的三维位pd算期望姿态角Θ(滚转、俯仰和偏航d,d,d)以及期望总拉力fd姿态控制：期望姿态角Θd算期望力矩τd控制分配：期望力矩和升力τd,fdn个电机的期望转速

,k1,2,,电机控制：期望转速d,k算电机入油门(电 占空比)

,k1,2,,

图.多旋翼底层控制层 1.多旋翼底层控制框架和模型化底层控fd τd控d姿态控位置控图.多旋翼全自主控制闭环f角Θ），其中内环。 1.多旋翼底层控制框架和模型化a基于第六章的多旋翼非线性模型，忽略bωJb ，apfsinsincossincos

fe

ge3

f

cossincossinsin pgfcos

其中 epp其中

, T 为对地位置，ev为对地速度bωyz eyz3机机体角速度R为旋转矩阵，J为转动惯量矩阵 3 1.多旋翼底层控制框架和模型化假设多旋翼处于悬停状态，即ppd0,dp0,0标称输入给定fmgτ0。则此时有sincos1,sincos1。cossin sin

可以认为Θh是输入进一步，因为gA以得到，所以可以可得到多旋翼的线性模水平通道

输入是gAh。水平道模型为h。水平 ppx,

sin,

Rdiag

其中

p

cos

y

1.多旋翼底层控制框架和模型化高度通道

线性模 vgzm姿态WW

=ω=ω

线性模 多旋翼底层控制框架和模型化原模

水平通道模高度通vg 姿态模=ω 位置控分根据给定目标轨迹pd的不同，可将位置控制分为三类：定点 2.位置控分dd3轨 33路径跟随（PathFollowing）[1]。期望轨pd是一条由参数3到0或者在0足够小的邻域modelinguncertainty.AutomaticControl,IEEETransactionson,2007,52(8):1362–1379. 2.位置控分根据解算出的姿态信息的不同，可将位置控制分为两产生期望欧拉角的位置控制器d,d,d 。产生期望旋转矩阵的位置控制位置控制器最终产生的期望值为旋转矩阵和拉力Rd, 2.位置控（1）传统的PID

gA

p

1hph当考虑定点控制当考虑定点控制 2.位置控产生期望欧拉角的位置控传统的PID mg mg dzd zdp zpzdvg 当考虑定点控制时，d

d

fdmgmkpzd

kpppzpz 2.位置控 k开源自驾仪PID1） ept0,我们先针

limtkvhdKp vhdKp phdphh期望vz期望vz

limt

eωt limtevt

limt0ezt t

limt0eΘtlimt0tlimt0eΘthhlimtepth

t 其中h

vhdlimtlimt0ht2.位置控产生期望欧拉角的位置控制（2）开源自驾仪PID

kk为了使limt t0,我们先针h

lim

limt

et zv

t gA

Kh

hK e

v

vp v

ezt

eΘt g1A1 vhi

t

limt0tdt如果

Θht

t

hhhlimt0epthh那

t

ev t2.位置控产生期望欧拉角的位置控制开源自驾仪PID

kk

limtklimt limt zlimt0eztvzdz

pzpzd

eωt

hlimt0h

limt0eΘtlimt0eΘt

t

t竖直方向速度能达到竖直方向速度能达到期望，高度也就能达到期h

ept 2.位置控产生期望欧拉角的位置控加饱和的PID传统自驾仪

就没意义位置误差很 角度误差很 小角度 设被破坏加加饱和非常 2.位置控产生期望欧拉角的位置控（3）加饱和的PID同样地，在开源自驾仪PID设计中，我们增加对h

和控制器右端的限hsatgdvhvhd,a1 11vhpe vhd,2其中a1a2。保方向饱和函数satgdxa的定义见10下面讨论保方向饱和函数satgdxa与传统的饱和函satxa的区别。虽然饱和函数限satxa的每个分量的绝对值不大于a，但它与x的方向可能不同。而保方向饱和函数satgdx不仅可以限制最终向量每个分量的绝对值不大于a，还可以保证satgdxa的方向与x相同 2.位置控产生期望欧拉角的位置控（3）加饱和的PIDTsatphdph,20 Ty

phd T传统饱和函T

保方向饱和函satgdx,ahp h

satgdphdph,20

导致方向偏图.两种饱和函数的作用效果对satx,a

传统饱和函

, ,satx,a kk

satx,a

asignxk,xk

水平面运动轨 2.位置控产生期望欧拉角的位置控（3）加饱和的PID其中a3。同样地，在开源自PID设计中，我们增加对ev和控制器的右端zz限 satgdvzvzd,a4z限其中a4a5对于一维变量，保方向饱和函数satgd和与传统饱和函数sat的作用相同 2.位置控内外环控制时，我们自然期望位置控制器可以直接给出期望的旋转矩阵Rd。因为旋转矩阵Rd需要直接通过向量进行设计，所以外环的位置控制可直接针对图.位置控制器给出Rd情况下的多旋翼总体控制 2.位置控多旋翼多旋翼位置模针对非线性耦合模型进行如下控制器设gefd 期望的加速，需要加饱其中期望的加速，需要加饱

,a vhpv vhi

vhdv 1 d d

kvzi

kv

,a2 先求解期望的旋转矩阵Rd。由上式可以得

mge3adfd 2.位置控d,3但是为了满足rT 1，我们只需要得到d,3ge3ge3

ge3然而Rd

rd,3，因此还要确定rd,1rd,2中的一个，我们就可以恢出 了。根据旋转矩阵的定义，r3 coscosrcossin 2.位置控因此，我们定义向cosd

d 在姿态角变化不大的情况下，可认为rd,1rd,1。这样rd,1和rd,3可以将rd,2定义

rd,3rd,3根据叉乘的定义，可以知道rd,2rd,3。进一步定义rd,1rd,2rd,3rd,1rd,3rd,1rd,2 2.位置控至此，可得期望的姿态矩阵Rd rd,3其满足RT I。下面进一步考虑期望升力f的计算，因为变量f和

耦合在一起，因此进行如下处理。首gefd

a两边同乘rT，利用rT

1可以得

d,3 mrTgea 如果限定fdfminfmax，那么姿态控制器可以写

ge

fminfmax,fmaxfminfmin gd

3.姿态控多旋翼采用分层控制外层控制器为内层控

kkΘhd或者RdtΘhtΘhdt0

或者

zz

0。不仅如此，一般要

limt

limt0eωt 可以认为姿态控制目标 或RtRt被limt0ezt “瞬间”实现。因此，目标像击鼓传花一样，传给

limt0eΘt态控制了。只要姿态控制被很好地实现水平位

evth

limt0tdt

epth 3.姿态控基本概多旋翼姿态控制是飞行器位置控制的基础。常见的刚体姿态描述方法拉角描述方法和旋转矩阵描表.姿态参数性能对姿态参优缺欧拉旋转 3.姿态控姿态控制的目标是已知参考姿态角Θ T，设计控制器τ使得

et0 d

t 中eΘΘΘd。这里Θhd是由位置控制器给定的，而d是任务规划给定的。为了达到这个目的我们先针设计角速度的期望ωd

ωdωd其中K 是正定的常值对角矩阵，所有元素都大于0。以上两式构成了角度控制环。接来的任务设计转矩的期望τd其中eωωωdKωpKωiKωd

τdτd=KωpeωKωieω。以上两式构成了角速度 3.姿态控基于旋转矩阵的控制器设计望坐标旋转矩阵与实际坐标旋转矩阵定义姿态 误差，并通过 误差设计姿态控制器。d多旋翼姿 控制的控制目标总结为limt

0定义姿 误差

1vexRTRRTR x2

xT

x,vexx

1 0 3.姿态控姿态角速 误差定义eω=ωR小角度假设下ωdΘd为期望的角速度eωω通过计算可得eRd。可以设计如下的PD控制器ττdKReR其中K, 33为正定增益矩阵上面设计的PD控制器只可以在悬停位置的小范围内保证系统 定。为了获得更大范围的稳定性，通过引入误差校正项，设计非线性控制器为τK KeJωRTRωRTRω R ω

略也可以获得满意的性能。但对于大机动飞行中后两项的值很大，对系统性能的影响很大，则必须予以 4.控制分基本概简单来说，控制分配问题可描述为：给定uvt，寻找δt使得uvtg其中g:mn为被控系统中执行机构控制输入到 分配问

uvt

gg上层控制和底层控制分uvtBδt其中Bmn为已知控制效率矩阵

有效分配，防止饱通过分配，提高对故障损伤的鲁棒 4.控制分+字形四旋翼的控制分配和多旋翼的控制分配模型见第6讲f

f

1

1

dcTsin

dcTsin

dcTsin

T 2 x

r 2

02

dc

dcT dcT McM n 3 McM n

r

c

c

4

nr对于四旋翼来说M44可逆，可直接求逆得到控制分配矩阵PM1，其中P

种分配是唯一的。然而，对于旋翼数大于4的多旋翼，分配可以有无穷多种。分配过程中能让某个旋翼的转速达到饱和。因此，好的控制分配算法就十分重要。在开源的自驾仪中，一般的算法是通过求伪逆得到控制分配矩阵，即 其中Pnr4

4nr4.控制分在实际中，通过控制器得到期望拉力fd和期望力矩τd后，进一步再通 d,1Pnr Pnr fdτd d,nr可得到每个螺旋桨的期望转速d,ii12,nr

与参数相在工程实际中，Mn中的参数未知，即cT,cM,d（具体定义参见第六讲未知时r？那这又是如何进行控制分配？ 4.控制分要说明这个问题，首先定义映射矩

sin dcsin dcsin c, ,d

nr

dc dc

dc

cM

r它满其中Padiag

MncT,cM,dPaMn cM。因此，有如下关M†c, ,dM† 已知参

未知参 4.控制分03以六旋翼为例M6cTcMd可以03

1

Mc,c,d

M†

1

13 3

dc dc

6

T T

13 3

cM

于图.常规布局的六旋图.常规布局的六旋

来补偿未知量。

fd

M†1,1,1P-1

aτ aτ

τd

dcT

d,6

5.电机控闭环控来感知转子的整流，利用高频脉宽调制模块 ）来控制电机的电压。下面给出利用电机油门实现电机控制的控制器设计。控制目标为：设计每个电机的输入油门d,k使limtkd,k0。以油作为输入的电机模型见第六章，对应到多旋翼上每个电机的动态模型k b Ck bTms1d,k其中kd,k 5.电机控开环控d,kad,k在开源自驾仪中，电机为开环控制fd,τd之后，期望的每个转子的转速为d,kk12,nr可通过控制分配直接得到，d,kad,k参数ab可以通过位置和姿态控制器中的PID参数来补d图.多旋翼动力系统的开环控制及工作 6.综合仿为了验证算法的有效性，我们利用进行了相关仿真。仿真中采用的四旋翼构型是十字形。它对于X字形构型的四旋翼，除了控制矩阵M4不同表.四旋翼主要模型参g9.8100ms2J0.0095kgm2d0.2223J0.0186kgm2m1.02303.7882e-06kgm21.4865e-07N/rpm280.58402.9250e-09Nm/rpm2b976.20000.0095kgm20.0760 6.综合仿控制目标为：以初始位置作为中心点，从中心点出发在个

4000Trpm,p0

3Tm,v

0TΘ

0T ω

0Tdeg/s.期望的航迹指令由时间序列给表.期望指令时间序时间zd（md（deg000050000000000000 6.综合仿姿态模指令模位置模图.综合控制仿真搭建框下面将给出基于此仿真搭建的两种控制方案的仿真结果基于欧拉角的姿态控制结合基于欧拉角的位置基于旋转矩阵的姿态控制结合基于旋转矩阵的位置 6.综合仿基于欧拉角的姿态控制结合基于欧拉角的位置 PQPQR

0

0ThetaTheta

0PsiPsi

PhiPhi04

004

00

图.航 效果

MotorMotorSpeedVxVyVxVy

VzVz

Motor

0

38.5

55Time55

5XX0

5YY0

ZZ0

3838.5

Motor Time

图.电机油门指令和电机转速响应 6.综合仿基于旋转矩阵的姿态控制结合基于旋转矩阵的位置

PP00PhiPhi0

QQ0ThetaTheta0

0RR0PsiPsi

4

4

图.航 效果MotorThrottleMotorThrottleCommand

MotorMotorSpeedVxVyVxVy

VzVz

Motor

5

5

0

0

Motor

XX

YY

ZZ

Motor

Time

图.电机油门指令和电机转速响 6.综合仿大机动控基于旋转矩阵的控制器相比于基于欧拉角控制器的一大优势就是其可以避免奇异问题，实现全姿态飞行，因此可以实现类似飞行表演中的大机动动基于旋转矩阵实现四旋翼的倒飞翻转。由下图可见，滚转角经历了一个由初始正180度（倒飞）到最终的0度（正飞）的过程。位置经过小幅波动后最终稳定在初始 1

1

2

1

0505 7.飞控原理与调滚转俯仰偏航PID 7.飞控原理与调高度控 7.飞控原理与调MissionPlanner调参 7.飞控原理与调PID控制简PIDProportionalIntegral控制是最早发展起来的控制策略之一，由于其算法简单、鲁棒性好和可靠性高，被广泛应用于工业过程控制，尤其适用于可建立精确数学模型的确定性控制系统。正因为它的简单易用性，因而成为了最早实用化的控制器，应用近百年仍然处于无法取代的位置。连续形式离散形式

utKPetKItkt

ukKPekKIeiKDekek 7.飞控原理与调yr 稳态误yr 稳态误Ot调节时上升时超调eer++y+uy+DKKI图.PID控制框效果指标

图.系统阶跃响应曲)输入y从10%增加到90%所消耗时间)。超调量(Overshoot)：在响应过程中，超出稳态值的最大偏离量与稳态值之比%ymaxy调节时间(SettlingTime)：输出曲线最终收敛于稳态值(5%以内)所用的时间 7.飞控原理与调PID稳定性要求超调量小无震荡，准确性要求稳态误差尽量小。PID调整的主要工作就是如何 I 现 I utKet ted0

KP比例增益。优点：调整系统的开环比例系数，提高系统的稳态精度，加快响应速度点：过大会使系统的超调量增大，而且会使系统稳定裕度变小，甚至不稳定KI积分增益。优点：消除稳态误差；缺点：影响系统的稳定性，使系统的稳定裕度减小。KD微分增益。优点：使系统的响应速度变快，超调减小，振荡减轻，对动态过程有“预测” 7.飞控原理与调PID调节准上升时超调调节时稳态误系统稳定PIKD更形象的说，P—比例控制系统的响应快速性，快速作用于输出，好比“现在”（现在就起用，快），I—积分控制系统的准确性，消除过去的累积误差，好比“过去”（清除过去积，回到准确轨道），微分控制系统的稳定性，具有超前控制作用，好比“（的理解PID的作用。在调整的时候，你所要做的任务就是在系统结构允许的情况下，在这三个参数之间权衡调整，达到最佳控制效果，实现稳快准的控制特点。 7.飞控原理与调PID中积分饱积分饱和(Integral ：如果执行机构已经到极限位置，仍然不能消除偏差时，尽小相。 ar0

r tab

图