【自动驾驶】LQR控制实现轨迹跟踪 | python实现 | c++实现参考资料1. 基本概念2. python实现4. c++代码实现

100 阅读 0 评论 66 点赞

我是靠谱客的博主机灵猎豹，最近开发中收集的这篇文章主要介绍【自动驾驶】LQR控制实现轨迹跟踪 | python实现 | c++实现参考资料1. 基本概念2. python实现4. c++代码实现，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

文章目录

参考资料
1. 基本概念
- 1.1 运动学模型的离散状态方程
- 1.2 LQR求解步骤
2. python实现
- 2.1 车辆模型
- 2.2 相关参数设置
- 2.3 生成轨迹曲线
- 2.4 角度归一化
- 2.5 解代数里卡提方程
- 2.6 LQR控制算法
- 2.7 主函数
4. c++代码实现

参考资料

基于运动模型的LQR优化
强化学习与最优控制：用于轨迹跟踪的 LQR
Linear–quadratic regulator (LQR) steering control
路径规划与轨迹跟踪系列算法
求解离散黎卡提矩阵代数方程

前文：LQR控制算法原理

1. 基本概念

LQR的控制理论原理推导可以翻看前文。

1.1 运动学模型的离散状态方程

对于一个离散时间系统：
$X (k + 1) = A X (k) + B u (k)$

其中， $R^{n times n}$ ， $R^{n times m}$ 。
关于最优问题，就在于如何选择合适的 $u_{0}, u_{1}, ldots$ ，使得状态量 $x_{0}, x_{1}, ldots$ 足够小，因此得到好的调节和控制；或者使得 $u_{0}, u_{1}, ldots$ 足够小，以使用更少的能量。这两个量通常相互制约，如果采用更大的输入 $u$ ，就会驱使状态量更快达到0。

这是一个典型的多目标优化最优控制问题，为了表示控制系统达到稳定控制所付出的代价，定义如下二次型代价函数：
$J=sum_{k=1}^{N}left(mathbf{X}^{T} Q mathbf{X}+mathbf{u}^{T} R mathbf{u}right)$

其中， $Q$ 为半正定的状态加权矩阵, $R$ 为正定的控制加权矩阵，且两者通常取为对角阵； $Q$ 矩阵元素变大意味着希望状态量 $X$ 能够快速趋近于零； $R$ 矩阵元素变大意味着希望控制输入能够尽可能小。

在轨迹跟踪中，前一项优化目标表示跟踪过程路径偏差的累积大小，第二项优化目标表示跟踪过程控制能量的损耗，这样就将轨迹跟踪控制问题转化为一个最优控制问题。

给定一个大小为 $n \times n$ 的实对称矩阵A ，若对于任意长度为 $n$ 的非零向量 $x$ ，有 $x^TAx>0$ 恒成立，则矩阵A是一个正定矩阵。

对于上述目标函数的优化求解，使用线性二次型调节器 ( Linear-Quadratic Regulator)，解出的最优控制规律u是关于状态变量 $X$ 的线性函数

$mathbf{u}=left[-left(R+B^{T} P Bright)^{-1} B^{T} P Aright] mathbf{X}=K mathbf{X}$
其中，P是下述黎卡提方程的解 :
$P=A^{T} P A-A^{T} P Bleft(R+B^{T} P Bright)^{-1} B^{T} P A+Q$

形如 $y'=P(x)y^2+Q(x)y+R(x)$ 的非线性微分方程称为黎卡提方程。针对黎卡提方程，可以采用循环迭代的思想求解P：
1）令等式右边的P_old=Q；
2）计算等式右边的值为P_new
3）比较P_old和P_new，若两者的差值小于预设值，则认为等式两边相等；否则再令P_old=P_new，继续循环。
备注
AtsushiSakai 的PythonRobotics的代码就是这么求解的

1.2 LQR求解步骤

综上，采用LQR算法进行控制率求解的步骤概括为：

