四轴实用教程

1、 小四轴飞行器制作过程电子1201 俞强 吴文杰 注：本文只讲我们做四轴遇到的实际的真正困扰我们的一些问题以及我们自身的经验，可以结合我们的报告来看，对于前人的经验以及我们觉得对我们有帮助的文章或帖子以及一些理论知识将在每一部分的讲解中给出，对于理论，本文尽量少讲甚至不讲，建议在看本文的每个部分的时候，写把给出的参考资料阅读一番。一飞行器大体结构1.飞行控制电路（飞控），稍后介绍，是飞行器设计的最关键部分。2.电机+桨：720空心杯电机，注意不人为要阻止电机转动，很容易损坏。720表示电机直径：7 MM ；电机长度：20 MM。根据飞行器的具体大小以及飞行测试的实际情况自行选择其他规格电机。桨

2、要注意有正桨和反桨的区别，见下图：注意：正桨逆时针转动，反桨逆时针转动。如果不确定，可以在安装好电机后，开电源看看是不是四个桨风都是向下吹的。对于相同的电机，桨的大小不同，飞行效果也不一样，小桨效率低，但抗风性能好。大桨效率高（就是省电），但抗风性能差。3.电池： 400mah，3.7v，20c锂电池基本知识:1) 1000mAh的意思是指充满电的情况下用1安培的电流放电, 可以放电一个小时.400mAh的电池在在1安培电流充放电情况下, 充放电时间大约是25分钟. 如果采用4安培的电流放电, 放电时间大约在6分钟(25/4)左右.2) 20c指放电倍率。放电倍率指的是放电电流, 以电池容量的

3、倍数计算. 上述电池的放电电流可根据下面的公式计算:400mAhX25C=10A。4. X 型四轴和 + 型四轴电机安放的区别：其实是要注意传感器mpu6050和电机的放置，因为传感器是有轴向的，图中大框为整块飞控，小框为mpu6050，飞行方向为上下左右。X型四轴飞控和电机臂的安放：+ 型四轴：二飞控电路1.飞控电路主要器件:1）mcu的选择：对于小四轴mcu实际上选择很多，只要满足：最少4路PWM,尽量有最少一路AD转换，尽量有硬件spi和i2c接口，频率够快即可。如avr， msp430，stm32等。还有人用stc产的增强型51单片机也做出来了，可以去看看2）mpu6050：mpu60

4、50集成了三轴加速度计和三轴陀螺仪，使用起来更方便，轴向融合度更高。下面先来介绍陀螺仪和加速度计（可参考MPU6050原始数据分析）。陀螺仪：陀螺仪的作用是测量载体绕x，y，z三个轴转动的角速度；角速度对时间积分就可以得到绕三个轴转过的角度。零点漂移：到飞机静止不动时，读取陀螺仪的数据，发现绕三个轴旋转的角速度不为0。由于零点漂移的存在，假设x轴的零漂为1度，积分360次后，累积的误差为360度，显然这是不合理的，因此传感器的读数需要进行处理。加速度计：测量的是物体的加速度，当物体静止或做匀速运动时，加速度计的输出是重力加速度在x，y，z三个轴上的分量。当飞机做加速运动时，此时，显然无法正确测

5、量重力加速度。因此加速度计的读数也需要处理。下面介绍mpu6050：MPU-60X0 是全球首例 9 轴运动处理传感器。它集成了 3 轴 MEMS 陀螺仪，3 轴 MEMS加速度计，以及一个可扩展的数字运动处理器 DMP（Digital Motion Processor），可用 I2C接口连接一个第三方的数字传感器，比如磁力计。扩展之后就可以通过其 I2C 或 SPI 接口输出一个 9 轴的信号（SPI 接口仅在 MPU-6000 可用）。MPU-60X0 也可以通过其 I2C 接口连接非惯性的数字传感器，比如压力传感器。MPU-60X0 对陀螺仪和加速度计分别用了三个 16 位的 ADC，将

6、其测量的模拟量转化为可输出的数字量。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的，陀螺仪可测范围为±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为±2，±4，±8，±16g。由于我们不使用它的dmp功能（该功能可以直接输出飞行器的姿态数据），因此，我们简单介绍几个常用的寄存器，详细说明请看芯片手册和寄存器手册：1.REGISTER 26 CONFIGURATION。EXT_SYNC_SET的值确定采样的值将代替传感器数据寄存器中的最低有效位。替换如下表所示：D

