青龙源码解析MPC

news/2024/9/28 1:13:26

1. 全身运动学

青龙全身共31个自由度。
2个7自由度臂，2个头部自由度，3个腰部自由度，每个腿是6个自由度(髋关节3DOF，膝关节1DOF，踝关节2DOF)
共7+7+2+3+6+6=31
再加上浮动基座6自由度，总共37自由度。

2. 变量：

输入：13 * 3 = 39的纬度；约束：32 * 3的纬度

  • Ac,Bc,A,B,Cc,C: 状态空间矩阵
    Ac,A: [12, 12]
    Bc,B: [12, 13]
    Cc,C: [12,1]

  • Aqp,bqp,cqp: QP问题的约束矩阵
    Aqp: [120,12]
    Aqp1: [120,120]
    Bqp1: [120,130]
    Bqp: [120.39]
    Cqp1: [120,1]
    Cqp: [120,1]

  • Ufe,Ufe_pre,xd,X_cur,X_cal,dX_cal: 输入，状态
    Ufe: [39,1] 输入 13 * 3
    Ufe_pre: [13,1]
    Xd: [120,1]
    X_cur: [12,1]
    X_cal: [12,1]
    dX_cal: [12,1]

  • L,K,M,alpha,H,c: QP相关矩阵
    L: [120,120] 优化变量状态权重（欧拉角，位置，角速度，速度）
    K: [39,39] 优化变量输入权重（接触力）
    M: [39,39]
    H: [39,39]
    c: [39,1]

  • u_low,u_up,As,bs: 输入约束，约束矩阵
    u_low,u_up: [39,1]
    As: [96,39]
    bs: [96,1]

3. MPC::cal()




1. 这段代码，Aqp1计算了A的迭代矩阵，10步是[120, 120]

for (int i = 0; i < mpc_N; i++) 
{Ac[i].block<3, 3>(0, 6) = R_curz[i].transpose();Ac[i].block<3, 3>(3, 9) = Eigen::MatrixXd::Identity(3,3);A[i] = Eigen::MatrixXd::Identity(nx,nx) + dt * Ac[i];
}
for (int i = 0; i < mpc_N; i++)Aqp.block<nx, nx>(i * nx, 0) = Eigen::MatrixXd::Identity(nx,nx);
for (int i = 0; i < mpc_N; i++)for (int j = 0; j < i + 1; j++)Aqp.block<nx, nx>(i * nx, 0) = A[j] * Aqp.block<nx, nx>(i * nx, 0);for (int i = 0; i < mpc_N; i++)for (int j = 0; j < i + 1; j++)Aqp1.block<nx, nx>(i * nx, j * nx) = Eigen::MatrixXd::Identity(nx,nx);
for (int i = 1; i < mpc_N; i++)for (int j = 0; j < i; j++)for (int k = j + 1; k < (i + 1); k++)Aqp1.block<nx, nx>(i * nx, j * nx) = A[k] * Aqp1.block<nx, nx>(i * nx, j * nx);

2. 这代代码，Bqp计算了B的迭代矩阵，注意与上图\(u_i\)不同的是，该程序的u为：\([m_0, f_0, m_1, f_1]\),先左腿；

另外，姿态和位置只用了速度积分，并没有用到加速度积分
这里注意Bqp1到B_tmp的转换？
这里给我的感觉是MPC主要跟踪状态，输入不需要跟踪，所以跟踪10步的状态，输入u产生3步的预测，最终也只会用到第一个u，这么处理减小矩阵运算？？？

for (int i = 0; i < mpc_N; i++)Bqp1.block<nx, nu>(i * nx, i * nu) = B[i];
Eigen::MatrixXd Bqp11 = Eigen::MatrixXd::Zero(nu * mpc_N, nu * ch);
Bqp11.setZero();
Bqp11.block<nu * ch, nu * ch>(0, 0) = Eigen::MatrixXd::Identity(nu * ch, nu * ch);
for (int i = 0; i < (mpc_N - ch); i++)Bqp11.block<nu, nu>(nu * ch + i * nu, nu * (ch - 1)) = Eigen::MatrixXd::Identity(nu, nu);Eigen::MatrixXd B_tmp = Eigen::MatrixXd::Zero(nx * mpc_N, nu * ch);
B_tmp = Bqp1 * Bqp11;
Bqp = Aqp1 * B_tmp;

