四足机器人优化方法初探:快速移植MIT Cheetha MPC控制代码

1背景

MIT猎豹在2019年末开源,提供了一个基于MPC的线性化模型预测控制框架,其核心是将四足机器人简化为单刚体后构建一个多点支撑的力平衡模型,并通过MPC的控制理论在线性化假设下采用QP优化来求解,实现在低成本嵌入式处理器上实时运算。基于MPC框架大大简化了四足机器人步态状态机的设计,在MIT原始步态规划中仅采用时间开环相序就可以实现不同的步态,而传统的方法则需要设计完善的状态机进行切换,另外由于MPC是以实现对轨迹跟踪为目标,因此可以考虑未来一段时间内系统支撑情况的变换提前控制,这样处理机器人腾空相有非常大的好处,而传统的方法例如我之前复现Cheetha3的Bound需要人工规划前馈力矩其调参困难而且严重依赖步态相序周期切换的稳定性

在2020年MIT又进一步增加了WBC实现了更快速度、更流畅优化的四足机器人奔跑,并且开始构建一个双足机器人工程的前期预告虽然不知道还会不会开源让国人抄一波。同期国内外基于该框架除了孵化很多创业公司外还对原始MIT开源代码中不涉及的地形处理、非线性建模以及更多约束进行改进优化。

虽然我在19年算国内比较早一批开始尝试运行MIT开源仿真的人,但实际上很长一段时间并没有更深入的研究,而是基于之前MOCO系列机器人的框架自己进行梳理VMC以及力控阻抗理论,并搭建自己的软硬件、操作系统以及开发仿真环境,同时由于一直以C为主要开发语言,对C++下工程搭建的项目涉及很少代码阅读比较费力,而MIT本身的代码工程管理有自己的体系里面涉及很多如LCM、QP优化库之前都没有涉及过。不过在2021年末圣诞节终于在自己的框架中完成了移植也算跟上了时代的步伐,当然这里面还是非常感谢于宪元、廖洽源、晚月君、bigban大哥等同志乐意分享,解答很多低级的疑惑,为大家提供很多免费的资料,可以算是真正推动国内机器人运动控制技术发展,能让更多人接触了解并快速入门。

2 MPC相关资料

目前国内设计MIT这套框架的相关资料已经很多,两年前刚接触他并作为课题的研究生也已经毕业,很多完整解析整套控制原理的学位论文也能查到,因此目前要了解MPC这套理论已经不是很难,而且MIT开源的代码本身也只是这套框架中的冰山一角,追溯BostonDynamics和ETH团队,目前基于非线性MPC、深度学习结合MPC的方案才是Altas或Anymal这样成熟机器人系统所采用的,这里给出了我收藏的比较好的介绍MIT控制框架的相关帖子:

其中盒子君的铁蛋与MIT代码解析部分详细的从底层硬件到上层软件算法说明了MIT整个控制算法的编程,通过结合其他资料,如于宪元或其他人相关学位论文对公式的分析,能很快理解MPC控制的基本框架,上述论文可以加入舵狗之家QQ群进行下载。

3 MPC和VMC的区别理解

在MOCO-8中采用了MIT早期基于虚拟模型VMC+QP分解(Balance Controller)的方案(早期由于四足机器人相关资料较少,在MPC开源后我一直想试试难道VMC就不能获得一个不错的控制效果吗?现在的答案是凑合吧),通过在实物上验证已经证明对于典型的Trot步态或站立步态该框架能获得较好的控制效果,并且对算力的要求也不高,之前也将其部署于树莓派4B上实现500Hz的伺服力控。对于Trot这样周期切换的步态实际上VMC和MPC的控制区别不大,特别是当VMC能运行在500Hz,而MPC仅能运行在30~50Hz,虽然MPC能实现预测,但对于Trot非扰动下通过微分阻尼也能具有一定的提前控制效果,但在特殊情况下MPC还是能实现VMC没法达到的性能。

首先是受扰动情况,VMC在Trot步态周期切换中实际可以做到每时每刻都至少有两条对角腿支撑的情况,而此时如对于12自由度机器人来说,其6维姿态与位置是完全受控制了,而当受扰后出现四腿腾空VMC反馈部分无法产生有效控制,如腾空时间太长在重新支撑后的冲击过大造成控制超调反而会造成机器人摔倒;MPC由于对未来一段时间内轨迹的跟踪,对于上面出现四腿腾空或其他欠驱动情况,能通过对未来支撑情况的预测修正来产生提前的控制量,从而让机器人更好地进行稳定,以MIT为例其仅建立了单刚体模型基于MPC就能较好地实现Bound、Pace、Pronk等具有腾空相的步态,在下面的视频中给出了两个例子能很好地对比MPC和VMC的区别,这里首先给出IIT的一个视频:

