PX4飞控之自主返航(RTL)控制逻辑

本文解析了PX4飞控1.5.5版本中自主返航模式的控制逻辑及算法。介绍了RTL模式下的初始化、主函数及退出过程,并详细阐述了爬升、返回、降落等子模式的工作原理。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

本文基于PX4飞控1.5.5版本,分析导航模块中自护返航模式的控制逻辑和算法。
自主返航模式和导航中的其他模式一样,在Navigator_main函数中一旦触发case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:任务指令,导航模式_navigation_mode = &_rtl;即进入自主返航模式。依次执行也分为初始化函数RTL::on_activation()、主函数RTL::on_active()、退出函数RTL::on_inactive()。
1.首先是初始化函数的控制逻辑:on_activation()
(1)将当前位置设为_mission_item,再赋值给当前任务航点。

set_current_position_item(&_mission_item);
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

保证第一个航点为当前位置,从而实现爬升等一系列动作。
(2)根据当前位置到home点的距离,当前高度等参数选择进入哪种子模式。
①如果已经落地,则进入落地状态

_rtl_state = RTL_STATE_LANDED;

②如果距离home点比较远,同时当前高度小于返航高度,则先爬升

_rtl_state = RTL_STATE_CLIMB;

③其他情况下,直接以当前高度飞往home点_rtl_state = RTL_STATE_RETURN;
注:主要针对当前高度大于返航高度的情况
2.主函数on_active()
主函数的功能在于返航状态切换,一个子返航模式完成后,进入下一个模式,顺序进行。然后设定对应的任务航点信息set_rtl_item(),然后进行航点位置控制。
RTL控制逻辑:
(1)爬升模式RTL_STATE_CLIMB
从当前位置爬升到指定高度climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();,经纬度不变。任务类型为航点任务_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
航点:指定返航高度、当前经纬度、不进行航向控制
(2)返回模式RTL_STATE_RETURN
在指定返回高度上到达home点上方
当距离home点的距离home_dist < _param_rtl_min_dist(默认5m),航向转为home点初始航向;
如果距离较远,同时上一航点有效,则航向为从上一航点指向home点;上一航点无效则航向从当前位置指向home点。

if (home_dist < _param_rtl_min_dist.get()) {
    _mission_item.yaw = _navigator->get_home_position()->yaw;
} else {
    if (pos_sp_triplet->previous.valid) {
    /* if previous setpoint is valid then use it to calculate heading to home */
        _mission_item.yaw = get_bearing_to_next_waypoint(
        pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
        _mission_item.lat, _mission_item.lon);
    } else {
    /* else use current position */
        _mission_item.yaw = get_bearing_to_next_waypoint(
        navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
        _mission_item.lat, _mission_item.lon);
    }
}

航点:home点经纬度,返航高度,进行航向控制
(3)RTL_STATE_DESCEND
降落,home点经纬度,高度为指定的落地高度_param_descend_alt

_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();

_param_land_delay如果在-0.1到0.1之间,则悬停loiter;否则直降降落landing(默认为-1,直接降落)
航点:home点经纬度,高度:落地高度QGC默认2.5m,航向为home点的原始航向
(4)RTL_STATE_LOITER
loiter:时间航向都不变,只是具有一定的悬停时间
在descend的航点处悬停一定时间,航向为home点的原始航向
loiter之后再land,
在land之前都是位置航点控制_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
(5)RTL_STATE_LAND
land:经纬度为home点的经纬度,高度为0(海拔)set_land_item
航点:航向为home处的原始航向,经纬度为home点高度,高度为海拔0m,任务模式为NAV_CMD_LAND,参看pos控制
在is_mission_item_reached中NAV_CMD_LAND
返回 _navigator->get_land_detected()->landed,因此在此处进行是否已落地检测,落地后即为真,从而进入下一个任务landed

### PX4速度控制代码逻辑及实现详解 #### 1. 控制架构概述 PX4的速度控制系统属于其多层控制体系的一部分,在这一框架下,速度控制器位于姿态控制器之上。这意味着速度控制器负责计算期望的姿态角或推力矢量,进而由底层的姿态控制器执行具体的姿态调整动作[^4]。 #### 2. 输入信号处理 对于来自地面站或其他传感器输入的目标速度指令,这些数据通常通过MAVLink消息传递给控制器。一旦接收到新的设定值,系统会将其存储于特定的数据结构中以便后续访问和处理。 #### 3. 速度误差计算 为了确定当前实际速度与目标速度之间的差异,PX4利用惯性测量单元(IMU)和其他导航设备提供的实时反馈信息来进行比较分析。具体而言,IMU可以提供加速度计读数用于积分得到速度估计;而GPS则能够给出更精确的位置变化率作为参考标准之一。 ```cpp // 计算水平方向上的速度误差 (伪代码) float velocity_error_x = target_velocity.x - current_velocity.x; float velocity_error_y = target_velocity.y - current_velocity.y; if (use_gps_for_vertical_control) { float velocity_error_z = gps_data.velocity.z - current_velocity.z; } else { float velocity_error_z = barometer_data.velocity.z - current_velocity.z; } ``` #### 4. PID调节机制应用 基于上述获得的速度偏差,采用PID(比例-积分-微分)算法来动态调整输出命令以最小化这种差距。该过程涉及到三个参数KP、KI 和KD 的设置,它们分别对应着即时响应强度、累积效应权重以及趋势预测系数。 ```cpp // 应用PID控制律生成所需的加速度修正项 (伪代码) float acceleration_command_x = kp * velocity_error_x + ki * integral_of_velocity_error_x + kd * derivative_of_velocity_error_x; float acceleration_command_y = kp * velocity_error_y + ki * integral_of_velocity_error_y + kd * derivative_of_velocity_error_y; float acceleration_command_z = ...; // 类似地应用于垂直维度 ``` #### 5. 输出转换为姿态/推力需求 最终,经过PID运算得出的结果会被进一步转化为适当的形式供下游模块使用——这可能是直接指定某个轴向上所需施加的力量大小,或者是间接指示机应该采取怎样的倾斜角度去达到预期效果。 ```cpp // 将加速请求映射成姿态角偏移 (伪代码) float pitch_angle_offset = map_acceleration_to_pitch(acceleration_command_x); float roll_angle_offset = map_acceleration_to_roll(acceleration_command_y); setpoint_attitude.pitch += pitch_angle_offset; setpoint_attitude.roll += roll_angle_offset; ```
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值