分享一个3天时间做得飞控程序，通俗易懂
本帖最后由 sdu1028 于 2014-5-14 14:56 编辑因为之前仿过MK的四轴，不过没做过软件。上次五一假期自己写了下飞控算法弥补一下。简单粗暴，直接有效
code全是自己做的，最简单的complementary filter，加RPY的PID控制。事实证明这种简单的算法还是最有效的，其实大部分时间都耗在调参数上。
用的Arduino的板子和GY-88 IMU snesor，还有一个超声测距模块,用来定高。
遥控用串口转蓝牙模块，还写了一个Android App来控制。
上video，还算稳定：
http://v.youku.com/v_show/id_XNzExNTc4ODIw.html
http://v.youku.com/v_show/id_XNzExODU1NTMy.html
附PID控制部分的code:
void loop()
{
dt_us_=micros()-time_us_;
time_us_=micros();
//There's no delays in this sketch while doing distance pings.
if (millis() >= pingTimer) {
pingTimer += pingSpeed; // Set the next ping time.
sonarAltitude.ping_timer(echoCheck); // Send out the ping
}
//update IMU data
IMU_proc();
rotation_x = imu_data - sensor_to_ground_bias;
rotation_y = imu_data - sensor_to_ground_bias;
rotation_z = imu_data - sensor_to_ground_bias;
//Caculate average rotation speed, degrees per 100ms
float drotation_x = 100.0f*(rotation_x - hrotation_x)/(((float)dt_us_)/1000.0f);
float drotation_y = 100.0f*(rotation_y - hrotation_y)/(((float)dt_us_)/1000.0f);
float drotation_z = 100.0f*(rotation_z - hrotation_z)/(((float)dt_us_)/1000.0f);
//serialPrintRotation();
//printserialChart();
getSerial3Cmd();
//PD Balance Control
adjustAC = - ( P_ * (rotation_x - desired_pitch_x) - D_ * drotation_x );
adjustBD = - ( P_ * (rotation_y - desired_pitch_y) - D_ * drotation_y );
//PD Yaw control
desired_heading = hrotation_z;
heading_integ_value = rotation_z-lock_yaw;// + I_HEADING_*(rotation_z -desired_heading);
float heading_trim = heading_integ_value*P_HEADING_ - drotation_z* D_HEADING_;
heading_trim = limitAbs(heading_trim, HEADING_TREIM_LIMIT);
//PID controller for altitude
float altitude_trim = - P_ALT_T_ * (sonar_altitude_cm_ - desired_altitude) - D_ALT_T_*daltitude_cm_;
altitude_trim = limitAbs(altitude_trim, MAX_ALTI_TRIM );
//limit max speed
speeda=limitMAXSPD(upspeed + adjustAC + heading_trim + altitude_trim);
speedb=limitMAXSPD(upspeed + adjustBD - heading_trim + altitude_trim);
speedc=limitMAXSPD(upspeed - adjustAC + heading_trim + altitude_trim);
speedd=limitMAXSPD(upspeed - adjustBD - heading_trim + altitude_trim);
//update speed
setFormatedSpeed(1, speeda); //a
setFormatedSpeed(2, speedb); //b
setFormatedSpeed(3, speedc); //c
setFormatedSpeed(4, speedd); //d
//Record history rotation
hrotation_x = rotation_x;
hrotation_y = rotation_y;
hrotation_z = rotation_z;
dt_us_=micros()-time_us_;
if(dt_us_ < CONTROL_CYCLE_US)
delayMicroseconds(CONTROL_CYCLE_US-dt_us_); //delay to 5ms ->200Hz
}
之前做两轮平衡载人小车，也是用这样简单粗暴的算法，没整那么多，也还行吧。。。 效果很不错啊，这是只用到了PD控制吗？调PD参数大概有什么诀窍吗？我用的也是这种方法，但是目前发现四轴很不稳定，卡在参数调节上了。还在考虑要不要寻找更复杂点的控制方法呢。 你的处理器是什么核心，貌似很牛的样子！ 简单的complementary filter 一旦yaw快速旋转就崩。。没有滤除向心加速度 qwe2231695 发表于 2014-5-13 22:52
简单的complementary filter 一旦yaw快速旋转就崩。。没有滤除向心加速度
对呀，所以在程序里面把yaw lock了 ad1984fr 发表于 2014-5-13 22:43
效果很不错啊，这是只用到了PD控制吗？调PD参数大概有什么诀窍吗？我用的也是这种方法，但是目前发现四轴很 ...
只是pd，调的时候把它绑起来，先只开一个轴，开到大概悬停时的油门，然后先p后d。能稳定飞了再yaw。然后定高。
控制器用的Arduino mega，就是avr sdu1028 发表于 2014-5-13 23:31
对呀，所以在程序里面把yaw lock了
恩。大概明白调试的流程了，我现在两个参数同时调，怎么都不行，原来是方法有问题。谢谢！目前我的YAW也lock了，感觉确实会崩。 真勇敢.大四轴也敢用手机控制. qwe2231695 发表于 2014-5-13 22:52
简单的complementary filter 一旦yaw快速旋转就崩。。没有滤除向心加速度
那怎么做才能滤除向心加速度呢？ SMC 发表于 2014-5-14 08:52
那怎么做才能滤除向心加速度呢？
我是加上了对合加速度的判断，如果和加速度比重力大于一定范围就丢弃这一帧的值
这是code：
float alpha = 0.98f;
//if acce is beyond normal gravity
if( abs( square((float)accel_x) + square((float)accel_y) + square((float)accel_z ) - GRAVITY_REF) > GRAVITY_REF*0.03f )
{
alpha=1.0f;
}
float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x;
float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y;
float angle_z = gyro_angle_z; //Accelerometer doesn't give z-angle sdu1028 发表于 2014-5-14 15:04
我是加上了对合加速度的判断，如果和加速度比重力大于一定范围就丢弃这一帧的值
这是code：
是个好主意，但我觉得应该还有别的法子解决。 sdu1028 发表于 2014-5-13 23:38
只是pd，调的时候把它绑起来，先只开一个轴，开到大概悬停时的油门，然后先p后d。能稳定飞了再yaw。然后 ...
不是“程序里面把yaw lock”了么? fengyunyu 发表于 2014-5-14 15:32
不是“程序里面把yaw lock”了么?
靠对Yaw的PID控制lock yaw mark一下备用{:smile:} 楼主可否分享一下定高的想法。因为在只有姿态控制的情况下，四旋翼一下子飞高一下子飞低，看着就不很爽。 976717326 发表于 2014-5-16 10:25
楼主可否分享一下定高的想法。因为在只有姿态控制的情况下，四旋翼一下子飞高一下子飞低，看着就不很爽。 ...
也是靠PID控制。高度小于30cm的时候油门是加油门，大于30cm的时候，高度PID控制启用，油门是改变PID的的desired hight
超声波测距做反馈。
当然会比较不靠谱，比如下面遇到一个花盆什么的就忽然高了。 sdu1028 发表于 2014-5-14 15:04
我是加上了对合加速度的判断，如果和加速度比重力大于一定范围就丢弃这一帧的值
这是code：
这么做是一种很危险的行为，虽然我没想出更好的办法，但是不推荐这样做 qwe2231695 发表于 2014-5-13 22:52
简单的complementary filter 一旦yaw快速旋转就崩。。没有滤除向心加速度
虽然你不愿意讲如何滤除向心加速度，但我在查这个的时候发现了一个更严重的问题。 SMC 发表于 2014-6-9 04:05
虽然你不愿意讲如何滤除向心加速度，但我在查这个的时候发现了一个更严重的问题。 ...
我也没有办法滤除啊，只有yaw转速大时优化一下而已。你还发现了什么。 MARK以备后用顺便帮顶! qwe2231695 发表于 2014-6-9 09:27
我也没有办法滤除啊，只有yaw转速大时优化一下而已。你还发现了什么。
这个问题可能只有我才犯得，就是直接积分每个轴数据当角度在有些情况下误差会很大。
mark。。。。。。。 MSRA的同学 你好。。 Surfire 发表于 2014-6-9 15:19
MSRA的同学 你好。。
楼上也在msra么 SMC 发表于 2014-6-9 04:02
这么做是一种很危险的行为，虽然我没想出更好的办法，但是不推荐这样做 ...
没有，实际证明这个方法很安全。现在加了optical的sensor后可以一直悬停十分钟到没电 SMC 发表于 2014-6-9 12:29
这个问题可能只有我才犯得，就是直接积分每个轴数据当角度在有些情况下误差会很大。
...
gyro有yaw的反馈，直接限制yaw的速度就是了。 感觉不错 很好 sdu1028 发表于 2014-6-10 02:21
楼上也在msra么
上个月底刚checkout
看到你的帖子小小的吃惊了一下 好像还看到过你的灰机 有点晃 跪拜大神。请问姿态解算的部分能否交流一下。
页:
[1]