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

refactor(base_move): simplify arc turn logic and remove precise turn functions

Browse Source 
- Simplified arc_turn_around_hori_line function:
  - Removed smooth stop method call
  - Added print statements for debugging
- Removed unused arc_turn_precise and arc_turn_around_hori_line_precise functions
- Updated task_1.py to use simplified arc_turn_around_hori_line
-
This commit is contained in:
havoc420ubuntu 2025-05-15 14:37:53 +00:00
parent aa855b160c
commit f484dab840
3 changed files with 15 additions and 289 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

290
base_move/move_base_hori_line.py
View File

@ -468,7 +468,12 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
pre_stop_yaw = ctrl.odo_msg.rpy[2]
# 使用专门的旋转停止函数替代原来的强制停止
ctrl.base_msg.stop_turn_smooth(current_vel=[current_v, 0, current_w], steps=5, delay=0.1, final_steps=2)
if observe:
print(f"使用平滑停止方法，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]")
print(f'{math.degrees(ctrl.odo_msg.rpy[2]):.2f}')
ctrl.base_msg.stop()
if observe:
print(f'{math.degrees(ctrl.odo_msg.rpy[2])}:.2f')
# 停下来后的最终角度
final_yaw = ctrl.odo_msg.rpy[2]
@ -520,260 +525,6 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
return True
def arc_turn_precise(ctrl, msg, radius, angle_deg, left=True, observe=False):
"""
实现更精确的圆弧转弯控制，特别改进了停止阶段的处理
参数:
ctrl: Robot_Ctrl对象
msg: robot_control_cmd_lcmt对象
radius: 圆弧半径(米)
angle_deg: 旋转角度(度)
left: True为左转，False为右转
observe: 是否打印调试信息
返回:
bool: 是否成功完成动作
"""
# 1. 参数准备
angle_rad = math.radians(angle_deg)
# 设定基础角速度，为更好的控制降低了角速度
base_w = 0.6 # 降低基础角速度提高精度
# 设定方向
w = base_w if left else -base_w # 左转为正，右转为负
v = abs(w * radius) # 线速度与角速度和半径成正比
# 计算理论时间
t = abs(angle_rad / w)
# 应用时间补偿系数
time_compensation = 0.92
t *= time_compensation
if observe:
print(f"圆弧参数: 半径={radius:.3f}米, 角度={angle_deg}度, 方向={'左' if left else '右'}")
print(f"速度参数: 角速度={w:.3f}rad/s, 线速度={v:.3f}m/s")
print(f"时间参数: 理论时间={abs(angle_rad / w):.2f}s, 实际时间={t:.2f}s")
# 2. 启动转向
msg.mode = 11
msg.gait_id = 26
msg.vel_des = [v, 0, w]
msg.duration = int((t + 2) * 1000) # 余量2秒
msg.step_height = [0.06, 0.06]
msg.life_count += 1
ctrl.Send_cmd(msg)
# 3. 记录初始状态
start_yaw = ctrl.odo_msg.rpy[2]
start_position = list(ctrl.odo_msg.xyz)
start_time = time.time()
# 4. 设置控制参数
target_angle = angle_rad if left else -angle_rad
distance_moved = 0
angle_turned = 0
timeout = t + 2 # 超时保护
current_v = v
current_w = w
# 分阶段控制参数
# 提前减速区间设置更大，从70%开始减速
slowdown_threshold = 0.7
# 慢速区间设置更大，从90%开始进入超慢速
precision_threshold = 0.9
# 5. 实时监控和控制
while abs(angle_turned) < abs(angle_rad) * 0.98 and time.time() - start_time < timeout:
# 计算已移动距离
current_position = ctrl.odo_msg.xyz
dx = current_position[0] - start_position[0]
dy = current_position[1] - start_position[1]
distance_moved = math.sqrt(dx*dx + dy*dy)
# 计算已旋转角度
current_yaw = ctrl.odo_msg.rpy[2]
angle_turned = current_yaw - start_yaw
# 角度归一化处理
while angle_turned > math.pi:
angle_turned -= 2 * math.pi
while angle_turned < -math.pi:
angle_turned += 2 * math.pi
# 计算完成比例
completion_ratio = abs(angle_turned) / abs(angle_rad)
# 分阶段减速控制
if completion_ratio > precision_threshold:
# 超低速阶段（精确控制）
speed_factor = 0.1 # 使用固定的极低速度
# 应用超低速
current_v = v * speed_factor
current_w = w * speed_factor
# 更新命令
msg.mode = 11
msg.gait_id = 26
msg.vel_des = [current_v, 0, current_w]
msg.duration = 150 # 更短的更新周期
msg.step_height = [0.06, 0.06]
msg.life_count += 1
ctrl.Send_cmd(msg)
if observe and time.time() % 0.5 < 0.02:
print(f"精确阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 速度: {speed_factor:.2f}, 角速度: {current_w:.3f}rad/s")
elif completion_ratio > slowdown_threshold:
# 一般减速阶段
speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold)
speed_factor = max(0.2, speed_factor * 0.8) # 整体降低一点速度
# 应用减速
current_v = v * speed_factor
current_w = w * speed_factor
# 更新命令
msg.mode = 11
msg.gait_id = 26
msg.vel_des = [current_v, 0, current_w]
msg.duration = 200
msg.step_height = [0.06, 0.06]
msg.life_count += 1
ctrl.Send_cmd(msg)
if observe and time.time() % 0.5 < 0.02:
print(f"减速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 速度因子: {speed_factor:.2f}, 角速度: {current_w:.3f}rad/s")
else:
if observe and time.time() % 0.5 < 0.02:
print(f"匀速阶段 - 已移动: {distance_moved:.3f}米, 已旋转: {math.degrees(angle_turned):.2f}度")
# 更短的循环检查间隔
time.sleep(0.03)
# 6. 使用改进的平滑停止方法
if observe:
print(f"旋转过程结束，使用平滑停止方法，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]")
# 记录停止前的角度
pre_stop_yaw = ctrl.odo_msg.rpy[2]
# 使用专门的旋转停止函数
# 增加步数和降低延迟以获得更平滑的停止
ctrl.base_msg.stop_turn_smooth(current_vel=[current_v, 0, current_w], steps=7, delay=0.08, final_steps=3)
# 7. 停止后的角度分析
final_yaw = ctrl.odo_msg.rpy[2]
final_angle_turned = final_yaw - start_yaw
# 角度归一化处理
while final_angle_turned > math.pi:
final_angle_turned -= 2 * math.pi
while final_angle_turned < -math.pi:
final_angle_turned += 2 * math.pi
final_angle_deg = math.degrees(final_angle_turned)
target_angle_deg = angle_deg if left else -angle_deg
if observe:
# 输出停止过程中角度变化
stop_angle_diff = math.degrees(final_yaw - pre_stop_yaw)
while stop_angle_diff > 180:
stop_angle_diff -= 360
while stop_angle_diff < -180:
stop_angle_diff += 360
print(f"停止过程中旋转了: {stop_angle_diff:.2f}度")
print(f"旋转完成，目标角度: {target_angle_deg:.2f}度, 实际角度: {final_angle_deg:.2f}度")
# 8. 微调处理
angle_error = abs(final_angle_deg - target_angle_deg)
# 如果误差超过阈值，进行微调
if angle_error > 2.0: # 降低误差阈值到2度
if observe:
print(f"角度误差: {angle_error:.2f}度，进行微调")
# 计算微调角度
adjust_angle = target_angle_deg - final_angle_deg
# 应用补偿系数
compensation_factor = 0.85
adjust_angle *= compensation_factor
if observe:
print(f"应用补偿系数{compensation_factor}后的微调角度: {adjust_angle:.2f}度")
# 使用turn_degree进行精确微调
turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False)
if observe:
print(f"微调结果: {'成功' if turn_success else '失败'}")
else:
if observe:
print(f"角度误差在允许范围内: {angle_error:.2f}度，无需微调")
# 9. 最终位置和角度
final_position = ctrl.odo_msg.xyz
final_yaw = ctrl.odo_msg.rpy[2]
if observe:
dx = final_position[0] - start_position[0]
dy = final_position[1] - start_position[1]
total_distance = math.sqrt(dx*dx + dy*dy)
# 计算理论圆弧长度
arc_length = abs(radius * angle_rad)
print(f"位移统计: 直线距离={total_distance:.3f}米, 圆弧长度={arc_length:.3f}米")
print(f"精确圆弧转弯完成")
return True
def arc_turn_around_hori_line_precise(ctrl, msg, target_distance=0.2, angle_deg=90, left=True, observe=False):
"""
使用精确圆弧转弯方法，对准前方横线，然后以计算出的距离为半径，做一个向左或向右的圆弧旋转。
参数:
ctrl: Robot_Ctrl对象
msg: robot_control_cmd_lcmt对象
target_distance: 横线前目标保持距离，默认0.2米
angle_deg: 旋转角度，支持任意角度
left: True为左转，False为右转
observe: 是否打印调试信息
返回:
bool: 是否成功完成动作
"""
# 1. 对准横线
print("校准到横向线水平")
aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
if not aligned:
print("无法校准到横向线水平，停止动作")
return False
# 2. 检测横线并计算距离
image = ctrl.image_processor.get_current_image()
edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe)
if edge_point is None or edge_info is None:
print("无法检测到横向线，停止动作")
return False
camera_height = 0.355 # 单位: 米
r = calculate_distance_to_line(edge_info, camera_height, observe=observe)
# 减去目标距离得到圆弧半径
r -= target_distance
if r is None or r < 0.1: # 半径太小无法执行圆弧
print(f"计算的半径太小: {r if r is not None else 'None'}，无法执行圆弧转弯")
return False
if observe:
print(f"当前距离: {r+target_distance:.3f}米，目标距离: {target_distance:.3f}米")
print(f"将使用半径: {r:.3f}米 执行圆弧转弯")
# 3. 使用精确圆弧转弯函数执行
return arc_turn_precise(ctrl, msg, r, angle_deg, left, observe)
# 用法示例
if __name__ == "__main__":
move_to_hori_line(None, None, observe=True)
@ -781,32 +532,3 @@ if __name__ == "__main__":
arc_turn_around_hori_line(None, None, angle_deg=90, left=True, observe=True)
# 180度右转
arc_turn_around_hori_line(None, None, angle_deg=180, left=False, observe=True)
# 使用精确版本做90度左转
arc_turn_around_hori_line_precise(None, None, angle_deg=90, left=True, observe=True)
"""
代码改进说明：
1. 添加了专用的平滑旋转停止函数 stop_turn_smooth (在 utils/base_msg.py 中):
- 分两个阶段减速：先减小角速度，再减小线速度
- 使用平方递减使角速度更快地降低
- 发送多次零速度命令确保完全停止
2. 改进了原始的 arc_turn_around_hori_line 函数:
- 使用平滑停止替代强制停止
- 添加停止过程中角度变化的监控
- 改进了角度误差计算和微调逻辑
3. 新增了 arc_turn_precise 函数，实现更精确的转弯控制:
- 速度更低，控制更精细
- 分三个阶段控制：匀速阶段、减速阶段、精确阶段
- 更频繁的控制更新和监控
- 更完善的停止策略
- 更严格的角度误差要求
4. 新增 arc_turn_around_hori_line_precise 结合横线检测和精确转弯功能:
- 保持原有的横线检测和校准功能
- 使用改进的精确转弯控制
这些改进有效解决了转弯后快速切换到零速度导致的角度偏移问题，提高了转弯的精度和稳定性。
"""