3. 这段代码，根据不同的摆动腿，增加重力加速度,先左腿后右腿

Eigen::Matrix<double, nu*ch, 1>		delta_U;
delta_U.setZero();
for (int i = 0; i < ch; i++){
if (legState[i] == DataBus::LSt)
delta_U(nu*i + 2) = m*g;
else if (legState[i] == DataBus::RSt)delta_U(nu*i + 8) = m*g;
else{delta_U(nu*i + 2) = 0.5*m*g;delta_U(nu*i + 8) = 0.5*m*g;}
}

4. 构建QP的矩阵：

H = 2 * (Bqp.transpose() * L * Bqp + alpha * K) + 1e-10*Eigen::MatrixXd::Identity(nx*mpc_N, nx*mpc_N);
c = 2 * Bqp.transpose() * L * (Aqp * X_cur - Xd) + 2 * alpha * K * delta_U;

5. 摩擦锥约束：


这里跟公式略有出入，但大差不差，约束的效果是产生的力尽量保持垂直

//friction constraint
Eigen::Matrix<double, ncfr_single, 3> Asfr111, Asfr11;
Eigen::Matrix<double, ncfr, nu> Asfr1;
Eigen::Matrix<double, ncfr * ch, nu * ch> Asfr;
Asfr111.setZero();
Asfr1.setZero();
Asfr.setZero();
Asfr111 <<-1.0, 0.0, -1.0 / sqrt(2.0) * miu,1.0, 0.0, -1.0 / sqrt(2.0) * miu,0.0, -1.0, -1.0 / sqrt(2.0) * miu,0.0, 1.0, -1.0 / sqrt(2.0) * miu;
Asfr11 = Asfr111 * R_w2f;
Asfr1.block<ncfr_single, 3>(0, 0) = Asfr11;
Asfr1.block<ncfr_single, 3>(ncfr_single, 6) = Asfr11;
for (int i = 0; i < ch; i++)Asfr.block<ncfr, nu>(ncfr * i, i * nu) = Asfr1;

Asfr是24*39的矩阵。最小值：-1e7，最大值根据双腿状态，哪个腿站立，哪个腿为0,双腿都站，都为0。意思是那条腿站立，那条腿进行约束

6. 水平方向力矩等效点不要超过足底多边形，ZMP？：

力矩等效点：\(M_xy = r_{xy} \times f_{xy} <= 足底多边形\)

//moment constraint x ydouble sign_xy[4]{1.0, -1.0, -1.0, 1.0};Eigen::Matrix<double, 3, 1> gxyz[4];gxyz[0] << 0.0, 1.0, 0.0;gxyz[1] << 0.0, 1.0, 0.0;gxyz[2] << 1.0, 0.0, 0.0;gxyz[3] << 1.0, 0.0, 0.0;Eigen::Matrix<double, 3, 1> r[4];Eigen::Matrix<double, 3, 1> p[4];Eigen::Matrix<double, ncstxya, 6> Astxy_r[4];Eigen::Matrix<double, ncstxy_single, 6> Astxy11;Eigen::Matrix<double, ncstxy, nu> Astxy1;Eigen::Matrix<double, ncstxy * ch, nu * ch> Astxy;Astxy_r[0].setZero();Astxy_r[1].setZero();Astxy_r[2].setZero();Astxy_r[3].setZero();Astxy11.setZero();Astxy1.setZero();Astxy.setZero();r[0] << 0.0, 1.0, 0.0;r[1] << 0.0, 1.0, 0.0;r[2] << 1.0, 0.0, 0.0;r[3] << 1.0, 0.0, 0.0;p[0] << delta_foot[0], 0.0, 0.0;p[1] << -delta_foot[1], 0.0, 0.0;p[2] << 0.0, delta_foot[2], 0.0;p[3] << 0.0, -delta_foot[3], 0.0;for (int i = 0; i < 4; i++) {Astxy_r[i].block<1, 3>(0, 0) =sign_xy[i] * gxyz[i].transpose() * R_w2f * R_f2w * r[i] * (R_f2w * r[i]).transpose() *CrossProduct_A(R_f2w * p[i]);Astxy_r[i].block<1, 3>(0, 3) = sign_xy[i] * gxyz[i].transpose() * R_w2f;Astxy11.block<ncstxya, 6>(i * ncstxya, 0) = Astxy_r[i];}Astxy1.block<ncstxy_single, 6>(0, 0) = Astxy11;Astxy1.block<ncstxy_single, 6>(ncstxy_single, 6) = Astxy11;for (int i = 0; i < ch; i++)Astxy.block<ncstxy, nu>(ncstxy * i, nu * i) = Astxy1;

