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

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

作者：从前慢现在也慢 | 2024-04-29 23:11:54

赞

踩

dlqr

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

Inverted Pendulum Control — PythonRobotics documentation

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

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

  •  代码如下代，内附官方和自己的注释：
  1. """
  2. Inverted Pendulum LQR control
  3. author: Trung Kien - letrungkien.k53.hut@gmail.com
  4. """
  5.  
  6. import math
  7. import time
  8.  
  9. import matplotlib.pyplot as plt
  10. import numpy as np
  11. from numpy.linalg import inv, eig
  12.  
  13. # Model parameters
  14.  
  15. l_bar = 2.0  # length of bar
  16. M = 1.0  # [kg]
  17. m = 0.3  # [kg]
  18. g = 9.8  # [m/s^2]
  19.  
  20. nx = 4  # number of state
  21. nu = 1  # number of input
  22. Q = np.diag([0.0, 1.0, 1.0, 0.0])  # state cost matrix。返回所给元素组成的对角矩阵
  23. R = np.diag([0.01])  # input cost matrix
  24.  
  25. delta_t = 0.1  # time tick [s]
  26. sim_time = 5.0  # simulation time [s]
  27.  
  28. show_animation = True #一个为真的变量
  29.  
  30. ## 1
  31. def main():
  32.     # x=[x,dot_x,seta,dot_seita]
  33.     x0 = np.array([
  34.         [0.0],
  35.         [0.0],
  36.         [0.3],#倾斜角
  37.         [0.0]
  38.     ])
  39.  
  40.     # x = np.copy(x0)
  41.     x = x0
  42.     time = 0.0
  43.  
  44.     while sim_time > time:
  45.         time += delta_t
  46.  
  47.         # calc control input
  48.         u = lqr_control(x)#调用函数
  49.  
  50.         # simulate inverted pendulum cart
  51.         x = simulation(x, u)#调用函数
  52.  
  53.         if show_animation:
  54.             plt.clf()#Clear the current figure.清楚每一帧的动画
  55.             print("X[0]:",x[0])
  56.             # print("X:",x)
  57.             px = float(x[0])#将一个字符串或数字转换为浮点数。输出位置
  58.             theta = float(x[2])#输出角度
  59.             plot_cart(px, theta)#调用函数
  60.             plt.xlim([-5.0, 2.0])
  61.             plt.pause(0.001)#暂停间隔
  62.  
  63.     print("Finish")
  64.     print(f"x={float(x[0]):.2f} [m] , theta={math.degrees(x[2]):.2f} [deg]")
  65.     if show_animation:
  66.         plt.show()
  67.  
  68.  
  69. ## 2
  70. def simulation(x, u):
  71.     '''
  72.     调整X
  73.     '''
  74.     A, B = get_model_matrix()#调用函数,得到AB矩阵
  75.     
  76.     x = A @ x + B @ u#矩阵乘法，必须声明numpy
  77.  
  78.     return x
  79.  
  80.  
  81. ## 3
  82. def solve_DARE(A, B, Q, R, maxiter=150, eps=0.01):
  83.     """
  84.     Solve a discrete time_Algebraic Riccati equation (DARE)求得离散时间黎卡提方程的解 矩阵P
  85.     """
  86.     P = Q#初始化
  87.  
  88.     for i in range(maxiter):
  89.         #矩阵P进行迭代，4×4的矩阵
  90.         Pn = A.T @ P @ A - A.T @ P @ B @ inv(R + B.T @ P @ B) @ B.T @ P @ A + Q
  91.         # print("Pn:",Pn)
  92.         #矩阵P收敛时，退出迭代循环
  93.         # print("abs(Pn - P).max:",abs(Pn - P).max(),i)
  94.         # print("Pn - P:",Pn - P,i)
  95.         if (abs(Pn - P)).max() < eps:
  96.             break
  97.         P = Pn
  98.  
  99.     return Pn#收敛的矩阵P
  100.  
  101.  
  102.  
  103.     # P = Q
  104.     # for i in range(50):
  105.     #     Pn = A.T @ P @ A - A.T @ P @ B @ inv(R + B.T @ P @ B ) @ B.T @ P @ A +Q 
  106.     #     # print("Pn:",Pn)
  107.     #     # print("P:",P)
  108.     #     if (abs(Pn - P)).max() < 0.01:
  109.     #         break
  110.     # return Pn
  111.  
  112. ## 4
  113. def dlqr(A, B, Q, R):
  114.     """
  115.     Solve the discrete time lqr controller.
  116.     x[k+1] = A x[k] + B u[k]
  117.     cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
  118.     # ref Bertsekas, p.151
  119.     """
  120.  
  121.     # first, try to solve the ricatti equation
  122.     #先求解迭代后收敛的矩阵P
  123.     P = solve_DARE(A, B, Q, R)
  124.  
  125.     # compute the LQR gain
  126.     #计算矩阵K，u=-KX
  127.     K = inv(B.T @ P @ B + R) @ (B.T @ P @ A)#u=-kx的k
  128.  
  129.     eigVals, eigVecs = eig(A - B @ K)#计算方阵的特征值和右特征向量
  130.  
  131.     return K, P, eigVals
  132.  
  133.  
  134. ## 5
  135. def lqr_control(x):
  136.     '''
  137.     得到输入u
  138.     '''
  139.     A, B = get_model_matrix()
  140.     start = time.time()
  141.     K, _, _ = dlqr(A, B, Q, R)
  142.     u = -K @ x
  143.     elapsed_time = time.time() - start
  144.     print(f"calc time:{elapsed_time:.6f} [sec]")
  145.     return u#返回输入u
  146.  
  147.  
  148. ## 6
  149. def get_numpy_array_from_matrix(x):
  150.     """
  151.     get build-in list from matrix,将多维数组降为一维
  152.     """
  153.     return np.array(x).flatten()#将多维数组降为一维
  154.  
  155.  
  156. ## 7
  157. def get_model_matrix():
  158.     '''
  159.     更新离散过程中的矩阵A、B
  160.     '''
  161.     A = np.array([
  162.         [0.0, 1.0, 0.0, 0.0],
  163.         [0.0, 0.0, m * g / M, 0.0],
  164.         [0.0, 0.0, 0.0, 1.0],
  165.         [0.0, 0.0, g * (M + m) / (l_bar * M), 0.0]
  166.     ])
  167.     
  168.     # A = np.eye(nx) + delta_t * A#单位矩阵+△t*A,收敛速度慢
  169.     A = inv(np.eye(4) - A *1/2 * delta_t) @ (np.eye(4) + A *1/2 * delta_t)#收敛更快
  170.  
  171.     B = np.array([
  172.         [0.0],
  173.         [1.0 / M],
  174.         [0.0],
  175.         [1.0 / (l_bar * M)]
  176.     ])
  177.     B = delta_t * B
  178.     
  179.     return A, B
  180.  
  181.  
  182. ## 8
  183. def flatten(a):
  184.     """
  185.     将多维数组降为一维
  186.     """
  187.     return np.array(a).flatten()
  188.  
  189.  
  190. ## 9
  191. def plot_cart(xt, theta):#xt：小球位置
  192.     """
  193.     画图
  194.     """
  195.     cart_w = 1.0#马车宽
  196.     cart_h = 0.5#马车高
  197.     radius = 0.1#马车轮半径
  198.  
  199.     cx = np.array([-cart_w / 2.0, cart_w / 2.0, cart_w /2.0, -cart_w / 2.0, -cart_w / 2.0])#马车顶点x坐标矩阵，闭合图形的顶点
  200.     cy = np.array([0.0, 0.0, cart_h, cart_h, 0.0])#马车顶点y坐标
  201.     cy += radius * 2.0#加轮高
  202.  
  203.     cx = cx + xt#调整马车位置,以球心坐标为基点变化
  204.  
  205.     bx = np.array([0.0, l_bar * math.sin(-theta)])#球心的x坐标
  206.     bx += xt
  207.     by = np.array([cart_h, l_bar * math.cos(-theta) + cart_h])#球心的y坐标
  208.     by += radius * 2.0
  209.  
  210.     ##画车轮的圆
  211.     #np.arange返回一个有终点和起点的固定步长的排列,第一个参数为起点，第二个参数为终点，第三个参数为步长
  212.     angles = np.arange(0.0, math.pi * 2.0, math.radians(3.0))#math.radians()返回弧度制。离散化
  213.     #每一步的x、y随球的旋转弧度变化
  214.     ox = np.array([radius * math.cos(a) for a in angles])#np.array()的作用就是按照一定要求将object转换为数组
  215.     oy = np.array([radius * math.sin(a) for a in angles])
  216.     #右轮
  217.     rwx = np.copy(ox) + cart_w / 4.0 + xt#右轮位置 = 离散矩阵 + 0.25 + 球位置
  218.     rwy = np.copy(oy) + radius#离散矩阵
  219.     #左轮
  220.     lwx = np.copy(ox) - cart_w / 4.0 + xt
  221.     lwy = np.copy(oy) + radius
  222.  
  223.     ##画球
  224.     wx = np.copy(ox) + bx[-1]
  225.     wy = np.copy(oy) + by[-1]
  226.  
  227.     plt.plot(flatten(cx), flatten(cy), "-b")#马车
  228.     plt.plot(flatten(bx), flatten(by), "-k")#摆杆
  229.     plt.plot(flatten(rwx), flatten(rwy), "-k")#右轮
  230.     plt.plot(flatten(lwx), flatten(lwy), "-k")#左球
  231.     plt.plot(flatten(wx), flatten(wy), "-k")#球
  232.     plt.title(f"x: {xt:.2f} , theta: {math.degrees(theta):.2f}")
  233.  
  234.     # for stopping simulation with the esc key.
  235.     plt.gcf().canvas.mpl_connect(
  236.         'key_release_event',
  237.         lambda event: [exit(0) if event.key == 'escape' else None])
  238.  
  239.     plt.axis("equal")
  240.  
  241.  
  242. if __name__ == '__main__':
  243.     main()

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

  1. import math
  2. import time
  3.  
  4. import matplotlib.pyplot as plt
  5. import numpy as np
  6. from numpy.linalg import inv, eig
  7.  
  8. l_bar = 2.0  # length of bar
  9. M = 1.0  # [kg]
  10. m = 0.3  # [kg]
  11. g = 9.8  # [m/s^2]
  12.  
  13. nx = 4  # number of state
  14. nu = 1  # number of input
  15. Q = np.diag([0.0, 1.0, 1.0, 0.0])  # state cost matrix。返回所给元素组成的对角矩阵
  16. R = np.diag([0.01])  # input cost matrix
  17.  
  18. delta_t = 0.1  # time tick [s]
  19. sim_time = 5.0  # simulation time [s]
  20.  
  21.  
  22. show_animation = True #一个为真的变量
  23.  
  24. #主函数√
  25. def  main():
  26.     time = 0.0
  27.     X0 = np.array([
  28.         [0.0],
  29.         [0.0],
  30.         [0.3],
  31.         [0.0]
  32.     ])
  33.  
  34.     X = X0
  35.     while sim_time > time:
  36.         time += delta_t
  37.         
  38.         A,B = get_A_B()
  39.         P = get_P(A,B,R,Q)
  40.         K = get_K(A,B,R,P)
  41.         u = get_u(K, X)
  42.         X = A @ X + B @ u
  43.  
  44.         if show_animation:
  45.             plt.clf()#Clear the current figure.清楚每一帧的动画
  46.             px = float(X[0])#将一个字符串或数字转换为浮点数。输出位置
  47.             theta = float(X[2])#输出角度
  48.             plot_cart(px, theta)#调用函数
  49.             plt.xlim([-5.0, 5.0])
  50.             plt.pause(0.001)#暂停间隔
  51.     print("Finish")
  52.     print(f"x={float(X[0]):.2f} [m] , theta={math.degrees(X[2]):.2f} [deg]")
  53.     if show_animation:
  54.         plt.show()
  55.  
  56.  
  57. #获取p矩阵√
  58. def get_P(A,B,R,Q):
  59.     P = Q
  60.     # Pn = A.T @ P @ A - A.T @ P @ B @ inv(R + B.T @ P @ B ) @ B.T @ P @ A +Q 
  61.     # print("Pn:",Pn)
  62.     for i in range(150):
  63.         Pn = A.T @ P @ A - A.T @ P @ B @ inv(R + B.T @ P @ B ) @ B.T @ P @ A +Q 
  64.         # print("Pn:",Pn)
  65.         # print("P:",P)
  66.         if (abs(Pn - P)).max()< 0.01:
  67.             break
  68.         P = Pn
  69.  
  70.     return Pn
  71.             
  72.  
  73.     
  74. #获取A,B√√√
  75. def get_A_B():
  76.     A0 = np.array([
  77.         [0.0, 1.0, 0.0, 0.0],
  78.         [0.0, 0.0, m * g / M, 0.0],
  79.         [0.0, 0.0, 0.0, 1.0],
  80.         [0.0, 0.0, g * (M + m) / (l_bar * M), 0.0]
  81.     ])
  82.     
  83.     B0 = np.array([
  84.         [0.0],
  85.         [1.0 / M],
  86.         [0.0],
  87.         [1.0 / (l_bar * M)]
  88.     ])
  89.  
  90.     A = inv(np.eye(4) - A0 *1/2 * delta_t) @ (np.eye(4) + A0 *1/2 * delta_t)
  91.     B = B0 * delta_t
  92.  
  93.     return A,B
  94.  
  95.  
  96.  
  97. #获取K矩阵
  98. def get_K(A,B,R,P):
  99.     K = inv(B.T @ P @ B + R) @(B.T @ P @ A)
  100.     return K
  101.  
  102.  
  103.  
  104. # 获取输入u
  105. def get_u(K, X):
  106.     u = -1 * K @ X
  107.     return u
  108.  
  109.  
  110. ## 8
  111. def flatten(a):
  112.     """
  113.     将多维数组降为一维
  114.     """
  115.     return np.array(a).flatten()
  116.  
  117.  
  118. def plot_cart(xt, theta):
  119.     """
  120.     画图
  121.     """
  122.     cart_w = 1.0
  123.     cart_h = 0.5
  124.     radius = 0.1
  125.  
  126.     cx = np.array([-cart_w / 2.0, cart_w / 2.0, cart_w /
  127.                    2.0, -cart_w / 2.0, -cart_w / 2.0])
  128.     cy = np.array([0.0, 0.0, cart_h, cart_h, 0.0])
  129.     cy += radius * 2.0
  130.  
  131.     cx = cx + xt
  132.  
  133.     bx = np.array([0.0, l_bar * math.sin(-theta)])
  134.     bx += xt
  135.     by = np.array([cart_h, l_bar * math.cos(-theta) + cart_h])
  136.     by += radius * 2.0
  137.  
  138.     angles = np.arange(0.0, math.pi * 2.0, math.radians(3.0))
  139.     ox = np.array([radius * math.cos(a) for a in angles])
  140.     oy = np.array([radius * math.sin(a) for a in angles])
  141.  
  142.     rwx = np.copy(ox) + cart_w / 4.0 + xt
  143.     rwy = np.copy(oy) + radius
  144.     lwx = np.copy(ox) - cart_w / 4.0 + xt
  145.     lwy = np.copy(oy) + radius
  146.  
  147.     wx = np.copy(ox) + bx[-1]
  148.     wy = np.copy(oy) + by[-1]
  149.  
  150.     plt.plot(flatten(cx), flatten(cy), "-b")
  151.     plt.plot(flatten(bx), flatten(by), "-k")
  152.     plt.plot(flatten(rwx), flatten(rwy), "-k")
  153.     plt.plot(flatten(lwx), flatten(lwy), "-k")
  154.     plt.plot(flatten(wx), flatten(wy), "-k")
  155.     plt.title(f"x: {xt:.2f} , theta: {math.degrees(theta):.2f}")
  156.  
  157.     # for stopping simulation with the esc key.
  158.     plt.gcf().canvas.mpl_connect(
  159.         'key_release_event',
  160.         lambda event: [exit(0) if event.key == 'escape' else None])
  161.  
  162.     plt.axis("equal")
  163.  
  164.  
  165. if __name__ == '__main__':
  166.     main()
  167.

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

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

  

闽ICP备14008679号