7、LPF(数字低通滤波器)由DLPF_CFG 配置。加速度计和陀螺仪根据DLPF_CFG 的值被过滤。下表显示过滤情况：2. REGISTER 25SAMPLE RATE DIVIDER。该寄存器用于 MPU-60X0的陀螺仪采样频率输出设置。传感器寄存器输出，FITO输出，DMP采样，Motion检测，ZeroMotion检测和 Free Fall检测都基于这个采样频率。配置后SMPLRT_DIV=0。采样频率=（陀螺仪或加速度计等的）输出频率/（1+SMPLRT_DIV）。3 REGISTER 27 GYROSCOPE CONFIGURATION这寄存器是用来触发陀螺仪自检和配置陀螺仪的满量

8、程范围。量程设为±2000度/s,即FS_SEL=3。XG_ST 该位置位 X轴进行自检。YG_ST 该位置位 Y轴进行自检。ZG_ST 该位置位 Z轴进行自检。FS_SEL 2位无符号数值.。选择陀螺仪的满量程范围。4 REGISTER 28ACCELEROMETER CONFIGURATION这寄存器是用来触发加速度计自检和配置加速度计的满量程范围。这个寄存器也可以用于配置数字高通滤波器（DHPF）。量程设为±8g，即AFS_SEL=2.AFS_SEL 用于选择加速度计的满量程范围，如下表：XA_ST 该位置 1，加速度计的 X轴执行自检。YA_ST 该位置 1，加速度

9、计的Y轴执行自检。ZA_ST 该位置 1，加速度计的 Z轴执行自检。AFS_SEL 2位无符号值。 选择加速度计的满量程范围。5.REGISTER 107 POWERMANAGEMENT 1。这个寄存器允许用户配置电源模式和时钟源。配置后唤醒mpu6000（设备刚上电处于睡眠模式，因此需要唤醒）。时钟选择内部时钟源的PLL with Z axis gyroscope reference。DEVICE_RESET=0， 该位置 1，重启内部寄存器到默认值。复位完成后该位自动清0。SLEEP=0， 该位置 1，MPU-60X0进入睡眠模式。CYCLE 当失能 SLEEP且 CYCLE位置 1，MP

10、U-60X0进入循环模式。在循环模式，设备在睡眠模式和唤醒模式间循环，根据 LP_WAKE_CTRL（寄存器 108）设定的速率从加速度计采集样品数据。TEMP_DIS=0， 该位置 1，失能温度传感器。CLKSEL =3，3位无符号数值。指定设备的系统时钟源。7.REGISTER 106 USER CONTROL这个寄存器允许用户使能或失能 FIFO缓冲区，I2C主机模式和主要 I2C接口。FIFO缓冲区，I2C主机，传感器信号通道和传感器寄存器也可以使用这个寄存器复位。当 I2C_MST_EN置 1，I2C主机模式使能。在这个模式下，MPU-60X0作为 I2C主机通过辅助I2C总线连接外

11、部传感器从机设备。当该位清0，辅助I2C总线（AUX_DA 和AUX_CL）逻辑上由主 I2C总线（SDA和 SCL）驱动。FIFO_EN=0，失能 FIFO缓冲区。 FIFO缓冲区不能读取或写入。FIFO缓冲区状态不能改变除非 MPU-60X0重新上电。I2C_MST_EN=0，辅助 I2C总线（AUX_DA 和 AUX_CL）逻辑上由主 I2C总线（SDA和 SCL）驱动。I2C_IF_DIS=1，MPU-6000:该位置 1，失能主 I2C接口并使能 SPI接口。FIFO_RESET=0， 当 FIFO_EN=0该位置 1，复位 FIFO缓冲区。复位后该位自动清 0,，此处置0不复位FI

12、FO。不使用FIFO，0或1都可以。I2C_MST_RESET=0， 当 I2C_MST_EN=0该位置 1，复位 I2C主机。复位后该位自动清 0。,SIG_COND_RESET=0，该位置 1，复位所有传感器的信号通道 (陀螺仪, 加速度计和温度传感器)。这个操作会清除传感器寄存器。复位后该位自动清0。如果只想复位信号通道(不复位传感器寄存器)，请使用寄存器 104，SIGNAL_PATH_RESET。8.REGISTER 117 WHOAMI（只读）这个寄存器用于标识设备的身份。WHO_AM_I的内容是 MPU-60X0的 7位 I2C地址的头 6位。最后一位地址由 AD0引脚确定。AD

