开源飞控PX4—Mission模式的部分逻辑梳理和S曲线的应用

andyjo

一、Mission模式的定义




Mission模式下的地面站界面（图片摘自PX4官方手册）

Mission模式的官方定义是：使飞机执行已上传到飞行控制器的预定义的自主任务。通常使用地面站（GCS）应用程序如QGroundControl创建和上传任务。Misson模式是近期PX4版本的新名字，之前也称为AUTO模式，同样也对应开源飞控APM的AUTO模式。翻译一下就是通过地面站或API等生成航点指令，并将航点信息存储在飞控内部，由飞控处理航点逻辑，实现航点轨迹跟踪。



Mission模式轨迹实现（图片摘自PX4官方手册）

新版PX4固件（1.11.3）更新的S曲线在Mission的应用，S曲线的原理在之前文章里有讲过，本文第五节会再做总结。本文的研究重点是如何将S曲线与Mission模式中的航迹跟踪结合起来。思考几个问题：

• S曲线的输入输出是什么？
  
• S曲线的输入如何由Miison模式产生的？
  
• 外部输入至Mission模式的航点如何规划？
  
• S曲线的输出如何作用于位置控制器/姿态控制器？
  

最终的跟踪效果如下图所示：



PX4航迹跟踪（图片摘自PX4官方手册）

二、三重目标位置结构体的定义 position_setpoint_triplet_s

Mission模式中有position_setpoint_triplet这个概念。这个名词怎么翻译都别扭，但是字面意思很好理解，“三个”“位置”的“设定值”，分别是previous setpoint（上一个航点），current setpoint（当前目标航点）和next setpoint（下一个航点）。这个结构体的定义可能封装在库中，直接全局搜索是查不到的，这里参考PX4的官网手册。
该结构体是在<position_setpoint_triplet.h>头文件里声明的：
struct position_setpoint_triplet_s {
     uint64_t timestamp;
     struct position_setpoint_s previous;
     struct position_setpoint_s current;
     struct position_setpoint_s next;
     //部分略
};

结构体成员previous、current、next同样是结构体position_setpoint_s，结构体成员表征目标航点的信息，部分有效部分无效，PX4在使用时判断十分详细，所以大家看到的代码就非常多，各取所需即可。
struct position_setpoint_s {
     uint64_t timestamp;       // 时间戳
     double lat;               // 目标纬度
     double lon;               // 目标经度
     float x;                  // 目标位置 （local坐标系）
     float y;           
     float z;           
     float vx;                 // 目标速度
     float vy;         
     float vz;         
     float loiter_radius;      // 悬停半径
     float a_x;                // 目标加速度
     float a_y;
     float a_z;
     float acceptance_radius;  // 到点判断半径
     float cruising_speed;     // 巡航速度
     // 部分略
};
三、三重目标位置的来源

这里牵扯到PX4的uORB消息机制：oURB是一种异步publish/subscribe的消息传递API，用于进程或线程间的通信。
以本文的position_setpoint_triplet_s为例：
uORB::Publication<position_setpoint_triplet_s>        _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)};
_pos_sp_triplet_pub中的_pub即为发布，实现该结构体实例的更新。在查找某主题在哪里更新时，查找该主题在哪个头文件发布即可。_pos_sp_triplet_pub发布的地方在两个头文件中：“navigator.h”和“mavlink_recevier.h”中。
从原理上分析也可以理解，该主题定义三重目标位置，目标位置在哪里更新呢？
一种是飞控内部更新，提前将航点信息存储在飞控内部，由navigator文件处理，生成三重目标位置，即为本文的Mission模式；
另一种是由上层通过Mavlink通讯机制传输，即为Offboard模式，实现避障、绕章等功能。
uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
_sub_triplet_setpoint中的_sub即为订阅，实现该结构体实例的调用，这部分在下节中讲解。
四、三重目标位置的使用