视频中对比了在采用视觉落足通过复杂地形下两算法的性能,VMC即使在补偿了重力后任然没法稳定跨越障碍,而基于MPC的框架能非常好滴跟踪期望轨迹,在上图中值得注意的还有VMC框架中质心仅是一个圆点,即每时每刻仅实现单刚体多支撑点静力学平衡的反馈控制,而MPC图中质心处存在期望轨迹。对于为何MPC在非平坦地面下具有更好的性能一个原因如下,我采用自己构建的机器人进行了越障仿真,视频中机器人在跨越障碍时会选择落足点不踏入空隙中

https://video.zhihu.com/video/1458451404386365440?player=%7B%22autoplay%22%3Afalse%2C%22shouldShowPageFullScreenButton%22%3Atrue%7D

VMC视觉越障

https://video.zhihu.com/video/1458451423311040512?player=%7B%22autoplay%22%3Afalse%2C%22shouldShowPageFullScreenButton%22%3Atrue%7DMPC视觉越障

视频中可以看到VMC方法在跨越第一个空隙后,虽然落足位置保证但是机器人出现了后倾导致重新踏入空隙中,而MPC速度运动均匀这也保证了视觉落足时的精确。对于前者首先为了跨越间隙,落足点位置必然不是SLIP模型计算出静加速度为0的稳定位置,这造成了在落足后对机器人产生了后向速度的扰动,同时落足点的前移导致质心靠后,也同时导致俯仰轴没法控住,最终耦合在一起导致机器人失稳踏入间隙;MPC由于预测时域覆盖了整个步态周期,考虑了摆动过程中的质心轨迹跟踪控制,从而不会在落足点偏离SLIP位置后发生很大的扰动,但是MPC毕竟控制频率还是比较低,如10个预测周期为例在原始代码中仅能保证支撑阶段产生10次不同的控制量,这也造成了其期望足端力是一段一段的平台(对于动态加减速不够平衡,力的不连续最终反映到执行器加速度不连续,对能耗和执行器寿命带来影响),而在加入WBC后其对上述基本力质量进行跟踪,进一步结合动力学产生0.5Khz的力控指令,从而相比原始MPC具有更好的动态性能。

可以看到采用MPC控制各个环节的闭环性能都很不错,其产生的足端力是一段一段的也是每个预测窗口时当前的最优控制量,特别是在越障中出现的扰动能快速地完成调节,但由于我未增加WBC使得机器人动态性能和控制精度还有很大的优化空间。

4 MPC的简单移植与测试

对于MIT原始的代码由于其采用C++类编程,而且继承了很多机器人的基类导致一开始移植起来感觉非常麻烦,但通过仔细阅读并和论文对比对于仅考虑支撑控制的MPC其仅仅需要如下的几个文件既可以,当然原始MIT由于将步态规划gait类深度融合在MPC步态代码中导致其看起来很混乱,我在原始VMC框架中支撑控制与摆动规划是独立的因此在此直接将MIT原始摆动部分完全删除,仅保留一些简单的接口和标志位即可:

(1)Robotstate

该函数去除了C++类,直接定义的机器人需要的状态变量

#pragma once
#include <Eigen/Dense>
#include "common_types.h"
#define MPC_PREDIC_LEN 10
#define FAST_MPC        1 //开启后MPC_PREDIC_LEN无法上10
#define EN_X_DRAG 1
using Eigen::Matrix;
using Eigen::Quaternionf;

	void RobotState_set(flt* p, flt* v, flt* q, flt* w, flt* r, flt yaw);
	void RobotState_print();

	typedef struct {
		Matrix<fpt, 3, 1> p, v, w;
		Matrix<fpt, 3, 4> r_feet;
		Matrix<fpt, 3, 3> R;
		Matrix<fpt, 3, 3> R_yaw;
		Matrix<fpt, 3, 3> I_body;
		Quaternionf q;
		fpt yaw;
		fpt m = 6;
	}RobotState;

	extern  RobotState robot_rs;