13、0引脚的值跟寄存器无关。寄存器的默认值为 0x68。位0 和7 保留。（硬编码为0）。9.REGISTERS 59 TO 64 ACCELEROMETERMEASUREMENTS（只读）这个寄存器存储最近加速度计的测量值。加速度计根据采样频率（由寄存器 25定义）写入到这些寄存器。每个 16位加速度计测量值的满量程定义在 ACCEL_FS（寄存器 28）。对于每个满量程的设置，ACCEL_xOUT里加速度计测量值的灵敏度最低分辨率（LSB）如下表：10.REGISTERS 67 TO 72 GYROSCOPEMEASUREMENTS（只读）这个寄存器存储最近陀螺仪的测量值。陀螺仪根据采样频率（

14、由寄存器 25定义）写入到这些寄存器。每个 16 位陀螺仪测量值的满量程定义在 FS_SEL（寄存器 27）。对于每个满量程的设置，GYRO_xOUT里陀螺仪测量值的灵敏度最低分辨率（LSB）如下表： 对于寄存器，我们需要注意REGISTER 117 WHOAMI（只读），这个寄存器与mpu6050的读写地址有关，AD0为mpu6050的一个引脚，由对该寄存器的描述可知，如果AD0接地，那么mpu6050的读地址为0xd0，如果AD0接高，那么mpu6050的读地址为0xd2。关于电源，MPU-60X0 可支持 VDD 范 围 2.5V±5% ，3.0V±5%，或 3.3V

15、±5%。另外MPU-6050 还有一个 VLOGIC 引脚，用来为 I2C 输出提供逻辑电平。VLOGIC 电压可取1.8±5%或者 VDD。对于VLOGIC引脚，最好接VDD，当然，还要看使用的mcu的高低电平范围是多少，否则，mpu6050的高低电平可能和mcu认为的高低电平不同。3）磁力计：又叫电子罗盘，磁力测量的是磁场强度在三个轴上的分量，但是在小四轴上，没有必要使用磁力计，因为电机和飞控电路靠的很近，我们知道，电机内部都是有磁铁的，因此会有很大的干扰，没有必要装磁力计。4）数据传输恩，飞机做好，那用什么来控制呢，最容易想到的是蓝牙，射频和wifi，wifi由于功耗

16、较大，我们暂不考虑。我们做的飞行器支持用hc-05（蓝牙）和nrf54l01+（射频）来传输数据。蓝牙模块实际上就相当于一个无线串口，蓝牙与蓝牙之间数据是透传的，也就是说配置好蓝牙模块之后，就可以把它当串口对待了。而射频接收则需要用另一块mcu来控制进行数据的接收并通过串口来发送数据给上位机。显然蓝牙更方便，但是射频可以支持更远距离的传输，只是要注意，电机对于射频的干扰是致命，因此设计电路的时候，电源部分要做好隔离，同时，射频极力推荐使用挪威原产nrf24l01+。台产的nrf24l01+，和普通的nrf24l01最好不要用。下面解释原因： 我们知道，如果用蓝牙或者串口收发数据的话，是不需要配

17、置某一方串口或蓝牙处于发送状态还是接受状态的，只需将数据写入发送缓冲区就可以发送数据，有数据接收只需要读接收缓冲寄存器。而nrf24l01则不同，在发送前必须配置为发送模式，接收前必须配置为接收模式，这对于双向通信显然是很麻烦的。但是，接受方在接受数据之后会向发送方发送应答信号表示已接收到数据，那么这个应答信号要是能顺便捎点其他信息给发送方，岂不是太方便了？挪威原产nrf24l01+完美解决了这个问题。三程序设计1）四轴飞行器的程序框架如下：1.初始化mcu和各个传感器2.读取传感器数据3对传感器原始数据进行处理4利用处理后的数据和上一次更新的姿态数据进行姿态更新5根据遥控信号和姿态对电机进行

18、控制，回到22）介绍陀螺仪和加速度计时说了，陀螺仪由于积分作用，它的累积误差来源于零漂。加速度计由于测量的是加速度，因此运动状态以及电机震动会干扰它对重力加速度的测量。对于以上干扰，我们对陀螺仪和加速度计进行如下处理： 1.飞行器水平静止放置时，读取200次传感器数据取平均，对于陀螺仪，这个数据就是零点漂移。 2.对于陀螺仪，以后每次用来姿态解算的数据都先减去1里获得平均值。3.对于加速度计，以后每次姿态解算前都读取20次加速度计数据并取平均，这样就可以有效的滤除高频分量，是加速度曲线稍微平滑。3）姿态解算在姿态解算前还得先讲讲四元数和欧拉角以及方向余弦矩阵，具体推导过程可以看秦永元的惯性导航

