"""
|
Offline simulation and path visualization
|
- Run complete trajectory with approach path
|
- Plot final path and control signals
|
- Save detailed logs for analysis
|
"""
|
import json
|
import time
|
from math import cos, sin, pi, hypot, atan2
|
import numpy as np
|
import matplotlib
|
# use non-interactive backend to avoid blocking GUI (run headless)
|
matplotlib.use('Agg')
|
import matplotlib.pyplot as plt
|
from mower_controller import MowerController, wrap_angle
|
from path_planner import plan_approach_path, compute_path_heading, combine_paths
|
|
|
def simulate_full_path(work_path, mower_start_pos, mower_start_heading, params=None,
|
approach_method='smooth'):
|
"""
|
Run complete simulation with approach path and return state history.
|
|
Args:
|
work_path: Planned work path [(x, y), ...]
|
mower_start_pos: (x, y) mower starting position
|
mower_start_heading: mower starting heading in radians
|
params: Controller parameters dict
|
approach_method: 'line' or 'smooth'
|
"""
|
# Generate approach path from mower start to work path start
|
work_path_heading = compute_path_heading(work_path, 0)
|
approach_path = plan_approach_path(mower_start_pos, mower_start_heading,
|
work_path[0], work_path_heading,
|
method=approach_method)
|
|
# Combine paths
|
combined_path, approach_end_idx = combine_paths(approach_path, work_path)
|
|
print(f"Generated approach path: {len(approach_path)} points")
|
print(f"Work path: {len(work_path)} points")
|
print(f"Combined path: {len(combined_path)} points (approach ends at idx {approach_end_idx})")
|
|
# Initialize controller with combined path
|
ctrl = MowerController(combined_path, params=params)
|
|
# Initial state
|
x, y = mower_start_pos
|
heading = mower_start_heading
|
t = 0.0
|
dt = 1.0/74.0 # control period
|
|
# History for plotting
|
history = {
|
't': [0.0],
|
'x': [x],
|
'y': [y],
|
'heading': [heading],
|
'forward': [0],
|
'turn': [0],
|
'approach_end_idx': approach_end_idx, # Store for visualization
|
'approach_path': approach_path,
|
'work_path': work_path,
|
}
|
|
finished = False
|
while not finished and t < 300.0: # timeout after 300s
|
# Update controller
|
ctrl.update_gps((x, y), heading)
|
ctrl.update_imu((0,0,0), (0,0,0))
|
|
# Get control signals
|
out = ctrl.compute_control(t)
|
f_sig = out['forward']
|
t_sig = out['turn']
|
|
# Convert to physical quantities
|
f_mps = (f_sig / 100.0) * ctrl.max_forward_mps
|
yawrate = (t_sig / 100.0) * ctrl.max_yawrate
|
|
# Update state
|
heading = wrap_angle(heading + yawrate * dt)
|
x = x + f_mps * cos(heading) * dt
|
y = y + f_mps * sin(heading) * dt
|
t += dt
|
|
# Record
|
history['t'].append(t)
|
history['x'].append(x)
|
history['y'].append(y)
|
history['heading'].append(heading)
|
history['forward'].append(f_sig)
|
history['turn'].append(t_sig)
|
|
# Check completion
|
if out['info'].get('status') == 'finished':
|
finished = True
|
print(f'Reached goal in {t:.1f}s')
|
|
if not finished:
|
print('Timeout!')
|
|
return history
|
|
|
def plot_results(history):
|
"""Plot path following results with approach and work paths."""
|
approach_path = history['approach_path']
|
work_path = history['work_path']
|
approach_end_idx = history['approach_end_idx']
|
|
fig = plt.figure(figsize=(15, 10))
|
gs = fig.add_gridspec(3, 2)
|
|
# Path plot (larger, spans two rows)
|
ax_path = fig.add_subplot(gs[0:2, 0])
|
|
# Plot approach path (dashed orange)
|
apx = [p[0] for p in approach_path]
|
apy = [p[1] for p in approach_path]
|
ax_path.plot(apx, apy, '--', color='orange', lw=2, label='Approach Path', alpha=0.7)
|
ax_path.scatter(apx, apy, c='orange', s=15, alpha=0.5)
|
|
# Plot work path (solid black)
|
wpx = [p[0] for p in work_path]
|
wpy = [p[1] for p in work_path]
|
ax_path.plot(wpx, wpy, '-k', lw=2, label='Work Path')
|
ax_path.scatter(wpx, wpy, c='k', s=20)
|
|
# Plot actual trajectory
|
ax_path.plot(history['x'], history['y'], '-r', lw=1.5, label='Actual', alpha=0.8)
|
|
# Add markers
|
ax_path.plot(history['x'][0], history['y'][0], 'go', markersize=12,
|
label='Mower Start', zorder=10)
|
ax_path.plot(work_path[0][0], work_path[0][1], 'bs', markersize=10,
|
label='Work Path Start', zorder=10)
|
ax_path.plot(history['x'][-1], history['y'][-1], 'ro', markersize=10,
|
label='End', zorder=10)
|
|
ax_path.set_aspect('equal')
|
ax_path.grid(True, alpha=0.3)
|
ax_path.set_xlabel('X (m)')
|
ax_path.set_ylabel('Y (m)')
|
ax_path.set_title('Path Following Results (Approach + Work)')
|
ax_path.legend(loc='best')
|
|
# XY position vs time
|
ax_xy = fig.add_subplot(gs[0:2, 1])
|
ax_xy.plot(history['t'], history['x'], '-b', label='X')
|
ax_xy.plot(history['t'], history['y'], '-r', label='Y')
|
ax_xy.set_xlabel('Time (s)')
|
ax_xy.set_ylabel('Position (m)')
|
ax_xy.grid(True)
|
ax_xy.legend()
|
|
# Control signals
|
ax_ctrl = fig.add_subplot(gs[2, :])
|
ax_ctrl.plot(history['t'], history['forward'], '-b', label='Forward')
|
ax_ctrl.plot(history['t'], history['turn'], '-r', label='Turn')
|
ax_ctrl.set_xlabel('Time (s)')
|
ax_ctrl.set_ylabel('Control Signal')
|
ax_ctrl.grid(True)
|
ax_ctrl.legend()
|
|
plt.tight_layout()
|
plt.savefig('simulation_results.png', dpi=300, bbox_inches='tight')
|
plt.show()
|
|
|
def main():
|
# Load planned work path
|
try:
|
with open('example_path.json', 'r') as f:
|
work_path = json.load(f)
|
except Exception as e:
|
print(f'Failed to load path: {e}')
|
return
|
|
# Define mower starting position (offset from work path start)
|
mower_start_pos = (work_path[0][0] + 3.5, work_path[0][1] + 1.0)
|
mower_start_heading = pi / 4 # 45 degrees
|
|
print(f"Mower starts at: {mower_start_pos}, heading: {mower_start_heading:.2f} rad")
|
print(f"Work path starts at: {work_path[0]}")
|
|
# Run simulation with stable high-precision parameters
|
params = {
|
# === 核心控制增益（修正后） ===
|
'k_heading': 2.0, # 航向增益
|
'k_xte': 0.8, # 横向误差增益：2.0 → 0.8 (大幅降低以防止过度修正)
|
'k_heading_d': 0.1, # 航向微分增益
|
'k_heading_i': 0.015, # 航向积分增益
|
'k_xte_i': 0.02, # 横向积分增益
|
|
# === 速度控制 ===
|
'base_speed': 0.8, # 基础速度：0.6 → 0.8
|
'max_forward_mps': 1.2, # 最大速度：0.95 → 1.2
|
'min_follow_speed': 0.40, # 最小速度：0.30 → 0.40
|
'max_reverse_mps': 0.25,
|
'max_yawrate': pi/2.5, # 转向速率：pi/3 → pi/2.5 (更快转向)
|
|
# === 前视距离 ===
|
'lookahead_min': 0.35, # 最小前视
|
'lookahead_max': 1.2, # 最大前视
|
'lookahead_time': 2.0, # 前视时间系数
|
|
# === 误差容忍度 ===
|
'max_xte':1.5, # 最大横向误差：0.8 → 1.0 (增大以减少归一化放大)
|
'goal_tolerance': 0.1, # 目标容忍
|
'large_err_threshold': 0.5, # 大误差阈值
|
|
# === 滤波参数 ===
|
'yawrate_alpha': 0.85, # 转向滤波
|
|
# === 速度缩放增益 ===
|
'curv_gain_scale': 2.0, # 曲率缩放
|
'heading_err_gain_scale': 1.5, # 航向误差缩放
|
'xte_gain_scale': 1.0, # 横向误差缩放：1.2 → 1.0 (移除额外放大)
|
|
# === 终点减速区域 ===
|
'slow_zone': 1.5,
|
'final_approach_dist': 0.8,
|
|
# === 起点阶段 ===
|
'start_min_speed': 0.28,
|
'start_slow_speed': 0.38,
|
|
# === 恢复模式 ===
|
'recovery_dist': 2.5,
|
'recovery_exit_dist': 0.6,
|
'recovery_speed': 0.6,
|
}
|
|
history = simulate_full_path(work_path, mower_start_pos, mower_start_heading,
|
params, approach_method='smooth')
|
plot_results(history)
|
|
|
if __name__ == '__main__':
|
main()
|
