多旋翼无人飞行器嵌入式飞控开发实战-基于STM32系列微控制器的代码实现-课件奚海蛟-第5章无人机参数存储 光流计OpticalFlow-PID位置算法

多旋翼无人机飞行器嵌入式飞控开发实战光流计OpticalFlow1234气压计MS56115光流计OpticalFlow无人机数传加速度计与陀螺仪MPU6050磁力计QMC5883L6遥控输入7电机控制光流计OpticalFlow通过串口通信原理，将光流模块采集到的数据，进行检验以及计算，实现无人机定高、定点的目的。0102光流计OpticalFlowSTMicroelectronics的VL53L1X是一种先进的ToF激光测距传感器，壮大了FlightSense™产品系列阵容。PMW3901是一个光流专用集成电路，它可以在内部计算光流，并在每帧之间提供像素差。VL53L1XVL53L1X系统由VL53L1X模块和运行在该模块上的驱动程序组成。PMW3901PMW3901基本上是一个跟踪传感器，类似于你可以在电脑鼠标中找到的东西，其适用范围在80毫米到无限宽之间。原理图通过串口4与光流模块连接。光流计OpticalFlow实现光流模块采集数据，无人机定高、定点。练习简述光流传感器的运行原理01简述光流传感器的数据解析的过程。02遥控输入1234气压计MS56115光流计OpticalFlow无人机数传加速度计与陀螺仪MPU6050磁力计QMC5883L6遥控输入7电机控制遥控输入熟悉飞控系统的遥控接收机输入接口信号的采集方法，能够采集多路的遥控信号，能够自行添加更多路的遥控数据。0102遥控输入遥控器发送PWM脉冲，接收机接收信号，然后单片机捕获接收到的信号。定时器TIM3和定时器TIM4来捕获接收机的1—8通道的pwm值。接收机通道与飞控通道对应连接原理图遥控输入能够采集无人机多路的遥控信号。练习利用其他通道实现遥控器的输入。01简述遥控器输入的过程。02电机控制1234气压计MS56115光流计OpticalFlow无人机数传加速度计与陀螺仪MPU6050磁力计QMC5883L6遥控输入7电机控制电机控制学习飞控系统的电调输出接口的原理和使用；了解电调工作条件需求；熟悉使用输出PWM驱动电调系统。0102电子调速器电调左端为输入端，外侧黑、红线接电池，内侧黑、红、白线接数字信号电路；右侧三条输出线接三相无刷电机。电调的作用就是把PWM方波信号调成三相交流电，任意改变接入电机的两根线，即可改变电机的转向。电子调速器与飞控连接图电调的输入信号由飞控板的引脚控制。由STM32定时器TIM1产生4路PWM方波脉冲，提供给四旋翼无人机的四个电调，控制无刷电机的转速。原理图电机控制使用输出PWM驱动电调系统。练习简述电调的原理。01简述电机控制的实现过程。02实现数字滤波器-FIR以及IIR、卡尔曼滤波以及扩展卡尔曼滤波、无人机的姿态解算以及角速度、角度PID控制器。第7章无人机飞控算法1234基于扩展卡尔曼滤的算法融合5无人机姿态解算数字滤波器-FIR数字滤波器-IIR基于卡尔曼滤波的角度融合6角速度、角度PID控制器设计数字滤波器-FIR1234基于扩展卡尔曼滤的算法融合5无人机姿态解算数字滤波器-FIR数字滤波器-IIR基于卡尔曼滤波的角度融合6角速度、角度PID控制器设计将“光标”飞控中传感器采集到的数据信息通过数字滤波器—FIR滤除杂波，保证传感器采集数据的准确有效性。0102数字滤波器-FIRFIR滤波器FIR滤波器是非递归型滤波器的简称，又叫做有限长单位冲激响应滤波器。在数字信号处理系统中较为基本的元件。数字滤波器系统函数为H(z)，脉冲响应为h(n)，输入时序为x(n)。

>

>设滤波器单位脉冲响应的长度为N，系统函数为：>传输函数

直接型设FIR滤波器的单位冲击响应h(n)为一个长度为N的序列，则滤波器系统函数为：A表示这一系统输入输出关系的差分方程为：B

差分方程对应的网络结构优点：简单直观，乘法运算量较少。缺点：调整零点较难，并且具有相位延迟性。级联型当需要控制滤波器的传输零点时，可将H(z)分解为实系数二阶因子的乘积形式。式中，H(z)为h(n)的z变换，β0k，β1k，β2k为实数。

级联型结构BA优点调整零点比直接型方便。缺点H(z)中的系数比直接型多，因而需要的乘法器多。线性相位型结构FIR滤波器的线性相位结构有偶对称和奇对称，不论h(n)为偶对称还是奇对称都有：

当N为奇数时，系统函数为：当N为偶数时，系统函数：FIR直接型信号流01N为偶数02N为奇数频率采样型频率采样型结构是一种用系数将滤波器参数化的一种实现结构。

