• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

【四足机器人那些事儿】MiniCheetah中的MPC控制

人工智能 我是。 2574次浏览 0个评论

一、MPC的力学原理

  刚体的力与加速度转矩与角加速度可以通过牛顿方程欧拉方程求出:  

1、牛顿公式:

  基本公式:  
【四足机器人那些事儿】MiniCheetah中的MPC控制   展开形式(n c为与地面接触点的数量):  
【四足机器人那些事儿】MiniCheetah中的MPC控制   将质量移至右边,可求得加速度:  
【四足机器人那些事儿】MiniCheetah中的MPC控制   写成更简洁的形式即:  
【四足机器人那些事儿】MiniCheetah中的MPC控制  

2、欧拉公式

  基本公式(ω为角速度,I 为转动惯量):  
【四足机器人那些事儿】MiniCheetah中的MPC控制   对上式做近似考虑:  
【四足机器人那些事儿】MiniCheetah中的MPC控制   因此有展开式:  
【四足机器人那些事儿】MiniCheetah中的MPC控制   其中:  
【四足机器人那些事儿】MiniCheetah中的MPC控制   同样将转动惯量移至右边,得到角加速度Θ  
【四足机器人那些事儿】MiniCheetah中的MPC控制  

3、对不同量的参考系作近似变换

  为了尽尽可能让系统保持线性关系,同时由于运动过程中滚转角α \alphaα和俯仰角β \betaβ较小,因此有以下近似变换:   角度变换:  
【四足机器人那些事儿】MiniCheetah中的MPC控制   惯量矩阵变换:  
【四足机器人那些事儿】MiniCheetah中的MPC控制   绕Z轴的变换的旋转矩阵:  
【四足机器人那些事儿】MiniCheetah中的MPC控制   Mit实验室给出的cheetah的转动惯量:  
【四足机器人那些事儿】MiniCheetah中的MPC控制  

4、状态迭代方程

  综合公式(1)所计算的加速度 P¨,公式(2)中所计算的角加速度 Θ¨,规定右上标为时刻k ,右下标为所在参考系 各状态量之间的迭代方式如下:

  • 旋转角:

 
【四足机器人那些事儿】MiniCheetah中的MPC控制   将式(3)~(6)整理写成矩阵形式如下:  
【四足机器人那些事儿】MiniCheetah中的MPC控制(7)   其中:  
【四足机器人那些事儿】MiniCheetah中的MPC控制  

二、构建二次规划问题

 

1、完整轨迹表示

  我们式(7)中的状态迭代方程写成如下形式 :  
【四足机器人那些事儿】MiniCheetah中的MPC控制   其中:
【四足机器人那些事儿】MiniCheetah中的MPC控制   此时式(8)是一个线性方程,我们根据其计算未来h步状态:  
【四足机器人那些事儿】MiniCheetah中的MPC控制   将上式写成矩阵运算形式:  
【四足机器人那些事儿】MiniCheetah中的MPC控制   现在我们得到了未来h步轨迹的状态矩阵以及其计算表达式(9)  

2、二次规划问题

  根据式(9),构造代价函数如下,由轨迹误差项和控制项组成:  
【四足机器人那些事儿】MiniCheetah中的MPC控制   式中L,K分别为权重矩阵(对角矩阵),由于状态X是一个矩阵,安装规范写法,表达如下:  
【四足机器人那些事儿】MiniCheetah中的MPC控制   接下来构造一个二次规划如下(最小化代价函数),该式的物理意义在于,使得计算轨迹尽可能与参考轨迹重合的同时,所使用到的力最小:  
【四足机器人那些事儿】MiniCheetah中的MPC控制   将式(10)展开,去掉与控制量f无关的项,可化成标准二次型如下:  
【四足机器人那些事儿】MiniCheetah中的MPC控制  

三、二次规划求解

  由于式(12)中数据量比较大,且与机器人系统的配置有关,因此我们从以下简单案例进行讲解: 对于二次多项式:   f(x)=2x12+x22+x1x2+x1+x2   求解其在约束条件下的最小值,即为二次规划问题,有以下表达:  
【四足机器人那些事儿】MiniCheetah中的MPC控制   根据约束条件可以得出:  
【四足机器人那些事儿】MiniCheetah中的MPC控制   利用python的cvxopt库可求解该问题:  

from cvxopt import matrix, solvers


P = matrix([[4.0, 1.0], [1.0, 2.0]])
q = matrix([1.0, 1.0])
G = matrix([[-1.0, 0.0], [0.0, -1.0]])
h = matrix([0.0, 0.0])
A = matrix([1.0, 1.0], (1, 2))
b = matrix([1.0])
result = solvers.qp(P, q, G, h, A, b)

print('x:\n', result['x'])

  结果如下:  

     pcost       dcost       gap    pres   dres
 0:  1.8889e+00  7.7778e-01  1e+00  3e-16  2e+00
 1:  1.8769e+00  1.8320e+00  4e-02  2e-16  6e-02
 2:  1.8750e+00  1.8739e+00  1e-03  2e-16  5e-04
 3:  1.8750e+00  1.8750e+00  1e-05  6e-17  5e-06
 4:  1.8750e+00  1.8750e+00  1e-07  2e-16  5e-08
Optimal solution found.
x: 
[ 2.50e-01]
[ 7.50e-01]

 


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明【四足机器人那些事儿】MiniCheetah中的MPC控制
喜欢 (0)

您必须 登录 才能发表评论!

加载中……