开源飞控PX4—Mission模式的部分逻辑梳理和S曲线的应用
一、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即为订阅，实现该结构体实例的调用，这部分在下节中讲解。
四、三重目标位置的使用
头文件&#34;FlightTaskAuto.hpp&#34;中订阅了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.getCurrentPosition(), _trajectory.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曲线规划。根据规划产生的期望位置、期望速度、期望加速度（对应角度）、期望加加速度（对应角速度），作为指令或前馈加入到位置控制和姿态控制中。
总结到位 再讲讲s曲线代码吧[看看你]
页:
[1]