在原始文件中仅有机器人的质量和刚体转动惯量是需要配置的参数:

#include "RobotState.h"
#include <Eigen/Dense>
#include <iostream>
#include <math.h>
#include "base_struct.h"
using std::cout;
using std::endl;
RobotState robot_rs;
void RobotState_set(flt* p_, flt* v_, flt* q_, flt* w_, flt* r_,flt yaw_)
{
    for(u8 i = 0; i < 3; i++)
    {
		robot_rs.p(i) = p_[i];
		robot_rs.v(i) = v_[i];
		robot_rs.w(i) = w_[i];
    }
	robot_rs.q.w() = q_[0];
	robot_rs.q.x() = q_[1];
	robot_rs.q.y() = q_[2];
	robot_rs.q.z() = q_[3];
	robot_rs.yaw = yaw_;

    //for(u8 i = 0; i < 12; i++)
    //    this->r_feet(i) = r[i];
    for(u8 rs = 0; rs < 3; rs++)
        for(u8 c = 0; c < 4; c++)
			robot_rs.r_feet(rs,c) = r_[rs*4 + c];

	robot_rs.R = robot_rs.q.toRotationMatrix();

	yaw_ = 0;
    fpt yc = cos(yaw_);
    fpt ys = sin(yaw_);


	robot_rs.R_yaw <<  yc,  -ys,   0,//线性化旋转航向矩阵
             ys,  yc,   0,
               0,   0,   1;

    Matrix<fpt,3,1> Id;
	robot_rs.m = Mw;
    Id << .15f, 0.22f, 0.15f;//转动惯量  X Y Z
	robot_rs.I_body.diagonal() = Id;

    //TODO: Consider normalizing quaternion??
}

void RobotState_print()
{
   cout<<"Robot State:"<<endl<<"Position\n"<< robot_rs.p.transpose()
       <<"\nVelocity\n"<< robot_rs.v.transpose()<<"\nAngular Veloctiy\n"
       << robot_rs.w.transpose()<<"\nRotation\n"<< robot_rs.R<<"\nYaw Rotation\n"
       << robot_rs.R_yaw<<"\nFoot Locations\n"<< robot_rs.r_feet<<"\nInertia\n"<< robot_rs.I_body<<endl;
}

(2)mpc_locomotion

在原始locomotion函数中函数接口需要机器人的基类,而其中的机器人期望质量、状态估计、腿部控制器都是从很多其他文件继承过来的导致最终简单的MPC接口需要包含数十个没用的头文件,这不但对编译造成影响,而对代码阅读和理解也造成影响,实际上MPC程序中仅仅需要机器人的状态反馈和期望轨迹就行,这里直接用了C的方式去除类定义结构体和全局变量

#pragma once
//#include "cppTypes.h"
#include "common_types.h"
using Eigen::Array4f;
using Eigen::Array4i;
using Eigen::Matrix;
using Eigen::Quaternionf;
#define T_MPC 0.003
#define MPC_USE_REAL_TD 1 //采用真实着地

typedef struct {
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	Vec4<float> contactEstimate;//接触估计
	Vec3<float> position;//位置
	Vec3<float> vBody;//速度
	Quat<float> orientation;//四元数
	Vec3<float> omegaBody;//角速度
	RotMat<float> rBody;//旋转矩阵
	Vec3<float> rpy;//欧拉角

	Vec3<float> omegaWorld;//世界坐标角速度
	Vec3<float> vWorld;//速度
	Vec3<float> aBody, aWorld;//加速度,世界坐标下加速度
}StateEstimate;


typedef struct  {
	// Instantaneous desired state command 瞬时期望状态指令
	Vec12<float> stateDes;

	Vec12<float> pre_stateDes;

	// Desired future state trajectory (for up to 10 timestep MPC) 期望的未来状态轨迹(最多10个时间步长MPC)
	Eigen::Matrix<float, 12, 30> stateTrajDes;
}DesiredStateData;

typedef struct {
 
}ControlFSMData1;

extern  ControlFSMData1 mpc_data;

在实际的程序中首先把MPC计算中需要的基本函数从其矩阵和四元数运算代码中摘取出来,所有原始类的成员均定义为全局变量:

#include "cppTypes.h"
#include "convexMPC_interface.h"
#include "SolverMPC.h"
#include "common_types.h"
#include "convexMPC_interface.h"
#include "RobotState.h"
#include "mpc_locomotion.h"
#include "base_struct.h"
#include "gait_math.h"
ControlFSMData1 mpc_data;

