飞控论文Word格式.docx

1、这种控制方式的好处是能对高度的变化进行第一时间的反应，因此高度控制较好，缺点是当油门失效时，比如发动机熄火发生时，由于高度降低飞控将使飞机保持经过限幅的最大仰角，最终由于动力的缺乏导致失速。但是以上仅仅是控制理论。在实际控制系统中，由于有些参量并不能较准确地测得，或者测量时数据不稳定，所以并不能完全按照上述的控制理论控制。例如空速的测量时相当不准确的，而且数据波动较严重，这样，就无法完全按照上述理论进行控制，必须在其基础上进行适当修改。以下以使用空速计情况和不使用空速计情况对APM飞控系统进行阐述。（1），使用空速计情况在使用空速计的情况下，升降舵是由空速控制。update_current_f

2、light_mode（ ）调用calc_nav_pitch（ ）调用nav_pitch = -g.pidNavPitchAirspeed.get_pid（airspeed_error, dTnav）。nav_pitch就是导航俯仰角，也就是说，使用空速计时，APM系统对利用空速偏差airspeed_error作为输入量进行导航级的俯仰角控制。 在使用空速计的情况下，油门是由飞机机械能偏差控制，也就是空速误差和高度误差共同决定。update_current_flight_mode（ ）调用calc_throttle（ ）调用g.channel_throttle.servo_out = g.thr

3、ottle_cruise + g.pidTeThrottle.get_pid（energy_error, dTnav）;g.channel_throttle.servo_out += （g.channel_pitch.servo_out * g.kff_pitch_to_throttle）;式中energy_error = airspeed_energy_error + （float）altitude_error * 0.098f，是空速动能偏差，加上飞机重力势能偏差。可以看出，油门是由设定的巡航油门g.throttle_cruise、机械能偏差PID调节量和升降舵通道补偿共同决定，但是巡航油

4、门是设定值，是固定的。g.kff_pitch_to_throttle默认是0，所以，实际上油门的增减是由机械能偏差控制的。所以，使用空速计时，APM飞控系统的油门升降舵控制属于空速控制升降，机械能控制油门方案，类似于第一种控制方案，但是又有点区别。（2），不使用空速计情况不使用空速计时，升降舵是由高度偏差控制。update_current_flight_mode（ ）中调用calc_nav_pitch（ ）调用nav_pitch = g.pidNavPitchAltitude.get_pid（altitude_error, dTnav）。所以升降舵的控制，是由高度误差altitude_erro

5、r作为PID调节的输入量。不使用空速计时，油门是由导航俯仰角控制。update_current_flight_mode（ ）调用calc_throttle（ ）调用if （nav_pitch = 0） g.channel_throttle.servo_out = throttle_target + （g.throttle_max - throttle_target） * nav_pitch / g.pitch_limit_max; else g.channel_throttle.servo_out = throttle_target - （throttle_target - g.throttl

6、e_min） * nav_pitch / g.pitch_limit_min;可以看出此时的油门控制是利用的是比例调节，依据的比例关系是=。二，如何让飞机飞往目标 要使飞机飞往目标，那就必须知道飞机当前位置、目标位置和当前航向等问题。在APM飞控系统中，GPS模块能够提供飞机当前经纬度信息，航迹方向和地速信息。根据这些信息，再用程序解算飞机当前位置和目标位置的关系，就能知道目标航向角target_bearing，知道了目标航向角target_bearing后就可以用于引导飞机飞向目标。但是仅用目标航向角进行导航，不能压航线飞行，为了解决这个问题，APM飞控系统中又增加了偏航距crosstrac

7、k_error的计算，并且根据偏航距，计算出需要的偏航修正量crosstrack_error * g.crosstrack_gain。使飞机能尽快飞到航线上。最后把目标航向角和偏航修正量组成导航航向角nav_bearing，提供给控制级PID。所以目标航向角的计算和偏航修正量的计算是构成如何让飞机飞往目标的核心。下面具体介绍APM中关于这部分的程序。APM飞控系统中的GPS信息只能每秒更新4-10次。所以，计算目标航向角和偏航修正量的程序都在每秒大约执行10次的medium_loop（ ）中。在medium_loop（ ） 的case 1中会执行navigate（ ），正是在这个函数中，执行了

8、导航航向角nav_bearing的计算。首先计算的是目标航向角。在navigate（ ）中有：target_bearing = get_bearing（¤t_loc, &next_WP）;nav_bearing = target_bearing;第一个语句中current_loc和next_WP是结构体，里面存储这一个位置点的经度、纬度、高度信息，current_lot中存储的是当前点，next_WP中存储的是目标点。根据这个进行在球体表面的三角函数计算（此文中，由于篇幅所限，很多东西不进行详细讲解），就可以得出目标航向target_bearing。接下来，要计算偏航修正量。nav

9、igate（ ）调用update_navigation（ ）调用verify_commands（ ）调用verify_nav_wp（ ）调用update_crosstrack（ ），这个函数中有：crosstrack_error = sin（radians（target_bearing - crosstrack_bearing） / 100） * wp_distance;nav_bearing += constrain（crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get（）, g.crosstrack_entr

