Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新task_3.py，增强爬坡和下坡阶段的监测功能，添加姿态判断参数，优化稳定性检测逻辑，使用滑动窗口记录高度和俯仰角，提升代码可读性和执行效率。

Browse Source
This commit is contained in:
Havoc 2025-05-28 13:05:02 +08:00
parent 918f5f20c5
commit aaa5ee7215
1 changed files with 97 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

123
task_3/task_3.py
View File

@ -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)