一级倒立摆控制 —— LQR 控制器 GAZEBO 仿真_lqr controller

系列文章目录

最优控制介绍
一级倒立摆控制 —— 系统建模（传递函数模型与状态空间方程表示）
一级倒立摆控制 —— 最优控制 线性二次型控制（LQR）及 MATLAB 实现
一级倒立摆控制 —— MPC 控制器设计及 MATLAB 实现
一级倒立摆控制 —— ROS2 仿真
一级倒立摆控制 —— LQR 控制器 GAZEBO 仿真

---

---

项目描述

该项目的目的是利用 ROS2 框架展示实时功能。本项目基于开放机器人组织之前的工作。本项目以这些软件包为基础。
这就是使用倒立摆进行实时演示的动机：

实时计算通常解决的控制问题的一个典型例子是平衡倒立摆。如果控制器意外地阻塞很长时间，摆就会倒下或变得不稳定。但是，如果控制器可靠地以比控制摆锤的电机运行速度更快的速度更新，摆锤就会成功地适应传感器数据，从而达到平衡摆锤的目的。

有关项目设计演示的详细介绍，请点击此处： 设计文章

---

一、

Cart-Pole 示例展示了如何用 Python 编写 Gazebo 系统。它实现了一个 LQR 控制器来平衡小车杆，使其始终处于垂直状态。

插件订阅 cart_pole/reset_angle 主题，该主题可用于反复重置杆角，并让控制器平衡车杆。

gz topic -t "cart_pole/reset_angle" -m gz.msgs.Double -p "data: 0.3"
1

注意:大角度会导致控制器失效，在这种情况下，模拟必须重置。

---

二、

这个演示还表明，通过将 LQR 控制器实现放在一个单独的模块中，插件可以提供一种在模拟器运行时热重新加载控制器的机制。
例如，这可以用于调整参数或快速迭代控制器设计。

为此，修改 lqr_controller.py 中的控制器增益 R 和 Q ，并通过发布到 cart_pole/reload_controller 主题来重新加载控制器

gz topic -t "cart_pole/reload_controller" -m gz.msgs.Empty -p ""
1

还可以通过订阅 cart_pole/state/position 和 cart_pole/state/velocity 来监控被控对象的状态。

这些主题各自发布一个 Vector2d ，其中 x 对应于小车的状态，y 对应于摆杆的状态。

---

三、仿真运行

需要设置插件的路径