19、第一章绪论和第九章捷联惯导系统。1.先建立两个三维直角坐标系，一个是机体坐标系（设为b系），它就用来代表飞机，飞机怎么转，它就怎么转。一个是地理坐标系（设为n系），他是固定不动的，方便起见，设它的xyz指向东北天。然后，有一个概念：任意从n到b的旋转都可以等效为三次旋转（这里我们约定旋转顺序为Z-X-Y,值得注意的是，矩阵的乘法是不满足交换律的，因此旋转顺序的不同对应的旋转矩阵不同，）：这里的，就是三个欧拉角yaw，pitch，roll（对于三个欧拉角的实际意义，可以看一下附带的动态图片pitch.gif,roll.gif,yaw.gif）2用欧拉角的表示的由n到b的旋转阵即姿态阵为：3.四元

20、数旋转矩阵的四元数表示：4由于上面两个旋转矩阵式是等价的，那么可以用作为欧拉角和四元数的相互转换 5.为什么要引入四元数我们知道，姿态更新的过程，主要的部分是陀螺仪的积分，如果不用四元数，我们可用7.2.3来计算，但是有缺点，看截图部分。由此我们引入四元数，用四元数来更新姿态，然后再由四元数求得欧拉角，他们都可以用来表示姿态，且是可以相互转换的。四元数微分方程四元数更新算法，一阶龙库法，同样4个量（入、P1-3）为四元数的四个值,即上面的q0-3。Wx,y,z是三个陀螺的这个周期的角速度，比如欧拉角微分方程中的0.01弧度/秒，T为更新周期,比如0.02秒。 四元数微分方程更新后的规

21、范化每个周期更新完四元数，需要对四元数做规范化处理。因为四元数本来就定义为四维单位向量。求q0-3的平方和，再开根号算出的向量长度length。然后每个q0-3除这个length。 6.姿态解算姿态解算就是姿态更新，说白了就是获得飞行器的姿态矩阵，在上述数学原理的讲解中已经说明了用四元数来更新姿态矩阵的方法。但是，上述理论都是以陀螺仪为主体的，那么加速度计有什么用呢？尽管姿态更新之前的陀螺仪数据已经经过了一定的处理，但这显然是不够，陀螺仪的零点漂移受温度等多种因素影响，并不是一个固定的值。因此如果依旧只用陀螺仪积分来进行姿态解算，时间久了仍然是有很大误差的。而加速度计不存在积分环节，

22、只不过电机扰动并较大，造成测量的重力加速度数据不准确。实际上，陀螺仪测量数据的零漂属于低频分量，加速度计的误差属于高频分量。因此可以对陀螺仪数据进行高通滤波，对加速度数据进行低通滤波，然后进行姿态解算，这就是互补滤波。其基本表达式为： angle=a*(angle+gyro*dt)+(1-a)*x_acc;(一个轴的表示，angle表示绕某个轴转动的姿态角，gyro*dt表示对陀螺仪数据积分，x_acc为加速度数据，a为参数，表示对陀螺仪数据和加速度数据的信任度。)上面的式子是互补滤波器的基本表达式，我们实际使用的是改进型的带PI修正的互补滤波器。详细专业内容可以参考论文基于改进互补滤波器的低

23、成本微小飞行器姿态估计方法（注意：我们的程序中没有对PI 补偿的增益进行分级调节）。姿态解算流程请看：互补滤波法.bmp,看上面一部分图片即可，我们没有用到磁力计，图片放大后看比较清晰。或者参考7.最后要提的2点是互补滤波法也可以使用卡尔曼滤波，梯度下降法代替，但卡尔曼滤波太过复杂，因此我们并未使用。并且这三种方法是对姿态数据进行误差补偿，使之更接近于真实姿态，而不属于四元数姿态更新的范畴。Mpu6050读取出来的数据在某些情况是不能直接使用，必须经过单位转换，至于怎么把原始数据转换成度或是弧度，在这里就不废话了，寄存器介绍那一部分有测量范围。写姿态更新程序的时候务必用定时器来产生精确地更新周