int iterationsBetweenMPC_default = 15;//MPC运行周期 2*15=30ms  *horizonLength(10)=0.3s->100Hz
int iterationsBetweenMPC = 15;//MPC运行周期 2*15=30ms  *horizonLength(10)=0.3s->100Hz
int horizonLength= MPC_PREDIC_LEN;//预测窗口
float cmpc_x_drag = 3;//拖拽参数
float ki_mpc[2] = { 1.0,2 };//Roll Pit积分系数
float dtMPC;
int iterationCounter = 0;
Vec3<float> f_ff[4];
Vec3<float> f_ff_n[4];
Vec3<float> f_ff_n_list[4][MPC_PREDIC_LEN];
Vec4<float> swingTimes;
Mat3<float> Kp, Kd, Kp_stance, Kd_stance;
bool firstRun = true;
bool firstSwing[4];
float swingTimeRemaining[4];
float stand_traj[6];
int current_gait=IDLE;//4 STAND
int gaitNumber;

Vec3<float> world_position_desired;
Vec3<float> rpy_int;
Vec3<float> rpy_comp;
Vec3<float> pFoot[4];

float x_comp_integral = 0;
float trajAll[12 * 36];
int _nMPC_segments;
int f_list_cnt = 0;
float f_list_cnt_timer = 0;
int* _mpc_table;
//trotting(horizonLength, Vec4<int>(0, 5, 5, 0), Vec4<int>(5, 5, 5, 5), "Trotting"),
//bounding(horizonLength, Vec4<int>(5, 5, 0, 0), Vec4<int>(5, 5, 5, 5), "Bounding"),
//pronking(horizonLength, Vec4<int>(0, 0, 0, 0), Vec4<int>(4, 4, 4, 4), "Pronking"),
//galloping(horizonLength, Vec4<int>(0, 2, 7, 9), Vec4<int>(6, 6, 6, 6), "Galloping"),
//standing(horizonLength, Vec4<int>(0, 0, 0, 0), Vec4<int>(10, 10, 10, 10), "Standing"),
//trotRunning(horizonLength, Vec4<int>(0, 5, 5, 0), Vec4<int>(3, 3, 3, 3), "Trot Running"),
//walking(horizonLength, Vec4<int>(0, 3, 5, 8), Vec4<int>(5, 5, 5, 5), "Walking"),
//walking2(horizonLength, Vec4<int>(0, 5, 5, 0), Vec4<int>(7, 7, 7, 7), "Walking2"),
//pacing(horizonLength, Vec4<int>(5, 0, 5, 0), Vec4<int>(5, 5, 5, 5), "Pacing")
Array4i _offsets= Vec4<int>(0, 0, 0, 0); // offset in mpc segments
Array4i _durations= Vec4<int>(100, 100, 100, 100); // duration of step in mpc segments
Array4f _offsetsFloat = Vec4<float>(0, 0, 0, 0); // offsets in phase (0 to 1)
Array4f _durationsFloat= Vec4<float>(100, 100, 100, 100); // durations in phase (0 to 1)

int _iteration;
int _nIterations = 0;//
float _phase;

void ConvexMPCLocomotion_updateMPCIfNeeded(int *mpcTable, bool omniMode,float dt);
StateEstimate seResult;
DesiredStateData stateCommand;

void setIterations(int iterationsPerMPC, int currentIteration)
{
	_nIterations = horizonLength;//
	_iteration = (currentIteration / iterationsPerMPC) % _nIterations;
	_phase = (float)(currentIteration % (iterationsPerMPC * _nIterations)) / (float)(iterationsPerMPC * _nIterations);
}

enum class CoordinateAxis { X, Y, Z };
/*!
 * Convert radians to degrees 转化弧度到度数
 */

float rad2deg(float rad) {

	return rad * float(180) / float(3.1415926);
}

/*!
 * Convert degrees to radians
 */

float deg2rad(float deg) {

	return deg * float(3.1415926) / float(180);
}

Mat3<float> coordinateRotation(CoordinateAxis axis, float theta) {

	float s = std::sin(theta);
	float c = std::cos(theta);

	Mat3<float> R1;

	if (axis == CoordinateAxis::X) {
		R1 << 1, 0, 0, 0, c, s, 0, -s, c;
	}
	else if (axis == CoordinateAxis::Y) {
		R1 << c, 0, -s, 0, 1, 0, s, 0, c;
	}
	else if (axis == CoordinateAxis::Z) {
		R1 << c, s, 0, -s, c, 0, 0, 0, 1;
	}

	return R1;
}

