"""
|
MowerController
|
|
控制器假设与契约：
|
- 坐标系：XY 平面单位为米（RTK 提供），Z 忽略用于平面跟踪。
|
- 航向（heading）：弧度，范围任意，控制器会处理环绕。
|
- GPS 频率（位置/速度/heading/attitude）约 10Hz；IMU（加速度、角速度）100Hz。
|
- 控制输出频率：74Hz，返回两个信号：forward（-100..100，对应 -1..+1 m/s）和 turn（-100..100，对应 -pi/2..+pi/2 rad/s，正为左转）。
|
- 路径文件：JSON 列表 [[x,y],[x,y],...]
|
|
设计说明：
|
- 使用纯追踪（pure pursuit）方法选取前视点，并基于角度误差使用 P 控制计算期望偏航速率（rad/s）。
|
- 前进速度与偏航误差耦合：偏航误差大时降低前进速度以保证可转弯能力。
|
|
接口：
|
- controller = MowerController(path_points, params)
|
- controller.update_gps(pos_xy, heading, vel_xy, timestamp)
|
- controller.update_imu(accel3, gyro3, timestamp)
|
- controller.compute_control(timestamp) -> {'forward': int, 'turn': int, 'info': {...}}
|
|
"""
|
from math import atan2, hypot, sin, cos, pi, acos
|
import time
|
|
|
def wrap_angle(a):
|
# wrap to [-pi, pi)
|
while a >= pi:
|
a -= 2 * pi
|
while a < -pi:
|
a += 2 * pi
|
return a
|
|
|
class MowerController:
|
def __init__(self, path_points, params=None):
|
# path_points: list of [x,y]
|
# 原始路径点
|
original_path = [tuple(p) for p in path_points]
|
if len(original_path) == 0:
|
raise ValueError("path_points cannot be empty")
|
|
# 保存原始路径用于拐点检测
|
self.original_path = original_path
|
|
# 路径加密（densify）：避免首段过长导致纯追踪前视点跳到远处
|
p_params = params or {}
|
max_seg_len = p_params.get('max_segment_length', 0.6) # 默认将长段切分为 <=0.6m
|
densified = []
|
for i in range(len(original_path) - 1):
|
x1, y1 = original_path[i]
|
x2, y2 = original_path[i + 1]
|
densified.append((x1, y1))
|
dx = x2 - x1
|
dy = y2 - y1
|
seg_len = hypot(dx, dy)
|
if seg_len > max_seg_len and seg_len > 1e-6:
|
# 需要插入若干中间点（不包含终点，终点下一循环添加）
|
steps = int(seg_len // max_seg_len)
|
# 使用均匀线性插值（1..steps），防止首段过长纯追踪直接跳过
|
for k in range(1, steps + 1):
|
ratio = k / (steps + 1)
|
densified.append((x1 + dx * ratio, y1 + dy * ratio))
|
# 添加最后一个点
|
densified.append(original_path[-1])
|
self.path = densified
|
self.n = len(self.path)
|
# 预计算每段长度及累积长度用于进度计算
|
self.segment_lengths = []
|
self.cum_lengths = [0.0]
|
for i in range(self.n - 1):
|
ax, ay = self.path[i]
|
bx, by = self.path[i+1]
|
L = hypot(bx-ax, by-ay)
|
self.segment_lengths.append(L)
|
self.cum_lengths.append(self.cum_lengths[-1] + L)
|
self.total_length = self.cum_lengths[-1] if self.cum_lengths else 0.0
|
# 进度单调性保护变量
|
self.last_progress = 0.0
|
|
# parameters and gains
|
p = params or {}
|
self.max_forward_mps = p.get('max_forward_mps', 0.6) # 更保守的速度
|
self.max_reverse_mps = p.get('max_reverse_mps', 0.2) # 保守的倒车速度
|
self.max_yawrate = p.get('max_yawrate', pi/4) # 更保守的转向速率
|
|
# 软启动和加减速
|
self.accel = p.get('accel', 0.2) # m/s^2
|
self.current_speed_target = 0.0 # 目标速度
|
self.current_speed = 0.0 # 实际速度
|
|
# 控制增益（提高响应速度和精度）
|
self.k_heading = p.get('k_heading', 1.8) # 大幅提高转向增益，加快航向收敛
|
self.k_heading_d = p.get('k_heading_d', 0.25) # 增加微分作用，抑制振荡
|
self.k_xte = p.get('k_xte', 1.2) # 大幅提高横向误差增益，激进纠正偏离
|
self.k_curv = p.get('k_curv', 0.2) # 降低曲率前馈增益
|
self.base_speed = p.get('base_speed', 0.5) # 提高基础速度
|
|
# 路径跟踪参数（减小lookahead以提高精度）
|
self.lookahead_time = p.get('lookahead_time', 2.0) # 减小前视时间
|
# 减小最小前视距离以提高轨迹精度
|
self.lookahead_min = p.get('lookahead_min', 0.35)
|
self.lookahead_max = p.get('lookahead_max', 1.5) # 减小最大前视距离
|
self.goal_tolerance = p.get('goal_tolerance', 0.3) # m
|
self.slow_zone = p.get('slow_zone', 1.5) # 减小减速区域
|
self.final_approach_dist = p.get('final_approach_dist', 0.8) # 减小精确接近区域
|
|
# 新增：横向误差相关
|
self.max_xte = p.get('max_xte', 1.0) # 减小最大横向误差限制，提高精度
|
self.xte_gain_scale = p.get('xte_gain_scale', 1.0) # 移除横向误差缩放，使用完整增益
|
|
# 速度控制缩放参数
|
self.curv_gain_scale = p.get('curv_gain_scale', 2.0) # 曲率缩放增益
|
self.heading_err_gain_scale = p.get('heading_err_gain_scale', 1.5) # 航向误差缩放增益
|
|
# 状态估计和滤波
|
self.yawrate_filter = 0.0
|
self.yawrate_alpha = p.get('yawrate_alpha', 0.90) # 进一步增加平滑度
|
self.last_heading_err = 0.0 # 用于D控制
|
self.last_time = None # 用于计算dt
|
self.last_curv = 0.0 # 用于平滑曲率变化
|
|
# 累积误差控制
|
self.heading_err_sum = 0.0 # 航向误差积分
|
self.xte_sum = 0.0 # 横向误差积分
|
self.max_sum = 2.0 # 积分限幅
|
self.k_heading_i = p.get('k_heading_i', 0.01) # 航向误差积分增益
|
self.k_xte_i = p.get('k_xte_i', 0.01) # 横向误差积分增益
|
|
# 错误恢复
|
self.consecutive_large_err = 0 # 连续大误差计数
|
self.large_err_threshold = p.get('large_err_threshold', 0.5) # 大误差阈值
|
self.max_large_err_count = 10 # 最大连续大误差次数
|
|
# (日志文件将在下面统一创建)
|
|
# state (most recent readings)
|
self.pos = None # (x,y)
|
self.heading = None # rad
|
self.vel = (0.0, 0.0) # vx, vy in m/s (ground frame)
|
self.imu_accel = None
|
self.imu_gyro = None
|
|
# control stages
|
self.STAGE_GOTO_START = 'goto_start' # 移动到起点阶段
|
self.STAGE_FOLLOW_PATH = 'follow_path' # 跟随路径阶段
|
self.stage = self.STAGE_GOTO_START
|
|
# 起点接近参数 - 收紧以确保真正到达起点
|
self.start_tolerance = p.get('start_tolerance', 0.12) # 位置容差（收紧到12cm）
|
self.start_heading_tolerance = p.get('start_heading_tolerance', 0.01) # 航向容差 (~14°，收紧）
|
self.start_approach_dist = p.get('start_approach_dist', 1.5) # 减速距离
|
self.start_slow_speed = p.get('start_slow_speed', 0.25) # 接近速度
|
self.start_min_speed = p.get('start_min_speed', 0.25) # 提高最小速度
|
# 新增：到起点时先转向再前进的阈值（若与起点方向误差大于此阈值，则先转向）
|
self.start_turn_first_heading = p.get('start_turn_first_heading', pi/50) # ~15度
|
# 路径跟随阶段的最小前进速度（防止映射/缩放导致速度归零）
|
self.min_follow_speed = p.get('min_follow_speed', 0.08)
|
# Recovery 模式参数
|
self.in_recovery = False
|
self.recovery_dist = p.get('recovery_dist', 2.0)
|
self.recovery_exit_dist = p.get('recovery_exit_dist', 0.8)
|
self.recovery_speed = p.get('recovery_speed', 0.55)
|
|
# progress - 只使用点索引，不使用segment
|
self.current_target_idx = 0
|
self.nearest_path_idx = 0 # 最近路径点索引
|
self.finished = False
|
# goto_start 子状态：'rotate'|'move'|'align'，保证顺序执行且不反复切换
|
self.goto_substage = 'rotate'
|
|
# 拐点转向参数
|
self.waypoint_approach_mode = False # 是否在接近拐点模式
|
self.waypoint_turn_mode = False # 是否在拐点转向模式
|
self.waypoint_turn_target_idx = None # 当前转向的拐点在原始路径中的索引
|
self.waypoint_turn_tolerance = p.get('waypoint_turn_tolerance', 0.12) # 到达拐点的距离容差(12cm)
|
self.waypoint_approach_dist = p.get('waypoint_approach_dist', 0.8) # 开始减速接近拐点的距离(80cm)
|
self.waypoint_turn_heading_tol = p.get('waypoint_turn_heading_tol', 0.1) # 转向完成的航向容差(~6度)
|
self.waypoint_turned = set() # 记录已经转向过的拐点，避免重复转向
|
# 创建日志文件
|
self.log_file = open('controller_log.csv', 'w')
|
# 简化日志格式：移除segment相关
|
self.log_file.write('t,x,y,heading,target_x,target_y,target_dist,target_idx,xte,heading_err,forward,turn,speed,stage,status\n')
|
|
def update_gps(self, pos_xy, heading, vel_xy=None, timestamp=None):
|
"""Update GPS-like measurements at ~10Hz.
|
|
pos_xy: (x,y) in meters
|
heading: radians
|
vel_xy: (vx, vy) in m/s in global frame (optional)
|
"""
|
self.pos = tuple(pos_xy)
|
self.heading = float(heading)
|
if vel_xy is not None:
|
self.vel = tuple(vel_xy)
|
|
def update_imu(self, accel3, gyro3, timestamp=None):
|
"""Update IMU (100Hz). accel3 and gyro3 are 3-element tuples."""
|
self.imu_accel = tuple(accel3)
|
self.imu_gyro = tuple(gyro3)
|
|
def _calc_xte(self, p1, p2, pos):
|
"""Calculate cross track error (perpendicular distance to line segment)"""
|
x, y = pos
|
x1, y1 = p1
|
x2, y2 = p2
|
|
# Convert to local coordinates
|
dx = x2 - x1
|
dy = y2 - y1
|
l2 = dx*dx + dy*dy
|
|
if l2 == 0: # points are identical
|
return hypot(x - x1, y - y1)
|
|
# Project point onto line
|
t = ((x - x1)*dx + (y - y1)*dy) / l2
|
|
if t < 0: # before start
|
return hypot(x - x1, y - y1)
|
elif t > 1: # after end
|
return hypot(x - x2, y - y2)
|
else: # projection point lies on segment
|
px = x1 + t*dx
|
py = y1 + t*dy
|
xte = hypot(x - px, y - py)
|
# Determine sign based on which side of the line the point is on
|
sign = 1 if (x - px)*dy - (y - py)*dx > 0 else -1
|
return sign * xte
|
|
def _is_waypoint_turn_needed(self, idx):
|
"""
|
检测路径点idx是否是拐点（需要原地转向）
|
|
拐点定义：前后两段路径方向变化超过45度
|
"""
|
if idx <= 0 or idx >= len(self.path) - 1:
|
return False
|
|
# 前一段方向
|
p_prev = self.path[idx - 1]
|
p_curr = self.path[idx]
|
p_next = self.path[idx + 1]
|
|
dx1 = p_curr[0] - p_prev[0]
|
dy1 = p_curr[1] - p_prev[1]
|
dx2 = p_next[0] - p_curr[0]
|
dy2 = p_next[1] - p_curr[1]
|
|
# 计算两段方向
|
heading1 = atan2(dy1, dx1)
|
heading2 = atan2(dy2, dx2)
|
|
# 方向变化角度
|
angle_diff = abs(wrap_angle(heading2 - heading1))
|
|
# 如果方向变化超过45度，认为是拐点
|
return angle_diff > pi / 4
|
|
def _find_nearest_original_waypoint(self):
|
"""
|
在原始路径中找到最接近当前位置的拐点
|
|
Returns: (waypoint_idx, distance) 或 (None, None)
|
"""
|
if self.pos is None:
|
return None, None
|
|
x, y = self.pos
|
nearest_waypoint_idx = None
|
nearest_dist = float('inf')
|
|
# 遍历原始路径的所有拐点
|
for i in range(1, len(self.original_path) - 1):
|
# 检查是否是拐点
|
p_prev = self.original_path[i - 1]
|
p_curr = self.original_path[i]
|
p_next = self.original_path[i + 1]
|
|
dx1 = p_curr[0] - p_prev[0]
|
dy1 = p_curr[1] - p_prev[1]
|
dx2 = p_next[0] - p_curr[0]
|
dy2 = p_next[1] - p_curr[1]
|
|
heading1 = atan2(dy1, dx1)
|
heading2 = atan2(dy2, dx2)
|
angle_diff = abs(wrap_angle(heading2 - heading1))
|
|
# 如果是拐点（方向变化>45度）
|
if angle_diff > pi / 4:
|
dist = hypot(p_curr[0] - x, p_curr[1] - y)
|
if dist < nearest_dist:
|
nearest_dist = dist
|
nearest_waypoint_idx = i
|
|
return nearest_waypoint_idx, nearest_dist
|
|
def _get_target_heading_at_waypoint(self, idx):
|
"""获取原始路径点idx处的目标航向（下一段的方向）"""
|
if idx >= len(self.original_path) - 1:
|
# 最后一个点，返回前一段方向
|
if idx > 0:
|
dx = self.original_path[idx][0] - self.original_path[idx-1][0]
|
dy = self.original_path[idx][1] - self.original_path[idx-1][1]
|
return atan2(dy, dx)
|
return 0.0
|
|
# 返回到下一个点的方向
|
dx = self.original_path[idx + 1][0] - self.original_path[idx][0]
|
dy = self.original_path[idx + 1][1] - self.original_path[idx][1]
|
return atan2(dy, dx)
|
|
def _find_lookahead_point(self, lookahead_dist):
|
"""
|
简化的Pure Pursuit前视点查找 - 只使用点索引
|
|
从nearest_path_idx开始向前搜索，找到第一个距离>=lookahead_dist的点
|
|
Returns: (lookahead_xy, curvature, lookahead_idx, nearest_idx)
|
"""
|
if self.pos is None:
|
return self.path[0], 0.0, 0, 0
|
|
x, y = self.pos
|
|
# 1. 找最近点 - 如果离路径很远,使用全局搜索;否则使用窗口搜索
|
# 先检查当前nearest_path_idx的距离
|
px, py = self.path[self.nearest_path_idx]
|
current_nearest_dist = hypot(px - x, py - y)
|
|
# 如果距离>1.0m,使用全局搜索
|
if current_nearest_dist > 1.0:
|
search_start = 0
|
search_end = len(self.path)
|
else:
|
# 使用窗口搜索
|
search_start = max(0, self.nearest_path_idx - 5)
|
search_end = min(len(self.path), self.nearest_path_idx + 30)
|
|
nearest_idx = self.nearest_path_idx
|
nearest_dist = float('inf')
|
|
for i in range(search_start, search_end):
|
px, py = self.path[i]
|
d = hypot(px - x, py - y)
|
if d < nearest_dist:
|
nearest_dist = d
|
nearest_idx = i
|
|
self.nearest_path_idx = nearest_idx # 更新最近点索引
|
|
# 2. 从nearest_idx开始向前搜索lookahead点
|
lookahead_idx = nearest_idx
|
for i in range(nearest_idx, len(self.path)):
|
px, py = self.path[i]
|
d = hypot(px - x, py - y)
|
if d >= lookahead_dist:
|
lookahead_idx = i
|
break
|
else:
|
# 没找到，使用最后一个点
|
lookahead_idx = len(self.path) - 1
|
|
# 3. 估计曲率（基于nearest_idx附近的点）
|
curvature = 0.0
|
if 1 <= nearest_idx < len(self.path) - 1:
|
p1 = self.path[nearest_idx - 1]
|
p2 = self.path[nearest_idx]
|
p3 = self.path[nearest_idx + 1]
|
|
v1 = (p2[0] - p1[0], p2[1] - p1[1])
|
v2 = (p3[0] - p2[0], p3[1] - p2[1])
|
|
l1 = hypot(v1[0], v1[1])
|
l2 = hypot(v2[0], v2[1])
|
|
if l1 > 1e-6 and l2 > 1e-6:
|
dot = v1[0]*v2[0] + v1[1]*v2[1]
|
cos_angle = max(-1.0, min(1.0, dot / (l1 * l2)))
|
angle_change = abs(pi - acos(cos_angle))
|
curvature = angle_change / max(l1, 1e-6)
|
|
return self.path[lookahead_idx], curvature, lookahead_idx, nearest_idx
|
|
def _find_nearest_original_waypoint(self):
|
"""
|
在原始路径中查找下一个未到达的拐点
|
按照路径顺序查找，不跳过
|
|
Returns: (waypoint_idx, distance) 或 (None, inf) 如果没有找到
|
"""
|
if not hasattr(self, 'original_path') or len(self.original_path) < 3:
|
return None, float('inf')
|
|
x, y = self.pos
|
|
# 遍历原始路径中的拐点（跳过起点和终点）
|
for i in range(1, len(self.original_path) - 1):
|
# 如果这个拐点已经转向过，跳过
|
if i in self.waypoint_turned:
|
continue
|
|
# 检查方向是否改变
|
prev_pt = self.original_path[i-1]
|
curr_pt = self.original_path[i]
|
next_pt = self.original_path[i+1]
|
|
# 计算前后方向向量
|
dx1 = curr_pt[0] - prev_pt[0]
|
dy1 = curr_pt[1] - prev_pt[1]
|
dx2 = next_pt[0] - curr_pt[0]
|
dy2 = next_pt[1] - curr_pt[1]
|
|
# 计算角度
|
angle1 = atan2(dy1, dx1)
|
angle2 = atan2(dy2, dx2)
|
angle_change = abs(wrap_angle(angle2 - angle1))
|
|
# 如果角度变化大于45度，认为是拐点
|
if angle_change > pi / 4:
|
# 找到第一个未完成的拐点，直接返回
|
dist = hypot(curr_pt[0] - x, curr_pt[1] - y)
|
return i, dist
|
|
return None, float('inf')
|
|
def _get_target_heading_at_waypoint(self, waypoint_idx):
|
"""
|
获取拐点处的目标航向（下一段路径的方向）
|
|
Args:
|
waypoint_idx: 拐点在原始路径中的索引
|
|
Returns: 目标航向（弧度）
|
"""
|
if waypoint_idx is None:
|
return 0.0 # 默认向东
|
|
# 如果是最后一个点，使用前一段的方向（保持不变）
|
if waypoint_idx >= len(self.original_path) - 1:
|
if waypoint_idx > 0:
|
prev_pt = self.original_path[waypoint_idx - 1]
|
curr_pt = self.original_path[waypoint_idx]
|
return atan2(curr_pt[1] - prev_pt[1], curr_pt[0] - prev_pt[0])
|
else:
|
return 0.0 # 只有一个点，默认向东
|
|
curr_pt = self.original_path[waypoint_idx]
|
next_pt = self.original_path[waypoint_idx + 1]
|
|
dx = next_pt[0] - curr_pt[0]
|
dy = next_pt[1] - curr_pt[1]
|
|
return atan2(dy, dx)
|
|
def _compute_xte_simple(self, pos):
|
"""
|
简化的横向误差计算 - 基于最近点
|
|
Returns: (xte, proj_x, proj_y)
|
"""
|
x, y = pos
|
nearest_idx = self.nearest_path_idx
|
|
# 计算到最近点的向量
|
px, py = self.path[nearest_idx]
|
|
# 如果有下一个点，计算到路径段的横向误差
|
if nearest_idx < len(self.path) - 1:
|
nx, ny = self.path[nearest_idx + 1]
|
|
# 段方向向量
|
dx = nx - px
|
dy = ny - py
|
seg_len2 = dx*dx + dy*dy
|
|
if seg_len2 > 1e-9:
|
# 投影到段上
|
t = ((x - px)*dx + (y - py)*dy) / seg_len2
|
t = max(0.0, min(1.0, t)) # 限制在[0,1]
|
|
proj_x = px + t * dx
|
proj_y = py + t * dy
|
|
# 横向误差（带符号）
|
xte_vec_x = x - proj_x
|
xte_vec_y = y - proj_y
|
xte_mag = hypot(xte_vec_x, xte_vec_y)
|
|
# 使用叉乘判断左右
|
cross = xte_vec_x * dy - xte_vec_y * dx
|
xte = xte_mag if cross > 0 else -xte_mag
|
|
return xte, proj_x, proj_y
|
|
# 否则直接用到最近点的距离
|
xte = hypot(x - px, y - py)
|
return xte, px, py
|
|
def compute_control(self, timestamp=None):
|
"""Compute forward and turn outputs mapped to [-100,100].
|
|
Returns dict with keys: forward(int), turn(int), info(dict)
|
"""
|
if self.finished:
|
return {'forward': 0, 'turn': 0, 'info': {'status': 'finished'}}
|
|
if self.pos is None or self.heading is None:
|
return {'forward': 0, 'turn': 0, 'info': {'status': 'no_state'}}
|
|
# ensure timestamp is numeric for logging/formatting
|
if timestamp is None:
|
timestamp = time.time()
|
|
# 计算时间增量
|
if self.last_time is None:
|
dt = 0.014 # 约74Hz
|
else:
|
dt = timestamp - self.last_time if timestamp else 0.014
|
self.last_time = timestamp if timestamp else (self.last_time + dt if self.last_time else 0.0)
|
|
x, y = self.pos
|
|
# 处理移动到起点阶段：使用持久子状态确保顺序执行：rotate -> move -> align
|
if self.stage == self.STAGE_GOTO_START:
|
start_point = self.path[0]
|
dx = start_point[0] - x
|
dy = start_point[1] - y
|
dist_to_start = hypot(dx, dy)
|
|
# 目标朝向指向起点
|
desired_heading_to_start = atan2(dy, dx)
|
heading_err_to_start = wrap_angle(desired_heading_to_start - self.heading)
|
|
# 初始化子状态（保守）
|
if not hasattr(self, 'goto_substage') or self.goto_substage is None:
|
self.goto_substage = 'rotate'
|
|
# 子状态：rotate -> move -> align
|
if self.goto_substage == 'rotate':
|
# 原地转向直到朝向误差足够小（移除距离退出条件，确保先转好）
|
if abs(heading_err_to_start) <= self.start_turn_first_heading:
|
self.goto_substage = 'move'
|
else:
|
yawrate_cmd = self.k_heading * heading_err_to_start * 1.3 # 稍微提高转向速度
|
turn_signal = int(round((yawrate_cmd / self.max_yawrate) * 100))
|
turn_signal = max(-100, min(100, turn_signal))
|
forward_signal = 0
|
# target is start_point for goto_start substage
|
target_x, target_y = start_point
|
target_dist = hypot(target_x - x, target_y - y)
|
target_idx = 0
|
self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
|
f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{target_idx},'
|
f'0.000,{heading_err_to_start:.3f},{forward_signal},{turn_signal},0.000,{self.stage},rotate_to_start\n')
|
self.log_file.flush()
|
return {'forward': forward_signal, 'turn': turn_signal, 'info': {
|
'status': 'rotate_to_start', 'stage': self.stage, 'heading_err': heading_err_to_start,
|
'dist_to_start': dist_to_start
|
}}
|
|
if self.goto_substage == 'move':
|
# 在移动子状态期间，不会回到 rotate 子状态，直到显式切换
|
desired_heading = desired_heading_to_start
|
heading_err = wrap_angle(desired_heading - self.heading)
|
|
# 如果已经接近起点则进入 align 子状态
|
if dist_to_start < self.start_tolerance:
|
self.goto_substage = 'align'
|
else:
|
# 常规：面向起点并前进
|
if dist_to_start < self.start_approach_dist:
|
dist_ratio = dist_to_start / self.start_approach_dist
|
speed_scale = 0.4 + 0.6 * dist_ratio # 提高最小速度比例
|
base_target_speed = self.start_slow_speed + (self.base_speed - self.start_slow_speed) * dist_ratio
|
else:
|
speed_scale = 1.0
|
base_target_speed = min(self.base_speed * 0.8, 0.5) # 限制最大速度避免过冲
|
|
# 航向误差缩放更宽松
|
heading_ratio = abs(heading_err) / pi
|
err_scale = 0.5 + 0.5 * (1.0 - heading_ratio) # 更宽松
|
target_speed = max(self.start_min_speed * 1.5, base_target_speed * speed_scale * err_scale)
|
target_speed = max(target_speed, self.start_min_speed * 1.2) # 确保最小速度
|
|
yawrate_cmd = self.k_heading * heading_err
|
forward_signal = int(round((target_speed / self.max_forward_mps) * 100))
|
forward_signal = max(-100, min(100, forward_signal))
|
turn_signal = int(round((yawrate_cmd / self.max_yawrate) * 100))
|
turn_signal = max(-100, min(100, turn_signal))
|
|
target_x, target_y = start_point
|
target_dist = hypot(target_x - x, target_y - y)
|
target_idx = 0
|
self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
|
f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{target_idx},'
|
f'0.000,{heading_err:.3f},{forward_signal},{turn_signal},{target_speed:.3f},{self.stage},moving_to_start\n')
|
self.log_file.flush()
|
|
return {'forward': forward_signal, 'turn': turn_signal, 'info': {
|
'status': 'moving_to_start', 'stage': self.stage, 'dist_to_start': dist_to_start,
|
'heading_err': heading_err
|
}}
|
|
# align 子状态：到达起点后进行原地朝向修正，完成后切换到跟踪
|
if self.goto_substage == 'align':
|
if len(self.path) > 1:
|
path_dx = self.path[1][0] - self.path[0][0]
|
path_dy = self.path[1][1] - self.path[0][1]
|
desired_start_heading = atan2(path_dy, path_dx)
|
else:
|
desired_start_heading = 0.0
|
heading_err_final = wrap_angle(desired_start_heading - self.heading)
|
# 如果最终朝向对齐或非常接近位置，则切换到跟踪
|
# 只有同时满足位置与航向条件才算真正到达起点，避免“偏近+朝向差”提前切换
|
if (dist_to_start < self.start_tolerance) and (abs(heading_err_final) < self.start_heading_tolerance):
|
self.stage = self.STAGE_FOLLOW_PATH
|
self.goto_substage = None
|
# 保持 current_target_idx = 0，让 pure pursuit 自然选择前视点
|
self.current_target_idx = 0
|
# 重置速度为较小值，避免 lookahead 过大
|
self.current_speed = self.start_min_speed
|
# 小幅前进以确保不卡在起点
|
f_sig = int(round((self.start_min_speed / self.max_forward_mps) * 100))
|
target_x, target_y = self.path[0] # 使用起点作为初始目标
|
target_dist = hypot(target_x - x, target_y - y)
|
target_idx = 0
|
self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
|
f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{target_idx},'
|
f'0.000,{heading_err_final:.3f},{f_sig},0,{self.start_min_speed:.3f},{self.stage},reached_start\n')
|
self.log_file.flush()
|
return {'forward': f_sig, 'turn': 0, 'info': {'status': 'reached_start', 'stage': self.stage, 'advanced_idx': self.current_target_idx}}
|
else:
|
# 在起点附近但朝向不对，进行原地修正（停止前进）
|
sign = 1 if heading_err_final >= 0 else -1
|
yawrate_cmd = sign * min(self.max_yawrate * 0.6, max(0.05, abs(heading_err_final) * 0.8))
|
turn_signal = int(round((yawrate_cmd / self.max_yawrate) * 100))
|
turn_signal = max(-100, min(100, turn_signal))
|
target_x, target_y = start_point
|
target_dist = hypot(target_x - x, target_y - y)
|
target_idx = 0
|
self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
|
f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{target_idx},'
|
f'0.000,{heading_err_final:.3f},0,{turn_signal},{0.000:.3f},{self.stage},align_at_start\n')
|
self.log_file.flush()
|
return {'forward': 0, 'turn': turn_signal, 'info': {'status': 'align_at_start', 'heading_err': heading_err_final}}
|
|
# 计算时间增量
|
if self.last_time is None:
|
dt = 0.014 # 约74Hz
|
else:
|
dt = timestamp - self.last_time if timestamp else 0.014
|
self.last_time = timestamp if timestamp else (self.last_time + dt if self.last_time else 0.0)
|
|
# 计算横向误差（使用简化方法）
|
x, y = self.pos
|
xte, proj_x, proj_y = self._compute_xte_simple(self.pos)
|
|
# 预先计算到终点的距离
|
final_dist = hypot(self.path[-1][0] - x, self.path[-1][1] - y)
|
|
# ========== 拐点接近和转向模式检测 ==========
|
# 检查是否需要进入拐点接近或转向模式
|
if not self.waypoint_approach_mode and not self.waypoint_turn_mode:
|
# 在原始路径中查找最近的拐点
|
waypoint_idx, waypoint_dist = self._find_nearest_original_waypoint()
|
|
# 如果拐点在接近距离内（2m）且未转向过，进入接近模式
|
# 注意：增大检测距离，确保在短边也能提前进入接近模式
|
if (waypoint_idx is not None and
|
waypoint_dist < 2.0 and
|
waypoint_idx not in self.waypoint_turned):
|
self.waypoint_approach_mode = True
|
self.waypoint_turn_target_idx = waypoint_idx # 记录当前接近的拐点
|
self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
|
f'{self.original_path[waypoint_idx][0]:.3f},'
|
f'{self.original_path[waypoint_idx][1]:.3f},'
|
f'{waypoint_dist:.3f},{waypoint_idx},'
|
f'{xte:.3f},0.000,0,0,0.000,{self.stage},enter_waypoint_approach\n')
|
self.log_file.flush()
|
|
# ========== 拐点接近模式：精确接近拐点位置 ==========
|
if self.waypoint_approach_mode:
|
waypoint_x, waypoint_y = self.original_path[self.waypoint_turn_target_idx]
|
dx = waypoint_x - x
|
dy = waypoint_y - y
|
waypoint_dist = hypot(dx, dy)
|
|
# 检查是否到达拐点（12cm容差）
|
if waypoint_dist < self.waypoint_turn_tolerance:
|
# 到达拐点，切换到转向模式
|
self.waypoint_approach_mode = False
|
self.waypoint_turn_mode = True
|
self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
|
f'{waypoint_x:.3f},{waypoint_y:.3f},'
|
f'{waypoint_dist:.3f},{self.waypoint_turn_target_idx},'
|
f'{xte:.3f},0.000,0,0,0.000,{self.stage},reached_waypoint\n')
|
self.log_file.flush()
|
else:
|
# 直接朝向拐点前进（忽略路径，直接目标点导航）
|
desired_heading_to_waypoint = atan2(dy, dx)
|
heading_err_to_waypoint = wrap_angle(desired_heading_to_waypoint - self.heading)
|
|
# 速度控制：接近时逐渐减速
|
if waypoint_dist < 0.5: # 50cm内减速
|
speed_ratio = waypoint_dist / 0.5
|
target_speed = self.start_min_speed * (0.4 + 0.6 * speed_ratio)
|
else:
|
target_speed = self.base_speed * 0.7
|
|
# 航向误差也会降低速度
|
heading_ratio = abs(heading_err_to_waypoint) / pi
|
err_scale = 0.5 + 0.5 * (1.0 - heading_ratio)
|
target_speed = target_speed * err_scale
|
target_speed = max(self.start_min_speed * 0.5, target_speed)
|
|
# 转向控制
|
yawrate_cmd = self.k_heading * heading_err_to_waypoint * 1.3 # 稍微提高转向增益
|
|
forward_signal = int(round((target_speed / self.max_forward_mps) * 100))
|
forward_signal = max(-100, min(100, forward_signal))
|
turn_signal = int(round((yawrate_cmd / self.max_yawrate) * 100))
|
turn_signal = max(-100, min(100, turn_signal))
|
|
self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
|
f'{waypoint_x:.3f},{waypoint_y:.3f},'
|
f'{waypoint_dist:.3f},{self.waypoint_turn_target_idx},'
|
f'{xte:.3f},{heading_err_to_waypoint:.3f},{forward_signal},{turn_signal},{target_speed:.3f},{self.stage},approaching_waypoint\n')
|
self.log_file.flush()
|
|
return {'forward': forward_signal, 'turn': turn_signal, 'info': {
|
'status': 'approaching_waypoint',
|
'waypoint_dist': waypoint_dist,
|
'heading_err': heading_err_to_waypoint,
|
'target_speed': target_speed
|
}}
|
|
# ========== 拐点转向模式：原地转向 ==========
|
if self.waypoint_turn_mode:
|
# 获取目标航向（下一段的方向）
|
target_heading = self._get_target_heading_at_waypoint(self.waypoint_turn_target_idx)
|
heading_err = wrap_angle(target_heading - self.heading)
|
|
# 检查是否转向完成
|
if abs(heading_err) < self.waypoint_turn_heading_tol:
|
# 转向完成，退出转向模式
|
self.waypoint_turned.add(self.waypoint_turn_target_idx) # 记录已转向
|
self.waypoint_turn_mode = False
|
self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
|
f'{self.original_path[self.waypoint_turn_target_idx][0]:.3f},'
|
f'{self.original_path[self.waypoint_turn_target_idx][1]:.3f},'
|
f'0.000,{self.waypoint_turn_target_idx},'
|
f'{xte:.3f},{heading_err:.3f},0,0,0.000,{self.stage},exit_waypoint_turn\n')
|
self.log_file.flush()
|
else:
|
# 原地转向：forward=0, turn基于航向误差
|
yawrate_cmd = self.k_heading * heading_err * 1.2 # 稍微提高转向增益
|
turn_signal = int(round((yawrate_cmd / self.max_yawrate) * 100))
|
turn_signal = max(-100, min(100, turn_signal))
|
|
self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
|
f'{self.original_path[self.waypoint_turn_target_idx][0]:.3f},'
|
f'{self.original_path[self.waypoint_turn_target_idx][1]:.3f},'
|
f'0.000,{self.waypoint_turn_target_idx},'
|
f'{xte:.3f},{heading_err:.3f},0,{turn_signal},0.000,{self.stage},waypoint_turning\n')
|
self.log_file.flush()
|
|
return {'forward': 0, 'turn': turn_signal, 'info': {
|
'status': 'waypoint_turning',
|
'heading_err': heading_err,
|
'target_heading': target_heading
|
}}
|
|
# 动态前视距离：基于当前速度和横向误差
|
actual_speed = abs(self.current_speed)
|
xte_mag = abs(xte)
|
|
# lookahead = lookahead_min + speed_component - xte_penalty
|
base_lookahead = self.lookahead_min + self.lookahead_time * actual_speed - 0.3 * min(xte_mag, 0.5)
|
lookahead = max(self.lookahead_min, min(self.lookahead_max, base_lookahead))
|
|
# 恢复模式判定
|
dist_to_path = abs(xte)
|
if not self.in_recovery and dist_to_path > self.recovery_dist:
|
self.in_recovery = True
|
self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
|
f'{proj_x:.3f},{proj_y:.3f},{dist_to_path:.3f},{self.nearest_path_idx},'
|
f'{xte:.3f},0.000,0,0,0.000,{self.stage},enter_recovery\n')
|
self.log_file.flush()
|
|
if self.in_recovery and dist_to_path < self.recovery_exit_dist:
|
self.in_recovery = False
|
self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
|
f'{proj_x:.3f},{proj_y:.3f},{dist_to_path:.3f},{self.nearest_path_idx},'
|
f'{xte:.3f},0.000,0,0,0.000,{self.stage},exit_recovery\n')
|
self.log_file.flush()
|
|
# ========== 标准 Pure Pursuit 控制 ==========
|
# 选择前视点
|
(lx, ly), path_curvature, lookahead_idx, nearest_idx = self._find_lookahead_point(lookahead)
|
self.current_target_idx = lookahead_idx
|
|
# 计算前视点方向
|
path_heading = atan2(ly - y, lx - x)
|
|
# 横向误差修正
|
xte_normalized = max(-1.0, min(1.0, xte / self.max_xte))
|
xte_correction = -self.k_xte * xte_normalized * self.xte_gain_scale
|
|
# 期望航向 = 前视方向 + xte修正
|
desired_heading = wrap_angle(path_heading + xte_correction)
|
heading_err = wrap_angle(desired_heading - self.heading)
|
|
# 计算航向误差的变化率
|
heading_err_d = (heading_err - self.last_heading_err) / dt if dt > 0 else 0
|
self.last_heading_err = heading_err
|
|
# 积分项计算（带防饱和）
|
self.heading_err_sum += heading_err * dt
|
self.xte_sum += xte * dt
|
self.heading_err_sum = max(-self.max_sum, min(self.max_sum, self.heading_err_sum))
|
self.xte_sum = max(-self.max_sum, min(self.max_sum, self.xte_sum))
|
|
# PID控制
|
yawrate_cmd = (self.k_heading * heading_err +
|
self.k_heading_d * heading_err_d +
|
self.k_heading_i * self.heading_err_sum +
|
self.k_xte_i * self.xte_sum)
|
|
# 速度控制：基于曲率、航向误差、横向误差
|
# 1) 曲率缩放
|
curv_scale = 1.0 / (1.0 + abs(path_curvature) * self.curv_gain_scale)
|
|
# 2) 航向误差缩放
|
heading_err_abs = abs(heading_err)
|
err_scale = 1.0 / (1.0 + heading_err_abs * self.heading_err_gain_scale)
|
|
# 3) 横向误差缩放
|
xte_abs = abs(xte)
|
xte_scale = 1.0 / (1.0 + xte_abs * self.xte_gain_scale)
|
|
# 混合缩放：几何平均 + 最小值
|
geom_scale = (curv_scale * err_scale * xte_scale) ** (1.0/3.0)
|
min_scale = min(curv_scale, err_scale, xte_scale)
|
blend_scale = 0.8 * geom_scale + 0.2 * min_scale
|
|
target_speed = self.base_speed * blend_scale
|
|
# 滤波和限幅
|
self.yawrate_filter = (self.yawrate_alpha * yawrate_cmd +
|
(1-self.yawrate_alpha) * self.yawrate_filter)
|
max_yawrate = self.max_yawrate
|
yawrate_cmd = max(-max_yawrate, min(max_yawrate, self.yawrate_filter))
|
|
# 恢复模式覆盖（优先级更高）
|
if self.in_recovery:
|
# 恢复模式：朝向投影点
|
path_heading = atan2(proj_y - y, proj_x - x)
|
xte_normalized = max(-1.0, min(1.0, xte / self.max_xte))
|
xte_correction = -self.k_xte * xte_normalized * self.xte_gain_scale
|
desired_heading = wrap_angle(path_heading + xte_correction)
|
heading_err = wrap_angle(desired_heading - self.heading)
|
yawrate_cmd = self.k_heading * heading_err
|
target_speed = self.recovery_speed
|
# 基于距离终点的距离调整速度
|
in_slow_zone = final_dist < self.slow_zone
|
in_final_approach = final_dist < self.final_approach_dist
|
|
# 错误恢复逻辑
|
if abs(xte) > self.large_err_threshold * 1.5 or abs(heading_err) > pi/2.5:
|
self.consecutive_large_err += 1
|
if self.consecutive_large_err > self.max_large_err_count:
|
# 重置到安全状态
|
self.consecutive_large_err = 0
|
self.heading_err_sum = 0
|
self.xte_sum = 0
|
else:
|
self.consecutive_large_err = 0
|
|
# 接近终点时调整速度
|
if not self.in_recovery:
|
if in_slow_zone:
|
dist_scale = max(0.3, min(1.0, final_dist / self.slow_zone))
|
if in_final_approach:
|
dist_scale *= max(0.4, final_dist / self.final_approach_dist)
|
target_speed *= dist_scale
|
|
# 在终点附近增加精度
|
if in_final_approach:
|
target_speed *= 0.6
|
if abs(xte) > 0.15 or abs(heading_err) > pi/10:
|
target_speed *= 0.7
|
|
# 强制最小速度（避免完全停滞）
|
if self.stage == self.STAGE_FOLLOW_PATH and not self.in_recovery:
|
dynamic_min = self.min_follow_speed if in_slow_zone else self.min_follow_speed * 1.5
|
if abs(heading_err) < 3.0 and target_speed < dynamic_min:
|
target_x, target_y = lx, ly
|
target_dist = hypot(target_x - x, target_y - y)
|
self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
|
f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{self.current_target_idx},'
|
f'{xte:.3f},{heading_err:.3f},0,0,'
|
f'{dynamic_min:.3f},{self.stage},enforce_min_speed\n')
|
self.log_file.flush()
|
target_speed = dynamic_min
|
|
# 软启动：限制加速度
|
accel = self.accel * (0.5 if in_slow_zone else 1.0)
|
speed_change = target_speed - self.current_speed
|
max_change = accel * dt
|
self.current_speed += max(-max_change, min(max_change, speed_change))
|
|
# 倒车控制
|
if self.stage == self.STAGE_GOTO_START:
|
if abs(heading_err) > 2.9:
|
self.current_speed = -self.max_reverse_mps * 0.5
|
else:
|
if abs(heading_err) > 2.9 and not self.in_recovery:
|
self.current_speed = -self.max_reverse_mps * 0.4
|
if self.current_speed < 0 and abs(heading_err) < 2.5:
|
self.current_speed = 0.0
|
|
# 终点检测（简化，只基于距离和点索引）
|
if final_dist < self.goal_tolerance:
|
self.finished = True
|
return {'forward': 0, 'turn': 0, 'info': {'status': 'finished'}}
|
|
# 扩展完成条件：接近终点且误差小
|
if (final_dist < self.goal_tolerance * 1.5 and
|
self.current_target_idx >= self.n - 2 and
|
abs(heading_err) < 0.3 and abs(xte) < 0.2):
|
self.finished = True
|
return {'forward': 0, 'turn': 0, 'info': {'status': 'finished'}}
|
|
# map to signals [-100,100]
|
# 记录映射前的速度信息以便诊断
|
log_t = timestamp if timestamp is not None else time.time()
|
pre_map_speed = self.current_speed
|
# 诊断日志（简化）
|
target_x, target_y = lx, ly
|
target_dist = hypot(target_x - x, target_y - y)
|
pre_map_forward = int(round((pre_map_speed / self.max_forward_mps) * 100)) if self.max_forward_mps != 0 else 0
|
pre_map_forward = max(-100, min(100, pre_map_forward))
|
pre_map_turn = int(round((yawrate_cmd / self.max_yawrate) * 100)) if self.max_yawrate != 0 else 0
|
pre_map_turn = max(-100, min(100, pre_map_turn))
|
self.log_file.write(f'{log_t:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
|
f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{self.current_target_idx},'
|
f'{xte:.3f},{heading_err:.3f},{pre_map_forward},{pre_map_turn},{pre_map_speed:.3f},'
|
f'{self.stage},pre_map\n')
|
self.log_file.flush()
|
|
forward_signal = int(round((self.current_speed / self.max_forward_mps) * 100)) if self.max_forward_mps != 0 else 0
|
forward_signal = max(-100, min(100, forward_signal))
|
|
# 强制最小前进信号（防止完全停止）
|
if forward_signal == 0 and not self.finished and abs(heading_err) < 3.0:
|
forced_signal = int(round((self.min_follow_speed / self.max_forward_mps) * 100)) if self.max_forward_mps != 0 else 0
|
if forced_signal != 0:
|
target_x, target_y = lx, ly
|
target_dist = hypot(target_x - x, target_y - y)
|
self.log_file.write(f'{log_t:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
|
f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{self.current_target_idx},'
|
f'{xte:.3f},{heading_err:.3f},{forced_signal},{int(round((yawrate_cmd / self.max_yawrate) * 100))},'
|
f'{self.min_follow_speed:.3f},{self.stage},force_min_forward\n')
|
self.log_file.flush()
|
forward_signal = forced_signal
|
self.current_speed = self.min_follow_speed
|
|
turn_signal = int(round((yawrate_cmd / self.max_yawrate) * 100)) if self.max_yawrate != 0 else 0
|
turn_signal = max(-100, min(100, turn_signal))
|
|
status = 'finished' if self.finished else 'running'
|
info = {
|
'status': status,
|
'pos': (x, y),
|
'heading': self.heading,
|
'desired_heading': desired_heading,
|
'heading_err': heading_err,
|
'current_speed': self.current_speed,
|
'yawrate_cmd': yawrate_cmd,
|
'target_xy': (lx, ly),
|
'path_curvature': path_curvature,
|
'final_dist': final_dist,
|
}
|
|
# 简化的日志记录
|
t = log_t
|
target_x, target_y = lx, ly
|
target_dist = hypot(target_x - x, target_y - y)
|
self.log_file.write(f'{t:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
|
f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{self.current_target_idx},'
|
f'{xte:.3f},{heading_err:.3f},{forward_signal},{turn_signal},{self.current_speed:.3f},'
|
f'{self.stage},{status}\n')
|
self.log_file.flush()
|
|
return {'forward': forward_signal, 'turn': turn_signal, 'info': info}
|
|
|
if __name__ == '__main__':
|
# quick manual test
|
import json
|
path = [[0,0],[5,0],[10,0]]
|
c = MowerController(path)
|
c.update_gps((0, -1.0), 0.0)
|
for i in range(20):
|
out = c.compute_control()
|
print(i, out)
|
# fake forward motion
|
f = out['forward'] / 100.0 * c.max_forward_mps
|
hd = c.heading + (out['turn'] / 100.0) * c.max_yawrate * 0.1
|
px = c.pos[0] + f * cos(hd) * 0.1
|
py = c.pos[1] + f * sin(hd) * 0.1
|
c.update_gps((px,py), hd)
|
