查看: 678|回复: 1

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

[复制链接]

40

主题

785

帖子

1555

积分

金牌飞友

Rank: 6Rank: 6

积分
1555
飞币
768
注册时间
2017-9-21
发表于 2022-10-29 01:36:32 | 显示全部楼层 |阅读模式
一、Mission模式的定义


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

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

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

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

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

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

  • S曲线的输入输出是什么?
  • S曲线的输入如何由Miison模式产生的?
  • 外部输入至Mission模式的航点如何规划?
  • S曲线的输出如何作用于位置控制器/姿态控制器?
最终的跟踪效果如下图所示:

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

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°,结果如下图所示,此时飞行器已飞过当前目标航点。相应的,设置上一航点为当前位置,设置当前航点不变,设置下一航点不变。

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

第二种情况 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,超过半径表示距离较远。相应的,设置上一航点为当前位置,设置当前航点为上一航点,设置下一航点为当前航点。即先向上一航点靠近,再飞向当前航点。

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

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

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

第四种情况: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曲线规划。

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

S曲线的一个栗子

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


  • Mission模式的数据流:

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


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

33

主题

789

帖子

1554

积分

金牌飞友

Rank: 6Rank: 6

积分
1554
飞币
763
注册时间
2017-9-12
发表于 2022-10-29 01:49:45 | 显示全部楼层
总结到位 再讲讲s曲线代码吧[看看你]
您需要登录后才可以回帖 登录 | 加入联盟

本版积分规则

快速回复 返回顶部 返回列表