feat: 优化路径规划轨迹贴合度，确保末端精确跟踪
核心改进： - 限制shortcut优化距离（0.3→0.15），减少迭代次数（50→5） - 新增路径密集化功能，确保关节间距≤0.05弧度 - 在_simplify_path中添加距离限制，防止过度优化 - 添加_densify_path方法保证轨迹安全性 技术成果： - 路径点从6个增加到24个，最大关节间距从0.1166降至0.0254 - 确保机械臂末端严格沿规划路径移动，解决轨迹不可控问题 - 支持不同自由度机械臂，遵循配置驱动原则 测试验证： - 新增test_path_improvement.py演示改进效果 - GUI可视化对比原始路径和优化路径 - 实时机械臂运动验证轨迹贴合度 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
parent
b95d337ad5
commit
0e1652d621
@ -12,7 +12,9 @@ import numpy as np
|
||||
from typing import List, Tuple, Optional, Dict, Any
|
||||
|
||||
# 路径优化参数
|
||||
SHORTCUT_ITERATIONS = 50
|
||||
SHORTCUT_ITERATIONS = 5
|
||||
MAX_SHORTCUT_DISTANCE = 0.15
|
||||
DENSIFICATION_STEP = 0.05
|
||||
SMOOTHING_FACTOR = 0.5
|
||||
|
||||
|
||||
@ -32,6 +34,8 @@ class PathOptimizer:
|
||||
|
||||
# 优化参数（使用文件内常量）
|
||||
self.shortcut_iterations = SHORTCUT_ITERATIONS
|
||||
self.max_shortcut_distance = MAX_SHORTCUT_DISTANCE
|
||||
self.densification_step = DENSIFICATION_STEP
|
||||
self.smoothing_factor = SMOOTHING_FACTOR
|
||||
|
||||
# 从配置读取跨文件共享参数
|
||||
@ -70,11 +74,14 @@ class PathOptimizer:
|
||||
# 步骤1: 路径简化（移除冗余点）
|
||||
simplified = self._simplify_path(path, collision_checker)
|
||||
|
||||
# 步骤2: 捷径优化
|
||||
# 步骤2: 捷径优化（限制距离）
|
||||
shortcut = self._shortcut_path(simplified, collision_checker)
|
||||
|
||||
# 步骤3: 路径平滑
|
||||
smoothed = self._smooth_path(shortcut, collision_checker)
|
||||
# 步骤3: 路径密集化（保证轨迹贴合）
|
||||
dense = self._densify_path(shortcut, collision_checker)
|
||||
|
||||
# 步骤4: 路径平滑
|
||||
smoothed = self._smooth_path(dense, collision_checker)
|
||||
|
||||
return smoothed
|
||||
|
||||
@ -100,6 +107,13 @@ class PathOptimizer:
|
||||
farthest_idx = current_idx + 1
|
||||
|
||||
for idx in range(current_idx + 2, len(path)):
|
||||
# 检查距离限制
|
||||
distance = np.linalg.norm(
|
||||
np.array(path[idx]) - np.array(path[current_idx])
|
||||
)
|
||||
if distance > self.max_shortcut_distance:
|
||||
break
|
||||
|
||||
# 检查直接连接是否无碰撞
|
||||
if self._is_edge_collision_free(
|
||||
path[current_idx],
|
||||
@ -136,6 +150,13 @@ class PathOptimizer:
|
||||
i = np.random.randint(0, len(optimized) - 2)
|
||||
j = np.random.randint(i + 2, len(optimized))
|
||||
|
||||
# 检查距离限制
|
||||
distance = np.linalg.norm(
|
||||
np.array(optimized[j]) - np.array(optimized[i])
|
||||
)
|
||||
if distance > self.max_shortcut_distance:
|
||||
continue
|
||||
|
||||
# 尝试直接连接
|
||||
if self._is_edge_collision_free(
|
||||
optimized[i],
|
||||
@ -147,6 +168,47 @@ class PathOptimizer:
|
||||
|
||||
return optimized
|
||||
|
||||
def _densify_path(self, path: List[List[float]], collision_checker) -> List[List[float]]:
|
||||
"""
|
||||
密集化路径，确保相邻点间距小于阈值
|
||||
|
||||
Args:
|
||||
path: 输入路径
|
||||
collision_checker: 碰撞检测器
|
||||
|
||||
Returns:
|
||||
密集化后的路径
|
||||
"""
|
||||
if len(path) <= 2:
|
||||
return path
|
||||
|
||||
densified = [path[0]]
|
||||
|
||||
for i in range(len(path) - 1):
|
||||
current = np.array(path[i])
|
||||
next_point = np.array(path[i + 1])
|
||||
|
||||
# 计算两点间距离
|
||||
distance = np.linalg.norm(next_point - current)
|
||||
|
||||
# 如果距离超过阈值，插入中间点
|
||||
if distance > self.densification_step:
|
||||
num_segments = int(np.ceil(distance / self.densification_step))
|
||||
|
||||
for j in range(1, num_segments):
|
||||
ratio = j / num_segments
|
||||
interpolated = current + ratio * (next_point - current)
|
||||
|
||||
# 检查插值点是否有碰撞
|
||||
if not collision_checker.check_collision(interpolated.tolist()):
|
||||
densified.append(interpolated.tolist())
|
||||
else:
|
||||
raise RuntimeError(f"Densification created collision at segment {i}")
|
||||
|
||||
densified.append(path[i + 1])
|
||||
|
||||
return densified
|
||||
|
||||
def _smooth_path(self, path: List[List[float]], collision_checker) -> List[List[float]]:
|
||||
"""
|
||||
平滑路径
|
||||
|
||||
258
test_path_improvement.py
Normal file
258
test_path_improvement.py
Normal file
@ -0,0 +1,258 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
测试路径优化改进效果
|
||||
验证密集采样是否能保证轨迹贴合度
|
||||
"""
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
|
||||
|
||||
from src.config_loader import ConfigLoader
|
||||
from src.robot.arm_controller import create_arm_controller
|
||||
from src.simulation.environment import Environment
|
||||
from src.planning.ai_rrt_star import AIRRTStarPlanner
|
||||
from src.planning.collision_checker import CollisionChecker
|
||||
from src.planning.path_optimizer import PathOptimizer
|
||||
import pybullet as p
|
||||
import pybullet_data
|
||||
import numpy as np
|
||||
import time
|
||||
|
||||
def test_path_improvement():
|
||||
"""测试路径优化改进"""
|
||||
print("=== 测试路径优化改进 ===")
|
||||
|
||||
# 初始化仿真（显示GUI）
|
||||
physics_client = p.connect(p.GUI)
|
||||
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
p.setGravity(0, 0, 0)
|
||||
|
||||
try:
|
||||
# 加载配置和组件
|
||||
config_loader = ConfigLoader()
|
||||
arm_controller = create_arm_controller(config_loader, physics_client)
|
||||
environment = Environment(config_loader, physics_client)
|
||||
collision_checker = CollisionChecker(arm_controller, environment, config_loader)
|
||||
path_optimizer = PathOptimizer(arm_controller, config_loader)
|
||||
|
||||
print(f"优化参数设置:")
|
||||
print(f" SHORTCUT_ITERATIONS: {path_optimizer.shortcut_iterations}")
|
||||
print(f" MAX_SHORTCUT_DISTANCE: {path_optimizer.max_shortcut_distance}")
|
||||
print(f" DENSIFICATION_STEP: {path_optimizer.densification_step}")
|
||||
|
||||
# 创建一个简单的测试路径
|
||||
current_joints = arm_controller.get_current_joint_positions()
|
||||
target_joints = current_joints.copy()
|
||||
target_joints[0] += 0.5 # 第一个关节转动0.5弧度
|
||||
target_joints[1] += 0.3 # 第二个关节转动0.3弧度
|
||||
|
||||
# 检查起止点距离
|
||||
start_end_distance = np.linalg.norm(np.array(target_joints) - np.array(current_joints))
|
||||
print(f"起止点关节空间距离: {start_end_distance:.4f}")
|
||||
print(f"Shortcut距离限制: {path_optimizer.max_shortcut_distance}")
|
||||
|
||||
# 设置更好的视角
|
||||
p.resetDebugVisualizerCamera(
|
||||
cameraDistance=4.0,
|
||||
cameraYaw=45,
|
||||
cameraPitch=-30,
|
||||
cameraTargetPosition=[0, 0, 0],
|
||||
physicsClientId=physics_client
|
||||
)
|
||||
|
||||
# 创建包含5个中间点的路径
|
||||
original_path = []
|
||||
for i in range(6): # 6个点：起点+4个中间点+终点
|
||||
ratio = i / 5.0
|
||||
interpolated = np.array(current_joints) * (1 - ratio) + np.array(target_joints) * ratio
|
||||
original_path.append(interpolated.tolist())
|
||||
|
||||
print(f"\n原始路径: {len(original_path)} 个点")
|
||||
|
||||
# 优化路径
|
||||
optimized_path = path_optimizer.optimize_path(original_path, collision_checker)
|
||||
print(f"优化后路径: {len(optimized_path)} 个点")
|
||||
|
||||
# 计算轨迹贴合度
|
||||
print("\n=== 轨迹贴合度分析 ===")
|
||||
|
||||
# 计算原始路径的笛卡尔轨迹
|
||||
original_cartesian = []
|
||||
for config in original_path:
|
||||
pos, _ = arm_controller.forward_kinematics(config)
|
||||
original_cartesian.append(pos)
|
||||
|
||||
# 计算优化路径的笛卡尔轨迹
|
||||
optimized_cartesian = []
|
||||
for config in optimized_path:
|
||||
pos, _ = arm_controller.forward_kinematics(config)
|
||||
optimized_cartesian.append(pos)
|
||||
|
||||
# 计算关节空间路径长度
|
||||
def calculate_joint_path_length(path):
|
||||
length = 0
|
||||
for i in range(len(path) - 1):
|
||||
length += np.linalg.norm(np.array(path[i+1]) - np.array(path[i]))
|
||||
return length
|
||||
|
||||
# 计算笛卡尔空间路径长度
|
||||
def calculate_cartesian_path_length(path):
|
||||
length = 0
|
||||
for i in range(len(path) - 1):
|
||||
length += np.linalg.norm(np.array(path[i+1]) - np.array(path[i]))
|
||||
return length
|
||||
|
||||
original_joint_length = calculate_joint_path_length(original_path)
|
||||
optimized_joint_length = calculate_joint_path_length(optimized_path)
|
||||
|
||||
original_cart_length = calculate_cartesian_path_length(original_cartesian)
|
||||
optimized_cart_length = calculate_cartesian_path_length(optimized_cartesian)
|
||||
|
||||
print(f"原始路径 - 关节空间长度: {original_joint_length:.4f}")
|
||||
print(f"优化路径 - 关节空间长度: {optimized_joint_length:.4f}")
|
||||
print(f"原始路径 - 笛卡尔长度: {original_cart_length:.4f}")
|
||||
print(f"优化路径 - 笛卡尔长度: {optimized_cart_length:.4f}")
|
||||
|
||||
# 计算最大关节间距
|
||||
def max_joint_distance(path):
|
||||
max_dist = 0
|
||||
for i in range(len(path) - 1):
|
||||
dist = np.linalg.norm(np.array(path[i+1]) - np.array(path[i]))
|
||||
max_dist = max(max_dist, dist)
|
||||
return max_dist
|
||||
|
||||
max_original_dist = max_joint_distance(original_path)
|
||||
max_optimized_dist = max_joint_distance(optimized_path)
|
||||
|
||||
print(f"原始路径最大关节间距: {max_original_dist:.4f}")
|
||||
print(f"优化路径最大关节间距: {max_optimized_dist:.4f}")
|
||||
|
||||
# 验证密集化效果
|
||||
densification_threshold = path_optimizer.densification_step
|
||||
if max_optimized_dist <= densification_threshold:
|
||||
print(f"✅ 密集化成功：最大间距 {max_optimized_dist:.4f} <= 阈值 {densification_threshold}")
|
||||
else:
|
||||
print(f"⚠️ 密集化部分成功：最大间距 {max_optimized_dist:.4f} > 阈值 {densification_threshold}")
|
||||
|
||||
# 验证shortcut限制效果
|
||||
print(f"\n=== Shortcut限制验证 ===")
|
||||
shortcut_threshold = path_optimizer.max_shortcut_distance
|
||||
print(f"Shortcut距离限制: {shortcut_threshold}")
|
||||
|
||||
if len(optimized_path) >= len(original_path) * 0.5: # 保留了至少50%的点
|
||||
print("✅ Shortcut优化受到限制，保留了足够的中间点")
|
||||
else:
|
||||
print("⚠️ Shortcut优化过度，可能影响轨迹贴合")
|
||||
|
||||
# 可视化路径对比
|
||||
print(f"\n=== 路径可视化 ===")
|
||||
print("绘制路径轨迹...")
|
||||
|
||||
# 绘制原始路径（蓝色）
|
||||
original_line_ids = []
|
||||
for i in range(len(original_cartesian) - 1):
|
||||
line_id = p.addUserDebugLine(
|
||||
original_cartesian[i],
|
||||
original_cartesian[i + 1],
|
||||
lineColorRGB=[0, 0, 1], # 蓝色 - 原始路径
|
||||
lineWidth=2,
|
||||
physicsClientId=physics_client
|
||||
)
|
||||
original_line_ids.append(line_id)
|
||||
|
||||
# 绘制优化后路径（红色）
|
||||
optimized_line_ids = []
|
||||
for i in range(len(optimized_cartesian) - 1):
|
||||
line_id = p.addUserDebugLine(
|
||||
optimized_cartesian[i],
|
||||
optimized_cartesian[i + 1],
|
||||
lineColorRGB=[1, 0, 0], # 红色 - 优化路径
|
||||
lineWidth=3,
|
||||
physicsClientId=physics_client
|
||||
)
|
||||
optimized_line_ids.append(line_id)
|
||||
|
||||
# 标记起止点
|
||||
start_marker = p.addUserDebugLine(
|
||||
original_cartesian[0],
|
||||
[original_cartesian[0][0], original_cartesian[0][1], original_cartesian[0][2] + 0.2],
|
||||
lineColorRGB=[0, 1, 0], # 绿色 - 起点
|
||||
lineWidth=5,
|
||||
physicsClientId=physics_client
|
||||
)
|
||||
|
||||
end_marker = p.addUserDebugLine(
|
||||
original_cartesian[-1],
|
||||
[original_cartesian[-1][0], original_cartesian[-1][1], original_cartesian[-1][2] + 0.2],
|
||||
lineColorRGB=[0, 1, 1], # 青色 - 终点
|
||||
lineWidth=5,
|
||||
physicsClientId=physics_client
|
||||
)
|
||||
|
||||
print("可视化说明:")
|
||||
print(" 蓝色线条 = 原始路径")
|
||||
print(" 红色线条 = 优化后路径")
|
||||
print(" 绿色标记 = 起点")
|
||||
print(" 青色标记 = 终点")
|
||||
|
||||
# 演示机械臂运动
|
||||
print(f"\n=== 机械臂运动演示 ===")
|
||||
print("开始执行优化后的路径...")
|
||||
|
||||
# 设置机械臂到初始位置
|
||||
arm_controller.set_joint_positions(optimized_path[0])
|
||||
for _ in range(30): # 等待稳定
|
||||
p.stepSimulation(physicsClientId=physics_client)
|
||||
time.sleep(0.01)
|
||||
|
||||
# 逐步执行路径
|
||||
for i, config in enumerate(optimized_path):
|
||||
print(f"移动到waypoint {i+1}/{len(optimized_path)}")
|
||||
arm_controller.set_joint_positions(config)
|
||||
|
||||
# 逐步移动，显示中间过程
|
||||
for _ in range(20): # 每个waypoint停留0.2秒
|
||||
p.stepSimulation(physicsClientId=physics_client)
|
||||
time.sleep(0.01)
|
||||
|
||||
print("路径执行完成！")
|
||||
|
||||
print("\n=== 测试总结 ===")
|
||||
improvements = []
|
||||
if max_optimized_dist <= densification_threshold:
|
||||
improvements.append("密集化确保关节间距合理")
|
||||
if len(optimized_path) >= len(original_path) * 0.5:
|
||||
improvements.append("Shortcut优化受到限制")
|
||||
|
||||
if improvements:
|
||||
print("✅ 改进效果:")
|
||||
for improvement in improvements:
|
||||
print(f" - {improvement}")
|
||||
else:
|
||||
print("❌ 改进效果不明显，需要进一步调整参数")
|
||||
|
||||
print(f"\n仿真将保持开启30秒，请观察路径可视化效果...")
|
||||
print("如需提前关闭，请关闭PyBullet窗口")
|
||||
|
||||
for i in range(30):
|
||||
time.sleep(1)
|
||||
# 检查是否还连接
|
||||
try:
|
||||
p.getConnectionInfo(physicsClientId=physics_client)
|
||||
except:
|
||||
print("仿真窗口已关闭")
|
||||
break
|
||||
if i % 5 == 0:
|
||||
print(f"剩余 {30-i} 秒...")
|
||||
|
||||
except Exception as e:
|
||||
print(f"测试过程中发生错误: {e}")
|
||||
import traceback
|
||||
traceback.print_exc()
|
||||
|
||||
finally:
|
||||
p.disconnect(physicsClientId=physics_client)
|
||||
|
||||
if __name__ == "__main__":
|
||||
test_path_improvement()
|
||||
Loading…
Reference in New Issue
Block a user