确定迭代范围 $N$ ，预设精度 $E P S$
设置迭代初始值 $P=Q_{f}$ ，其中 $Q_f=Q$
循环迭代， $t = 1, \dots, N$
$P_{new}=Q+A^{T} P A-A^{T} P Bleft(R+B^{T} P Bright)^{-1} B^{T} P A\$
若 $P_{new}-P||<EPS$ ：跳出循环；否则： $P=P_{new}$
计算反馈系数 $K=-left(R+B^{T} P_{new}Bright)^{-1} B^{T} P_{new} A$
最终得优化的控制量 $u^{*}=K X$

2. python实现

2.1 车辆模型

我们以后轴中心为车辆中心的单车运动学模型为例，它的离散状态方程如下：

$v_{r} sin psi_{r} \ 0 & 1 & Tv_{r} cos psi_{r} \ 0 & 0 & 1 end{array}right] mathbf{X}(k)+left[begin{array}{ccc} T cos psi_{r} & 0 \ T sin psi_{r} & 0 \ frac{Ttan delta_{r}}{L} & frac{v_{r}T}{L cos ^{2} delta_{r}} end{array}right]mathbf{u}(k) \ &=A mathbf{X}(k)+B mathbf{u}(k) end{aligned}$
式中， $v_r$ 代表参考轨迹上每一个轨迹点要求的速度值； $delta_r$ 是每一个轨迹点的参考前轮转角控制量。 $mathbf{X}=x-x_{ref}$ 为状态量误差， $mathbf{u}=u-u_{ref}$ 为控制量误差。

期望的系统响应特性有以下两点:

跟踪偏差能够快速、稳定地趋近于零，并保持平衡；
前轮转角控制输入尽可能小。
采用线性二次调节原理解决这个问题。

python实现代码如下。

import math


class KinematicModel_3:
  """假设控制量为转向角delta_f和加速度a
  """

  def __init__(self, x, y, psi, v, L, dt):
    self.x = x
    self.y = y
    self.psi = psi
    self.v = v
    self.L = L
    # 实现是离散的模型
    self.dt = dt

  def update_state(self, a, delta_f):
    self.x = self.x+self.v*math.cos(self.psi)*self.dt
    self.y = self.y+self.v*math.sin(self.psi)*self.dt
    self.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dt
    self.v = self.v+a*self.dt

  def get_state(self):
    return self.x, self.y, self.psi, self.v

  def state_space(self, ref_delta, ref_yaw):
    """将模型离散化后的状态空间表达

    Args:
        delta (_type_): 参考输入

    Returns:
        _type_: _description_
    """
    A = np.matrix([
      [1.0,0.0,-self.v*self.dt*math.sin(ref_yaw)],
      [0.0, 1.0, self.v*self.dt*math.cos(ref_yaw)],
      [0.0,0.0,1.0]])
    
    B = np.matrix([
        [self.dt*math.cos(ref_yaw), 0],
        [self.dt*math.sin(ref_yaw), 0],
      [self.dt*math.tan(ref_delta)/self.L, self.v*self.dt/(self.L*math.cos(ref_delta)*math.cos(ref_delta))]
                  ])
    return A,B

2.2 相关参数设置

N=100 # 迭代范围
EPS = 1e-4 # 迭代精度
Q = np.eye(3)*3
R = np.eye(2)*2.
dt=0.1 # 时间间隔，单位：s
L=2 # 车辆轴距，单位：m
v = 2 # 初始速度
x_0=0 # 初始x
y_0=-3 #初始y
psi_0=0 # 初始航向角

2.3 生成轨迹曲线