STEP1H(k)与系统函数之间的关系可用内插公式为：其零点为：STEP2

信号流图数字滤波器-FIR将“光标”飞控中传感器采集到的数据信息通过数字滤波器—FIR滤除杂波，保证传感器采集数据的准确有效性。练习简述FIR数字滤波器运行原理。01重新导出22阶汉明窗口滤波器。02数字滤波器-IIR1234基于扩展卡尔曼滤的算法融合5无人机姿态解算数字滤波器-FIR数字滤波器-IIR基于卡尔曼滤波的角度融合6角速度、角度PID控制器设计数字滤波器-IIR将“光标”飞控中传感器采集到的数据信息通过数字滤波器—IIR滤除杂波，保证传感器采集数据的准确有效性。0102数字滤波器-IIRIIR（InfiniteImpulseResponse）数字滤波器，又名无限脉冲响应数字滤波器，或递归滤波器。IIR与FIR滤波器对比数字滤波器根据其冲激响应函数的时域性分为两种，无限长冲激响应（IIR）数字滤波器和有限长冲激响应（FIR）滤波器。IIR数字滤波器与FIR数字滤波器相比相位特性差且可以用于较少的结束获得很高的选择性但是具有结构简单具有经济高效的特点IIR滤波器差分方程的一般表达式:运算量小IIR数字滤波器设计的方法直接设计法01不使用模拟滤波器，直接进行数字滤波设计的方法。不需要事先给定参数的方法。间接设计法02间接设计发是借助成熟的模拟滤波器设计方法设计的，根据数字滤波器设计指标（事先给定参数）来设计相对应的过渡模拟滤波器，再将过渡模拟滤波器转换为数字滤波器。原型低通滤波器--巴特沃斯低通滤波器巴特沃斯低通滤波器其中，N为滤波器的次数，Ωc是截止频率。计算出阻带衰减：将巴特沃斯低通滤波器的振幅特性，直接带入上式得到：可以解得次数N为：巴斯沃斯滤波器的传递函数巴斯沃斯低通滤波器的传递函数，可以由其振幅特性的分母多项式求得。其分母多项式为：根据S解开，可以得到极点。当N为偶数的时候，求得：同理得当N为奇数的时候，求得：利用欧拉公式求得的极点：巴斯沃斯滤波器的传递函数选取稳定极点，取模拟滤波器的传递函数求得：设计完乘法器过后，可计算出其滤波系数：双1次z变换的原理模拟滤波器的传递函数为：传递函数进行拉普拉斯逆变换，可以得：其中，x(t)表示输入，y(t)表示输出。然后我们需要将其离散化，可得到：之后使用z变换，再将其简化。可以得：从而，我们可以得到s平面到z平面的映射关系：然后，将关系式

与s=δ+jΩ带入得到：可以得到Ω与ω的对应关系为：数字滤波器-IIR将“光标”飞控中传感器采集到的数据信息通过数字滤波器—IIR滤除杂波，保证传感器采集数据的准确有效性。练习简述IIR滤波器运行原理。01简述IIR滤波器实现过程。02基于卡尔曼滤波的角度融合1234基于扩展卡尔曼滤的算法融合5无人机姿态解算数字滤波器-FIR数字滤波器-IIR基于卡尔曼滤波的角度融合6角速度、角度PID控制器设计通过学习卡尔曼滤波工作原理与实战演练，掌握并运用卡尔曼滤波进行滤波和数据融合。0102FreeRTOS+TCP客户端TCP协议简介卡尔曼滤波（Kalmanfiltering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。卡尔曼滤波原理系统的状态方程和测量方程状态方程：观测方程：当前状态当前状态公式为：式中，

，B=[T0]，X(k|k-1)是利用k预测的结果，X(k-1|k-1)是k-1时刻的最优结果。则有对应于X(k│k-1)的协方差为：式中，P(k│k-1)是X(k│k-1)对应的协方差，AT表示A的转置矩阵，Q是系统过程的协方差。即对系统的状态更新。则状态k的最优化估算值X(k│k)为：其中H=[10]。卡尔曼增益(KalmanGain)卡尔曼增益公式为：此时，已经得到了k状态下最优的估算值X(k│k)。但是还要更新k状态下X(k│k)的协方差：

可改写为：

基于卡尔曼滤波的角度融合运用卡尔曼滤波进行滤波和数据融合。练习简述卡尔曼滤波运行原理。01简述卡尔曼滤波黄金公式含义。02基于扩展卡尔曼滤的算法融合1234基于扩展卡尔曼滤的算法融合5无人机姿态解算数字滤波器-FIR数字滤波器-IIR基于卡尔曼滤波的角度融合6角速度、角度PID控制器设计基于扩展卡尔曼滤的算法融合通过学习扩展卡尔曼滤波工作原理与实战演练，掌握并运用扩展卡尔曼滤波进行滤波和数据融合。0102扩展卡尔曼滤扩展卡尔曼滤波器（ExtendedKalmanFilter）是标准卡尔曼滤波在非线性情形下的一种扩展形式，它是一种高效率的递归滤波器（自回归滤波器）。EKF的基本思想是利用泰勒级数展开将非线性系统线性化，然后采用卡尔曼滤波框架对信号进行滤波，因此它是一种次优滤波。扩展卡尔曼滤波EKF卡曼滤波预测方程：A卡曼滤波更新方程：B

