Merge branch 'main' of ssh://120.27.199.238:222/Havoc420mac/mi-task into main
This commit is contained in:
commit
aa855b160c
@ -460,12 +460,20 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
|
||||
|
||||
time.sleep(0.05)
|
||||
|
||||
# Stop
|
||||
ctrl.base_msg.stop_force()
|
||||
|
||||
# 使用改进的平滑停止方法替代强制停止
|
||||
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=5, delay=0.1, final_steps=2)
|
||||
|
||||
# 停下来后的最终角度
|
||||
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
|
||||
@ -476,8 +484,15 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
|
||||
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}度")
|
||||
|
||||
|
||||
# 检查是否需要微调
|
||||
angle_error = abs(final_angle_deg - target_angle_deg)
|
||||
if angle_error > 3.0: # 如果误差超过3度
|
||||
@ -495,13 +510,270 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
|
||||
print(f"应用补偿系数{compensation_factor}后的微调角度: {adjust_angle:.2f}度")
|
||||
|
||||
# 使用turn_degree函数进行微调
|
||||
turn_degree(ctrl, msg, adjust_angle, absolute=False)
|
||||
turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False)
|
||||
|
||||
if observe:
|
||||
print(f"微调结果: {'成功' if turn_success else '失败'}")
|
||||
|
||||
if observe:
|
||||
print("圆弧转弯完成")
|
||||
|
||||
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)
|
||||
@ -509,3 +781,32 @@ 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 结合横线检测和精确转弯功能:
|
||||
- 保持原有的横线检测和校准功能
|
||||
- 使用改进的精确转弯控制
|
||||
|
||||
这些改进有效解决了转弯后快速切换到零速度导致的角度偏移问题，提高了转弯的精度和稳定性。
|
||||
"""
|
||||
|
||||
@ -77,4 +77,91 @@ class BaseMsg:
|
||||
self.ctrl.Send_cmd(self.msg)
|
||||
|
||||
# 等待最后的命令完成
|
||||
time.sleep(delay)
|
||||
time.sleep(delay)
|
||||
|
||||
def stop_turn_smooth(self, current_vel=None, steps=5, delay=0.1, final_steps=2):
|
||||
"""
|
||||
专为旋转动作设计的平滑停止方法，通过更精细的步骤减小速度
|
||||
|
||||
参数:
|
||||
current_vel: 当前速度列表 [x, y, w]，如果为None则使用当前消息中的速度
|
||||
steps: 减速步骤数，默认为5步（比普通停止更多步骤）
|
||||
delay: 每步之间的延迟时间(秒)，默认为0.1秒（更快的响应）
|
||||
final_steps: 最后完全停止前的额外零速度指令次数，增强稳定性
|
||||
"""
|
||||
|
||||
# 如果没有提供当前速度，使用消息中的当前速度
|
||||
if current_vel is None:
|
||||
current_vel = self.msg.vel_des.copy() if hasattr(self.msg, 'vel_des') else [0, 0, 0]
|
||||
|
||||
# 保存当前消息的一些参数
|
||||
current_mode = self.msg.mode
|
||||
current_gait_id = self.msg.gait_id
|
||||
|
||||
# 检查是否有速度需要减小
|
||||
if all(abs(v) < 0.01 for v in current_vel):
|
||||
# 速度已经很小，直接停止
|
||||
self.stop_force()
|
||||
return
|
||||
|
||||
# 1. 先针对角速度进行单独减速，确保旋转先停稳
|
||||
if len(current_vel) >= 3 and abs(current_vel[2]) > 0.05: # 角速度
|
||||
# 首先只减小角速度，保持线速度
|
||||
for i in range(1, steps + 1):
|
||||
# 计算这一步的减速比例 - 角速度减速更快
|
||||
ratio = (steps - i) / steps
|
||||
ratio_w = ratio * ratio # 角速度使用平方递减，减速更快
|
||||
|
||||
# 计算新的速度，只改变角速度
|
||||
new_vel = current_vel.copy()
|
||||
new_vel[2] = current_vel[2] * ratio_w
|
||||
|
||||
# 更新消息
|
||||
self.msg.mode = current_mode
|
||||
self.msg.gait_id = current_gait_id
|
||||
self.msg.vel_des = new_vel
|
||||
self.msg.duration = int(delay * 1000) # 毫秒
|
||||
self.msg.life_count += 1
|
||||
|
||||
# 发送命令
|
||||
self.ctrl.Send_cmd(self.msg)
|
||||
|
||||
# 等待这一步完成
|
||||
time.sleep(delay)
|
||||
|
||||
# 2. 再减小整体速度
|
||||
current_vel_no_w = current_vel.copy()
|
||||
if len(current_vel) >= 3:
|
||||
current_vel_no_w[2] = 0 # 角速度已减为0
|
||||
|
||||
for i in range(1, steps + 1):
|
||||
# 计算这一步的减速比例
|
||||
ratio = (steps - i) / steps
|
||||
|
||||
# 计算新的速度
|
||||
new_vel = [v * ratio for v in current_vel_no_w]
|
||||
if len(current_vel) >= 3:
|
||||
new_vel[2] = 0 # 确保角速度为0
|
||||
|
||||
# 更新消息
|
||||
self.msg.mode = current_mode
|
||||
self.msg.gait_id = current_gait_id
|
||||
self.msg.vel_des = new_vel
|
||||
self.msg.duration = int(delay * 1000) # 毫秒
|
||||
self.msg.life_count += 1
|
||||
|
||||
# 发送命令
|
||||
self.ctrl.Send_cmd(self.msg)
|
||||
|
||||
# 等待这一步完成
|
||||
time.sleep(delay)
|
||||
|
||||
# 3. 发送多次零速度命令，确保完全停止
|
||||
for _ in range(final_steps):
|
||||
self.msg.mode = current_mode
|
||||
self.msg.gait_id = current_gait_id
|
||||
self.msg.vel_des = [0, 0, 0]
|
||||
self.msg.duration = int(delay * 1000)
|
||||
self.msg.life_count += 1
|
||||
self.ctrl.Send_cmd(self.msg)
|
||||
time.sleep(delay)
|
||||
Loading…
x
Reference in New Issue
Block a user