devbox
盐析白兔
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

自动驾驶算法详解(3): LQR算法进行轨迹跟踪，lqr_speed_steering_control( )的python实现_lqr轨迹跟踪

作者：盐析白兔 | 2024-04-20 08:25:47

赞

踩

lqr轨迹跟踪

前言：

LQR算法在自动驾驶应用中，一般用在NOP、TJA、LCC这些算法的横向控制中，一般与曲率的前馈控制一起使用，来实现轨迹跟踪的目标，通过控制方向盘转角来实现横向控制。

本文将使用python来实现 lqr_speed_steering_control( ) 轨迹跟踪算法的demo，通过同时控制转角与加速度来实现轨迹跟踪。

如果对自动驾驶规划、控制、apollo算法细节、感知融合算法感兴趣，可以关注我的主页：

https://blog.csdn.net/nn243823163/category_11685852.htmlhttps://blog.csdn.net/nn243823163/category_11685852.html

最新免费文章推荐：

prescan联合simulink进行FCW的仿真_自动驾驶 Player的博客-CSDN博客

Apollo Planning决策规划代码详细解析 (1)：Scenario选择

自动驾驶算法详解 (1) : Apollo路径规划 Piecewise Jerk Path Optimizer的python实现

自动驾驶算法详解(5): 贝塞尔曲线进行路径规划的python实现

使用Vscode断点调试apollo的方法_自动驾驶 Player的博客-CSDN博客

Apollo规划决策算法仿真调试(1): 使用Vscode断点调试apollo的方法

Apollo规划决策算法仿真调试(4):动态障碍物绕行_自动驾驶 Player的博客-CSDN博客

正文如下：

一、LQR问题模型建立：

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

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

 

LQR控制的步骤：

选择参数矩阵Q,R
求解Riccati方程得到矩阵P
根据P计算 K = R − 1 B T P K=R^{-1}B^{T}P K=R−1BTP
计算控制量 u = − K x u=-Kx u=−Kx

系统状态矩阵：

 输入矩阵：

 A矩阵：

 B矩阵：

 二、代码实现

  1. # 导入相关包
  2.  
  3. import math
  4.  
  5. import sys
  6.  
  7. import os
  8.  
  9. import matplotlib.pyplot as plt
  10.  
  11. import numpy as np
  12.  
  13. import scipy.linalg as la
  14.  
  15.  
  16. # cubic_spline_planner为自己实现的三次样条插值方法
  17.  
  18. try:
  19.  
  20.     from cubic_spline_planner import *
  21.  
  22. except ImportError:
  23.  
  24.     raise

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

  1. # 设置轨迹会经过的点
  2.  
  3. ax = [0.0, 6.0, 12.5, 10.0, 17.5, 20.0, 25.0]
  4.  
  5. ay = [0.0, -3.0, -5.0, 6.5, 3.0, 0.0, 0.0]
  6.  
  7. goal = [ax[-1], ay[-1]]
  8.  
  9.  
  10.  
  11. # 使用三次样条插值方法，根据途经点生成轨迹，x、y、yaw、曲率k，距离s
  12.  
  13. cx, cy, cyaw, ck, s = calc_spline_course(
  14.  
  15.         ax, ay, ds=0.1)
  16.  
  17.  
  18.  
  19. # 绘制规划好的轨迹
  20.  
  21. plt.plot(ax, ay, "xb", label="waypoints")
  22.  
  23. plt.plot(cx, cy, "-r", label="target course")

生成的轨迹如下：

  

设置期望速度：

  1. # 设置目标速度
  2.  
  3. target_speed = 10.0 / 3.6  # simulation parameter km/h -> m/s
  4.  
  5.  
  6.  
  7.  
  8.  
  9. speed_profile = [target_speed] * len(cyaw)
  10.  
  11.  
  12.  
  13. direction = 1.0
  14.  
  15.  
  16.  
  17. # 转弯幅度较大时将速度设置为0，并将速度方向翻转
  18.  
  19. # Set stop point
  20.  
  21. for i in range(len(cyaw) - 1):
  22.  
  23.     dyaw = abs(cyaw[i + 1] - cyaw[i])
  24.  
  25.     switch = math.pi / 4.0 <= dyaw < math.pi / 2.0
  26.  
  27.  
  28.  
  29.     if switch:
  30.  
  31.         direction *= -1
  32.  
  33.  
  34.  
  35.     if direction != 1.0:
  36.  
  37.         speed_profile[i] = - target_speed
  38.  
  39.     else:
  40.  
  41.         speed_profile[i] = target_speed
  42.  
  43.  
  44.  
  45.     if switch:
  46.  
  47.         speed_profile[i] = 0.0
  48.  
  49.  
  50.  
  51. # 靠近目的地时，速度降低       
  52.  
  53. # speed down
  54.  
  55. for i in range(40):
  56.  
  57.     speed_profile[-i] = target_speed / (50 - i)
  58.  
  59.     if speed_profile[-i] <= 1.0 / 3.6:
  60.  
  61.         speed_profile[-i] = 1.0 / 3.6
  62.  
  63.  
  64.  
  65. plt.plot(speed_profile, "-b", label="speed_profile")