Mat3<float> rpyToRotMat(const Vec3<float> v) {
	Mat3<float> m = coordinateRotation(CoordinateAxis::X, v[0]) *
		coordinateRotation(CoordinateAxis::Y, v[1]) *
		coordinateRotation(CoordinateAxis::Z, v[2]);
	return m;
}

Quat<float> rotationMatrixToQuaternion(
	Mat3<float> r1) {
	Quat<float> q;
	Mat3<float> r = r1.transpose();
	float  tr = r.trace();
	if (tr > 0.0) {
		float  S = sqrt(tr + 1.0) * 2.0;
		q(0) = 0.25 * S;
		q(1) = (r(2, 1) - r(1, 2)) / S;
		q(2) = (r(0, 2) - r(2, 0)) / S;
		q(3) = (r(1, 0) - r(0, 1)) / S;
	}
	else if ((r(0, 0) > r(1, 1)) && (r(0, 0) > r(2, 2))) {
		float  S = sqrt(1.0 + r(0, 0) - r(1, 1) - r(2, 2)) * 2.0;
		q(0) = (r(2, 1) - r(1, 2)) / S;
		q(1) = 0.25 * S;
		q(2) = (r(0, 1) + r(1, 0)) / S;
		q(3) = (r(0, 2) + r(2, 0)) / S;
	}
	else if (r(1, 1) > r(2, 2)) {
		float  S = sqrt(1.0 + r(1, 1) - r(0, 0) - r(2, 2)) * 2.0;
		q(0) = (r(0, 2) - r(2, 0)) / S;
		q(1) = (r(0, 1) + r(1, 0)) / S;
		q(2) = 0.25 * S;
		q(3) = (r(1, 2) + r(2, 1)) / S;
	}
	else {
		float  S = sqrt(1.0 + r(2, 2) - r(0, 0) - r(1, 1)) * 2.0;
		q(0) = (r(1, 0) - r(0, 1)) / S;
		q(1) = (r(0, 2) + r(2, 0)) / S;
		q(2) = (r(1, 2) + r(2, 1)) / S;
		q(3) = 0.25 * S;
	}
	return q;
}

Quat<float> rpyToQuat(const Vec3<float> rpy) {
 
Mat3<float> R1 = rpyToRotMat(rpy);
Quat<float> q = rotationMatrixToQuaternion(R1);
return q;
}

int* mpc_gait()
{
	for (int i = 0; i < _nIterations; i++)
	{
		int iter = (i + _iteration + 1) % _nIterations;
		Array4i progress = iter - _offsets;
		for (int j = 0; j < 4; j++)
		{
			if (progress[j] < 0) progress[j] += _nIterations;

			if (vmc_all.gait_mode == STAND_RC || vmc_all.gait_mode == TROT) {
			
				if (seResult.contactEstimate[j] > 0.5) {
					_mpc_table[i * 4 + j] = 1;//着地
					//printf("sg\n");
				}
				else {
					_mpc_table[i * 4 + j] = 0;
					//printf("slf\n");
				}
			}
			else {

				if (progress[j] < _durations[j])
					_mpc_table[i * 4 + j] = 1;//着地
				else
					_mpc_table[i * 4 + j] = 0;
			}
		}
	}

	return _mpc_table;
}

定义两个接口函数,将自己的传感器数据传入原始类继承的状态估计器seResult结构体和期望指令stateCommand中:

