飞控算法讲解

SuccessAlwaysBelongsToThosePrepared!DroneTechnologySeminarInshanghai无人机技术研讨会锡月科技曹君超飞控算法01两种结构“×”字模式：Pitch和Roll与1,3、2,4两组电机呈45°夹角。“十”字模式：Pitch对应2,4电机的对轴，Roll对应1,3电机的对轴，夹角为0。稳定灵活四旋翼飞行状态思考一下：四个电机转速完全相同，无人机能平衡嘛？答：四旋翼重心不一定在中心上，所以无法平衡。达到平衡状态后不改变四个电机的转速，无人机能一直平衡下去嘛？

答：因为有外界的干扰，四旋翼四个旋翼的转速要一直处于调节状态才能保持平衡。六种飞行状态四旋翼垂直运动示意图垂直运动在四旋翼完成4轴平衡的条件下，同时增加四个电机的输出功率，旋翼转速增加使得总的拉力增大，当总拉力足以克服四旋翼无人机受到的重力时，四旋翼飞行器便离地垂直上升；反之，同时减小四个电机的输出功率，四旋翼飞行器则垂直下降，直至平衡落地，实现了沿z轴的垂直运动。当外界扰动量为零时，在旋翼产生的升力等于飞行器的自身的重力时，飞行器便保持悬停状态。六种飞行状态四旋翼俯仰运动示意图俯仰运动

在四旋翼完成4轴平衡的条件下，在图中，电机1、4的转速上升，电机2、3的转速下降（改变量大小应相等，在PID程序的实现中也有体现）。由于旋翼1、4的转速即升力上升，旋翼2、3的转速即升力下降，产生的不平衡力矩使机身绕Roll轴旋转，同理，当电机1、4的转速下降，电机2、3的转速上升，机身便绕Roll轴向另一个方向运动，实现飞行器的俯仰运动。六种飞行状态四旋翼翻滚运动示意图翻滚运动在图中，在四旋翼完成4轴平衡的条件下，提高3、4号电机的转速，减慢1、2号电机的转速，则可使机身绕Pitch轴的正向或者反向进行运动，实现飞行器的翻滚运动。

六种飞行状态四旋翼航向运动示意图航向运动

在图中，当电机1和电机3的转速上升，电机2和电机4的转速下降时，旋翼1和旋翼3对机身的反扭矩大于旋翼2和旋翼4对机身的反扭矩，机身便在富余反扭矩的作用下绕z轴转动，实现飞行器的航向运动，转向与电机1、电机3的转向相反。六种飞行状态四旋翼前后运动示意图前后运动

如果想要实现四旋翼无人机在水平面内前后、左右的运动，必须在水平面内对飞行器施加一定的力。在图中，增加1、4号电机转速，使拉力增大，相应减小2、3号电机转速，使拉力减小，同时反扭矩仍然要保持平衡。按俯仰理论，飞行器首先发生一定程度的倾斜，从而使旋翼拉力产生水平分量，因此可以实现无人机的前飞运动。向后飞行与向前飞行正好相反。六种飞行状态四旋翼前后运动示意图侧向运动与前后运动对称，与翻滚运动同轴。无人机状态空间方程控制原理Throttle_Info.M1=-pidRatePitch.value-pidRateRoll.value+pidAccHeight.value-pidRateYaw.value+throttleBasic;

Throttle_Info.M2=+pidRatePitch.value-pidRateRoll.value+pidAccHeight.value+pidRateYaw.value+throttleBasic;Throttle_Info.M3=+pidRatePitch.value+pidRateRoll.value+pidAccHeight.value-pidRateYaw.value+throttleBasic;

Throttle_Info.M4=-pidRatePitch.value+pidRateRoll.value+pidAccHeight.value+pidRateYaw.value+throttleBasic;X型的四旋翼每个对轴由四个电机同时控制，根据电机序号的标定，我们可以将3个欧拉角的控制量分解到4个电机上，高度的控制量四个电机是一致的。最后需要给每个电机加上一个油门基础量。四旋翼控制的油门基础量油门基础量的选择：大约占电机最高转速的70%左右。能够提供给四旋翼一个起飞的油门，注意并不是靠地效的反作用力是四旋翼脱离地面的量。油门基础量与控制量的占比：

