新增go_straight_until_bar函数以控制机器人沿直线行走至检测到栏杆；优化follow_left_side_track函数的PID参数和稳定性要求，增强响应速度和稳定性；调整detect_left_side_track函数的搜索区域和评分逻辑，以提高线段检测的准确性和灵活性。
This commit is contained in:
parent
009b8e616a
commit
e77d5655ae
@ -196,6 +196,12 @@ def go_straight(ctrl, msg, distance, speed=0.5, observe=False):
|
||||
|
||||
return go_success
|
||||
|
||||
def go_straight_until_bar(ctrl, msg, distance, speed=0.5, observe=False):
|
||||
"""
|
||||
控制机器人沿直线行走指定距离，直到检测到栏杆
|
||||
"""
|
||||
pass
|
||||
|
||||
# 用法示例
|
||||
if __name__ == "__main__":
|
||||
# 这里是示例代码，实际使用时需要提供合适的ctrl和msg对象
|
||||
|
||||
@ -1224,13 +1224,13 @@ def follow_dual_tracks(ctrl, msg, speed=0.5, max_time=30, target_distance=None,
|
||||
|
||||
def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=30, observe=False):
|
||||
"""
|
||||
控制机器狗向左侧移动并靠近左侧的黄色轨迹线
|
||||
控制机器狗向左侧移动并靠近左侧的黄色轨迹线，只进行侧向移动，不进行前进
|
||||
|
||||
参数:
|
||||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||||
target_distance: 目标与左侧线的像素距离，默认为150像素（保持一定间距）
|
||||
speed: 移动速度(米/秒)，默认为0.3米/秒
|
||||
speed: 侧向移动的最大速度(米/秒)，默认为0.3米/秒
|
||||
max_time: 最大执行时间(秒)，默认为30秒
|
||||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||||
|
||||
@ -1256,22 +1256,22 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
|
||||
if hasattr(ctrl, 'place_marker'):
|
||||
ctrl.place_marker(start_position[0], start_position[1], start_position[2] if len(start_position) > 2 else 0.0, 'green', observe=True)
|
||||
|
||||
# PID控制参数 - 侧向移动的PID参数
|
||||
kp_side = 0.002 # 比例系数
|
||||
# PID控制参数 - 侧向移动的PID参数，根据近距离跟踪需求调整
|
||||
kp_side = 0.0025 # 比例系数
|
||||
ki_side = 0.0001 # 积分系数
|
||||
kd_side = 0.0005 # 微分系数
|
||||
kd_side = 0.001 # 微分系数
|
||||
|
||||
# 记录目标到达状态和稳定计数
|
||||
target_reached = False
|
||||
stable_count = 0
|
||||
required_stable_count = 10 # 需要连续稳定的检测次数
|
||||
required_stable_count = 8 # 需要连续稳定的检测次数
|
||||
|
||||
# PID控制变量
|
||||
previous_error = 0
|
||||
integral = 0
|
||||
|
||||
# 最大侧向速度限制
|
||||
max_side_velocity = 0.3 # m/s
|
||||
max_side_velocity = speed # 使用参数传入的速度作为最大侧向速度
|
||||
|
||||
# 检测成功计数器
|
||||
detection_success_count = 0
|
||||
@ -1284,6 +1284,9 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
|
||||
filter_size = 5
|
||||
distance_queue = []
|
||||
|
||||
# 动态调整目标距离的阈值
|
||||
target_reached_threshold = 25 # 在目标距离±25像素范围内视为达到目标
|
||||
|
||||
# 开始跟踪循环
|
||||
while time.time() - start_time < max_time:
|
||||
# 获取当前图像
|
||||
@ -1321,7 +1324,7 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
|
||||
error = target_distance - filtered_distance
|
||||
|
||||
# 检查是否达到目标位置
|
||||
if abs(error) < 20: # 误差小于20像素视为达到目标
|
||||
if abs(error) < target_reached_threshold: # 误差小于阈值视为达到目标
|
||||
stable_count += 1
|
||||
if stable_count >= required_stable_count:
|
||||
if not target_reached:
|
||||
@ -1348,38 +1351,41 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
|
||||
# 计算侧向速度
|
||||
side_velocity = p_control + i_control + d_control
|
||||
|
||||
# 应用非线性控制，加快大距离靠近，减缓小距离微调
|
||||
if abs(error) > 100:
|
||||
# 大误差时，增强响应
|
||||
side_velocity *= 1.2
|
||||
elif abs(error) < 40:
|
||||
# 小误差时，减弱响应，增加稳定性
|
||||
side_velocity *= 0.7
|
||||
|
||||
# 限制侧向速度范围
|
||||
side_velocity = max(-max_side_velocity, min(max_side_velocity, side_velocity))
|
||||
|
||||
# 根据目标状态调整速度
|
||||
forward_speed = speed
|
||||
# 如果已达到目标，进一步降低侧向速度以减少振荡
|
||||
if target_reached:
|
||||
# 如果已达到目标，降低侧向速度以减少振荡
|
||||
side_velocity *= 0.5
|
||||
# 降低前进速度让侧向移动更平稳
|
||||
forward_speed *= 0.7
|
||||
side_velocity *= 0.4
|
||||
|
||||
if observe and time.time() % 0.5 < 0.02:
|
||||
debug(f"误差: {error:.1f}px, P: {p_control:.4f}, I: {i_control:.4f}, D: {d_control:.4f}", "控制")
|
||||
debug(f"控制指令: 前进={forward_speed:.2f}m/s, 侧向={side_velocity:.2f}m/s", "速度")
|
||||
debug(f"控制指令: 侧向={side_velocity:.2f}m/s", "速度")
|
||||
|
||||
# 设置速度命令 - [前进速度, 侧向速度, 角速度]
|
||||
# 侧向速度为正表示向左移动，为负表示向右移动
|
||||
msg.vel_des = [forward_speed, side_velocity, 0]
|
||||
# 前进速度保持为0，只进行侧向移动
|
||||
msg.vel_des = [0, side_velocity, 0]
|
||||
|
||||
# 使用轨迹线斜率进行小角度调整，确保机器人方向与轨迹线平行
|
||||
# 斜率大于0表示向右倾斜，需要顺时针旋转(负角速度)
|
||||
# 斜率小于0表示向左倾斜，需要逆时针旋转(正角速度)
|
||||
if not track_info["is_vertical"]: # 对于非垂直线，进行角度校正
|
||||
slope = track_info["slope"]
|
||||
# 计算旋转角度 - 垂直线的ideal_angle应该是0
|
||||
angle_correction = -math.atan(1/slope) if abs(slope) > 0.01 else 0
|
||||
# 将角度校正应用为小的角速度
|
||||
angular_velocity = angle_correction * 0.2 # 角速度控制系数
|
||||
angular_velocity = angle_correction * 0.25
|
||||
# 限制角速度范围
|
||||
angular_velocity = max(-0.2, min(0.2, angular_velocity))
|
||||
angular_velocity = max(-0.25, min(0.25, angular_velocity))
|
||||
|
||||
if abs(angular_velocity) > 0.02: # 只在需要明显校正时应用
|
||||
if abs(angular_velocity) > 0.01: # 只在需要明显校正时应用
|
||||
msg.vel_des[2] = angular_velocity
|
||||
if observe and time.time() % 0.5 < 0.02:
|
||||
debug(f"应用角度校正，斜率: {slope:.2f}, 角速度: {angular_velocity:.3f}", "校正")
|
||||
@ -1389,9 +1395,6 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
|
||||
|
||||
# 如果之前有有效检测结果，使用上一次的控制值但降低强度
|
||||
if last_valid_track_info is not None and len(distance_queue) > 0:
|
||||
# 使用上一次的距离，但降低侧向速度
|
||||
reduced_speed = speed * 0.5 # 降低前进速度
|
||||
|
||||
# 如果上次距离已经接近目标，维持当前位置
|
||||
last_error = target_distance - distance_queue[-1]
|
||||
if abs(last_error) < 50: # 如果接近目标距离
|
||||
@ -1400,19 +1403,19 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
|
||||
info("接近目标位置，保持当前侧向位置", "保持")
|
||||
else:
|
||||
# 向目标方向缓慢移动
|
||||
side_velocity = 0.05 * (1 if last_error > 0 else -1)
|
||||
side_velocity = 0.08 * (1 if last_error > 0 else -1)
|
||||
if observe:
|
||||
info(f"距离目标较远，缓慢向{'左' if side_velocity > 0 else '右'}移动", "调整")
|
||||
|
||||
msg.vel_des = [reduced_speed, side_velocity, 0]
|
||||
msg.vel_des = [0, side_velocity, 0]
|
||||
if observe:
|
||||
warning(f"使用降低强度的控制: 前进={reduced_speed:.2f}m/s, 侧向={side_velocity:.2f}m/s", "恢复")
|
||||
warning(f"使用降低强度的控制: 侧向={side_velocity:.2f}m/s", "恢复")
|
||||
else:
|
||||
# 如果从未检测到轨迹线，降低速度直接向前
|
||||
reduced_speed = speed * 0.3
|
||||
msg.vel_des = [reduced_speed, 0, 0]
|
||||
# 如果从未检测到轨迹线，默认向左侧移动一点，尝试找到轨迹线
|
||||
default_side_velocity = 0.1
|
||||
msg.vel_des = [0, default_side_velocity, 0]
|
||||
if observe:
|
||||
warning(f"无法检测到左侧轨迹线，降低速度前进: {reduced_speed:.2f}m/s", "警告")
|
||||
warning(f"无法检测到左侧轨迹线，向左移动尝试寻找: {default_side_velocity:.2f}m/s", "警告")
|
||||
|
||||
# 发送命令
|
||||
msg.life_count += 1
|
||||
@ -1438,11 +1441,11 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
|
||||
if observe:
|
||||
info("开始平滑停止", "停止")
|
||||
|
||||
# 先降低速度再停止，实现平滑停止
|
||||
slowdown_steps = 5
|
||||
# 平滑停止 - 由于没有前进速度，只需要停止侧向移动
|
||||
slowdown_steps = 3
|
||||
for i in range(slowdown_steps, 0, -1):
|
||||
slowdown_factor = i / slowdown_steps
|
||||
msg.vel_des = [speed * slowdown_factor, 0, 0]
|
||||
msg.vel_des = [0, 0, 0] # 完全停止所有移动
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
time.sleep(0.1)
|
||||
|
||||
37
logs/robot_2025-05-25.log
Normal file
37
logs/robot_2025-05-25.log
Normal file
@ -0,0 +1,37 @@
|
||||
2025-05-25 19:50:01 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码
|
||||
2025-05-25 19:50:02 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码
|
||||
2025-05-25 19:50:03 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测
|
||||
2025-05-25 19:50:04 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 65 条直线
|
||||
2025-05-25 19:50:05 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 8 条垂直线
|
||||
2025-05-25 19:50:07 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250525_195007_428255.jpg
|
||||
2025-05-25 19:50:07 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250525_195007_428255', 'center_point': (834, 1080), 'deviation': -126, 'left_track_mid_x': 397.0, 'right_track_mid_x': 1351.5, 'track_width': 954.5, 'center_slope': -2.8529411764705883}
|
||||
2025-05-25 19:50:15 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-25 19:50:16 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-25 19:50:17 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码
|
||||
2025-05-25 19:50:18 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-25 19:50:19 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 36 条直线
|
||||
2025-05-25 19:50:20 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 4 条垂直线
|
||||
2025-05-25 19:50:21 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点
|
||||
2025-05-25 19:50:22 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250525_195022_290308.jpg
|
||||
2025-05-25 19:50:22 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250525_195022_290308.jpg
|
||||
2025-05-25 19:50:22 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250525_195022_290308', 'tracking_point': (94, 1079), 'ground_intersection': (92, 1080), 'distance_to_left': 215.0, 'slope': -0.756198347107438, 'line_mid_x': 215.0}
|
||||
2025-05-25 19:52:13 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-25 19:52:14 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-25 19:52:15 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码
|
||||
2025-05-25 19:52:16 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-25 19:52:17 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 74 条直线
|
||||
2025-05-25 19:52:18 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 2 条垂直线
|
||||
2025-05-25 19:52:19 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点
|
||||
2025-05-25 19:52:20 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250525_195220_395525.jpg
|
||||
2025-05-25 19:52:20 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250525_195220_395525.jpg
|
||||
2025-05-25 19:52:20 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250525_195220_395525', 'tracking_point': (549, 1071), 'ground_intersection': (543, 1080), 'distance_to_left': 584.5, 'slope': -1.619718309859155, 'line_mid_x': 584.5}
|
||||
2025-05-25 19:52:26 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||
2025-05-25 19:52:27 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||
2025-05-25 19:52:28 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码
|
||||
2025-05-25 19:52:29 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||
2025-05-25 19:52:31 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 79 条直线
|
||||
2025-05-25 19:52:32 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 4 条垂直线
|
||||
2025-05-25 19:52:33 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点
|
||||
2025-05-25 19:52:34 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250525_195234_036434.jpg
|
||||
2025-05-25 19:52:34 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250525_195234_036434.jpg
|
||||
2025-05-25 19:52:34 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250525_195234_036434', 'tracking_point': (133, 1077), 'ground_intersection': (128, 1080), 'distance_to_left': 246.0, 'slope': -0.7035398230088495, 'line_mid_x': 246.0}
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 46 KiB After Width: | Height: | Size: 47 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 64 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 63 KiB |
BIN
res/path/test/result_image_20250514_024347.png
Normal file
BIN
res/path/test/result_image_20250514_024347.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 59 KiB |
@ -1248,11 +1248,10 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
|
||||
|
||||
# 计算图像中间和左侧区域的范围
|
||||
center_x = width // 2
|
||||
# 主要关注视野的左半部分
|
||||
left_region_width = center_x
|
||||
left_region_height = height
|
||||
# 主要关注视野的左半部分，但稍微扩大一点以确保捕捉到左侧线
|
||||
left_region_width = int(center_x * 1.2) # 扩大左侧搜索区域
|
||||
left_bound = 0
|
||||
right_bound = center_x
|
||||
right_bound = min(width, left_region_width)
|
||||
bottom_bound = height
|
||||
top_bound = 0
|
||||
|
||||
@ -1268,17 +1267,17 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
|
||||
# 转换到HSV颜色空间以便更容易提取黄色
|
||||
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
|
||||
|
||||
# 黄色的HSV范围 - 扩大范围以更好地捕捉不同光照条件下的黄色
|
||||
lower_yellow = np.array([15, 80, 80]) # 更宽松的黄色下限
|
||||
upper_yellow = np.array([35, 255, 255]) # 更宽松的黄色上限
|
||||
# 黄色的HSV范围 - 进一步扩大范围以适应不同光照条件
|
||||
lower_yellow = np.array([12, 70, 70]) # 更宽松的黄色下限
|
||||
upper_yellow = np.array([38, 255, 255]) # 更宽松的黄色上限
|
||||
|
||||
# 创建黄色的掩码
|
||||
mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
|
||||
|
||||
# 形态学操作以改善掩码
|
||||
kernel = np.ones((5, 5), np.uint8) # 增大kernel尺寸
|
||||
kernel = np.ones((5, 5), np.uint8)
|
||||
mask = cv2.dilate(mask, kernel, iterations=1)
|
||||
mask = cv2.erode(mask, np.ones((3, 3), np.uint8), iterations=1) # 添加腐蚀操作去除噪点
|
||||
mask = cv2.erode(mask, np.ones((3, 3), np.uint8), iterations=1)
|
||||
|
||||
if observe:
|
||||
debug("步骤2: 创建黄色掩码", "处理")
|
||||
@ -1301,9 +1300,9 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
|
||||
cv2.imshow("边缘检测", edges)
|
||||
cv2.waitKey(delay)
|
||||
|
||||
# 霍夫变换检测直线
|
||||
lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=25,
|
||||
minLineLength=height*0.15, maxLineGap=50) # 调整参数以检测更长的线段
|
||||
# 霍夫变换检测直线 - 降低minLineLength以更好地检测近距离的线段
|
||||
lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=20,
|
||||
minLineLength=height*0.1, maxLineGap=60) # 调整参数以检测更短的线段
|
||||
|
||||
if lines is None or len(lines) == 0:
|
||||
error("未检测到直线", "失败")
|
||||
@ -1318,7 +1317,7 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
|
||||
cv2.imshow("检测到的直线", lines_img)
|
||||
cv2.waitKey(delay)
|
||||
|
||||
# 筛选左侧区域内的近似垂直线
|
||||
# 筛选左侧区域内的近似垂直线，放宽条件以捕获更多可能的线
|
||||
left_vertical_lines = []
|
||||
for line in lines:
|
||||
x1, y1, x2, y2 = line[0]
|
||||
@ -1333,8 +1332,8 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
|
||||
else:
|
||||
slope = (y2 - y1) / (x2 - x1)
|
||||
|
||||
# 筛选接近垂直的线 (斜率较大)
|
||||
if abs(slope) > 0.7: # 设置较宽松的垂直线斜率阈值
|
||||
# 放宽垂直线的斜率范围，以适应近距离时线的倾斜
|
||||
if abs(slope) > 0.5: # 降低垂直线斜率阈值
|
||||
line_length = np.sqrt((x2-x1)**2 + (y2-y1)**2)
|
||||
# 计算线的中点坐标
|
||||
mid_x = (x1 + x2) / 2
|
||||
@ -1360,15 +1359,24 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
|
||||
cv2.imshow("左侧垂直线", left_lines_img)
|
||||
cv2.waitKey(delay)
|
||||
|
||||
# 按线段长度和位置进行评分，优先选择更长且更靠近图像左边的线
|
||||
# 修改评分函数，优先选择最左侧的线
|
||||
def score_left_line(line_info):
|
||||
_, mid_x, _, _, length = line_info
|
||||
# 线段越长分数越高
|
||||
_, mid_x, mid_y, slope, length = line_info
|
||||
|
||||
# 线段越长分数越高，但降低权重
|
||||
length_score = min(1.0, length / (height * 0.3))
|
||||
# 越靠近左边分数越高
|
||||
|
||||
# 越靠近左边分数越高，增加权重
|
||||
position_score = 1.0 - (mid_x / center_x)
|
||||
# 综合评分
|
||||
return length_score * 0.7 + position_score * 0.3
|
||||
|
||||
# 优先选择在图像下半部分的线段
|
||||
height_score = min(1.0, mid_y / (height * 0.6))
|
||||
|
||||
# 斜率得分，垂直度越高越好
|
||||
slope_score = min(1.0, abs(slope) / 20) if abs(slope) < 100 else 1.0
|
||||
|
||||
# 综合评分，加大位置权重
|
||||
return length_score * 0.2 + position_score * 0.5 + height_score * 0.2 + slope_score * 0.1
|
||||
|
||||
# 对线段进行评分并排序
|
||||
left_vertical_lines = sorted(left_vertical_lines, key=score_left_line, reverse=True)
|
||||
@ -1387,9 +1395,6 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
|
||||
tracking_point = (x2, y2) if y2 > y1 else (x1, y1)
|
||||
|
||||
# 计算线与地面的交点
|
||||
# 使用线段的方程: (y - y1) = slope * (x - x1)
|
||||
# 地面对应图像底部: y = height
|
||||
# 解这个方程得到交点的x坐标
|
||||
if abs(slope) < 0.01: # 几乎垂直
|
||||
ground_intersection_x = x1
|
||||
else:
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user