7. 垂直方向力矩等效点不要超过足底多边形，ZMP？

循环计算每个腿4个接触点与Z方向相关的力矩

for (int i = 0; i < 4; i++) {Astz_r[i].block<1, 3>(0, 0) =  -sqrt(p[i](0) * p[i](0) + p[i](1) * p[i](1) + p[i](2) * p[i](2)) * miu *Eigen::Matrix<double, 1, 3>(0.0, 0.0, 1.0) * R_w2f;Astz_r[i].block<1, 3>(0, 3) = Eigen::Matrix<double, 1, 3>(0.0, 0.0, 1.0) * R_w2f;Astz_r[i].block<1, 3>(1, 0) = Astz_r[i].block<1, 3>(0, 0);Astz_r[i].block<1, 3>(1, 3) = -1*Astz_r[i].block<1, 3>(0, 3);Astz11.block<ncstza, 6>(i * ncstza, 0) = Astz_r[i];}Astz1.block<ncstz_single, 6>(0, 0) = Astz11;Astz1.block<ncstz_single, 6>(ncstz_single, 6) = Astz11;for (int i = 0; i < ch; i++)Astz.block<ncstz, nu>(ncstz * i, nu * i) = Astz1;

8. QP初始化：

res = QP.init(qp_H, qp_c, qp_As, qp_lu, qp_uu, qp_lbA, qp_ubA, nWSR, &cpu_time, xOpt_iniGuess);

qp_H,qp_c:QP目标函数
qp_As：约束矩阵
qp_lu, qp_uu：变量上下界，lu <= x <= uu
qp_lbA, qp_ubA: 约束边界，lbA <= A * x <= ubA; 如果lbA和ubA相等，就是等式约束
nWSR: 输入工作集重新计算次数，输出实际
cpu_time：输入允许QP初始化的最大时间，输出实际时间
xOpt_iniGuess：初值猜想

9. 结果计算：

dX_cal计算\(\dot{x} = Ax + Bu\)
delta_X根据\(\dot{x}\)进行加速度积分，得到这个控制周期的状态增量
X_cal根据离散状态方程，计算最优控制下，当前的状态。
Ufe_pre是最优控制的第一个点,也会用到WBC中

dX_cal = Ac[0] * X_cur + Bc[0] * Ufe.block<nu,1>(0,0);Eigen::Matrix<double, nx, 1>    delta_X;delta_X.setZero();for (int i = 0; i < 3; i++){delta_X(i) = 0.5*dX_cal(i+6)*dt*dt;delta_X(i+3) = 0.5*dX_cal(i+9)*dt*dt;delta_X(i+6) = dX_cal(i+6)*dt;delta_X(i+9) = dX_cal(i+9)*dt;}X_cal = (Aqp * X_cur + Bqp * Ufe).block<nx,1>(nx*0,0) + delta_X;Ufe_pre = Ufe.block<nu, 1>(0, 0);

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.ryyt.cn/news/64945.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈，一经查实，立即删除！