24、期。四PID控制器对于PID的详细资料请参阅AN_SPMC75_0012PID调节控制做电机速度控制有入门到精通吃透PID1）采用位置式PID控制，位置式PID公式如下 其中ek为当前实际姿态角减去期望姿态角和的差值，而uk则是作为pwm的占空比，来控制电机的转速；Kp，Ki，Kd是三个参数，下面介绍这三个参数的作用：1. Kp是比例系数，强制回复力是系统平衡的主力，它总是使飞行器的实际姿态不断逼近期望姿态，太小会导致飞行器回复力太小，导致飞行器无法回复，太大容易超调，所谓超调就是指恢复到平衡位置之后往另一个方向偏，从而引发低频振荡。要注意的是，如果只有Kp，那么四轴是肯定会震荡的，

25、因此需要Kd来抑制这个低频震荡。2. Kd是阻尼力参数，始终是抑制飞行器姿态的变化，姿态变化的越快，阻尼力越大，因此Kd*（ek-ek-1）这一项是负的，这一点一定要注意，否则pid永远调不出来。Kd的乘积项可以直接用角速度代替。Kd项可以抑制Kp的低频震荡，Kd越大，抑制效果越好，但是太大会引起高频震荡。因此Kp和Kd具体的大小需要不断地调试才能获得。3. Ki是积分项的参数，他也是回复力，但比Kp柔和，Ki只能是辅助Kp，在小四轴上，甚至可以不用Ki这一项。2）调试PID 可以参阅： 调PID就是通过不断地实验获取合理的KP,KI,KD这三个参数的值，要注意，对于小四轴来说，如果只有参数K

26、p，那么四轴要么一下子就倒下去再也起不来，要么就是倒向另一边之后突然电机力量加大，朝另一个方向翻滚，不停地翻滚，因此必须适当的加入Kd进行抑制。在调PID之前，非常非常重要的一点，就是按照你的程序定好的机体坐标系和飞行模式（X型or +型）安放你的电机。我们小四轴为X型，示意如下：1,2,3,4是电机编号以便说明2143上图对应的机体坐标系如下：z轴方向为垂直射出纸面XY同时，在我们的程序中，按照右手定则约定：右手握住x轴，拇指指向x轴正方向，那么另外四个手指指尖的朝向就是机体绕x轴旋转的正方向，同时，我们约定横滚角roll就是绕x轴。右手按同样方法握住Y轴和Z轴可知绕这两个轴旋转的正方向，同

27、样，我们约定绕Y轴旋转为俯仰角pitch，绕z轴旋转为航向角yaw。对于pid的程序，有一点需要说明：Moto_PWM_1 = rc_in->THROTTLE - 1000 + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;Moto_PWM_2 = rc_in->THROTTLE - 1000 - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;Moto_PWM_3 = rc_in->THROTTLE - 1000 + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;Moto_P

28、WM_4 = rc_in->THROTTLE - 1000 - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;上面一段程序中，Moto_PWM_1/2/3/4是四个电机的pwm值，rc_in->THROTTLE是接收到的遥控信号的基础油门值（这里要减1000是因遥控油门数据为1000到2000之间，而我们程序的PWM值得范围是0到1000之间，不用深究），后面的PID_ROL.OUT ， PID_PIT.OUT ， PID_YAW.OUT才是三个轴的PID控制器的输出。那么怎么来区分这个pid输出是减还是加呢？我们假设现在要让飞行器向右飞，那么按照

29、前面的分析我们应该给个正的roll的期望角，记为exp_roll,那么ek=0-exp_roll(我们假设原来飞行器是水平的，即实际姿态角为0)。 然后，我们想象一下，飞行器如果要向右飞，显然2，4号电机应该加大转速；1，3电机应该减小转速，这样才能向右倾斜。那么表现在公式上：Moto_PWM_1 = rc_in->THROTTLE - 1000 + PID_ROL.OUT;Moto_PWM_2 = rc_in->THROTTLE - 1000 - PID_ROL.OUT;Moto_PWM_3 = rc_in->THROTTLE - 1000 + PID_ROL.OUT;Moto_PWM_4 = rc_in->THROTTLE - 1000 - PID_ROL.OUT;可能有人觉得奇怪，这个式子怎么和上面的分析相反呢？怎么1,3是+ PID_ROL.OUT，而2,4却是 - PID_ROL.OUT呢？这是因为我们上面举得例子中ek=-exp_roll是个小于0的值，那么PID的输出肯定也是个负值。为什么呢，首先，ek带入PID的公式