油门基础量占比为80%左右或以上，控制量只是根据外界干扰和重心偏移所修正的一个油门转速，所以只需要较小的控制量就能使四旋翼保持平衡。思考：如果油门基础量较小，控制量较大，会发生什么情况？答：一旦控制飞行时，调节平衡能力就较差，且受到外界干扰后，控制量较大，四旋翼处于较不稳定状态。控制难点自身中心偏移自身气流干扰外界气流干扰多变量、非线性、强耦合欠驱动系统传感器精度低、误差积累PID控制器（比例-积分-微分控制器），由比例单元(P)、积分单元(I)和微分单元(D)组成。透过Kp，Ki和Kd三个参数的设定。PID控制器主要适用于基本上线性，且动态特性不随时间变化的系统。PID控制器自动控制系统的特点稳定性（P和I降低系统稳定性，D提高系统稳定性）在平衡状态下，系统受到某个干扰后，经过一段时间其被控量可以达到某一稳定状态；准确性（P和I提高稳态精度，D无作用）系统处于稳态时，其稳态误差；快速性（P和D提高响应速度，I降低响应速度）：系统对动态响应的要求。一般由过渡时间的长短来衡量。自动控制系统的特点动态特性（暂态特性，由于系统惯性引起）系统突加给定量（或者负载突然变化）时，其系统输出的动态响应曲线。延迟时间、上升时间、峰值时间、调节时间、超调量和振荡次数。在通常情况下,上升时间和峰值时间用来评价系统的响应速度；超调量用来评价系统的阻尼程度；调节时间同时反应响应速度和阻尼程度；稳态特性在参考信号输出下，经过无穷时间，其系统输出与参考信号的误差。影响因素：系统结构、参数和输入量的形式等分离四轴做平衡控制分离四轴做平衡控制（单级PID）纯比例调节（P项）找到P0引起等幅震荡加入微分，反向抑制震荡，但存在一些静差加入微弱的积分项，使静差减小，在-1°~1°间010203分离四轴做平衡控制（单级PID）单侧人为扰动后的自稳数据分离四轴做平衡控制（单级PID）Kd过小的控制效果图Kd过大的控制效果图无法消除震荡无法稳定在某一值掩盖了Kp调节作用使系统不存在分离四轴做平衡控制（单级PID）Ki过小的控制效果图Ki过大的控制效果图调整率缓慢引起震荡单级PID

当前角度误差=期望角度–实际角度;PID_P项=Kp*当前角度误差;PID_I项+=Ki*当前角度误差;PID_I项进行积分限幅;//当前角度的微分（原理上为当前角度误差–上一次角度//误差，换一个角度来讲角度的微分就是角速度，所以一//般用陀螺仪的角速度数据来替代角度的微分）PID_D项=Kd*当前角速度的微分（陀螺仪数据）;单环PID总输出=PID_P项+PID_I项+PID_D项;单环PID总输出限幅;

编程思路单级PID例程请见：Single-loopPID_control

CUSTOMIZEConvenientlyiteratetop-linealignmentsforwirelessmetrics.姿态控制要点输出给电机期望角度比例+姿态解析角速度采样积分微分比例积分微分+++++-内环：角速度环外环：角度环-特点增加了控制变量，增加抗干扰性内环粗调，外环细调内环起到超前调节的总用，增加鲁棒性提高了系统工作频率，减小震荡周期，调节时间缩短，系统的快速性增强串级PID

当前角度误差=期望角度–实际角度;PID_P项=Kp*当前角度误差;PID_I项+=Ki*当前角度误差;PID_I项进行积分限幅;//当前角度的微分（原理上为当前角度误差–上一次角度//误差，换一个角度来讲角度的微分就是角速度，所以一//般用陀螺仪的角速度数据来替代角度的微分）PID_D项=Kd*当前角速度的微分（陀螺仪数据）;单环PID总输出=PID_P项+PID_I项+PID_D项;单环PID总输出限幅;

