基于LQR的倒立摆控制——python代码——dlqr步骤推导

已于 2023-02-26 10:20:33 修改
阅读量1.8k 收藏 26
点赞数 4
分类专栏： 自动驾驶规划控制 文章标签： python numpy 开发语言
于 2022-12-26 20:58:54 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_42806827/article/details/128449260
版权

推荐一个自动控制小车开源项目：本文结合老王自动驾驶控制算法第五讲的离散LQR进行学习复盘

Inverted Pendulum Control — PythonRobotics documentation

  • dlqr原理（老王的拉格朗日乘子法）

  • 由于函数较多，写了一个框架方便理解。

  •  代码如下代，内附官方和自己的注释：
"""
Inverted Pendulum LQR control
author: Trung Kien - letrungkien.k53.hut@gmail.com
"""

import math
import time

import matplotlib.pyplot as plt
import numpy as np
from numpy.linalg import inv, eig

# Model parameters

l_bar = 2.0  # length of bar
M = 1.0  # [kg]
m = 0.3  # [kg]
g = 9.8  # [m/s^2]

nx = 4  # number of state
nu = 1  # number of input
Q = np.diag([0.0, 1.0, 1.0, 0.0])  # state cost matrix。返回所给元素组成的对角矩阵
R = np.diag([0.01])  # input cost matrix

delta_t = 0.1  # time tick [s]
sim_time = 5.0  # simulation time [s]

show_animation = True #一个为真的变量

## 1
def main():
    # x=[x,dot_x,seta,dot_seita]
    x0 = np.array([
        [0.0],
        [0.0],
        [0.3],#倾斜角
        [0.0]
    ])

    # x = np.copy(x0)
    x = x0
    time = 0.0

    while sim_time > time:
        time += delta_t

        # calc control input
        u = lqr_control(x)#调用函数

        # simulate inverted pendulum cart
        x = simulation(x, u)#调用函数

        if show_animation:
            plt.clf()#Clear the current figure.清楚每一帧的动画
            print("X[0]:",x[0])
            # print("X:",x)
            px = float(x[0])#将一个字符串或数字转换为浮点数。输出位置
            theta = float(x[2])#输出角度
            plot_cart(px, theta)#调用函数
            plt.xlim([-5.0, 2.0])
            plt.pause(0.001)#暂停间隔

    print("Finish")
    print(f"x={float(x[0]):.2f} [m] , theta={math.degrees(x[2]):.2f} [deg]")
    if show_animation:
        plt.show()


## 2
def simulation(x, u):
    '''
    调整X
    '''
    A, B = get_model_matrix()#调用函数,得到AB矩阵
    
    x = A @ x + B @ u#矩阵乘法，必须声明numpy

    return x


## 3
def solve_DARE(A, B, Q, R, maxiter=150, eps=0.01):
    """
    Solve a discrete time_Algebraic Riccati equation (DARE)求得离散时间黎卡提方程的解 矩阵P
    """
    P = Q#初始化

    for i in range(maxiter):
        #矩阵P进行迭代，4×4的矩阵
        Pn = A.T @ P @ A - A.T @ P @ B @ inv(R + B.T @ P @ B) @ B.T @ P @ A + Q
        # print("Pn:",Pn)
        #矩阵P收敛时，退出迭代循环
        # print("abs(Pn - P).max:",abs(Pn - P).max(),i)
        # print("Pn - P:",Pn - P,i)
        if (abs(Pn - P)).max() < eps:
            break
        P = Pn

    return Pn#收敛的矩阵P



    # P = Q
    # for i in range(50):
    #     Pn = A.T @ P @ A - A.T @ P @ B @ inv(R + B.T @ P @ B ) @ B.T @ P @ A +Q 
    #     # print("Pn:",Pn)
    #     # print("P:",P)
    #     if (abs(Pn - P)).max() < 0.01:
    #         break
    # return Pn

## 4
def dlqr(A, B, Q, R):
    """
    Solve the discrete time lqr controller.
    x[k+1] = A x[k] + B u[k]
    cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
    # ref Bertsekas, p.151
    """

    # first, try to solve the ricatti equation
    #先求解迭代后收敛的矩阵P
    P = solve_DARE(A, B, Q, R)

    # compute the LQR gain
    #计算矩阵K，u=-KX
    K = inv(B.T @ P @ B + R) @ (B.T @ P @ A)#u=-kx的k

    eigVals, eigVecs = eig(A - B @ K)#计算方阵的特征值和右特征向量

    return K, P, eigVals