相关文章

网络安全C10-2024.9.21-burpsuite安装使用过程

网络安全C10-2024.9.21-burpsuite安装使用过程

1、安装burp，分别在本机上实现全局代理和局部代理，提供设置过程的说明文档； 确认burpsuite监听地址和端口：全局代理：全局上网生效，设备--->网络和Internet--->开启“使用代理服务器” 局部代理：仅浏览器生效，使用firefox浏览设置2、利用burp实现对https站点的抓包…
阅读更多...
openwrt释放DHCP地址池命令

openwrt释放DHCP地址池命令

ssh连上openwrt后台 vim打开/tmp/dhcp.leases文件 删除你想要释放的ip那一行保存退出 然后执行/etc/init.d/dnsmasq restart 就可以释放掉这个ip地址将其分配给其它设备了
阅读更多...
一次基于AST的大规模代码迁移实践

一次基于AST的大规模代码迁移实践

在研发项目过程中，我们经常会遇到技术架构迭代更新的需求，通过技术的迭代更新，让项目从新的技术特性中受益，但由于很多新的技术迭代版本并不能完全向下兼容，包含了很多非兼容性的改变（Breaking Changes），因此我们需要设计一款工具，帮助我们完成大规模代码自动迁移问题…
阅读更多...
Leetcode 706. 设计哈希映射

Leetcode 706. 设计哈希映射

1.题目基本信息 1.1.题目描述 不使用任何内建的哈希表库设计一个哈希映射（HashMap）。 实现 MyHashMap 类：MyHashMap() 用空映射初始化对象 void put(int key, int value) 向 HashMap 插入一个键值对 (key, value) 。如果 key 已经存在于映射中，则更新其对应的值 value 。 i…
阅读更多...
易基因：eLife：早期中度产前酒精暴露(PAE)和母体饮食介导子代DNA甲基化变化 | WGBS研究

易基因：eLife：早期中度产前酒精暴露(PAE)和母体饮食介导子代DNA甲基化变化 | WGBS研究

大家好，这里是专注表观组学十余年，领跑多组学科研服务的易基因。 妊娠期间饮酒是导致儿童神经发育障碍的主要原因之一，高剂量饮酒可导致儿童出现认知、行为和神经发育障碍，但目前对于低至中等饮酒水平的影响尚不完全清楚。DNA甲基化是一种表观遗传机制，涉及在碳-1代谢过程…
阅读更多...
实战教程！Zabbix 监控 Spark 中间件配置教程

实战教程！Zabbix 监控 Spark 中间件配置教程

本文将介绍以JMX方式监控Spark中间件。JMX具有跨平台、灵活性强、监控能力强、易于集成与扩展、图形化界面支持以及安全性与可配置性等多方面的优势，是监控Spark等复杂Java应用程序的重要工具之一。 Apache Spark 是一个开源的大数据处理框架，它提供了快速、通用和可扩展的数…
阅读更多...
Linux单机最大并发到底是多少？

Linux单机最大并发到底是多少？

Linux单机最大并发到底是多少？ - 知乎 (zhihu.com) 所谓C10K就是单机1w并发问题，其中IO复用epoll/kqueue/iocp等技术对于C10k问题的解决起到了非常重要的作用。 所谓C10W就是单机1000w并发问题，未来待解决 ========================================== 五元组数 一个五元组可…
阅读更多...
PyG的安装

PyG的安装

PyG的安装 很早就想了解一下图神经网络，终于有时间学习一下了，下面记录一下安装 PyG 的过程。 PyG GitHub官网地址：GitHub - pyg-team/pytorch_geometric: Graph Neural Network Library for PyTorch 这个官网我觉得很好的一点是他一直在更新，而且基本上所有的图神经网络模…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023

友情链接： 我的编程经验分享 一条长河网 尧图网站开发 尧图建网站 尧图建网站 广利置业