|
楼主最近有段时间没来了,既然来了,就顺便谈谈飞控的代码写法吧。
楼主发的帖子比较少,关于小四轴的硬件设计,大家就参照楼主发的下面两个帖子吧。(由于时间久远,换了ID)
帖子一:圆点博士微型四轴飞行器开工拉...有钱出钱,没钱出力 http://www.amobbs.com/thread-5504090-1-1.html
帖子二:圆点博士小四轴2013版全部资料大放送 http://www.amobbs.com/thread-5571228-1-1.html
看完上面两个帖子,小四轴的硬件设计肯定没有问题啦。下面楼主谈下简单飞控算法的写法。
飞控的算法代码一般包括下面三个部分:滤波,姿态,PID
1,滤波可以用互补滤波来实现,互补滤波的资料很多,大家随便就能找到。基本公式是:
2,滤波完就是四元数拉。直接用老外Madgwick的IMU就可以。超级简单
3, PID的代码其实也很简单,主要是要了解其中的原理,才能更好地调整参数。为了方便新手们理解,楼主建立了一个数学模型来让大家了解。(只针对新手,老手就算了)
========圆点博士小四轴之PID控制模式分析=======
PID控制的P是Proportional的缩写, 是比例的意思,I是Integral的缩写,是积分的意思,D是Derivative的缩写,是微分的意思。所以,PID就是我们常说的比例,积分,微分控制。
我们首先来看一个PID控制模型曲线图:
该图包含了比例控制,比例+积分控制,比较+积分+微分控制的电机响应图的对比。
下面我们对曲线进行具体分析:
PID中的比例控制是最容易理解的,比例控制就是把角度的误差乘以一个常数作为输出驱动。假定我们有一个理想模型的电机,1V电压的变化会带来小四轴1度的角度改变。假定现在电机控制电压是5V,小四轴在某一轴上的偏角是5度,目标角度是100度。我们把当前的电压量定义为Vin,把输出控制量定义为Vout。假定P等于0.2,那么比例控制的结果就是:
第一次:Vout=Vin+(100-5)*P=5V+19V=24V,得到电机电压是24V,对应的小四轴角度是24度,距离目标角度的误差是100-24=76度。
第二次:Vout=Vin+(100-24)*P=24V+15V=39V, 从而引起的角度是39度。
我们看到,在这么的一个比例控制系统下,小四轴角度在慢慢地向目标角度靠近。
PID中的积分控制就是把把所有角度误差相加起来,然后乘上一个常数作为输出驱动。在上述例子中,假定I=0.2, 我们来看看比例和积分控制同时起作用下的系统反应。
第一次:Vout=Vin+(100-5)*P+(100-5)*I=5V+19V+19V=43V,这时候小四轴角度为43度。
由于第一次控制前的误差是100-5=95,第二次控制前的误差是100-43=57,所以积分结果是152。
第二次:Vout=Vin+(100-43)*P+((100-5)+(100-43))*I=43V+11V+30V=84V, 这时候小四轴角度变为84度。
第三次:Vout=Vin+(100-84)*P+((100-5)+(100-43)+(100-84))*I=84+3V+33V=120V。这时小四轴角度变为120度。
我们看到,在增加了积分控制后,小四轴角度在快速向目标角度靠近。
PID中的微分控制就是把角度的变化乘上一个常数来作为电机驱动输出。在上述例子中,假定D=0.2, 我们来看看比例,积分和微分共同控制下的系统反应。假定第一次前,电机转速保持5转,那么第一次前的角度变化为0。
第一次:Vout=Vin+(100-5)*P+(100-5)*I-(5-5)*D=5V+19V+19V-0V=43V,这时候小四轴角度为43度。
和上一次相比,角度从5度变化到了43度,所以小四周角度变化是43-5=38度。
第二次:Vout=Vin+(100-43)*P+((100-5)+(100-43))*I-(43-5)*D=43V+11V+30V-7V=77V, 这时候小四周角度77度。
把上述的计算结果列出来,我们看到:
从上面的数据,我们可以看到:
1,单独比例控制的时候,数据慢慢接近目标 (图表中的红色线)
2,加入积分控制之后,数据快速接近目标 (图表中的蓝色线)
3,微分控制起到抑制变化的作用。(图表中的绿色线)
有了这些理论基础,就可以写PID控制代码拉。
========圆点博士小四轴之PID控制代码分析=======
在圆点博士小四轴2014版代码里,我们只使用到PD参数。
首先我们来看PID中的比例控制。跟上一节模型提到的一样,比例是针对误差的控制。
首先我们获取小四轴当前角度。
bs004_angle_cur_pitch=bs004_imu_pitch;
bs004_angle_cur_roll =bs004_imu_roll;
把当前角度和目标角度相减,就可以得到角度偏差。
bs004_angle_err_pitch=bs004_angle_cur_pitch-bs004_angle_target_pitch;
bs004_angle_err_roll=bs004_angle_cur_roll-bs004_angle_target_roll;
然后进行比例控制:
bs004_fly_m1=bs004_fly_m1
+bs004_pitch_p*bs004_angle_err_pitch
-bs004_roll_p *bs004_angle_err_roll
-bs004_yaw_p*bs004_angle_err_yaw;
bs004_fly_m2=bs004_fly_m2
-bs004_pitch_p*bs004_angle_err_pitch
-bs004_roll_p *bs004_angle_err_roll
+bs004_yaw_p*bs004_angle_err_yaw;
bs004_fly_m3=bs004_fly_m3
-bs004_pitch_p*bs004_angle_err_pitch
+bs004_roll_p *bs004_angle_err_roll
-bs004_yaw_p*bs004_angle_err_yaw;
bs004_fly_m4=bs004_fly_m4
+bs004_pitch_p*bs004_angle_err_pitch
+bs004_roll_p *bs004_angle_err_roll
+bs004_yaw_p*bs004_angle_err_yaw;
在上一节模型中,我们提到PID中的微分控制针对的是角度变化而进行的控制。
所以我们首先要得到当前角度和上一次角度的差异。
bs004_angle_dif_pitch=bs004_angle_cur_pitch-bs004_angle_last_pitch;
bs004_angle_dif_roll =bs004_angle_cur_roll-bs004_angle_last_roll;
bs004_angle_dif_yaw =bs004_angle_last_yaw-bs004_angle_cur_yaw;
然后进行微分控制:
bs004_fly_m1=bs004_fly_m1
+bs004_pitch_d*bs004_angle_dif_pitch
-bs004_roll_d *bs004_angle_dif_roll
-bs004_yaw_d*bs004_angle_dif_yaw;
bs004_fly_m2=bs004_fly_m2
-bs004_pitch_d*bs004_angle_dif_pitch
-bs004_roll_d *bs004_angle_dif_roll
+bs004_yaw_d*bs004_angle_dif_yaw;
bs004_fly_m3=bs004_fly_m3
-bs004_pitch_d*bs004_angle_dif_pitch
+bs004_roll_d *bs004_angle_dif_roll
-bs004_yaw_d*bs004_angle_dif_yaw;
bs004_fly_m4=bs004_fly_m4
+bs004_pitch_d*bs004_angle_dif_pitch
+bs004_roll_d *bs004_angle_dif_roll
+bs004_yaw_d*bs004_angle_dif_yaw;
从上面的描述我们可以看出,小四轴的PID控制还是比较简单的。
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
|