期望速度如下：

 定义求解所需要的数据结构与方法：

  1. # 定义LQR 计算所需要的数据结构，以及DLQR的求解方法
  2.  
  3.  
  4.  
  5. # State 对象表示自车的状态，位置x、y，以及横摆角yaw、速度v
  6.  
  7. class State:
  8.  
  9.  
  10.  
  11.     def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
  12.  
  13.         self.x = x
  14.  
  15.         self.y = y
  16.  
  17.         self.yaw = yaw
  18.  
  19.         self.v = v
  20.  
  21.  
  22.  
  23. # 更新自车的状态，采样时间足够小，则认为这段时间内速度相同，加速度相同，使用匀速模型更新位置
  24.  
  25. def update(state, a, delta):
  26.  
  27.  
  28.  
  29.     if delta >= max_steer:
  30.  
  31.         delta = max_steer
  32.  
  33.     if delta <= - max_steer:
  34.  
  35.         delta = - max_steer
  36.  
  37.  
  38.  
  39.     state.x = state.x + state.v * math.cos(state.yaw) * dt
  40.  
  41.     state.y = state.y + state.v * math.sin(state.yaw) * dt
  42.  
  43.     state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
  44.  
  45.     state.v = state.v + a * dt
  46.  
  47.  
  48.  
  49.     return state
  50.  
  51.  
  52.  
  53.  
  54.  
  55. def pi_2_pi(angle):
  56.  
  57.     return (angle + math.pi) % (2 * math.pi) - math.pi
  58.  
  59.  
  60.  
  61.  
  62.  
  63. # 实现离散Riccati equation 的求解方法
  64.  
  65. def solve_dare(A, B, Q, R):
  66.  
  67.     """
  69.     solve a discrete time_Algebraic Riccati equation (DARE)
  71.     """
  72.  
  73.     x = Q
  74.  
  75.     x_next = Q
  76.  
  77.     max_iter = 150
  78.  
  79.     eps = 0.01
  80.  
  81.  
  82.  
  83.     for i in range(max_iter):
  84.  
  85.         x_next = A.T @ x @ A - A.T @ x @ B @ \
  86.  
  87.                 la.inv(R + B.T @ x @ B) @ B.T @ x @ A + Q
  88.  
  89.         if (abs(x_next - x)).max() < eps:
  90.  
  91.             break
  92.  
  93.         x = x_next
  94.  
  95.  
  96.  
  97.     return x_next
  98.  
  99.  
  100.  
  101. # 返回值K 即为LQR 问题求解方法中系数K的解
  102.  
  103. def dlqr(A, B, Q, R):
  104.  
  105.     """Solve the discrete time lqr controller.
  107.     x[k+1] = A x[k] + B u[k]
  109.     cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
  111.     # ref Bertsekas, p.151
  113.     """
  114.  
  115.  
  116.  
  117.     # first, try to solve the ricatti equation
  118.  
  119.     X = solve_dare(A, B, Q, R)
  120.  
  121.  
  122.  
  123.     # compute the LQR gain
  124.  
  125.     K = la.inv(B.T @ X @ B + R) @ (B.T @ X @ A)
  126.  
  127.  
  128.  
  129.     eig_result = la.eig(A - B @ K)
  130.  
  131.  
  132.  
  133.     return K, X, eig_result[0]
  134.  
  135.  
  136.  
  137. # 计算距离自车当前位置最近的参考点
  138.  
  139. def calc_nearest_index(state, cx, cy, cyaw):
  140.  
  141.     dx = [state.x - icx for icx in cx]
  142.  
  143.     dy = [state.y - icy for icy in cy]
  144.  
  145.  
  146.  
  147.     d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]
  148.  
  149.  
  150.  
  151.     mind = min(d)
  152.  
  153.  
  154.  
  155.     ind = d.index(mind)
  156.  
  157.  
  158.  
  159.     mind = math.sqrt(mind)
  160.  
  161.  
  162.  
  163.     dxl = cx[ind] - state.x
  164.  
  165.     dyl = cy[ind] - state.y
  166.  
  167.  
  168.  
  169.     angle = pi_2_pi(cyaw[ind] - math.atan2(dyl, dxl))
  170.  
  171.     if angle < 0:
  172.  
  173.         mind *= -1
  174.  
  175.  
  176.  
  177.     return ind, mind