## 5
def lqr_control(x):
    '''
    得到输入u
    '''
    A, B = get_model_matrix()
    start = time.time()
    K, _, _ = dlqr(A, B, Q, R)
    u = -K @ x
    elapsed_time = time.time() - start
    print(f"calc time:{elapsed_time:.6f} [sec]")
    return u#返回输入u


## 6
def get_numpy_array_from_matrix(x):
    """
    get build-in list from matrix,将多维数组降为一维
    """
    return np.array(x).flatten()#将多维数组降为一维


## 7
def get_model_matrix():
    '''
    更新离散过程中的矩阵A、B
    '''
    A = np.array([
        [0.0, 1.0, 0.0, 0.0],
        [0.0, 0.0, m * g / M, 0.0],
        [0.0, 0.0, 0.0, 1.0],
        [0.0, 0.0, g * (M + m) / (l_bar * M), 0.0]
    ])
    
    # A = np.eye(nx) + delta_t * A#单位矩阵+△t*A,收敛速度慢
    A = inv(np.eye(4) - A *1/2 * delta_t) @ (np.eye(4) + A *1/2 * delta_t)#收敛更快

    B = np.array([
        [0.0],
        [1.0 / M],
        [0.0],
        [1.0 / (l_bar * M)]
    ])
    B = delta_t * B
    
    return A, B


## 8
def flatten(a):
    """
    将多维数组降为一维
    """
    return np.array(a).flatten()


## 9
def plot_cart(xt, theta):#xt：小球位置
    """
    画图
    """
    cart_w = 1.0#马车宽
    cart_h = 0.5#马车高
    radius = 0.1#马车轮半径

    cx = np.array([-cart_w / 2.0, cart_w / 2.0, cart_w /2.0, -cart_w / 2.0, -cart_w / 2.0])#马车顶点x坐标矩阵，闭合图形的顶点
    cy = np.array([0.0, 0.0, cart_h, cart_h, 0.0])#马车顶点y坐标
    cy += radius * 2.0#加轮高

    cx = cx + xt#调整马车位置,以球心坐标为基点变化

    bx = np.array([0.0, l_bar * math.sin(-theta)])#球心的x坐标
    bx += xt
    by = np.array([cart_h, l_bar * math.cos(-theta) + cart_h])#球心的y坐标
    by += radius * 2.0

    ##画车轮的圆
    #np.arange返回一个有终点和起点的固定步长的排列,第一个参数为起点，第二个参数为终点，第三个参数为步长
    angles = np.arange(0.0, math.pi * 2.0, math.radians(3.0))#math.radians()返回弧度制。离散化
    #每一步的x、y随球的旋转弧度变化
    ox = np.array([radius * math.cos(a) for a in angles])#np.array()的作用就是按照一定要求将object转换为数组
    oy = np.array([radius * math.sin(a) for a in angles])
    #右轮
    rwx = np.copy(ox) + cart_w / 4.0 + xt#右轮位置 = 离散矩阵 + 0.25 + 球位置
    rwy = np.copy(oy) + radius#离散矩阵
    #左轮
    lwx = np.copy(ox) - cart_w / 4.0 + xt
    lwy = np.copy(oy) + radius

    ##画球
    wx = np.copy(ox) + bx[-1]
    wy = np.copy(oy) + by[-1]

    plt.plot(flatten(cx), flatten(cy), "-b")#马车
    plt.plot(flatten(bx), flatten(by), "-k")#摆杆
    plt.plot(flatten(rwx), flatten(rwy), "-k")#右轮
    plt.plot(flatten(lwx), flatten(lwy), "-k")#左球
    plt.plot(flatten(wx), flatten(wy), "-k")#球
    plt.title(f"x: {xt:.2f} , theta: {math.degrees(theta):.2f}")

    # for stopping simulation with the esc key.
    plt.gcf().canvas.mpl_connect(
        'key_release_event',
        lambda event: [exit(0) if event.key == 'escape' else None])

    plt.axis("equal")


if __name__ == '__main__':
    main()

自己根据老王的dlqr推导过程，仿着开源代码修改了自己的代码，由于开源代码有些地方较为冗长，自己在函数顺序方面做了改进，图像plot部分未做修改

import math
import time

import matplotlib.pyplot as plt
import numpy as np
from numpy.linalg import inv, eig