10、y_angle.get（）;第一句是计算偏航距的，偏航距是飞机当前位置点到航线的距离，事实上就是求一个点到一条线之间的距离。wp_distance是这个直角三角形的斜边，target_bearing - crosstrack_bearing正是偏航距对应的边相对的那个锐角。第二句中crosstrack_error * g.crosstrack_gain使用偏航距乘以偏航修正增益就得出需要的偏航距修正量，然后使用constrain（ ）函数将偏航距修正量限制在-g.crosstrack_entry_angle.get（）与g.crosstrack_entry_angle.get（）之间。g.cr

11、osstrack_entry_angle.get（）其实就是最大的偏航距修正量。在上一段中target_bearing计算时已经有nav_bearing = target_bearing。现在又nav_bearing += constrain（crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get（）, g.crosstrack_entry_angle.get（），这样其实就把目标航向角和偏航距修正都加到了nav_bearing 中。3、如何让飞机按照导航级控制信息飞行在导航级，我们已经解算出了让飞机保持高度和空速

12、飞行所需要的俯仰角和油门，以及按航线飞向目标所需要的导航航向。这就解决了如何引导飞机进行飞行的问题。也就是说，飞控已经知道该怎么让飞机飞行了，现在就要解决飞控如何具体控制飞机的问题，也就是说如何控制各个舵机或者油门。（1）油门的控制油门的控制，前面已经提到，其实油门的控制量是在导航级完成的。并不传给控制级程序解算，直接就交给舵机控制级去控制舵机。（2）升降舵的控制对于升降舵的控制，在导航级已经给出了需要的俯仰角nav_pitch，此时，控制级的任务就是通过控制舵机让飞机保持规定的俯仰角nav_pitch。飞控通过惯性测量单元的DCM 算法能测量出飞机当前的俯仰角dcm.pitch_sensor

13、，然后利用目标俯仰角与当前俯仰角的差值作为控制级升降PID调节的输入，进行PID控制运算。程序如下：long tempcalc = nav_pitch +fabs（dcm.roll_sensor * g.kff_pitch_compensation） + （g.channel_throttle.servo_out * g.kff_throttle_to_pitch） - （dcm.pitch_sensor - g.pitch_trim）;g.channel_pitch.servo_out = g.pidServoPitch.get_pid（tempcalc, delta_ms_fast_loo

14、p, speed_scaler）;语句中作为PID控制的输入量是tempcalc，而tempcalc除了包含目标俯仰角与当前俯仰角的差值（nav_pitch-dcm.pitch_sensor）外，还包含了fabs（dcm.roll_sensor * g.kff_pitch_compensation）、（g.channel_throttle.servo_out * g.kff_throttle_to_pitch）和g.pitch_trim。其作用如下：fabs（dcm.roll_sensor * g.kff_pitch_compensation）作用： 加入这个是因为飞机滚转时时会掉高度，所以提

15、前加入了掉高度的预判fabs（dcm.roll_sensor * g.kff_pitch_compensation）。其中g.kff_pitch_compensation默认值是0.3。（g.channel_throttle.servo_out * g.kff_throttle_to_pitch）作用： 其中g.kff_throttle_to_pitch值为0，也就是默认不加入这个影响。所以在此可以忽略油门控制量对升降舵控制的影响。当然，也可以通过地面站调节这个值。加入的目的也是用于预判。g.pitch_trim作用： 这是升降舵的微调值，调试飞机时，为了使飞机平飞，会调节升降微调，对升降通道

16、的微调会记录在其中。dcm.pitch_sensor - g.pitch_trim正是平飞需要的仰角。语句中的speed_scaler是一个缩放因子，用于缩放输出的控制量，它与空速或油门有关。（3）副翼的控制飞机的转弯靠的是滚转副翼，同时拉升降舵，为了消除侧滑还需要打方向舵。所以要想让飞机转弯，导航级会根据转弯的大小通过PID算法给出需要的侧倾量。前面已经看到升降通道在发现飞机侧倾时会根据g.kff_pitch_compensation给出侧倾时需要的升降舵补偿。即使这个补偿不够，升降通道也会在发现飞机掉高度后拉升降舵。所以，升降舵总能配合副翼在侧倾时不掉高度进行转弯。导航级只要给出需要的侧倾

17、量nav_roll就行。既然导航级已经给出了当前需要的导航航向nav_roll（也就是导航侧倾角），那么控制级的任务就是控制飞机保持这个侧倾角。因此飞控就用DCM算法得出的飞机当前侧倾角dcm.roll_sensor与导航侧倾角nav_roll之间的偏差作为控制级侧倾的PID调节输入量，进行PID解算出需要的调节量。g.channel_roll.servo_out = g.pidServoRoll.get_pid（nav_roll - dcm.roll_sensor）, delta_ms_fast_loop, speed_scaler）;（4）方向舵的控制在飞行中，方向舵是配合飞机转弯用的，用