设置起点参数：

  1. # 设置起点的参数
  2.  
  3. T = 500.0  # max simulation time
  4.  
  5. goal_dis = 0.3
  6.  
  7. stop_speed = 0.05
  8.  
  9.  
  10.  
  11. state = State(x=-0.0, y=-0.0, yaw=0.0, v=0.0)
  12.  
  13.  
  14.  
  15. time = 0.0
  16.  
  17. x = [state.x]
  18.  
  19. y = [state.y]
  20.  
  21. yaw = [state.yaw]
  22.  
  23. v = [state.v]
  24.  
  25. t = [0.0]
  26.  
  27.  
  28.  
  29. pe, pth_e = 0.0, 0.0

使用LQR算法计算轨迹跟踪需要的加速度与前轮转角：

  1. # 配置LQR 的参数
  2.  
  3. # === Parameters =====
  4.  
  5.  
  6.  
  7. # LQR parameter
  8.  
  9. lqr_Q = np.eye(5)
  10.  
  11. lqr_R = np.eye(2)
  12.  
  13. dt = 0.1  # time tick[s]，采样时间
  14.  
  15. L = 0.5  # Wheel base of the vehicle [m]，车辆轴距
  16.  
  17. max_steer = np.deg2rad(45.0)  # maximum steering angle[rad]
  18.  
  19.  
  20.  
  21. show_animation = True
  22.  
  23.  
  24.  
  25. while T >= time:
  26.  
  27.     ind, e = calc_nearest_index(state, cx, cy, cyaw)
  28.  
  29.  
  30.  
  31.     sp = speed_profile
  32.  
  33.     tv = sp[ind]
  34.  
  35.  
  36.  
  37.     k = ck[ind]
  38.  
  39.     v_state = state.v
  40.  
  41.     th_e = pi_2_pi(state.yaw - cyaw[ind])
  42.  
  43.  
  44.  
  45.     # 构建LQR表达式，X(k+1) = A * X(k) + B * u(k), 使用Riccati equation 求解LQR问题
  46.  
  47. #    dt表示采样周期，v表示当前自车的速度
  48.  
  49. #    A = [1.0, dt, 0.0, 0.0, 0.0
  50.  
  51. #          0.0, 0.0, v, 0.0, 0.0]
  52.  
  53. #          0.0, 0.0, 1.0, dt, 0.0]
  54.  
  55. #          0.0, 0.0, 0.0, 0.0, 0.0]
  56.  
  57. #          0.0, 0.0, 0.0, 0.0, 1.0]
  58.  
  59.     A = np.zeros((5, 5))
  60.  
  61.     A[0, 0] = 1.0
  62.  
  63.     A[0, 1] = dt
  64.  
  65.     A[1, 2] = v_state
  66.  
  67.     A[2, 2] = 1.0
  68.  
  69.     A[2, 3] = dt
  70.  
  71.     A[4, 4] = 1.0
  72.  
  73.  
  74.  
  75.     # 构建B矩阵，L是自车的轴距
  76.  
  77.     # B = [0.0, 0.0
  78.  
  79.     #    0.0, 0.0
  80.  
  81.     #    0.0, 0.0
  82.  
  83.     #    v/L, 0.0
  84.  
  85.     #    0.0, dt]
  86.  
  87.     B = np.zeros((5, 2))
  88.  
  89.     B[3, 0] = v_state / L
  90.  
  91.     B[4, 1] = dt
  92.  
  93.  
  94.  
  95.     K, _, _ = dlqr(A, B, lqr_Q, lqr_R)
  96.  
  97.  
  98.  
  99.     # state vector，构建状态矩阵
  100.  
  101.     # x = [e, dot_e, th_e, dot_th_e, delta_v]
  102.  
  103.     # e: lateral distance to the path， e是自车到轨迹的距离
  104.  
  105.     # dot_e: derivative of e， dot_e是自车到轨迹的距离的变化率
  106.  
  107.     # th_e: angle difference to the path， th_e是自车与期望轨迹的角度偏差
  108.  
  109.     # dot_th_e: derivative of th_e， dot_th_e是自车与期望轨迹的角度偏差的变化率
  110.  
  111.     # delta_v: difference between current speed and target speed，delta_v是当前车速与期望车速的偏差
  112.  
  113.     X = np.zeros((5, 1))
  114.  
  115.     X[0, 0] = e
  116.  
  117.     X[1, 0] = (e - pe) / dt
  118.  
  119.     X[2, 0] = th_e
  120.  
  121.     X[3, 0] = (th_e - pth_e) / dt
  122.  
  123.     X[4, 0] = v_state - tv
  124.  
  125.  
  126.  
  127.     # input vector，构建输入矩阵u
  128.  
  129.     # u = [delta, accel]
  130.  
  131.     # delta: steering angle，前轮转角
  132.  
  133.     # accel: acceleration，自车加速度
  134.  
  135.     ustar = -K @ X
  136.  
  137.  
  138.  
  139.     # calc steering input
  140.  
  141.     ff = math.atan2(L * k, 1)  # feedforward steering angle
  142.  
  143.     fb = pi_2_pi(ustar[0, 0])  # feedback steering angle
  144.  
  145.     delta = ff + fb
  146.  
  147.  
  148.  
  149.     # calc accel input
  150.  
  151.     accel = ustar[1, 0]
  152.  
  153.  
  154.  
  155.     dl, target_ind, pe, pth_e, ai = delta, ind, e, th_e, accel
  156.  
  157.  
  158.  
  159.     state = update(state, ai, dl)
  160.  
  161.  
  162.  
  163.     if abs(state.v) <= stop_speed:
  164.  
  165.         target_ind += 1
  166.  
  167.  
  168.  
  169.     time = time + dt
  170.  
  171.  
  172.  
  173.     # check goal
  174.  
  175.     dx = state.x - goal[0]
  176.  
  177.     dy = state.y - goal[1]
  178.  
  179.     if math.hypot(dx, dy) <= goal_dis:
  180.  
  181.         print("Goal")
  182.  
  183.         break
  184.  
  185.  
  186.  
  187.     x.append(state.x)
  188.  
  189.     y.append(state.y)
  190.  
  191.     yaw.append(state.yaw)
  192.  
  193.     v.append(state.v)
  194.  
  195.     t.append(time)
  196.  
  197.  
  198.  
  199.     if target_ind % 100 == 0 and show_animation:
  200.  
  201.         plt.cla()
  202.  
  203.         # for stopping simulation with the esc key.
  204.  
  205.         plt.gcf().canvas.mpl_connect('key_release_event',
  206.  
  207.                 lambda event: [exit(0) if event.key == 'escape' else None])
  208.  
  209.         plt.plot(cx, cy, "-r", label="course")
  210.  
  211.         plt.plot(x, y, "ob", label="trajectory")
  212.  
  213.         plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
  214.  
  215.         plt.axis("equal")
  216.  
  217.         plt.grid(True)
  218.  
  219.         plt.title("speed[km/h]:" + str(round(state.v * 3.6, 2))
  220.  
  221.                   + ",target index:" + str(target_ind) + ", time si: " + str(time))
  222.  
  223.         plt.pause(0.1)

