Python基于梯度下降的路径规划算法：从原理到实践

def cost_function(path, obstacles):
    # 路径长度代价（欧氏距离累加）
    length_cost = sum(np.hypot(path[i+1][0]-path[i][0], 
                              path[i+1][1]-path[i][1]) 
                     for i in range(len(path)-1))
    
    # 障碍物避障代价（指数衰减模型）
    obstacle_cost = 0
    for obs in obstacles:
        min_dist = min(np.hypot(p[0]-obs[0], p[1]-obs[1]) for p in path)
        obstacle_cost += np.exp(-min_dist/0.5)  # 0.5为衰减系数
    
    return length_cost + 0.3*obstacle_cost  # 0.3为权重系数

def compute_gradient(path, obstacles):
    gradients = []
    for i in range(1, len(path)-1):  # 跳过起点和终点
        # 路径长度梯度（前后点连线的垂直方向）
        prev_vec = np.array(path[i-1]) - np.array(path[i])
        next_vec = np.array(path[i+1]) - np.array(path[i])
        length_grad = prev_vec/np.linalg.norm(prev_vec) + next_vec/np.linalg.norm(next_vec)
        
        # 障碍物梯度（指向最近障碍物的反方向）
        obs_grad = np.zeros(2)
        for obs in obstacles:
            vec = np.array(path[i]) - np.array(obs)
            dist = np.linalg.norm(vec)
            if dist < 2.0:  # 只考虑2米范围内的障碍物
                obs_grad += vec/(dist+1e-6)  # 避免除零
        
        gradients.append(-0.5*length_grad - 0.1*obs_grad)  # 合并梯度
    return gradients

import numpy as np
import matplotlib.pyplot as plt

def gradient_descent_planner(start, goal, obstacles, max_iter=100, lr=0.01):
    # 初始化路径（直线连接+中间点）
    path = [start]
    for t in np.linspace(0, 1, 8):  # 8个中间点
        path.append([start[0]+t*(goal[0]-start[0]), 
                     start[1]+t*(goal[1]-start[1])])
    path.append(goal)
    
    for _ in range(max_iter):
        gradients = compute_gradient(path, obstacles)
        
        # 更新中间点（保持起点终点不变）
        for i in range(1, len(path)-1):
            path[i] = path[i] + lr * gradients[i-1]
        
        # 早停机制：当路径变化小于阈值时终止
        if np.linalg.norm(gradients[-1]) < 1e-3:
            break
    
    return path

# 参数设置
start = [0, 0]
goal = [5, 5]
obstacles = [[2, 2], [3, 3], [4, 2.5]]

# 运行算法
optimized_path = gradient_descent_planner(start, goal, obstacles)

# 可视化结果
plt.figure(figsize=(8,6))
plt.plot([p[0] for p in optimized_path], 
         [p[1] for p in optimized_path], 'b-o', label='Optimized Path')
for obs in obstacles:
    plt.plot(obs[0], obs[1], 'ro', markersize=10, label='Obstacle')
plt.plot(start[0], start[1], 'go', markersize=10, label='Start')
plt.plot(goal[0], goal[1], 'mo', markersize=10, label='Goal')
plt.legend()
plt.grid(True)
plt.show()

#!/usr/bin/env python
import rospy
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
import numpy as np

class GradientPlanner:
    def __init__(self):
        self.path_pub = rospy.Publisher('/optimized_path', Path, queue_size=10)
        self.obstacles = []  # 从传感器数据更新
    
    def update_obstacles(self, msg):
        self.obstacles = [(p.pose.position.x, p.pose.position.y) 
                         for p in msg.poses]
    
    def plan(self, start, goal):
        path = gradient_descent_planner(start, goal, self.obstacles)
        ros_path = Path()
        for p in path:
            pose = PoseStamped()
            pose.pose.position.x = p[0]
            pose.pose.position.y = p[1]
            ros_path.poses.append(pose)
        self.path_pub.publish(ros_path)

def simulated_annealing(current_cost, new_cost, temperature):
    if new_cost < current_cost or np.random.rand() < np.exp((current_cost-new_cost)/temperature):
        return True
    return False

def lr_scheduler(base_lr, epoch, decay_rate=0.95):
    return base_lr * (decay_rate ** (epoch//10))

def compute_3d_gradient(path, obstacles):
    # 路径长度梯度计算扩展z轴
    prev_vec = np.array(path[i-1]) - np.array(path[i])
    next_vec = np.array(path[i+1]) - np.array(path[i])
    length_grad = np.concatenate([
        prev_vec[:2]/np.linalg.norm(prev_vec[:2]),
        [prev_vec[2]/np.abs(prev_vec[2]) if np.abs(prev_vec[2])>1e-6 else 0]
    ]) + ...  # 类似处理next_vec
    return gradients