cd CartPole
export GZ_SIM_RESOURCE_PATH=`pwd`/:$GZ_SIM_RESOURCE_PATH
export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`/plugins:$GZ_SIM_SYSTEM_PLUGIN_PATH 
1
2
3

gz sim -v 4 "harmonic_demo/harmonic.sdf"
1

四、代码阅读

lqr_controller.py

4.1 导入库

import numpy as np
from scipy import linalg 
1
2

4.2 定义 LQR 控制器类

lqr 类有两个方法，一个初始化方法，一个计算控制输入。

class LqrController(object):
    def __init__(self, mass_cart, mass_point_mass, pole_length):
    def compute(self, x):
1
2
3

初始化函数定义了倒立摆的状态空间方程，求解 ricatti 方程及计算最优控制器增益；

    def __init__(self, mass_cart, mass_point_mass, pole_length):
        a = 9.81 / (pole_length * 4.0 / 3 - mass_point_mass / (mass_point_mass +
                                                               mass_cart))

        A = np.array([[0, 1, 0, 0],
                      [0, 0, a, 0],
                      [0, 0, 0, 1],
                      [0, 0, a, 0]])

        b = -1 / (pole_length * (4.0 / 3 - mass_point_mass / (mass_point_mass + mass_cart)))
        B = np.array([[0], [1 / (mass_point_mass + mass_cart)], [0], [b]])

        R = np.eye(1)
        Q = np.eye(4)
        Q[0, 0] = 10
        Q[1, 1] = 10

        # solve ricatti equation    计算 ricatti 方程
        P = linalg.solve_continuous_are(A, B, Q, R)

        # calculate optimal controller gain 计算最优控制器增益
        self.K = np.dot(np.linalg.inv(R),
                        np.dot(B.T, P))
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

计算控制输入函数

    def compute(self, x):
        # 计算控制输入
        u = -np.dot(self.K, x)
        return u
1
2
3
4

lqr 类由 LQR 控制器公式。

cart_pole_system.py

4.3 导入库

from gz.sim8 import Model, Link, Joint
from gz.transport13 import Node
from gz.msgs10.double_pb2 import Double
from gz.msgs10.vector2d_pb2 import Vector2d
from gz.msgs10.empty_pb2 import Empty
import sdformat14 as sdformat
import numpy as np
from threading import Lock
import importlib
from . import lqr_controller
1
2
3
4
5
6
7
8
9
10

4.4 定义插件类

倒立摆模型插件类有 6 个方法

class CartPoleSystem(object):
    def configure(self, entity, sdf, ecm, event_mgr):
    def init_controller(self):
    def pre_update(self, info, ecm):
    def reset_angle_cb(self, msg):
    def reload_controller_cb(self, msg):
    def publish_state(self, x):
1
2
3
4
5
6
7

configure() 方法主要完成实例化Model,Joint,Link类，检查Joint,Link位置，速度，获取.sdf文件中倒立摆各参数的值，发布倒立摆位置，速度等话题。

    def configure(self, entity, sdf, ecm, event_mgr):
        self.model = Model(entity)
        self.cart_link = Link(self.model.link_by_name(ecm, "cart"))
        self.point_mass_link = Link(self.model.link_by_name(ecm, "point_mass"))
        self.cart_joint = Joint(self.model.joint_by_name(ecm, "cart_joint"))
        self.pole_joint = Joint(self.model.joint_by_name(ecm, "pole_joint"))

        initial_angle = sdf.get_double("initial_angle", 0)[0]

        assert self.cart_joint.valid(ecm)
        assert self.pole_joint.valid(ecm)

        self.cart_joint.enable_position_check(ecm)
        self.pole_joint.enable_position_check(ecm)
        self.cart_joint.enable_velocity_check(ecm)
        self.pole_joint.enable_velocity_check(ecm)

        self.pole_joint.reset_position(ecm, [initial_angle])
        self.init_controller()

        self.node = Node()
        reset_angle_topic = sdf.get_string("reset_angle_topic", "reset_angle")[0]
        print("Subscribing to", reset_angle_topic)
        self.node.subscribe(Double, reset_angle_topic, self.reset_angle_cb)

        self.new_reset_angle = None
        self.reset_angle_lock = Lock()

        reload_controller_topic = sdf.get_string("reload_controller_topic", "reload_controller")[0]
        print("Subscribing to", reload_controller_topic)
        self.node.subscribe(Empty, reload_controller_topic, self.reload_controller_cb)
        self.controller_lock = Lock()

        state_topic = sdf.get_string("state_topic", "state")[0]
        position_topic = state_topic + "/position"
        velocity_topic = state_topic + "/velocity"
        print(f"Advertising to {position_topic} and {velocity_topic}")
        self.position_pub = self.node.advertise(position_topic, Vector2d)
        self.velocity_pub = self.node.advertise(velocity_topic, Vector2d)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39

init_controller() 方法主要完成小车、摆杆质量，摆杆长度赋值，初始化控制器

    def init_controller(self):
        # TODO Get these from the model
        # TODO (azeey) Add API in sim::Link to get the mass of the link
        cart_mass = 0.25
        point_mass = 0.03
        pole_length = 0.4

        self.controller = lqr_controller.LqrController(cart_mass,
                                                       point_mass, pole_length)
1
2
3
4
5
6
7
8
9

pre_update() 方法主要完成发布小车位置速度话题，给小车施加作用力

    def pre_update(self, info, ecm):
        if info.paused:
            return

        if len(self.cart_joint.position(ecm)) == 0:
            return

        with self.reset_angle_lock:
            if self.new_reset_angle is not None:
                self.pole_joint.reset_position(ecm, [self.new_reset_angle])
                self.new_reset_angle = None

        x = np.array([
            self.cart_joint.position(ecm)[0],
            self.cart_joint.velocity(ecm)[0],
            self.pole_joint.position(ecm)[0],
            self.pole_joint.velocity(ecm)[0]
        ])

        self.publish_state(x)   # 发布位置、速度话题

        with self.controller_lock:
            u = self.controller.compute(x)

        self.cart_joint.set_force(ecm, [u]) #在这个关节上施加力。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

reset_angle_cb() 方法接收重置角度

    def reset_angle_cb(self, msg):
        with self.reset_angle_lock:
            self.new_reset_angle = msg.data
            print("Resetting angle to", self.new_reset_angle)
1
2
3
4

reload_controller_cb() 方法重新加载控制器

    def reload_controller_cb(self, msg):
        with self.controller_lock:
            print("Reloading controller")
            importlib.reload(lqr_controller)
            self.init_controller()
1
2
3
4
5

publish_state() 方法发布位置速度话题

    def publish_state(self, x):
        position_msg = Vector2d()
        position_msg.x = x[0]
        position_msg.y = x[2]

        velocity_msg = Vector2d()
        velocity_msg.x = x[1]
        velocity_msg.y = x[3]

        self.position_pub.publish(position_msg)
        self.velocity_pub.publish(velocity_msg)
1
2
3
4
5
6
7
8
9
10
11

类外定义了 get_system() 方法

def get_system():
    return CartPoleSystem()
1
2

4.5 model.sdf

<?xml version="1.0" encoding="UTF-8"?>
<sdf version="1.11">
  <model name="CartPole">
    <joint name="fix_to_world" type="fixed">
      <parent>world</parent>
      <child>base</child>
    </joint>

    <link name="base">
      <visual name="shaft">
        <pose degrees="true">0 0 0   0 90 0</pose>
        <geometry>
          <cylinder>
            <length>1.70</length>
            <radius>0.01</radius>
          </cylinder>
        </geometry>
        <material>
          <diffuse>0.0 1.0 1.0 0.4</diffuse>
          <specular>1.0 1.0 1.0 0.4</specular>
        </material>
      </visual>
      <visual name="lower_limit">
        <pose>-0.870 0 0   0 0 0</pose>
        <geometry>
          <box>
            <size>0.05 0.05 0.15</size>
          </box>
        </geometry>
        <material>
          <diffuse>0.2 0.1 0.1</diffuse>
          <specular>0.2 0.2 0.2</specular>
        </material>
      </visual>
      <visual name="upper_limit">
        <pose>0.870 0 0   0 0 0</pose>
        <geometry>
          <box>
            <size>0.05 0.05 0.15</size>
          </box>
        </geometry>
        <material>
          <diffuse>0.2 0.1 0.1</diffuse>
          <specular>0.2 0.2 0.2</specular>
        </material>
      </visual>
    </link>
    <joint name="cart_joint" type="prismatic">
      <parent>base</parent>
      <child>cart</child>
      <axis>
        <xyz>1 0 0</xyz>
        <limit>
          <lower>-0.80</lower>
          <upper>0.80</upper>
          <effort>100</effort>
        </limit>
      </axis>
    </joint>

    <link name="cart">
      <inertial auto="true"/>
      <visual name="visual">
        <geometry>
          <box>
            <size>0.1 0.05 0.05</size>
          </box>
        </geometry>
        <material>
          <diffuse>0.3 0.1 0.1</diffuse>
          <specular>0.1 0.1 0.1</specular>
        </material>
      </visual>
      <collision name="collision">
        <density>1000</density>
        <geometry>
          <box>
            <size>0.1 0.05 0.05</size>
          </box>
        </geometry>
      </collision>
    </link>

    <joint name="pole_joint" type="revolute">
      <pose relative_to="cart"/>
      <parent>cart</parent>
      <child>pole</child>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>

    <link name="pole">
      <pose>0 0.040 0.2   0 0 0</pose>
      <inertial auto="true"/>
      <visual name="visual">
        <geometry>
          <cylinder>
            <length>0.4</length>
            <radius>0.01</radius>
          </cylinder>
        </geometry>
        <material>
          <diffuse>0.3 0.3 0.3</diffuse>
        </material>
      </visual>
      <collision name="collision">
        <density>1</density>
        <geometry>
          <cylinder>
            <length>0.4</length>
            <radius>0.01</radius>
          </cylinder>
        </geometry>
      </collision>
    </link>

    <joint name="pole_point_mass_joint" type="fixed">
      <parent>pole</parent>
      <child>point_mass</child>
    </joint>

    <link name="point_mass">
      <pose relative_to="pole">0 0.0 0.2   0 0 0</pose>
      <inertial auto="true"/>
      <visual name="visual">
        <geometry>
          <sphere>
            <radius>0.02</radius>
          </sphere>
        </geometry>
        <material>
          <diffuse>0.3 0.3 0.3</diffuse>
        </material>
      </visual>
      <collision name="collision">
        <density>1000</density>
        <geometry>
          <sphere>
            <radius>0.02</radius>
          </sphere>
        </geometry>
      </collision>
    </link>

    <plugin filename="gz-sim-python-system-loader-system" name="gz::sim::systems::PythonSystemLoader">
      <module_name>cart_pole_controller</module_name>
      <initial_angle>0.2</initial_angle>
      <reset_angle_topic>cart_pole/reset_angle</reset_angle_topic>
      <reload_controller_topic>cart_pole/reload_controller</reload_controller_topic>
      <state_topic>cart_pole/state</state_topic>
    </plugin>
  </model>
</sdf>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154

---