Python車輛導航專案實現指南

全面的車輛導航算法設計與實踐

主要收穫

• 路徑規劃與動態更新：利用Reeds-Shepp算法生成並實時更新全局路徑。
• 局部導航曲線擬合：轉換局部路徑點並擬合成平滑的導航曲線。
• MPC控制與可視化：應用模型預測控制器(MPC)跟蹤導航曲線，並使用Matplotlib進行路徑可視化。

專案概述

本專案旨在實現一個基於Python的車輛導航系統，主要功能包括路徑規劃、動態路徑更新、局部導航曲線擬合、控制參數輸出，以及路徑的可視化展示。系統將結合Reeds-Shepp曲線路徑生成與模型預測控制器(MPC)來實現精確的導航控制。

技術架構與流程

1. 導入必要的函式庫

首先，需要導入Python中的相關函式庫以實現路徑規劃、控制和可視化功能。必須安裝以下函式庫：

pip install -U rsplan matplotlib do-mpc scipy

2. Reeds-Shepp路徑規劃

使用rsplan函式庫中的Reeds-Shepp算法生成全局路徑，該算法考慮了轉向半徑，能夠生成前進與倒車的軌跡。

from rsplan import ReedsShepp

# 初始化Reeds-Shepp路徑規劃器
rs_planner = ReedsShepp()

# 定義初始和目標姿態 (x, y, yaw)
start_pose = (0, 0, 0)  # 初始位置與航向
goal_pose = (10, 5, np.pi / 2)  # 目標位置與航向

# 生成全局路徑
path = rs_planner.path(start_pose, goal_pose, TURN_RADIUS=2.0, STEP_SIZE=0.1)
global_path_points = np.array([(p[0], p[1], p[2]) for p in path.path_points])

3. 動態路徑更新

隨著載具位置的變化，系統需要動態更新路徑。這包括重新計算路徑點並調整控制參數以適應新的位置。

def update_global_path(current_pose, goal_pose):
    # 重新生成路徑
    new_path = rs_planner.path(current_pose, goal_pose, TURN_RADIUS=2.0, STEP_SIZE=0.1)
    return np.array([(p[0], p[1], p[2]) for p in new_path.path_points])

4. 局部導航曲線擬合

為了實現精確的導航控制，需要將最近的若干路徑點轉換到車輛的局部坐標系，並擬合出平滑的導航曲線。

def transform_to_vehicle_frame(global_path, current_pose, N=5):
    x0, y0, theta0 = current_pose
    local_points = global_path[:N]
    x_local, y_local = [], []

    for x, y, yaw in local_points:
        dx = x - x0
        dy = y - y0
        x_transformed = dx * np.cos(-theta0) - dy * np.sin(-theta0)
        y_transformed = dx * np.sin(-theta0) + dy * np.cos(-theta0)
        x_local.append(x_transformed)
        y_local.append(y_transformed)

    return np.array(x_local), np.array(y_local)

def fit_local_curve(x_local, y_local):
    def poly2(x, a, b, c):
        return a * x**2 + b * x + c

    params, _ = curve_fit(poly2, x_local, y_local)
    return params

5. 模型預測控制器(MPC)設置

MPC是一種先進的控制方法，能夠根據當前狀態和未來預測來計算最優控制參數。本專案使用do-mpc函式庫來實現MPC控制器。

from do_mpc.model import Model
from do_mpc.controller import MPC

def setup_mpc():
    model = Model('continuous')
    x = model.set_variable('_x', 'x')
    y = model.set_variable('_x', 'y')
    theta = model.set_variable('_x', 'theta')
    v = model.set_variable('_u', 'v')
    omega = model.set_variable('_u', 'omega')

    # 定義動力學方程
    model.set_rhs('x', v * np.cos(theta))
    model.set_rhs('y', v * np.sin(theta))
    model.set_rhs('theta', omega)
    model.setup()

    mpc = MPC(model)
    mpc.set_param(n_horizon=10, t_step=0.1, state_discretization='discrete')
    mpc.setup()
    return mpc

mpc = setup_mpc()

6. 控制參數輸出與位置更新

根據MPC計算出的控制參數來調整載具的線速度和角速度，並更新當前的姿態。

def update_pose(current_pose, control):
    x, y, theta = current_pose
    v, omega = control
    x += v * np.cos(theta) * STEP_SIZE
    y += v * np.sin(theta) * STEP_SIZE
    theta += omega * STEP_SIZE
    return (x, y, theta)

7. 路徑可視化

使用Matplotlib函式庫對全局和局部路徑進行可視化，便於分析導航效果。

import matplotlib.pyplot as plt

def visualize_paths(global_path, local_x, local_y, params):
    plt.figure(figsize=(10, 6))
    # 全局路徑
    plt.plot(global_path[:, 0], global_path[:, 1], '--', color='grey', label='Global Path')
    
    # 局部路徑點
    plt.scatter(local_x, local_y, color='blue', label='Local Waypoints')
    
    # 擬合曲線
    x_fit = np.linspace(min(local_x), max(local_x), 100)
    y_fit = params[0] * x_fit**2 + params[1] * x_fit + params[2]
    plt.plot(x_fit, y_fit, color='red', label='Fitted Local Curve')
    
    plt.xlabel('X (m)')
    plt.ylabel('Y (m)')
    plt.title('Path Planning & Tracking')
    plt.legend()
    plt.grid(True)
    plt.show()

8. 主程序執行

將上述各部分整合，實現完整的導航流程：