18、来消除飞机转弯时的侧滑，也就是用来辅助转弯用的。只有在着陆以后，方向舵才能控制机轮以控制飞机转向。在导航级并没有对飞机方向舵的控制，只在控制级才有。stabilize（ ）调用calc_nav_yaw（speed_scaler）中有：g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out + g.pidServoRudder.get_pid（error, delta_ms_fast_loop, speed_scaler）;语句中g.channel_roll.servo_out是副翼的控制量，g.kff_r

19、udder_mix是方向与升降之间的关联增益。error是飞机横向的加速度，就是侧滑加速度，是DCM算法解算出来的。由此可以看出方向舵的控制由副翼控制量和横向加速度PID调节量共同决定。这样，通过两级PID控制，飞机就能按照预定的要求飞行。这两级PID控制就是APM飞控系统的核心。当然，APM飞控系统的内容远不止这些，这只不过是是飞控中最核心的部分。其中涉及到的APM飞控系统的DCM算法也是飞控系统的重要组成部分。这里不详细介绍，但是APM飞控DCM算法在进行校准时忽略了纵向加速度，认为纵向加速度始终为0，所以这是APM飞控DCM算法的一个重大缺陷。在此，由于本文是写给搞飞控是专业人士看的，所

20、以不再详细讲述DCM算法。四，APM飞控系统主程序安排。void loop（） / We want this to execute at 50Hz if possible / - if （millis（）-fast_loopTimer 19） 1 delta_ms_fast_loop = millis（） - fast_loopTimer; 2 load= （float）（fast_loopTimeStamp - fast_loopTimer）/delta_ms_fast_loop; /cpu使用率 3 G_Dt=（float）delta_ms_fast_loop/1000.f; /陀螺仪积分

21、时间（DCM算法） 4 fast_loopTimer = millis（）; 5 mainLoop_count+; 6 / Execute the fast loop / - fast_loop（）; 7 / Execute the medium loop / - medium_loop（）; 8 counter_one_herz+; 9 if（counter_one_herz = 50） 10 one_second_loop（）; 11 counter_one_herz = 0; 12 if（millis（）-perf_mon_timer20000） 13 /性能监控时间到，执行性能监控 i

22、f （mainLoop_count != 0） 14 gcs.send_message（MSG_PERF_REPORT）; 15 if （g.log_bitmask & MASK_LOG_PM） 16 Log_Write_Performance（）; 17 resetPerfData（）; 18 fast_loopTimeStamp = millis（）; 19 以上是飞控系统的主循环程序，有19条语句，我在右侧标了语句号。循环的频率是50Hz，与标准舵机控制频率相同，这是通过第一条语句if （millis（）-fast_loopTimer 19）实现的，这个语句中millis（ ）函数在程序

23、开始运行后开始执行，中间不停顿，返回的是从程序开始一直到当前的时间（毫秒），在第5条语句有fast_loopTimer = millis（）; 所以if （millis（）-fast_loopTimer 19）的意思就是如果现在的时间距离上一次执行时间超过了19ms，也就是20ms的时候，就会执行一次下面所有的程序，如果不满足条件，就一直等待。接下来从第2条语句到第6条，除了第三条语句是计算主程序执行一次的时间外（可以认为是CPU使用率），其他的都是做标记用。接下来的程序是执行fast_loop（）、medium_loop（）、one_second_loop（）以及20秒一次的性能监视。此外，

24、在medium_loop（ ）中还会调用slow_loop（ ）（执行周期1/3s）。其他语句是辅助作用。飞控系统的主程序执行的就是这几个子程序。以下一一说明功能。（1） fast_loop（ ） ： 这是飞控系统控制的控制核心之一。执行频率50Hz。void fast_loop（） / This is the fast loop - we want it to execute at 50Hz if possible / - if （delta_ms_fast_loop G_Dt_max） G_Dt_max = delta_ms_fast_loop; / Read radio /读取遥控信号

25、/ - read_radio（）; /APM_RC.InputCh（CH_ROLL）- ch1_temp- g.channel_roll.set_pwm（ch1_temp）（即转为control_in） /油门还有control_failsafe（g.channel_throttle.radio_in）; /g.channel_throttle.servo_out = g.channel_throttle.control_in; /airspeed_nudge,throttle_nudge, / check for loss of control signal failsafe conditi

26、on /检查丢失控制信号故障安 / - check_short_failsafe（）; /关于failsafe和ch3_failsafe的处理 / Read Airspeed /读取空速 / - if （g.airspeed_enabled = true & HIL_MODE != HIL_MODE_ATTITUDE） /使能空速计真实飞行时为1read_airspeed（）; /读取空速，并calc_airspeed_errors（）; else if （g.airspeed_enabled = true & HIL_MODE = HIL_MODE_ATTITUDE） /真实飞行时为0 calc_airspeed_errors（）; #if HIL_MODE = HIL_MODE_SENSORS /飞行模式时为0 / update hil before dcm update hil.update（）; #endif dcm.update_DCM（G_Dt）; /更新DCM / uses the yaw from the DCM to give more accurate turns /使用从DCM获得的偏航数据，进行更精确的转弯 calc_bearing_error（）; /计算得出bearing_error # if HIL_MODE = HIL_MODE_DISABLED