头文件"FlightTaskAuto.hpp"中订阅了position_setpoint_triplet的主题，表示该文件调用了三重目标位置的航点信息。其中，整个_evaluateTriplets()函数都在评估航点信息的有效性、有效性、有效性....以及是否通过和上一帧对比，判读外部传入的航点信息是否被更新。如果航点更新，那么就调用_updateInternalWaypoints()函数规划真正用于位置控制的目标航点信息。其实就是根据当前位置和三个目标航点位置对比，重新排列。贴代码，上图。
void FlightTaskAuto::_updateInternalWaypoints()
{
        // The internal Waypoints might differ from _triplet_prev_wp, _triplet_target and _triplet_next_wp.
        // The cases where it differs:
        // 1. The vehicle already passed the target -> go straight to target
        // 2. The vehicle is more than cruise speed in front of previous waypoint -> go straight to previous waypoint
        // 3. The vehicle is more than cruise speed from track -> go straight to closest point on track
        switch (_current_state) {
        case State::target_behind:
                _target = _triplet_target;
                _prev_wp = _position;
                _next_wp = _triplet_next_wp;
                break;

        case State::previous_infront:
                _next_wp = _triplet_target;
                _target = _triplet_prev_wp;
                _prev_wp = _position;
                break;

        case State::offtrack:
                _next_wp = _triplet_target;
                _target = matrix::Vector3f(_closest_pt(0), _closest_pt(1), _triplet_target(2));
                _prev_wp = _position;
                break;

        case State::none:
                _target = _triplet_target;
                _prev_wp = _triplet_prev_wp;
                _next_wp = _triplet_next_wp;
                break;

        default:
                break;

        }
}

函数根据_current_state状态分为了四个case，重点在_current_state如果计算的，该变量是在_getCurrentState()函数中定义的，变量名起得简单粗暴：
State FlightTaskAuto::_getCurrentState()
{
        // Calculate the vehicle current state based on the Navigator triplets and the current position.
        Vector2f u_prev_to_target = Vector2f(_triplet_target - _triplet_prev_wp).unit_or_zero();
        Vector2f pos_to_target(_triplet_target - _position);
        Vector2f prev_to_pos(_position - _triplet_prev_wp);
        // Calculate the closest point to the vehicle position on the line prev_wp - target
        _closest_pt = Vector2f(_triplet_prev_wp) + u_prev_to_target * (prev_to_pos * u_prev_to_target);

        State return_state = State::none;

        if (u_prev_to_target * pos_to_target < 0.0f) {
                // Target is behind.
                return_state = State::target_behind;

        } else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) {
                // Current position is more than cruise speed in front of previous setpoint.
                return_state = State::previous_infront;

        } else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _mc_cruise_speed) {
                // Vehicle is more than cruise speed off track.
                return_state = State::offtrack;

        }

        return return_state;
}
第一种情况 State::target_behind:
u_prev_to_target * pos_to_target <0.0f
两个向量点乘结果为负，表示两个向量夹角大于90°，结果如下图所示，此时飞行器已飞过当前目标航点。相应的，设置上一航点为当前位置，设置当前航点不变，设置下一航点不变。



第二种情况 State::previous_infront:
u_prev_to_target * prev_to_pos <0.0f&& prev_to_pos.length()> _mc_cruise_speed
向量u_prev_to_target和 向量prev_to_pos 的夹角大于90°，且此时距离上一航点的距离较远。图中紫色圆圈半径为_mc_cruise_speed，超过半径表示距离较远。相应的，设置上一航点为当前位置，设置当前航点为上一航点，设置下一航点为当前航点。即先向上一航点靠近，再飞向当前航点。



第三种情况 State::offtrack:
Vector2f(Vector2f(_position)- _closest_pt).length()> _mc_cruise_speed
closest_pt为当前位置pos距离航线最近的点，它和当前位置的距离即为侧偏距，如果侧偏距较大，表示飞行器离航线较远。相应的，设置上一航点为当前位置，设置当前航点为closest_pt，设置下一航点为当前航点。即如果侧偏距较大，则优先消除侧偏距。



第四种情况：State::none:
飞行器位置偏差不大，大致向当前航点飞行，那么保持position_setpoint_triplet不变。
小结：整个规划过程可以总结为：当前航点为大目标，根据飞行器当前位置设计一个个小目标（牵引点），通过跟踪小目标，一步一步跟踪至当前航点，完成大目标。通过牵引点的方式实现航迹跟踪和简单的侧偏控制。
五、_target数据流