13
task_1/task_1.py
View File

@ -1,5 +1,4 @@
import time
import cv2
import sys
import os
@ -10,15 +9,21 @@ from base_move.move_base_hori_line import move_to_hori_line, arc_turn_around_hor
observe = True
def run_task_1(ctrl, msg):
print('Running task 1...')
print('🐶 Running task 1...')
# v2
arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=False, observe=observe)
print('😺 task 1 - 1')
arc_turn_around_hori_line(ctrl, msg, angle_deg=70, left=False, observe=observe)
print('😺 task 1 - 2')
move_to_hori_line(ctrl, msg, target_distance=1, observe=observe)
arc_turn_around_hori_line(ctrl, msg, angle_deg=180, left=True, observe=observe)
return
print('😺 task 1 - 3')
arc_turn_around_hori_line(ctrl, msg, angle_deg=160, left=True, observe=observe)
print('😺 task 1 - 4')
move_to_hori_line(ctrl, msg, observe=observe)

1
utils/base_msg.py
View File

@ -79,7 +79,6 @@ class BaseMsg:
# 等待最后的命令完成
time.sleep(delay)
def stop_turn_smooth(self, current_vel=None, steps=5, delay=0.1, final_steps=2):
"""
专为旋转动作设计的平滑停止方法，通过更精细的步骤减小速度