current_pose = start_pose
for i in range(0, len(global_path_points), 5):
    # 更新局部路徑點
    local_x, local_y = transform_to_vehicle_frame(global_path_points[i:], current_pose)
    params = fit_local_curve(local_x, local_y)
    
    # MPC控制計算
    control = mpc.make_step(current_pose)
    
    # 更新位置
    current_pose = update_pose(current_pose, control)
    
    # 可視化
    visualize_paths(global_path_points, local_x, local_y, params)
    
    # 動態更新全局路徑
    if i + 1 < len(global_path_points):
        current_pose = global_path_points[i + 1]

詳盡功能解析

1. 路徑生成與動態更新

通過Reeds-Shepp算法生成包含座標、航向與運動方向的全局路徑點，並根據當前位置動態更新路徑，以適應載具的即時變化。

2. 局部導航曲線擬合

提取最近的5個路徑點，基於當前姿態轉換至車輛坐標系，然後使用二次多項式進行曲線擬合，從而生成平滑的導航曲線。

3. MPC控制器應用

使用MPC控制器根據擬合出的局部導航曲線計算最佳的線速度與角速度，確保載具能夠精確地跟蹤導航路徑。

4. 路徑可視化

利用Matplotlib將全局路徑、局部路徑點及擬合曲線進行可視化展示，便於對導航過程進行監控與調試。

完整代碼實現

Python車輛導航系統

import numpy as np
import matplotlib.pyplot as plt
from rsplan import ReedsShepp
from do_mpc.model import Model
from do_mpc.controller import MPC
from scipy.optimize import curve_fit

# 導入Reeds-Shepp路徑規劃器
rs_planner = ReedsShepp()

# 定義初始與目標姿態 (x, y, yaw)
start_pose = (0, 0, 0)  # 初始位置與航向
goal_pose = (10, 5, np.pi / 2)  # 目標位置與航向

# 生成全局路徑
path = rs_planner.path(start_pose, goal_pose, TURN_RADIUS=2.0, STEP_SIZE=0.1)
global_path_points = np.array([(p[0], p[1], p[2]) for p in path.path_points])

# 設置MPC控制器
def setup_mpc():
    model = Model('continuous')
    x = model.set_variable('_x', 'x')
    y = model.set_variable('_x', 'y')
    theta = model.set_variable('_x', 'theta')
    v = model.set_variable('_u', 'v')
    omega = model.set_variable('_u', 'omega')

    # 定義動力學方程
    model.set_rhs('x', v * np.cos(theta))
    model.set_rhs('y', v * np.sin(theta))
    model.set_rhs('theta', omega)
    model.setup()

    mpc = MPC(model)
    mpc.set_param(n_horizon=10, t_step=0.1, store_full_solution=True)
    mpc.setup()
    return mpc

mpc = setup_mpc()

# 更新全局路徑
def update_global_path(current_pose, goal_pose):
    new_path = rs_planner.path(current_pose, goal_pose, TURN_RADIUS=2.0, STEP_SIZE=0.1)
    return np.array([(p[0], p[1], p[2]) for p in new_path.path_points])

# 轉換至車輛坐標系
def transform_to_vehicle_frame(global_path, current_pose, N=5):
    x0, y0, theta0 = current_pose
    local_points = global_path[:N]
    x_local, y_local = [], []

    for x, y, yaw in local_points:
        dx = x - x0
        dy = y - y0
        x_transformed = dx * np.cos(-theta0) - dy * np.sin(-theta0)
        y_transformed = dx * np.sin(-theta0) + dy * np.cos(-theta0)
        x_local.append(x_transformed)
        y_local.append(y_transformed)

    return np.array(x_local), np.array(y_local)

# 二次曲線擬合
def fit_local_curve(x_local, y_local):
    def poly2(x, a, b, c):
        return a * x<b>2 + b * x + c

    params, _ = curve_fit(poly2, x_local, y_local)
    return params

# 更新姿態
def update_pose(current_pose, control, step_size=0.1):
    x, y, theta = current_pose
    v, omega = control
    x += v * np.cos(theta) * step_size
    y += v * np.sin(theta) * step_size
    theta += omega * step_size
    return (x, y, theta)

# 可視化路徑
def visualize_paths(global_path, local_x, local_y, params):
    plt.figure(figsize=(10, 6))
    # 全局路徑
    plt.plot(global_path[:, 0], global_path[:, 1], '--', color='grey', label='Global Path')
    
    # 局部路徑點
    plt.scatter(local_x, local_y, color='blue', label='Local Waypoints')
    
    # 擬合曲線
    x_fit = np.linspace(min(local_x), max(local_x), 100)
    y_fit = params[0] * x_fit</b>2 + params[1] * x_fit + params[2]
    plt.plot(x_fit, y_fit, color='red', label='Fitted Local Curve')
    
    plt.xlabel('X (m)')
    plt.ylabel('Y (m)')
    plt.title('Path Planning & Tracking')
    plt.legend()
    plt.grid(True)
    plt.show()

# 主程序執行
current_pose = start_pose
for i in range(0, len(global_path_points), 5):
    # 轉換並擷取局部路徑點
    local_x, local_y = transform_to_vehicle_frame(global_path_points[i:], current_pose)
    params = fit_local_curve(local_x, local_y)
    
    # MPC控制計算
    control = mpc.make_step(current_pose)
    
    # 更新位置
    current_pose = update_pose(current_pose, control)
    
    # 可視化
    visualize_paths(global_path_points, local_x, local_y, params)
    
    # 動態更新全局路徑
    if i + 1 < len(global_path_points):
        current_pose = global_path_points[i + 1]

參考資料

github.com

rsplan 官方 GitHub

scipy.org

SciPy 官方網站

以上代碼提供了完整的車輛導航系統實現方案，具體應用中可根據實際需求進行調試與優化。請確保所有依賴函式庫已正確安裝，並根據實際車輛模型調整MPC參數以獲得最佳導航效果。