class MyReferencePath:
    def __init__(self):
        # set reference trajectory
        # refer_path包括4维：位置x, 位置y， 轨迹点的切线方向, 曲率k 
        self.refer_path = np.zeros((1000, 4))
        self.refer_path[:,0] = np.linspace(0, 100, 1000) # x
        self.refer_path[:,1] = 2*np.sin(self.refer_path[:,0]/3.0)+2.5*np.cos(self.refer_path[:,0]/2.0) # y
        # 使用差分的方式计算路径点的一阶导和二阶导，从而得到切线方向和曲率
        for i in range(len(self.refer_path)):
            if i == 0:
                dx = self.refer_path[i+1,0] - self.refer_path[i,0]
                dy = self.refer_path[i+1,1] - self.refer_path[i,1]
                ddx = self.refer_path[2,0] + self.refer_path[0,0] - 2*self.refer_path[1,0]
                ddy = self.refer_path[2,1] + self.refer_path[0,1] - 2*self.refer_path[1,1]
            elif i == (len(self.refer_path)-1):
                dx = self.refer_path[i,0] - self.refer_path[i-1,0]
                dy = self.refer_path[i,1] - self.refer_path[i-1,1]
                ddx = self.refer_path[i,0] + self.refer_path[i-2,0] - 2*self.refer_path[i-1,0]
                ddy = self.refer_path[i,1] + self.refer_path[i-2,1] - 2*self.refer_path[i-1,1]
            else:      
                dx = self.refer_path[i+1,0] - self.refer_path[i,0]
                dy = self.refer_path[i+1,1] - self.refer_path[i,1]
                ddx = self.refer_path[i+1,0] + self.refer_path[i-1,0] - 2*self.refer_path[i,0]
                ddy = self.refer_path[i+1,1] + self.refer_path[i-1,1] - 2*self.refer_path[i,1]
            self.refer_path[i,2]=math.atan2(dy,dx) # yaw
            # 计算曲率:设曲线r(t) =(x(t),y(t)),则曲率k=(x'y" - x"y')/((x')^2 + (y')^2)^(3/2).
            # 参考：https://blog.csdn.net/weixin_46627433/article/details/123403726
            self.refer_path[i,3]=(ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) # 曲率k计算
            
    def calc_track_error(self, x, y):
        """计算跟踪误差

        Args:
            x (_type_): 当前车辆的位置x
            y (_type_): 当前车辆的位置y

        Returns:
            _type_: _description_
        """
        # 寻找参考轨迹最近目标点
        d_x = [self.refer_path[i,0]-x for i in range(len(self.refer_path))] 
        d_y = [self.refer_path[i,1]-y for i in range(len(self.refer_path))] 
        d = [np.sqrt(d_x[i]**2+d_y[i]**2) for i in range(len(d_x))]
        s = np.argmin(d) # 最近目标点索引


        yaw = self.refer_path[s, 2]
        k = self.refer_path[s, 3]
        angle = normalize_angle(yaw - math.atan2(d_y[s], d_x[s]))
        e = d[s]  # 误差
        if angle < 0:
            e *= -1

        return e, k, yaw, s

2.4 角度归一化

def normalize_angle(angle):
    """
    Normalize an angle to [-pi, pi].

    :param angle: (float)
    :return: (float) Angle in radian in [-pi, pi]
    copied from https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/stanley_control/stanley_control.html
    """
    while angle > np.pi:
        angle -= 2.0 * np.pi

    while angle < -np.pi:
        angle += 2.0 * np.pi

    return angle

2.5 解代数里卡提方程

def cal_Ricatti(A,B,Q,R):
    """解代数里卡提方程

    Args:
        A (_type_): 状态矩阵A
        B (_type_): 状态矩阵B
        Q (_type_): Q为半正定的状态加权矩阵, 通常取为对角阵；Q矩阵元素变大意味着希望跟踪偏差能够快速趋近于零；
        R (_type_): R为正定的控制加权矩阵，R矩阵元素变大意味着希望控制输入能够尽可能小。

    Returns:
        _type_: _description_
    """
    # 设置迭代初始值
    Qf=Q
    P=Qf
    # 循环迭代
    for t in range(N):
        P_=Q+A.T@P@A-A.T@P@B@np.linalg.pinv(R+B.T@P@B)@B.T@P@A
        if(abs(P_-P).max()<EPS):
            break
        P=P_
    return P_

2.6 LQR控制算法

def lqr(robot_state, refer_path, s0, A, B, Q, R):
    """
    LQR控制器
    """
    # x为位置和航向误差
    x=robot_state[0:3]-refer_path[s0,0:3]


    P = cal_Ricatti(A,B,Q,R)



    K = -np.linalg.pinv(R + B.T @ P @ B) @ B.T @ P @ A
    u = K @ x
    u_star = u #u_star = [[v-ref_v,delta-ref_delta]] 
    # print(u_star)
    return u_star[0,1]