在卡尔曼滤波中假设AB其状态转移是线性的，因此我们可以直接用矩阵F表示其线性特征。其状态和观测都是高斯分布。推导卡尔曼滤波

FreeRTOS的套接字连接接收函数

求偏导令等于0推导卡尔曼增益将真实测量值代替之后化简Σt第二种方法

计算联合高斯分布求条件高斯分布A舒尔补恒等式展开对比C计算高斯联合分布的指数二次项BD得到新的均值和方差求解EKF

基于扩展卡尔曼滤的算法融合运用扩展卡尔曼滤波进行滤波和数据融合。练习简述扩展卡尔曼滤波的原理。01简述扩展卡尔曼滤波的实现过程。02无人机姿态解算1234基于扩展卡尔曼滤的算法融合5无人机姿态解算数字滤波器-FIR数字滤波器-IIR基于卡尔曼滤波的角度融合6角速度、角度PID控制器设计无人机姿态解算掌握欧拉角转四元数、四元数转方向余弦矩阵、静态姿态角计算、方向余弦矩阵计算转欧拉角、一节龙格库塔法求解四元数以及通过磁力计与加速度计补偿姿态角。0102无人机姿态解算无人机控制中，主要就是姿态解算和姿态控制部分，位置控制可用远程遥控控制，而姿态控制一般由无人机系统自动完成。无人机控制框图无人机姿态解算框图姿态解算方法的比较名词解释惯性测量单元IMU姿态航向参考系统AHRS地磁角速度重力MARG微机电系统MEMS可垂直起降TOL方向余弦矩阵DCM天文导航系统CNS全球导航卫星系统GNSS自由度维数DOF无人驾驶飞行器UAV惯性导航系统INS扩展卡尔曼滤波EKF无损卡尔曼滤波UKF姿态数据重力0102地磁电子罗盘0503陀螺仪加速度计04导航的基本原则导航的基本原则就是保证两个基本坐标系的正确转化，没有误差。只有实现了这个原则，载体才可以在自己的坐标系中完成一系列动作而被转换到地理坐标系中看起来是正确的。AHRS和IMU的差异AHRS由加速度计、磁场计、陀螺仪构成，AHRS的真正参考来自于地球的重力场和地球的磁场，它的静态精度取决于对磁场的测量精度和对重力的测量精度，而陀螺仪决定了他的动态性能。IMU（Inertialmeasurementunit）学名惯性测量单元，所有的运动都可以分解为一个直线运动和一个旋转运动，故这个惯性测量单元就是测量这两种运动，直线运动通过加速度计可以测量，旋转运动则通过陀螺。大地坐标系与机体坐标系关系定义三个单位向量欧拉角俯仰角（pitch）θ01滚转角（横滚角roll）φ02偏航角（方位角yaw）ψ03方向角转动示意图偏航角（方位角yaw）ψ俯仰角（pitch）θ滚转角（横滚角roll）φ飞机空间角的确定数学表达为求欧拉角姿态矩阵各次基本旋转对应的变换矩阵反求欧拉角四元数四元数是简单的超复数。四元数运算四元数的形式：并且四元数与旋转关系四元数是可以描述三维空间中刚体的旋转计算四元数旋转平面为垂直P点与P'点的共面且垂直于旋转轴v，定义v0是四元数P的矢量部分P在旋转平面上的投影矢量，v2是四元数P'的矢量部分P'在旋转平面上的投影矢量，因此相当于矢量v0围绕旋转轴v旋转θ角度到v2。四元数的姿态估计经运算可得

，从而得到D即为机体坐标系b到大地坐标系的方向余弦矩阵机体坐标转大地坐标矩阵其中四元数求解方向余弦矩阵A将单位四元数代入，并得到方向余弦矩阵B采用一阶龙格库塔法，最终求得无人机姿态解算计算转欧拉角、求解四元数以及通过磁力计与加速度计补偿姿态角。角速度、角度PID控制器设计1234基于扩展卡尔曼滤的算法融合5无人机姿态解算数字滤波器-FIR数字滤波器-IIR基于卡尔曼滤波的角度融合6角速度、角度PID控制器设计角速度、角度PID控制器设计通过学习角速度、角度PID控制器设计以及参数调整，掌握PID的控制原理、参数调节以及在无人机中的应用。0102PID算法在过程控制中，按偏差的比例（P）、积分（I）和微分（D）