From aaa5ee721541e08b1d76cc77f12c0c182d95cfd7 Mon Sep 17 00:00:00 2001 From: Havoc <2993167370@qq.com> Date: Wed, 28 May 2025 13:05:02 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0task=5F3.py=EF=BC=8C=E5=A2=9E?= =?UTF-8?q?=E5=BC=BA=E7=88=AC=E5=9D=A1=E5=92=8C=E4=B8=8B=E5=9D=A1=E9=98=B6?= =?UTF-8?q?=E6=AE=B5=E7=9A=84=E7=9B=91=E6=B5=8B=E5=8A=9F=E8=83=BD=EF=BC=8C?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=A7=BF=E6=80=81=E5=88=A4=E6=96=AD=E5=8F=82?= =?UTF-8?q?=E6=95=B0=EF=BC=8C=E4=BC=98=E5=8C=96=E7=A8=B3=E5=AE=9A=E6=80=A7?= =?UTF-8?q?=E6=A3=80=E6=B5=8B=E9=80=BB=E8=BE=91=EF=BC=8C=E4=BD=BF=E7=94=A8?= =?UTF-8?q?=E6=BB=91=E5=8A=A8=E7=AA=97=E5=8F=A3=E8=AE=B0=E5=BD=95=E9=AB=98?= =?UTF-8?q?=E5=BA=A6=E5=92=8C=E4=BF=AF=E4=BB=B0=E8=A7=92=EF=BC=8C=E6=8F=90?= =?UTF-8?q?=E5=8D=87=E4=BB=A3=E7=A0=81=E5=8F=AF=E8=AF=BB=E6=80=A7=E5=92=8C?= =?UTF-8?q?=E6=89=A7=E8=A1=8C=E6=95=88=E7=8E=87=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- task_3/task_3.py | 123 +++++++++++++++++++++++++++++++++++++---------- 1 file changed, 97 insertions(+), 26 deletions(-) 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)