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

## 一、

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

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


## 二、

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


## 三、仿真运行

需要设置插件的路径

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

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


## 四、代码阅读

lqr_controller.py

### 4.1 导入库

import numpy as np
from scipy import linalg


### 4.2 定义 LQR 控制器类

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

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


    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))


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


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


### 4.4 定义插件类

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):


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)


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)


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]) #在这个关节上施加力。


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)


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

    def reload_controller_cb(self, msg):
with self.controller_lock:
print("Reloading controller")
importlib.reload(lqr_controller)
self.init_controller()


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)


def get_system():
return CartPoleSystem()


### 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月前

49 1
|
9天前
|

23 2
|
1月前
|

27 1
|
1月前
|

27 1
|
1月前

25 1
|
1月前
|

62 0
|
30天前
|

25 0
|
2月前

**摘要：** 构建基于PID的直流电机位置控制系统，利用PID的简易性和有效性实现精确控制。在MATLAB2022a中进行系统仿真，展示结果。控制器基于误差(e(t))生成控制信号(u(t))，由比例(K_p)、积分(K_i)和微分(K_d)项构成。系统采用三层控制环：位置环设定速度参考，速度环调节实际速度，电流环确保电流匹配，以达成期望位置。
65 14
|
30天前
MATLAB - 控制小车上的倒立摆
MATLAB - 控制小车上的倒立摆
22 0
|
4月前
|

LabVIEW开发无刷直流电机磁场定向控制器（FOC）/空间矢量控制器
LabVIEW开发无刷直流电机磁场定向控制器（FOC）/空间矢量控制器
48 2