void convert_mpc_data_in(void)//转换我的数据到MPC MIT
{
	int id_swap[4] = { 0,2,1,3 };//Moco定义顺序
	double FALL[3], TALL[3], Fp[2], PC[3], P1[3], P2[3], P3[3], P4[3];
	char G[4];
	double F1c[3], F2c[3], F3c[3], F4c[3];

	FALL[Xrw] = robotwb.exp_force.x;//前
	FALL[Yrw] = robotwb.exp_force.y;
	FALL[Zrw] = robotwb.exp_force.z;//高度
	TALL[Xrw] = -robotwb.exp_torque.x;//ROL
	TALL[Yrw] = robotwb.exp_torque.y;//PIT
	TALL[Zrw] = robotwb.exp_torque.z * -1;

	G[0] = robotwb.Leg[id_swap[0]].is_ground;
	G[1] = robotwb.Leg[id_swap[1]].is_ground;
	G[2] = robotwb.Leg[id_swap[2]].is_ground;
	G[3] = robotwb.Leg[id_swap[3]].is_ground;
	seResult.contactEstimate[0] = G[0];
	seResult.contactEstimate[1] = G[1];
	seResult.contactEstimate[2] = G[2];
	seResult.contactEstimate[3] = G[3];
#if 0
	G[0] = G[1] = G[2] = G[3] = 1;
#endif

	//世界系下的足端位置
	pFoot[0][Xrw] = (robotwb.Leg[id_swap[0]].epos_n.x)+ vmc_all.pos_n.x;
	pFoot[0][Yrw] = (-robotwb.Leg[id_swap[0]].epos_n.y) - vmc_all.pos_n.y;
	pFoot[0][Zrw] = (robotwb.Leg[id_swap[0]].epos_n.z) + vmc_all.pos_n.z;

	pFoot[1][Xrw] = (robotwb.Leg[id_swap[1]].epos_n.x) + vmc_all.pos_n.x;
	pFoot[1][Yrw] = (-robotwb.Leg[id_swap[1]].epos_n.y)- vmc_all.pos_n.y;
	pFoot[1][Zrw] = ( robotwb.Leg[id_swap[1]].epos_n.z) + vmc_all.pos_n.z;

	pFoot[2][Xrw] = (robotwb.Leg[id_swap[2]].epos_n.x) + vmc_all.pos_n.x;
	pFoot[2][Yrw] = (-robotwb.Leg[id_swap[2]].epos_n.y) - vmc_all.pos_n.y;
	pFoot[2][Zrw] = (robotwb.Leg[id_swap[2]].epos_n.z) + vmc_all.pos_n.z;

	pFoot[3][Xrw] = (robotwb.Leg[id_swap[3]].epos_n.x) + vmc_all.pos_n.x;
	pFoot[3][Yrw] = (-robotwb.Leg[id_swap[3]].epos_n.y) - vmc_all.pos_n.y;
	pFoot[3][Zrw] = (robotwb.Leg[id_swap[3]].epos_n.z) + vmc_all.pos_n.z;
	//printf("%f %f %f\n", pFoot[0][Xrw], pFoot[0][Yrw], pFoot[0][Zrw]);

	//Vec4<float> contactEstimate;//接触估计
	//Vec3<float> position;//位置
	//Vec3<float> vBody;//速度
	//Quat<float> orientation;//四元数
	//Vec3<float> omegaBody;//角速度
	//RotMat<float> rBody;//旋转矩阵
	//Vec3<float> rpy;//欧拉角

	//Vec3<float> omegaWorld;//世界坐标角速度
	//Vec3<float> vWorld;//速度
	//Vec3<float> aBody, aWorld;//加速度,世界坐标下加速度
	//float* p = seResult.position.data();
	//float* v = seResult.vWorld.data();
	//float* w = seResult.omegaWorld.data();
	//float* q = seResult.orientation.data();
	seResult.position[0] = vmc_all.pos_n.x;
	seResult.position[1] = -vmc_all.pos_n.y;
	seResult.position[2] = vmc_all.pos_n.z;
	//printf("pos=%f %f %f\n", seResult.position[0], seResult.position[1], seResult.position[2]);
	seResult.vWorld[0] = vmc_all.spd_n.x;
	seResult.vWorld[1] = -vmc_all.spd_n.y;
	seResult.vWorld[2] = -vmc_all.spd_n.z;//原理向上为-我的   改为-好像不行
	//printf("spd=%f %f %f\n", seResult.vWorld[0], seResult.vWorld[1], seResult.vWorld[2]);
	seResult.omegaWorld[0] = vmc_all.att_rate[ROLr] / 57.3;
	seResult.omegaWorld[1] = -vmc_all.att_rate[PITr] / 57.3;
	seResult.omegaWorld[2] = -vmc_all.att_rate[YAWr] / 57.3;
	seResult.omegaBody[0] = seResult.omegaWorld[0];
	seResult.omegaBody[1] = seResult.omegaWorld[1];
	seResult.omegaBody[2] = seResult.omegaWorld[2];

	seResult.rpy[0] = vmc_all.att[ROLr] / 57.3;
	seResult.rpy[1] = -vmc_all.att[PITr] / 57.3;
	seResult.rpy[2] = 0;// -vmc_all.att[YAWr] / 57.3;//我不考虑机头

	seResult.orientation = rpyToQuat(seResult.rpy);

	stateCommand.stateDes[3] = vmc_all.tar_att[ROLr] / 57.3;
	stateCommand.stateDes[4] = -vmc_all.tar_att[PITr] / 57.3;

	stand_traj[0] = robotwb.exp_pos_n.x;
	stand_traj[1] = -robotwb.exp_pos_n.y;


	stateCommand.stateDes[6] = robotwb.exp_spd_ng.x;
	stateCommand.stateDes[7] = 0;
	//printf("%f\n", stateCommand.stateDes[6]);
}