编程思路串级PID输出给电机期望角度比例+姿态解析角速度采样积分微分比例积分微分+++++-内环：角速度环外环：角度环-特点增加了控制变量，增加抗干扰性内环粗调，外环细调内环起到超前调节的总用，增加鲁棒性提高了系统工作频率，减小震荡周期，调节时间缩短，系统的快速性增强串级PID当前角度误差=期望角度–实际角度;外环PID_P项=Kp*当前角度误差;外环PID_I项+=Ki*当前角度误差;外环PID_I项进行积分限幅;外环PID_D项=Kd*（本次角度误差–上次角度误差）/控制周期;外环PID总输出=外环PID_P项+外环PID_I项+外环PID_D项;外环PID总输出限幅;当前角速度误差=外环PID总输出–当前角速度;（期望角速度为0）;内环PID_P项=Kp*当前角速度误差;内环PID_I项+=Ki*当前角速度误差;内环PID_I项进行积分限幅;内环PID_D项=Kd*（本次角速度误差–上次角速度误差）/控制周期;内环PID总输出=内环PID_P项+内环PID_I项+内环PID_D项;内环PID总输出限幅;编程思路打舵实验与PID调参角速度环打舵：观察静态稳定性观察响应率角度环打舵观察静态水平稳定性观察正负十度内的跟踪情况根据以上情况进行相应的PID调参例程请见：Doubleclosed-loopPID_control

CUSTOMIZEConvenientlyiteratetop-linealignmentsforwirelessmetrics.PID参数整定要点角速度环打舵：观察静态稳定性观察响应率角度环打舵观察静态水平稳定性观察正负十度内的跟踪情况P太小：不能修正角速度误差表现为很“软”倾斜后难以修正，打舵响应也差。P太大：在平衡位置容易震荡，打舵回中或给干扰（用手突加干扰）时会震荡。合适的P：能较好的对打舵进行响应，又不太会震荡，但是舵量回中后会回弹好几下才能停止（没有D）。D的效果：加快打舵响应，最大的作用是能很好地抑制舵量回中后的震荡，可谓立竿见影。D太大：会在横滚俯仰混控时表现出来（尽管在“烤四轴”时的表现可能很好），具体表现是四轴抓在手里推油门会抽搐。如果这样，只能回到“烤四轴”降低D，同时P也只能跟着降低。D调整完后可以再次加大P值，以能够跟随打舵为判断标准。加入I：由于重心偏高，质量不均造成偏离，I可以一定程度上修正偏离。

CUSTOMIZEConvenientlyiteratetop-linealignmentsforwirelessmetrics.高度控制要点悬停油门=加速度环控制量（积分起主导作用）+电机参数表单电压下所对应的悬停油门注*（如果电机参数曲线为各个电压下，可由参数表直接实时算出悬停油门）此时输出Z轴的升力F=mg

当需要输出a（OriginalAccZ.value）的加速度时，输出力F1=m(g+a)注*（如果整机质量已知，即可算出每个电机需要提供的升力，并通过电机参数表曲线将升力转换为对应油门）此处因为要通用于不同轴距的多个机型，所以m、F与油门的关系量都未知，因此有以下关系式：F1/F=1+a/g因此此时应输出：悬停油门*(1+a/g)*/UAVThrust.HeightThrust=OriginalAccZ.value+UAVThrust.BasicThrust;观测量反馈算法这个算法需要明确的是所需的高度信息是地理坐标系下的相对高度，整个算法的核心思想是由地理坐标系下的加速度通过积分，来获得速度、位置信息，而这个数据的精确程度是由机体测量的加速度通过减去偏差，再转换到地理坐标系求得的。这里超声波的作用就是计算一个校正系数来对加速度偏移量进行校正。卡尔曼滤波器卡尔曼黄金五条公式观测量反馈滤波高度数据融合加计数据以60HZ频率从US100超声波模块获取原始高度数据高度数据获取中位值滤波简单滤波处理实时高度以500HZ频率从MPU6500获取原始加速计数据加速计数据获取均值滤波简单滤波处理实时速度用于控制用于控制像素点到实际距离的转换像素点焦距实际高度实际距离需要已知条件：1、CCD或CMOS大小2、焦距3、像素4、同步的高度数据IMU与图像数据融合卡尔曼滤波观测量反馈滤波三轴加速计数据加速计数据XY轴数据摄像头数据（转换到单位M）XY二维图像位置信息滤波获得x轴的实时位置和速度信息X轴位置和速度滤波获得y轴的实时位置和速度信息Y轴位置和速度滤波获得z轴的实时位置和速度信息Z轴位置和速度1、融合频率合适过高会产生高延迟2、R参数为传感器误差的协方差3、Q参数为预测量的协方差4、R参数不变，增大Q是相信预测模型，反之则相信观测量。卡尔曼滤波要点

CUSTOMIZEConvenien