在第四节中，更新了当前航点_target的信息，在void FlightTaskAutoMapper类的成员函数_preparePositionSetpoints()中，更新了_position_setpoint。函数中速度设定值_velocity_setpoint为空，因为AUTO模式的目标速度是根据目标航点位置计算的，所以不需要外部输入指令：
void FlightTaskAutoMapper::_preparePositionSetpoints()
{
        // Simple waypoint navigation: go to xyz target, with standard limitations
        _position_setpoint = _target;
        _velocity_setpoint.setNaN(); // No special velocity limitations
}
在“FlightTaskAutoLineSmoothVel”文件中调用_position_setpoint，根据S曲线实时规划的目标位置（每帧都在更新）和航点目标位置的误差更新目标速度_velocity_setpoint，个人认为使用当前的位置反馈求取位置误差也可以：
void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
{
        // Interface: A valid position setpoint generates a velocity target using conservative motion constraints.
        // If a velocity is specified, that is used as a feedforward to track the position setpoint
        // (ie. it assumes the position setpoint is moving at the specified velocity)
        // If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.

                const bool xy_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1));
                const bool z_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(2));

        if (xy_pos_setpoint_valid && z_pos_setpoint_valid) {
                // Use 3D position setpoint to generate a 3D velocity setpoint
                // 三维轨迹规划 代码略
        }

        else if (xy_pos_setpoint_valid) {
                // Use 2D position setpoint to generate a 2D velocity setpoint

                // Get various path specific vectors
                Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
                Vector2f pos_traj_to_dest_xy = Vector2f(_position_setpoint.xy()) - pos_traj;
                Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());

                Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * _getMaxXYSpeed();

                for (int i = 0; i < 2; i++) {
                        // If available, use the existing velocity as a feedforward, otherwise replace it
                        if (PX4_ISFINITE(_velocity_setpoint(i))) {
                                _velocity_setpoint(i) += vel_sp_constrained_xy(i);

                        } else {
                                _velocity_setpoint(i) = vel_sp_constrained_xy(i);
                        }
                }
        }

        else if (z_pos_setpoint_valid) {
                // Use Z position setpoint to generate a Z velocity setpoint
                // 高度轨迹规划 代码略
        }
}
FlightTaskAutoLineSmoothVel的类函数_generateTrajectory()中，调用_velocity_setpoint进行S曲线规划。
六、S曲线在AUTO模式中的应用

在前五节中，一步一步推导，将三重目标位置 position_setpoint_triplet转换为目标位置_position_setpoint，进一步转化为目标速度_velocity_setpoint，作为指令输入给S曲线规划。



S曲线的一个栗子

关于S曲线可以参考之前的文章：
PX4的S曲线，近似于速度的指令规划，是一个指令柔化的过程，保证加速度的连续性，不会出现明显的跳变。类似APM的平方根控制器（_sqrt_controller）。一句话概括，你只管生成目标速度，剩下的全看S曲线规划。根据规划产生的期望位置、期望速度、期望加速度（对应角度）、期望加加速度（对应角速度），作为指令或前馈加入到位置控制和姿态控制中。
七、总结

• Mission模式的数据流：
  

• 由地面站等传入航点信息，存储至飞控内部；
  
• 由navigator和mission_block模块处理航点信息，更新/发布三重目标位置；
  
• FlightTaskAuto模块调用三航点信息，根据当前位置重新排列航点优先级，更新实时目标位置和目标速度；
  
• S曲线根据目标速度规划速度/加速度/加加速度曲线，作为指令或前馈加入控制器中。
  

• FlightTaskAuto模块规划三航点过程可以总结为：当前航点为大目标，根据飞行器当前位置设计一个个小目标（牵引点），通过跟踪小目标，一步一步跟踪至当前航点，完成大目标。通过牵引点的方式实现航迹跟踪和简单的侧偏控制。
  
• PX4的S曲线，近似于速度的指令规划，是一个指令柔化的过程，保证加速度的连续性，不会出现明显的跳变。类似APM的平方根控制器（_sqrt_controller）。一句话概括，你只管生成目标速度，剩下的全看S曲线规划。根据规划产生的期望位置、期望速度、期望加速度（对应角度）、期望加加速度（对应角速度），作为指令或前馈加入到位置控制和姿态控制中。
  

zzxx110

总结到位 再讲讲s曲线代码吧[看看你]