一、开篇
姿态控制篇终于来了、来了、来了~~~
心情爽不爽？愉悦不愉悦？开心不开心？
喜欢的话就请我吃顿饭吧，哈哈。
其实这篇blog一周前就应该写的，可惜被上一篇blog霸占了。但是也不算晚，整理了很多算法基础知识，使得本篇blog更充实。一人之力总是有限的，难免有不足之处，大家见谅，有写的不好的地方劳烦指正。看到标题了吧，属于连载篇，所以后续还会有相关问题的补充的。
三、实验平台
Software Version：PX4Firmware
Hardware Version：pixhawk
IDE：eclipse Juno （Windows）
四、基础知识
1、写在前面
无人机控制部分主要分为两个部分，姿态控制部分和位置控制部分；位置控制可用远程遥控控制，而姿态控制一般由无人机系统自动完成。姿态控制是非常重要的，因为无人机的位置变化都是由姿态变化引起的。
下图阐释了PX4源码中的两个环路控制，分为姿态控制和位置控制。
补充：关于Pixhawk原生固件中姿态(估计/控制)和位置(估计/控制)源码的应用问题
PX4Fireware原生固件中的modules中姿态估计有多种：Attitude_estimator_ekf、Attitude_estimator_q、ekf_att_pos_estimator。
位置估计有：ekf_att_pos_estimator、local_position_estimator、position_estimator_inav
姿态控制有：fw_att_control、mc_att_control、mc_att_control_multiplatform、vtol_att_control
位置控制有：fw_pos_control_l1、fw_pos_control_l1、mc_pos_control_multiplatform
四旋翼用到以上哪些估计和控制算法呢？这部分在启动代码rc.mc_app里面有详细的说明。
默认的是：
姿态估计 Attitude_estimator_q
位置估计 position_estimator_inav
姿态控制 mc_att_control
位置控制 mc_pos_control
2、飞行控制（该部分属于理论概述）
飞行控制分为姿态控制和位置控制，该文章主讲姿态控制。
所谓姿态控制，主要就是在前期姿态解算的基础上对四旋翼飞行器进行有效的飞行控制，以达到所需要的控制效果。在这种情况下，算法要学会如何连续地做决策，并且算法的评价应该根据其所做选择的长期质量来进行。举一个具体的例子，想想无人机飞行所面临的难题：每不到一秒，算法都必须反复地选择最佳的行动控制。控制过程还是以经典的PID反馈控制器为主（在控制环路中可以添加smith预测器）。那么如何实现控制呢?
以四旋翼飞行器为例，主要就是通过改变旋翼的角速度来控制四旋翼无人机。每个旋翼产生一个推力（F1、F2、F3、F4）和一个力矩，其共同作用构成四旋翼无人机的主推力、偏航力矩、俯仰力矩和滚转力矩。在四旋翼无人机中，正对的一对旋翼旋转方向一致，另外一对与之相反，来抵消静态平稳飞行时的回转效应和气动力矩。升降以及RPY的实现不在赘述。控制对象就是四旋翼无人机，其动力学模型可以描述为：将其视为有一个力和三个力矩的三维刚体。如下给出了小角度变化条件下的四旋翼无人机的近似动力学模型：
PS：PX4的姿态控制部分使用的是roll-pitch和yaw分开控制的（是为了解耦控制行为），即tilt和torsion两个环节。感性认识一下，如下图：
根据经验所得，控制toll-pitch比控制yaw更容易实现。比如同样是实现10°的变化，roll-pitch需要60ms左右；但是yaw控制器却需要接近150ms。（上面两幅图是出自DJI某哥写的论文里面，仅作参考，结合理解Pixhawk）
控制流程：
1）、预处理：各参数的初始化。
2）、稳定roll-pitch的角速度。
3）、稳定roll-pitch的角度。
4）、稳定yaw的角速度。
5）、稳定yaw的角度。
其中在第五步中有一个yaw的前馈控制（MC_YAW_FF）：There is MC_YAW_FF parameter that controls how much of userinput need to feed forward to yaw rate controller. 0 means very slow control,controller will start to move yaw only when sees yaw position error, 1 meansvery fast responsive control, but with some overshot, controller will move yawimmediately, always keeping yaw error near zero。This parameter is not critical and can be tuned in flight, inworst case yaw responce will be sluggish or too fast. Play with FF parameter toget comfortable responce. Valid range is 0…1. Typical value is 0.8…0.9. (Foraerial video optimal value may be much smaller to get smooth responce.) Yawovershot should be not more than 2-5%。
摘自：https://pixhawk.org/users/multirotor_pid_tuning
3、 进入姿态控制源码的前期过程
首先感性认识一下姿态控制部分的框架，控制部分分为内外环控制，内环控制角速度、外环控制角度。控制过程是先根据目标姿态（target）和当前姿态（current）求出偏差角，然后通过角速度来修正这个偏差角，最终到达目标姿态。
和姿态解算算法的流程几乎类似，主要的代码流程首先就是按照C++语言的格式引用C语言的main函数，但是在该处变成了：
extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[])。
然后捏：跳转到所谓的main函数，该部分有个要注意的点，如下代码所示：即mc_att_control::g_control = new MulticopterAttitudeControl;//重点（934），new关键词应该不陌生吧，类似于C语言中的malloc，对变量进行内存分配的，即对姿态控制过程中使用到的变量赋初值。
int mc_att_control_main(int argc, char *argv[])
{
if (argc < 2) {
warnx("usage: mc_att_control {start|stop|status}");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (mc_att_control::g_control != nullptr) {
warnx("already running");
return 1;
}
mc_att_control::g_control = new MulticopterAttitudeControl;//重点
if (mc_att_control::g_control == nullptr) {
warnx("alloc failed");
return 1;
}
if (OK != mc_att_control::g_control->start()) {//跳转
delete mc_att_control::g_control;
mc_att_control::g_control = nullptr;
warnx("start failed");
return 1;
}
return 0;
}
然后捏：start函数
Int MulticopterAttitudeControl::start()
{
ASSERT(_control_task == -1);
/* start the task */
_control_task = px4_task_spawn_cmd("mc_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1500,
(px4_main_t)&MulticopterAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
其中上面有个封装了nuttx自带的生成task的任务创建函数（他把优先级什么的做了重新的define，这么做是便于代码阅读）：px4_task_spawn_cmd()，注意它的用法。其函数原型是：
px4_task_t px4_task_spawn_cmd(con
最低0.47元/天 解锁文章
6602
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