l_bar = 2.0  # length of bar
M = 1.0  # [kg]
m = 0.3  # [kg]
g = 9.8  # [m/s^2]

nx = 4  # number of state
nu = 1  # number of input
Q = np.diag([0.0, 1.0, 1.0, 0.0])  # state cost matrix。返回所给元素组成的对角矩阵
R = np.diag([0.01])  # input cost matrix

delta_t = 0.1  # time tick [s]
sim_time = 5.0  # simulation time [s]


show_animation = True #一个为真的变量

#主函数√
def  main():
    time = 0.0
    X0 = np.array([
        [0.0],
        [0.0],
        [0.3],
        [0.0]
    ])

    X = X0
    while sim_time > time:
        time += delta_t
        
        A,B = get_A_B()
        P = get_P(A,B,R,Q)
        K = get_K(A,B,R,P)
        u = get_u(K, X)
        X = A @ X + B @ u

        if show_animation:
            plt.clf()#Clear the current figure.清楚每一帧的动画
            px = float(X[0])#将一个字符串或数字转换为浮点数。输出位置
            theta = float(X[2])#输出角度
            plot_cart(px, theta)#调用函数
            plt.xlim([-5.0, 5.0])
            plt.pause(0.001)#暂停间隔
    print("Finish")
    print(f"x={float(X[0]):.2f} [m] , theta={math.degrees(X[2]):.2f} [deg]")
    if show_animation:
        plt.show()


#获取p矩阵√
def get_P(A,B,R,Q):
    P = Q
    # Pn = A.T @ P @ A - A.T @ P @ B @ inv(R + B.T @ P @ B ) @ B.T @ P @ A +Q 
    # print("Pn:",Pn)
    for i in range(150):
        Pn = A.T @ P @ A - A.T @ P @ B @ inv(R + B.T @ P @ B ) @ B.T @ P @ A +Q 
        # print("Pn:",Pn)
        # print("P:",P)
        if (abs(Pn - P)).max()< 0.01:
            break
        P = Pn

    return Pn
            

    
#获取A,B√√√
def get_A_B():
    A0 = np.array([
        [0.0, 1.0, 0.0, 0.0],
        [0.0, 0.0, m * g / M, 0.0],
        [0.0, 0.0, 0.0, 1.0],
        [0.0, 0.0, g * (M + m) / (l_bar * M), 0.0]
    ])
    
    B0 = np.array([
        [0.0],
        [1.0 / M],
        [0.0],
        [1.0 / (l_bar * M)]
    ])

    A = inv(np.eye(4) - A0 *1/2 * delta_t) @ (np.eye(4) + A0 *1/2 * delta_t)
    B = B0 * delta_t

    return A,B



#获取K矩阵
def get_K(A,B,R,P):
    K = inv(B.T @ P @ B + R) @(B.T @ P @ A)
    return K



# 获取输入u
def get_u(K, X):
    u = -1 * K @ X
    return u


## 8
def flatten(a):
    """
    将多维数组降为一维
    """
    return np.array(a).flatten()


def plot_cart(xt, theta):
    """
    画图
    """
    cart_w = 1.0
    cart_h = 0.5
    radius = 0.1

    cx = np.array([-cart_w / 2.0, cart_w / 2.0, cart_w /
                   2.0, -cart_w / 2.0, -cart_w / 2.0])
    cy = np.array([0.0, 0.0, cart_h, cart_h, 0.0])
    cy += radius * 2.0

    cx = cx + xt

    bx = np.array([0.0, l_bar * math.sin(-theta)])
    bx += xt
    by = np.array([cart_h, l_bar * math.cos(-theta) + cart_h])
    by += radius * 2.0

    angles = np.arange(0.0, math.pi * 2.0, math.radians(3.0))
    ox = np.array([radius * math.cos(a) for a in angles])
    oy = np.array([radius * math.sin(a) for a in angles])

    rwx = np.copy(ox) + cart_w / 4.0 + xt
    rwy = np.copy(oy) + radius
    lwx = np.copy(ox) - cart_w / 4.0 + xt
    lwy = np.copy(oy) + radius

    wx = np.copy(ox) + bx[-1]
    wy = np.copy(oy) + by[-1]

    plt.plot(flatten(cx), flatten(cy), "-b")
    plt.plot(flatten(bx), flatten(by), "-k")
    plt.plot(flatten(rwx), flatten(rwy), "-k")
    plt.plot(flatten(lwx), flatten(lwy), "-k")
    plt.plot(flatten(wx), flatten(wy), "-k")
    plt.title(f"x: {xt:.2f} , theta: {math.degrees(theta):.2f}")

    # for stopping simulation with the esc key.
    plt.gcf().canvas.mpl_connect(
        'key_release_event',
        lambda event: [exit(0) if event.key == 'escape' else None])

    plt.axis("equal")