结果可视化：

  1. if show_animation:  # pragma: no cover
  2.  
  3.         plt.close()
  4.  
  5.         plt.subplots(1)
  6.  
  7.         plt.plot(ax, ay, "xb", label="waypoints")
  8.  
  9.         plt.plot(cx, cy, "-r", label="target course")
  10.  
  11.         plt.plot(x, y, "-g", label="tracking")
  12.  
  13.         plt.grid(True)
  14.  
  15.         plt.axis("equal")
  16.  
  17.         plt.xlabel("x[m]")
  18.  
  19.         plt.ylabel("y[m]")
  20.  
  21.         plt.legend()
  22.  
  23.  
  24.  
  25.         plt.subplots(1)
  26.  
  27.         plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
  28.  
  29.         plt.grid(True)
  30.  
  31.         plt.legend()
  32.  
  33.         plt.xlabel("line length[m]")
  34.  
  35.         plt.ylabel("yaw angle[deg]")
  36.  
  37.  
  38.  
  39.         plt.subplots(1)
  40.  
  41.         plt.plot(s, ck, "-r", label="curvature")
  42.  
  43.         plt.grid(True)
  44.  
  45.         plt.legend()
  46.  
  47.         plt.xlabel("line length[m]")
  48.  
  49.         plt.ylabel("curvature [1/m]")
  50.  
  51.  
  52.  
  53.         plt.show()

轨迹跟踪结果：

 三、结果分析

LQR算法一般用在NOP、TJA、LCC这些功能的横向控制，几种典型工况的轨迹跟踪效果如下：

1、正常变道工况 

2、转弯工况

 3、轴距对控制效果的影响

L = 0.5

 L = 2.5

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/盐析白兔/article/detail/456354
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号