这是一段md，第一次写md，可能有一些小问题，不过尽量写好一点点
保姆级资料，现代人比较浮躁，还是希望你沉下心来看，阅读时间大概20分钟
任务目标：仿真环境内实现单轮倒立摆模型
方法：LQR
添加需要的python库
import sys
#import pidcontrol as pid
import numpy as np
import pybullet as p
import math
import time
import pybullet_data
import controlpy
import pos_ang设置物理属性，质量与转动惯量、相对尺寸可以从solidworks里面得到
M-小轮质量，m-倒立摆质量，b-小车与地面的摩擦系数
M = 0.061;
m = 0.425;
b = 1;
I = 0.0060700391309238813;
g = 9.8;
l = 0.191333;建立物理模型，得到状态空间方程x_dot=Ax+Bu
控制量有[x,x_dot,Θ,Θ_dot]
本例采用力矩控制
A,B的参数列于下方
A = (np.array([[0,1,0,0],
[0,0,-(m*g)/M,0],
[0,0,0,1],
[0,0,((m+M)*g)/(M*l),0]]))
B = np.array([[0],
[1/M],
[0],
[-1/(M*l)]])数据的取得：synthesizeData函数
需要得到的数据量无外乎就是当前的状态空间，用pybullet内部的API或自定义的函数来完成
目前需要补充的是：
没有加滤波器，对于目前状态的获取是直接取得瞬时状态，不能完成丝滑如德芙的操作，可能会导致机器误判形势
简单介绍一下有几个API
getBasePositionAndOrientation：获取base_link的笛卡尔坐标、代表转角的四元数
返回值是一个tuple，具体来说就是[[X,Y,Z],[x,y,z,w]]
getEulerFromQuaternion：从四元数内获取欧拉角，方便机器人的Θ的获取
返回值是欧拉角
欧拉角补充
odom：我自己写的，获取到当前的位置，采用的是到原点的距离作为测量量来实现的
至于速度、角速度的话，不能直接使用API来实现，我就采用了一个奇怪的方法：
由于在pybullet内部，计算一次模拟的steptime是 1/240s，就使用了一个变量来存一下上一时刻的角度和位置，由此计算出速度与角速度，可以使用全局变量也可以定义一个对象属性
def synthesizeData (robot):
'''
Purpose:
---
Calculate the current state(position , velocity , orienation etc.)
Input Arguments:
---
`robot` : integer
object id of bot spawned in pybullet
Returns:
---
`data` : 1D array
list of information required for calculation
Example call:
---
data=synthesizeData(robot)
'''
#state matrix will be [x x. theta theta.](and the x is along the x direction)
data = np.zeros([4,1])
data[2][0] = (p.getEulerFromQuaternion(p.getBasePositionAndOrientation(robot)[1])[1])
odom = pos_ang.odom(p.getBasePositionAndOrientation(robot)[0][:2])
global pos_pos
data[0][0] = odom
data[1][0] = (pos_pos-odom)*240
pos_pos = odom
global pos_pitch
vel_pitch = -(pos_pitch - (p.getEulerFromQuaternion(p.getBasePositionAndOrientation(robot)[1])[1]))*240
pos_pitch = p.getEulerFromQuaternion(p.getBasePositionAndOrientation(robot)[1])[1]
data[3][0] = vel_pitch
return data定义自平衡LQR类（从git项目得到的代码）
构造函数内的各参数的含义：
xvelMin-最小速度
xvelMax-最大速度 用于debug，这些参数是不用管的
yMin-最小Yaw值
yMax-最大Yaw值
问题：为什么是偏航角yaw？我取得的不应该是pitch吗？这一段代码取自github，可能里面有问题，我认为是pitch角，所以说想吧命名改了，俯仰角就是我们的Θ，根据下面这个经典的图例，可以看出咱状态量里面的就是pitch呀
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-OFzFdctl-1657766369639)(/home/lock/图片/2022-07-14 10-18-56 的屏幕截图.png)]
y_-存储之前的pitch值，可以作为一个寄存器
Q-Q矩阵，用于控制参数回复到零点的速度
R-R矩阵，用于控制对输入硬件的要求，R越大要求越低
K,S,e是控制矩阵（K是反馈的增益，其它的S和e的作用我就不是很清楚咯，评论区见）
trq = balance.callback(data)#call to function to implement algorithm在后面有这么一句，力矩等于callback的返回值，意味着咱的输出就是由这个函数决定的
在这里面得到了增益矩阵k，这个是非常重要的，u(1)的就由u(1)=K*x(0)计算得到，但是S和e是什么意思我就不太清楚了，有明白的小伙伴可以在评论区留言一下
class SelfBalanceLQR:
'''
Purpose:
---
Class Describing the gains of LQR and various functions
Functions:
---
__init__ : Called by default to initilize the variables in LQR
callback : It takes the data from sensors/bots and accordingly predicts the next state and respecive action
callback_q : It can be used to change the value of gains during execution
callback_r : It can be used to change the value of gains during execution
Example initialization and function call:
---
balance=SelfBalanceLQR()
vel=balance.callback(data)
'''
def __init__(self):
self.xvelMin=-.01# x velocity
self.xvelMax =0
self.yMin = -0.01#pitch
self.yMax = 0
self.y_ = 0
self.Q = np.array([[10, 0 ,0 ,0],[0,10,0,0],[0,0,100,0],[0,0,0,1]])
self.R = [[100]]
self.K,self.S,self.e = controlpy.synthesis.controller_lqr(A,B,self.Q,self.R)
print(K)
def callback(self, data):
np_x = data
y = p.getBasePositionAndOrientation(robot)[1][2]
u_t=-np.matmul(self.K, np_x)
#print(np_x,self.K,u_t)
#print(np_x.shape,self.K.shape,u_t.shape,A.shape,B.shape)
xvel = (np.matmul(A, np_x)+np.matmul(B, u_t))[1] ##### might have to change this based on your state equation
self.y_ = y # storing th previous value of state
return u_t接下来就是常规操作，启动pybullet，加载模型，运行仿真，使用以上的函数来达到LQR平衡
if __name__ == "__main__":
'''
Purpose:
--
Setup the pybullet environment and calculation of state variables and respective action to balance the bot
'''
# connecting to pybullet
id = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
plane = p.loadURDF("/home/lock/.local/lib/python3.8/site-packages/pybullet_data/plane.urdf")
p.setGravity(0, 0, -9.8)
robot = p.loadURDF("/home/lock/桌面/dachuang/urdf/wide.SLDASM/urdf/wide.SLDASM.urdf" , [0,0,0.45])
left_joint=0
right_joint=1
maxForce = 0
mode = p.VELOCITY_CONTROL
p.setJointMotorControl2(robot, left_joint,controlMode=mode, force=maxForce p.changeDynamics(robot,left_joint,lateralFriction=1,spinningFriction=1,rollingFriction=0.001)
balance = SelfBalanceLQR()
while(True):
data = synthesizeData(robot)#get data from simulation and bot
trq = balance.callback(data)#call to function to implement algorithm
print(trq)
# 设置机器关节的转矩大小,转矩是电机的一般控制量，选择为仿真的输入量
p.setJointMotorControl2(robot, left_joint, p.TORQUE_CONTROL, force = -trq)
p.stepSimulation()
# 设置相机跟随
location, _ = p.getBasePositionAndOrientation(robot)
p.resetDebugVisualizerCamera(
cameraDistance=0.5,
cameraYaw=110,
cameraPitch=-30,
cameraTargetPosition=location
)
time.sleep(1/240)