if __name__ == '__main__':
    main()

儒雅芝士
关注
  • 4
    点赞
  • 踩
  • 26
    收藏
    觉得还不错? 一键收藏
  • 2
    评论
专栏目录
MoJoCo 入门教程（六）Python LQR 教程
weixin_46300916的博客
04-08 948
本笔记本提供了一个使用 MuJoCo 物理原理的 LQR 控制器示例。版权声明根据 Apache License 2.0 版（"许可协议"）授权；除非遵守许可协议，否则不得使用本文件。您可以从 http://www.apache.org/licenses/LICENSE-2.0 获取许可证副本。除非适用法律要求或书面同意，否则根据许可协议分发的软件均按 "原样 "分发，不附带任何明示或暗示的保证或条件。请参阅许可协议，了解有关许可协议下的权限和限制的具体语言。
伺服系统LQR最优控制仿真程序
03-14
为一Matlab仿真程序，采用线性二次型最优控制方法实现伺服系统的高精度控制。
倒立摆LQR控制程序
06-28
在matlab中编程实现倒立摆的状态空间模型建立，并进行LQR控制
求解离散Raccati方程的和连续Raccati方程的解
qq_43526137的博客
08-31 889
DLQR
笔记92：连续状态空间方程转化为离散状态空间方程的详细推导 + 离散LQR控制器详细推导
最新发布
weixin_44063383的博客
05-29 513
a对LQR算法本质的总结：​​​​​​​利用性能函数J作为目标；利用Frenet上的动力学方程作为（等式）约束条件；使用拉格朗日乘子法计算J的极小值点处U的值在这一步中经过数学推导，可以不用计算复杂的拉格朗日乘子法，直接用现成的里卡提公式就可以计算出极小值点处U的值）U_%7Bk%7DJ1%7DU_%7Bk%7D1%7DU_%7Bk%7DU_%7Bk%7D1%7DU_%7Bk%7DX_%7Bk%7DJU_%7Bk%7DJJU_%7Bk%7D1%7DU_%7Bk%7DP_%7Bk%7D。
LQR线性二次型调节器3（Discrete-time system Linear-Quadratic Regulator design，离散系统分析及MATALB实例）
weixin_40857506的博客
07-09 3104
### 3.1 离散系统中，函数*dare*()和*dlqr*()计算的结果相等，*K*,*P*,*r*完全相同==； ### 3.2 公式*k*=(*B^T^PB*+*R*)^-1^*B^T^PA*求解的*k*和*dlqr*()函数、*dare*()函数求解的*k*结果相同==； ### 3.3 函数*dare*()和*dlqr*()都可以直接求解Riccati方程的解*P*、以及最优反馈增益矩阵K，且他们求解的P和*dlyap*()函数求解的*P*不同； ### 3.4 以上结论同连续系..
LQR算法原理和代码实现
weixin_44924725的博客
03-22 7514
本文讲解线性二次优化器LQR的原理和C++的代码实现，同时在CARLA-ROS联合仿真环境验证算法效果。
自动驾驶控制算法---求解误差微分方程（LQR算法及基于CARLA的Python代码实现）
weixin_46072670的博客
11-01 2162
使用LQR算出反馈控制的k值，然后利用k去算出前馈控制，此时施加控制u就满足误差稳态是0的控制要求了。
hwv1_python倒立摆_基于神经网络的小车倒立摆控制_
09-30
在本项目中，我们主要探讨的是“hwv1_python倒立摆_基于神经网络的小车倒立摆控制”。倒立摆是一种经典的控制理论问题，它涉及到动态系统稳定性、控制策略和机器学习等多个领域。在这个项目中，我们使用Python语言来...
基于LQR控制器的倒立摆平衡车稳定性控制matlab仿真+仿真录像
10-21
1.版本：matlab2021a，我录制了仿真操作录像，可以跟着操作出仿真结果 2.领域：LQR控制器 3.内容：基于LQR控制器的倒立摆平衡车稳定性控制matlab仿真，显示平衡车动态调整过程 4.适合人群：本，硕等教研学习使用
基于LQR控制器控制一阶倒立摆,使用simulink的SimMechanics工具箱一阶倒立摆的物理仿真模型+操作视频
06-07
2.内容：基于LQR控制器控制一阶倒立摆,使用simulink的SimMechanics工具箱一阶倒立摆的物理仿真模型+操作视频 3.用处：用于LQR控制器控制一阶倒立摆编程学习 4.指向人群：本硕博等教研学习使用 5.运行注意事项： ...
lqr_s_zuizhong_倒立摆控制_
09-29
总之，"lqr_s_zuizhong_倒立摆控制_"项目是利用LQR算法来解决倒立摆的控制问题，通过MATLAB/Simulink模型进行仿真验证。通过对模型的深入理解和优化，我们可以实现对倒立摆的高精度位置和角度跟踪，从而提高其稳定性...
LQR matlab 仿真程序m文件
06-27
其中的文件均为m文件，自己写的关于lqr的仿真代码，包含了关于lqr的理论知识。
倒立摆系统的设计与控制
01-05
倒立摆系统的设计与稳定性研究与控制论的应用
倒立摆程序
12-19
本文件夹包含一级，二级，三级倒立摆的相关程序供大家讨论与学习
基于LQR 和模糊插值的五级倒立摆控制
01-15
首先给出五级倒立摆的数学模型并验证了它的可控性; 然后在分析与倒立摆系统稳定性有关因素...基于二元分片插值函数的模糊控制器, 成功地实现了具有定位功能的五级倒立摆的控制. 仿真结果表明该方法是有 效的.</p>
自动驾驶算法详解(3): LQR算法进行轨迹跟踪，lqr_speed_steering_control( )的python实现
自动驾驶Player的博客
04-27 7493
前言： LQR算法在自动驾驶应用中，一般用在NOP、TJA、LCC这些算法的横向控制中，一般与曲率的前馈控制一起使用，来实现轨迹跟踪的目标，通过控制方向盘转角来实现横向控制。 本文将使用python来实现 lqr_speed_steering_control( ) 轨迹跟踪算法的demo，通过同时控制转角与加速度来实现轨迹跟踪。 如果对自动驾驶规划、控制、apollo算法细节、感知融合算法感兴趣，可以关注我的主页： https://blog.csdn.net/nn243823163/category
【强化学习】深度Q网络(DQN)求解倒立摆问题 + Pytorch代码实战
热门推荐
知不足而奋进，望远山而前行
10-22 2万+
Agent 必须在两个动作之间做出决定 - 向左或向右移动推车 - 以使连接到它的杆保持直立。 深度 Q\mathrm{Q}Q 网络算法是这样的，我们初始化两个网络 ：估计网络 QQQ 和 目标网络 Q^，Q^\hat{Q} ， \hat{Q}Q^​，Q^​ 就等于 QQQ ，一开始 目标网络 Q^\hat{Q}Q^​ 与原来的 QQQ 网络是一样的。在每一个回合中，我们用演员与环境交互，在每一次交互的过程中，都会得到一个 状态 sts_tst​ ，会采取某一个动作 at。a_{t 。}at。​ 怎么知道采
基于LQR算法跟踪圆形轨迹，MATLAB 代码
ilovewold的博客
05-15 2801
希望做一个分享者 轨迹跟踪效果图 位姿误差图 参考博文 LQR 的直观推导及简单应用_白巧克力亦唯心的博客-CSDN博客_lqr算法 关键词：泰勒展开；黎卡提方程求解；状态反馈 clear all; close all; clc; %% 参数 ts=0.1; t=30; d=1.45; %% 初始状态 x(1)=0; y(1)=0; xita(1)=0; v=3; w=0.2; u=[v;w]; Z(:,1)=[-3;-3;0.2]; for k=1:1:t/ts .
单级倒立摆基于LQR的模糊控制.pdf
07-08
单级倒立摆基于LQR的模糊控制.pdf单级倒立摆基于LQR的模糊控制.pdf单级倒立摆基于LQR的模糊控制.pdf单级倒立摆基于LQR的模糊控制.pdf单级倒立摆基于LQR的模糊控制.pdf单级倒立摆基于LQR的模糊控制.pdf单级倒立摆基于LQR的模糊控制.pdf单级倒立摆基于LQR的模糊控制.pdf单级倒立摆基于LQR的模糊控制.pdf

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值