px4固定翼无人机姿态控制理解

Posted 2020-09-29

　　学习px4代码也有一段时间了，所以想写一写，自己的一些学习心得吧，也算是笔记吧。

　　在px4这套代码中，每一个功能都是一个模块，例如姿态控制，也就是一个应用程序，我们可以把它添加到初始话脚本里，让它自启动。需要注意的就是在一个应用程序就是处理订阅的消息，然后发布处理过后的消息。这种消息机制就是uorb消息机制，可以找资料学习它具体实现的一个过程。

　　接下来，就逐步学习一下这个姿态控制的模块。

　　首先就是应用程序的入口，“extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[])”，在这个函数里就实现了，这个程序是否已近启动，如果没有启动就会注册函数来启动。

　　task_main()这个函数就是整个姿态控制的关键，具体姿态控制的算法就是在这里面实现。刚开始就介绍说每个应用程序会订阅相应的消息，然后处理，最后发布。

所以刚开始就是一些消息的订阅：

_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));//姿态设定点

_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));//飞机状态

_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);//加速度值
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));//控制模式

_params_sub = orb_subscribe(ORB_ID(parameter_update));//参数更新

_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));//手动控制的设定

_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));//车辆全球位置

_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));//飞机状态

_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));//着陆探测

接下来就是以check的方式获取订阅消息的值：

vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_status_poll();
vehicle_land_detected_poll();

接下来就是以阻塞等待方式检查参数是否更新，以及当前的飞机状态。阻塞等待这种获取订阅消息的方式，具体是怎么实现的可以查找资料进行学习。

这个if函数就是姿态控制运行的开始，如果姿态发生了改变，就运行这个if

if (fds[1].revents & POLLIN) 

那么要知道姿态是否发生改变，那么就需要知道当前的姿态，这通过获取当前姿态的消息，然后得到四元数，转化为旋转矩阵，进而求得姿态角：

 1 orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
 2 
 3 
 4 /* get current rotation matrix and euler angles from control state quaternions */
 5 math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
 6 _R = q_att.to_dcm();
 7 
 8 math::Vector<3> euler_angles;
 9 euler_angles = _R.to_euler();
10 _roll    = euler_angles(0);
11 _pitch   = euler_angles(1);
12 _yaw     = euler_angles(2);

由于姿态控制算法在一些模式下面是不会估算姿态设定点的，所以要确认这些标志。

_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;//判断垂尾，用于自主起飞。

判断是否安全故障，如果安全故障开启，就会设定降落伞。

接下来是襟翼的设定，分为手动控制和自动控制

 1             float flap_control = 0.0f;
 2 
 3             /* map flaps by default to manual if valid */
 4             if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled
 5                 && fabsf(_parameters.flaps_scale) > 0.01f) {
 6                 flap_control = 0.5f * (_manual.flaps + 1.0f) * _parameters.flaps_scale;
 7 
 8             } else if (_vcontrol_mode.flag_control_auto_enabled
 9                    && fabsf(_parameters.flaps_scale) > 0.01f) {
10                 flap_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f;
11             }
12 
13             // move the actual control value continuous with time, full flap travel in 1sec
14             if (fabsf(_flaps_applied - flap_control) > 0.01f) {
15                 _flaps_applied += (_flaps_applied - flap_control) < 0 ? deltaT : -deltaT;
16 
17             } else {
18                 _flaps_applied = flap_control;
19             }

然后是襟副翼的设定，分为手动控制和自动控制

 1 if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled
 2                 && fabsf(_parameters.flaperon_scale) > 0.01f) {
 3                 flaperon_control = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale;
 4 
 5             } else if (_vcontrol_mode.flag_control_auto_enabled
 6                    && fabsf(_parameters.flaperon_scale) > 0.01f) {
 7                 flaperon_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f;
 8             }
 9 
10             // move the actual control value continuous with time, full flap travel in 1sec
11             if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) {
12                 _flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? deltaT : -deltaT;
13 
14             } else {
15                 _flaperons_applied = flaperon_control;
16             }

接下来就是通过订阅的消息来判断是否能够控制姿态

也就是这一行代码：

1 if (_vcontrol_mode.flag_control_attitude_enabled)

在这个if语句里面执行的主要有判断空速是否有效，如果无效设定空速为参数设定，如果有效设定空速为测量或者计算的空速。

通过vehicle_global_position来计算飞机的地面速度。

如果自问模式下面，我们需要通过遥控器来产生姿态设定点；

计算机体坐标系下飞机的速度；

准备姿态控制器运行需要的参数；

 1                 control_input.roll = _roll;
 2                 control_input.pitch = _pitch;
 3                 control_input.yaw = _yaw;
 4                 control_input.roll_rate = _ctrl_state.roll_rate;
 5                 control_input.pitch_rate = _ctrl_state.pitch_rate;
 6                 control_input.yaw_rate = _ctrl_state.yaw_rate;
 7                 control_input.speed_body_u = speed_body_u;
 8                 control_input.speed_body_v = speed_body_v;
 9                 control_input.speed_body_w = speed_body_w;
10                 control_input.acc_body_x = _accel.x;
11                 control_input.acc_body_y = _accel.y;
12                 control_input.acc_body_z = _accel.z;
13                 control_input.roll_setpoint = roll_sp;
14                 control_input.pitch_setpoint = pitch_sp;
15                 control_input.yaw_setpoint = yaw_sp;
16                 control_input.airspeed_min = _parameters.airspeed_min;
17                 control_input.airspeed_max = _parameters.airspeed_max;
18                 control_input.airspeed = airspeed;
19                 control_input.scaler = airspeed_scaling;
20                 control_input.lock_integrator = lock_integrator;
21                 control_input.groundspeed = groundspeed;
22                 control_input.groundspeed_scaler = groundspeed_scaler;

前面的判断就是为运行姿态控制器所准备的。

1 _roll_ctrl.control_attitude(control_input);
2 _pitch_ctrl.control_attitude(control_input);
3 _yaw_ctrl.control_attitude(control_input);
4 _wheel_ctrl.control_attitude(control_input);

上面就是计算目标与当前姿态的角度误差值,对于roll和pitch是计算角度误差，然后算出角速率，对于yaw速率的计算是，假设在没有侧向力的情况下,通过计算可以得到相应的yaw速率：

就是通过如下计算公式得到：

以及：

                                                                                                                                                                                                                                                             、

计算得到相应的yaw速率；

由于滚转，俯仰和偏航速率是在地面坐标系下，因此，要通过坐标转换转换到机体坐标系下，也就是：

                                                                                                                                                                           通过上述转换就把角速率转换到机体坐标系下了，接下来是通过误差，运用pid来控制姿态，下面的还需要研究一下。先这样吧。

      以上内容均属自己理解，有错误之处见谅，共同讨论。