横向LQR、纵向PID控制进行轨迹跟踪以及python实现

横向LQR、纵向PID控制进行轨迹跟踪以及python实现

附赠自动驾驶最全的学习资料和量产经验：链接

一、LQR问题模型建立：

理论部分比较成熟，这里只介绍demo所使用的建模方程：

使用离散代数黎卡提方程求解

系统状态矩阵：

输入矩阵：

A矩阵：

B矩阵：

二、代码实现

# 导入相关包

import math
import sys
import os

import matplotlib.pyplot as plt
import numpy as np
import scipy.linalg as la

# cubic_spline_planner为自己实现的三次样条插值方法
try:
    from cubic_spline_planner import *
except ImportError:
    raise

设置轨迹途经点并生成轨迹：

# 设置轨迹会经过的点
ax = [0.0, 6.0, 12.5, 10.0, 17.5, 20.0, 25.0]
ay = [0.0, -3.0, -5.0, 6.5, 3.0, 0.0, 0.0]
goal = [ax[-1], ay[-1]]

# 使用三次样条插值方法，根据途经点生成轨迹，x、y、yaw、曲率k，距离s
cx, cy, cyaw, ck, s = calc_spline_course(
        ax, ay, ds=0.1)

# 绘制规划好的轨迹
plt.plot(ax, ay, "xb", label="waypoints")
plt.plot(cx, cy, "-r", label="target course")

生成的轨迹如下：

设置期望速度：

# 设置目标速度
target_speed = 10.0 / 3.6  # simulation parameter km/h -> m/s


speed_profile = [target_speed] * len(cyaw)

direction = 1.0

# 转弯幅度较大时将速度设置为0，并将速度方向翻转
# Set stop point
for i i