diff --git a/task_3/task_3.py b/task_3/task_3.py index 017835e..614e485 100644 --- a/task_3/task_3.py +++ b/task_3/task_3.py @@ -5,6 +5,7 @@ import toml import copy import math import lcm +import numpy as np # 添加父目录到路径，以便能够导入utils sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) @@ -103,36 +104,73 @@ def run_task_3(ctrl, msg): max_iterations = 250 # 最大循环次数，作为安全保障 min_iterations = 150 # 最小循环次数，作为安全保障 + # 姿态判断参数 + pitch_threshold = 0.05 # 俯仰角阈值（弧度） + angular_rate_threshold = 0.03 # 角速度阈值（弧度/秒） + # 阶段控制 climbing_detected = False # 是否检测到正在爬坡 + + # 高度变化记录 + height_window = [] + pitch_window = [] + window_size = 8 + + # 记录起始姿态和高度 + start_height = ctrl.odo_msg.xyz[2] + info(f"开始监测爬坡过程，初始高度: {start_height:.3f}", "监测") + for i in range(max_iterations): # 发送控制命令维持心跳 ctrl.Send_cmd(msg) + # 获取当前状态数据 + vz = ctrl.odo_msg.vxyz[2] # Z轴速度 + current_height = ctrl.odo_msg.xyz[2] # 当前高度 + current_pitch = ctrl.odo_msg.rpy[1] # 当前俯仰角 + pitch_rate = ctrl.odo_msg.omegaBody[1] # 俯仰角速度 + vbody_z = ctrl.odo_msg.vBody[2] # 机体坐标系Z速度 + + # 更新滑动窗口数据 + height_window.append(current_height) + pitch_window.append(current_pitch) + if len(height_window) > window_size: + height_window.pop(0) + pitch_window.pop(0) + # 每10次迭代打印一次当前信息 if i % 10 == 0: - # 获取当前Z轴位置和速度 - current_vz = ctrl.odo_msg.vxyz[2] # z轴速度 - info(f"当前Z轴速度={current_vz:.3f}", "监测") + info(f"当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测") - # 获取z轴速度 - vz = ctrl.odo_msg.vxyz[2] - - # 检测是否开始爬坡阶段 - 使用z轴速度判断 + # 检测是否开始爬坡阶段 if not climbing_detected and vz > climb_speed_threshold: climbing_detected = True - info(f"检测到开始爬坡，Z轴速度: {vz:.3f}, 当前高度: {ctrl.odo_msg.xyz[2]:.3f}", "监测") + info(f"检测到开始爬坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}, 俯仰角: {current_pitch:.3f}", "监测") - # 只有在检测到爬坡后，才开始监控Z轴是否停止增加 - if i > min_iterations and climbing_detected: - # 如果Z轴速度接近于0或者为负，表示已经停止爬升或开始下降 - if abs(vz) < z_speed_threshold: # or vz < 0: + # 多条件判断是否完成爬坡 + if i > min_iterations and climbing_detected and len(height_window) == window_size: + # 计算高度和俯仰角的稳定性 + height_std = np.std(height_window) # 高度标准差 + pitch_std = np.std(pitch_window) # 俯仰角标准差 + + # 多条件综合判断 + position_stable = abs(vz) < z_speed_threshold # 垂直速度稳定 + attitude_stable = abs(current_pitch) < pitch_threshold # 俯仰角接近水平 + angular_rate_stable = abs(pitch_rate) < angular_rate_threshold # 角速度稳定 + height_stable = height_std < 0.01 # 高度变化小 + pitch_stable = pitch_std < 0.01 # 俯仰角变化小 + vbody_stable = abs(vbody_z) < 0.01 # 机体Z方向速度稳定 + + # 综合判断条件 + if (position_stable and attitude_stable and angular_rate_stable) or \ + (position_stable and height_stable and pitch_stable) or \ + (vbody_stable and attitude_stable and height_stable): stable_count += 1 if stable_count >= stable_threshold: - info(f"Z轴速度趋近于0，停止循环。当前速度: {vz:.3f}", "监测") + info(f"检测到已完成爬坡：\n - Z轴速度: {vz:.3f}\n - 俯仰角: {current_pitch:.3f}\n - 角速度: {pitch_rate:.3f}\n - 当前高度: {current_height:.3f}\n - 上升了: {current_height - start_height:.3f}米", "监测") break else: - # 如果Z轴仍有明显上升速度，重置稳定计数 + # 重置稳定计数 stable_count = 0 time.sleep(0.2) @@ -217,40 +255,73 @@ def run_task_3(ctrl, msg): min_iterations = 150 # 最小循环次数，确保有足够的时间开始动作 start_height = ctrl.odo_msg.xyz[2] # 记录起始高度 + # 姿态判断参数 + pitch_threshold = 0.05 # 俯仰角阈值（弧度） + angular_rate_threshold = 0.03 # 角速度阈值（弧度/秒） + # 阶段控制 descending_detected = False # 是否检测到正在下坡 flat_ground_detected = False # 是否检测到已到达平地 + # 高度变化记录 + height_window = [] + pitch_window = [] + window_size = 8 + info(f"开始监测下坡过程，初始高度: {start_height}", "监测") for i in range(max_iterations): # 发送控制命令维持心跳 ctrl.Send_cmd(msg) - # 获取z轴速度和当前高度 - vz = ctrl.odo_msg.vxyz[2] - current_height = ctrl.odo_msg.xyz[2] + # 获取当前状态数据 + vz = ctrl.odo_msg.vxyz[2] # Z轴速度 + current_height = ctrl.odo_msg.xyz[2] # 当前高度 + current_pitch = ctrl.odo_msg.rpy[1] # 当前俯仰角 + pitch_rate = ctrl.odo_msg.omegaBody[1] # 俯仰角速度 + vbody_z = ctrl.odo_msg.vBody[2] # 机体坐标系Z速度 + + # 更新滑动窗口数据 + height_window.append(current_height) + pitch_window.append(current_pitch) + if len(height_window) > window_size: + height_window.pop(0) + pitch_window.pop(0) # 每10次迭代打印一次当前信息 if observe and i % 10 == 0: - info(f"当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}", "监测") + info(f"当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测") - # 检测是否开始下坡阶段 - 使用z轴速度判断（负值表示下降） + # 检测是否开始下坡阶段 if not descending_detected and vz < descent_speed_threshold: descending_detected = True - info(f"检测到开始下坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}", "监测") + info(f"检测到开始下坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}, 俯仰角: {current_pitch:.3f}", "监测") - # 只有在检测到下坡后，才开始监控是否到达平地 - if i > min_iterations and descending_detected: - # 如果Z轴速度接近于0，表示已经停止下降（到达平地） - if abs(vz) < z_speed_threshold: + # 多条件判断是否到达平地 + if i > min_iterations and descending_detected and len(height_window) == window_size: + # 计算高度和俯仰角的稳定性 + height_std = np.std(height_window) # 高度标准差 + pitch_std = np.std(pitch_window) # 俯仰角标准差 + + # 多条件综合判断 + position_stable = abs(vz) < z_speed_threshold # 垂直速度稳定 + attitude_stable = abs(current_pitch) < pitch_threshold # 俯仰角接近水平 + angular_rate_stable = abs(pitch_rate) < angular_rate_threshold # 角速度稳定 + height_stable = height_std < 0.01 # 高度变化小 + pitch_stable = pitch_std < 0.01 # 俯仰角变化小 + vbody_stable = abs(vbody_z) < 0.01 # 机体Z方向速度稳定 + + # 综合判断条件 + if (position_stable and attitude_stable and angular_rate_stable) or \ + (position_stable and height_stable and pitch_stable) or \ + (vbody_stable and attitude_stable and height_stable): stable_count += 1 if stable_count >= stable_threshold: - info(f"检测到已到达平地，Z轴速度趋近于0，停止循环。当前速度: {vz:.3f}, 当前高度: {current_height:.3f}, 下降了: {start_height - current_height:.3f}米", "监测") + info(f"检测到已到达平地：\n - Z轴速度: {vz:.3f}\n - 俯仰角: {current_pitch:.3f}\n - 角速度: {pitch_rate:.3f}\n - 高度: {current_height:.3f}\n - 下降了: {start_height - current_height:.3f}米", "监测") flat_ground_detected = True break else: - # 如果Z轴仍有明显下降速度，重置稳定计数 + # 重置稳定计数 stable_count = 0 time.sleep(0.2)