这里期望质量stateCommand[0~2]为期望全局位置[3~5]为期望姿态[6~8]为期望速度剩下为期望角速度,进一步写一个输出转换函数将MPC最优控制赋值给自己的足端力接口

void convert_mpc_data_out(float dt) {//转换MIT 到我的数据

	//MIT     1         0
	//        3         2
	//MOCO    2         0
	//        3         1
	int id_swap[4] = { 0,2,1,3 };

	robotwb.Leg[id_swap[0]].tar_force_dis_n_mpc.x = f_ff_n[0][Xrw];// *G[0];
	robotwb.Leg[id_swap[0]].tar_force_dis_n_mpc.y = -f_ff_n[0][Yrw];// * G[0];
	robotwb.Leg[id_swap[0]].tar_force_dis_n_mpc.z = f_ff_n[0][Zrw];// * G[0];

	robotwb.Leg[id_swap[1]].tar_force_dis_n_mpc.x = f_ff_n[1][Xrw];// * G[1];
	robotwb.Leg[id_swap[1]].tar_force_dis_n_mpc.y = -f_ff_n[1][Yrw];// * G[1];
	robotwb.Leg[id_swap[1]].tar_force_dis_n_mpc.z = f_ff_n[1][Zrw];//* G[1];

	robotwb.Leg[id_swap[2]].tar_force_dis_n_mpc.x = f_ff_n[2][Xrw];// * G[2];
	robotwb.Leg[id_swap[2]].tar_force_dis_n_mpc.y = -f_ff_n[2][Yrw];// * G[2];
	robotwb.Leg[id_swap[2]].tar_force_dis_n_mpc.z = f_ff_n[2][Zrw];// * G[2];

	robotwb.Leg[id_swap[3]].tar_force_dis_n_mpc.x = f_ff_n[3][Xrw];// * G[3];
	robotwb.Leg[id_swap[3]].tar_force_dis_n_mpc.y = -f_ff_n[3][Yrw];// * G[3];
	robotwb.Leg[id_swap[3]].tar_force_dis_n_mpc.z = f_ff_n[3][Zrw];//* G[3];
}

剩下就是最核心的MPC步态部分,这里已经完全将摆动与步态规划剥离,仅采用支撑标志位进行MPC控制:

void ConvexMPCLocomotion_run(float dt){//ControlFSMData1 & data) {
	static int init = 0;
	if (!init)
	{
		init = 1;
		f_list_cnt = 0;//list力预测输出
		_mpc_table = new int[horizonLength * 4];
	}
	bool omniMode = false;//万向模式
	//dt = T_MPC;

	gaitNumber = vmc_all.gait_mode;
	//计算覆盖一个步态周期下需要的迭代次数
	if (gaitNumber == STAND_RC || gaitNumber == IDLE)
		iterationsBetweenMPC = iterationsBetweenMPC_default;
	else
		iterationsBetweenMPC=(int)((vmc_all.gait_time[1]+vmc_all.delay_time[2]/2) / horizonLength / dt / 2);

	dtMPC = dt * iterationsBetweenMPC;//MPC一个窗口对应的间隔时间 2*15=30ms  同时也是MPC刷新的频率
	//printf(" iterationsBetweenMPC=%d dtMPC=%f T_g=%f\n", iterationsBetweenMPC, dtMPC, vmc_all.gait_time[0]);

	convert_mpc_data_in();//获取我的数据

	// Check if transition to standing
	if (((gaitNumber == STAND_RC) && current_gait != STAND_RC) || firstRun)
	{
		//printf("MPC::Transition to standing\n");
		stand_traj[