2.7 主函数

from celluloid import Camera # 保存动图时用，pip install celluloid
# 使用随便生成的轨迹
def main():

    reference_path = MyReferencePath()
    goal = reference_path.refer_path[-1,0:2]



    # 运动学模型
    ugv = KinematicModel_3(x_0, y_0, psi_0, v, L, dt)
    x_ = []
    y_ = []
    fig = plt.figure(1)
    # 保存动图用
    camera = Camera(fig)
    # plt.ylim([-3,3])
    for i in range(500):
        robot_state = np.zeros(4)
        robot_state[0] = ugv.x
        robot_state[1] = ugv.y
        robot_state[2]=ugv.psi
        robot_state[3]=ugv.v
        e, k, ref_yaw, s0 = reference_path.calc_track_error(robot_state[0], robot_state[1])
        ref_delta = math.atan2(L*k,1)
        A, B = ugv.state_space(ref_delta,ref_yaw)
        delta = lqr(robot_state, reference_path.refer_path,s0, A, B, Q, R)
        delta = delta+ref_delta

        ugv.update_state(0, delta)  # 加速度设为0，恒速

        x_.append(ugv.x)
        y_.append(ugv.y)

        # 显示动图
        plt.cla()
        plt.plot(reference_path.refer_path[:,0], reference_path.refer_path[:,1], "-.b",  linewidth=1.0, label="course")
        plt.plot(x_, y_, "-r", label="trajectory")
        plt.plot(reference_path.refer_path[s0,0], reference_path.refer_path[s0,1], "go", label="target")
        # plt.axis("equal")
        plt.grid(True)
        plt.pause(0.001)

        # camera.snap()
        # 判断是否到达最后一个点
        if np.linalg.norm(robot_state[0:2]-goal)<=0.1:
            print("reach goal")
            break
    # animation = camera.animate()
    # animation.save('trajectory.gif')



main()

跟踪效果如下：

在这里插入图片描述

完整python代码文件见github仓库

4. c++代码实现

由于在自动驾驶中算法实现一般使用C++，所以我也使用C++实现了相关功能，代码结构与python代码实现类似，这边就不再做相关代码解释了。完整代码详见另一个github仓库。

最后

以上就是机灵猎豹为你收集整理的【自动驾驶】LQR控制实现轨迹跟踪 | python实现 | c++实现参考资料1. 基本概念2. python实现4. c++代码实现的全部内容，希望文章能够帮你解决【自动驾驶】LQR控制实现轨迹跟踪 | python实现 | c++实现参考资料1. 基本概念2. python实现4. c++代码实现所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。

本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。

本文分类：控制理论
浏览次数：100 次浏览
发布日期：2023-05-07 12:05:02
本文链接：https://www.kaopuke.com/article/k-p-k_14_ujo_6_f3_13__7__18_z.html

【自动驾驶】LQR控制实现轨迹跟踪 | python实现 | c++实现参考资料1. 基本概念2. python实现4. c++代码实现

概述

文章目录

参考资料

1. 基本概念

1.1 运动学模型的离散状态方程

1.2 LQR求解步骤

2. python实现

2.1 车辆模型

2.2 相关参数设置

2.3 生成轨迹曲线

2.4 角度归一化

2.5 解代数里卡提方程

2.6 LQR控制算法

2.7 主函数

4. c++代码实现

最后

评论列表共有 0 条评论

发表评论取消回复

【自动驾驶】LQR控制实现轨迹跟踪 | python实现 | c++实现参考资料1. 基本概念2. python实现4. c++代码实现

概述

文章目录

参考资料

1. 基本概念

1.1 运动学模型的离散状态方程

1.2 LQR求解步骤

2. python实现

2.1 车辆模型

2.2 相关参数设置

2.3 生成轨迹曲线

2.4 角度归一化

2.5 解代数里卡提方程

2.6 LQR控制算法

2.7 主函数

4. c++代码实现

最后

相关文章

评论列表共有 0 条评论

发表评论 取消回复

发表评论取消回复