四足机器人优化方法初探:快速移植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[0] = seResult.position[0];
		stand_traj[1] = seResult.position[1];
		stand_traj[2] = vmc_all.tar_pos.z;
		stand_traj[3] = 0;
		stand_traj[4] = 0;
		stand_traj[5] = seResult.rpy[2];
		world_position_desired[0] = stand_traj[0];
		world_position_desired[1] = stand_traj[1];
	}

	Vec3<float> v_des_robot(stateCommand.stateDes[6], stateCommand.stateDes[7], 0);
	Vec3<float> v_des_world = v_des_robot;
	Vec3<float> v_robot = seResult.vWorld;

	//Integral-esque pitche and roll compensation
	if (fabs(v_robot[0]) > 0.2)   //avoid dividing by zero
	{
		rpy_int[1] += T_MPC*(stateCommand.stateDes[4] - vmc_all.ground_att_cmd[PITr] / 57.3 - seResult.rpy[1]) / v_robot[0];//PITr
	}
	if (fabs(v_robot[1]) > 0.1)//ROLr
	{
		rpy_int[0] += T_MPC*(stateCommand.stateDes[3]  - seResult.rpy[0]) / v_robot[1];
	}

	//Integral-esque pitche and roll compensation
   //pitche和roll 积分达到目标值 
	rpy_int[0] = fminf(fmaxf(rpy_int[0], -.35), .35);//积分
	rpy_int[1] = fminf(fmaxf(rpy_int[1], -.35), .35);
	rpy_comp[1] = v_robot[0] * rpy_int[1];
	rpy_comp[0] = v_robot[1] * rpy_int[0] * (gaitNumber != PRONK);  //turn off for pronking

	//非站定下目标位置 通过累加目标速度完成
	if (vmc_all.gait_mode!=STAND_RC){//步态模式移动
		world_position_desired += T_MPC * Vec3<float>(v_des_world[0], v_des_world[1], 0);
	}

	// some first time initialization
	if (firstRun&&seResult.position[0]!=0)//初始化期望上电
	{
		world_position_desired[0] = seResult.position[0];
		world_position_desired[1] = seResult.position[1];
		world_position_desired[2] = seResult.rpy[2];//初始化航向
		firstRun = false;
		printf("firstRun MPC\n");
	}

	// calc gait
	setIterations(iterationsBetweenMPC, iterationCounter);// ?
	iterationCounter++;//MPC控制频率计数用

	int* mpcTable = mpc_gait();//MPC 预测着地状态 从当期计算开始
	
	ConvexMPCLocomotion_updateMPCIfNeeded(mpcTable, omniMode,dt);//求解MPC问题

	convert_mpc_data_out(dt);
}

最终,计数器iterationsBetweenMPC 达到控制频率后运行一次MPC求解,更新相关矩阵进行预测:

void ConvexMPCLocomotion_updateMPCIfNeeded(int *mpcTable, bool omniMode,float dt) {

	if ((iterationCounter % iterationsBetweenMPC) == 0)//控制周期
	{
		int* mpcTable_real = mpc_gait_real();//MPC 预测着地状态 从当期计算开始
		float* p = seResult.position.data();
		float* v = seResult.vWorld.data();
		float* w = seResult.omegaWorld.data();
		float* q = seResult.orientation.data();
		float r[12];
		Vec3<float> position_temp= seResult.position;//位置
	
		for (int i = 0; i < 12; i++)
			r[i] = pFoot[i % 4][i / 4] - position_temp[i / 4];//pos 0 1 2除4来保证顺序  支撑点矢量

		float Q[12] = {
			0.25, 0.25, 10,
			2, 2, 50,
			0, 0, 0.3,
			0.2, 0.2, 0.1 };

		float Q_stand[12] = { 
			5, 8, 15, //R P Y
			5, 0, 120, //X Y Z
			0, 0, 0.3, //DR DP DY只有航向角速度
			0.5,0, 0.5 //VX VY VZ
		};//MPC权重


		float Q_trot[12] = { 
			25, 35, 3,//ROL PIT
			4, 0, 150,
			0.35, 0.25, 0.3, //增加ROLL让速度连续
			12, 0, 2 };


		float yaw = seResult.rpy[2];
		float* weights = Q;
		float alpha = 5e-5; // make setting eventually 越小力月大

		Vec3<float> v_des_robot(stateCommand.stateDes[6], stateCommand.stateDes[7], 0);
		Vec3<float> v_des_world =  v_des_robot;

		if (vmc_all.gait_mode == STAND_RC)//站立
		{
			for (int i = 0; i < 12; i++)
				Q[i] = Q_stand[i];

			float trajInitial[12] = { (float)stateCommand.stateDes[3],//roll
									 (float)stateCommand.stateDes[4]-vmc_all.ground_att_cmd[PITr]/57.3 /*-hw_i->state_estimator->se_ground_pitch*/, //pitch
									 (float)stand_traj[5]/*+(float)stateCommand->data.stateDes[11]*/,//yaw

									 (float)stand_traj[0]/*+(float)fsm->main_control_settings.p_des[0]*/,//X
									 (float)stand_traj[1]/*+(float)fsm->main_control_settings.p_des[1]*/,//Y
									 (float )vmc_all.tar_pos.z,//(float)0.26/*fsm->main_control_settings.p_des[2]*/,
									 0,0,0,0,0,0 };
			//变成mpc问题需要的格式
			for (int i = 0; i < horizonLength; i++)
				for (int j = 0; j < 12; j++)
					trajAll[12 * i + j] = trajInitial[j];
		}

		else if(vmc_all.gait_mode==TROT)//正常步态
		{

			for (int i = 0; i < 12; i++)
				Q[i] = Q_trot[i];
#if 0//不控制位置
			world_position_desired[0] = position_temp[0];
			world_position_desired[1] = position_temp[1];
#endif
			const float max_pos_error = .1;//轨迹跟踪误差
			float xStart = world_position_desired[0];
			float yStart = world_position_desired[1];

			//printf("orig \t%.3f\t%.3f\n", xStart, yStart);
			//printf("ref: \t%.3f\t%.3f\n", p[0], p[1]);
//在误差范围内,更新目标值
			if (xStart - p[0] > max_pos_error) xStart = p[0] + max_pos_error;
			if (p[0] - xStart > max_pos_error) xStart = p[0] - max_pos_error;

			if (yStart - p[1] > max_pos_error) yStart = p[1] + max_pos_error;
			if (p[1] - yStart > max_pos_error) yStart = p[1] - max_pos_error;

			world_position_desired[0] = xStart;
			world_position_desired[1] = yStart;

//机体初始参考轨迹
			float trajInitial[12] = {(float)rpy_comp[0] * ki_mpc[0],  // 0 roll
									 (float)rpy_comp[1] * ki_mpc[1] - vmc_all.ground_att_cmd[PITr] / 57.3*1/*-hw_i->state_estimator->se_ground_pitch*/,    // 1 pitch
									 (float)stateCommand.stateDes[5],    // 2 yaw
									 xStart,                                   // 3 posx
									 yStart,                                   // 4 posy
									 (float)vmc_all.tar_pos.z,//(float)0.26,      // 5		                    posz
									 0,                                        // 6
									 0,                                        // 7
									 (float)stateCommand.stateDes[11],  // 8//转向速度
									 v_des_world[0],                           // 9
									 v_des_world[1],                           // 10
									 0 };                                       // 11

			//变成mpc问题需要的格式
			//轨迹为当前时刻向后预测一个步态周期的轨迹 按匀速运动计算
			for (int i = 0; i < horizonLength; i++)
			{
				for (int j = 0; j < 12; j++)
					trajAll[12 * i + j] = trajInitial[j];
#if 1
				if (i == 0) // start at current position  TODO consider not doing this
				{
					//trajAll[3] = hw_i->state_estimator->se_pBody[0];
					//trajAll[4] = hw_i->state_estimator->se_pBody[1];
					trajAll[2] = seResult.rpy[2];
				}
				else
				{
					trajAll[12 * i + 3] = trajAll[12 * (i - 1) + 3] + dtMPC * v_des_world[0];
					trajAll[12 * i + 4] = trajAll[12 * (i - 1) + 4] + dtMPC * v_des_world[1];
					trajAll[12 * i + 2] = trajAll[12 * (i - 1) + 2] + dtMPC * stateCommand.stateDes[11];

				}
#endif
			}
		}

		Vec3<float> pxy_act(p[0], p[1], 0);
		Vec3<float> pxy_des(world_position_desired[0], world_position_desired[1], 0);
		//Vec3<float> pxy_err = pxy_act - pxy_des;
		float pz_err = p[2] - vmc_all.pos_n.z;
		Vec3<float> vxy(seResult.vWorld[0], seResult.vWorld[1], 0);


		setup_problem(dtMPC, horizonLength, vmc_robot_p.ground_mu, robotwb.max_force.z);//设置MPC参数

		update_x_drag(x_comp_integral);//z轴方向加速度受x轴方向速度的影响程度
		
		if (vxy[0] > 0.3 || vxy[0] < -0.3) {
			x_comp_integral +=  cmpc_x_drag * pz_err * dtMPC / vxy[0];
		}

		update_problem_data_floats(p, v, q, w, r, yaw, weights, trajAll, alpha, mpcTable);

		for (int leg = 0; leg < 4; leg++)
		{
			for (int axis = 0; axis < 3; axis++) {
				f_ff_n[leg][axis] = get_solution(leg * 3 + axis);//世界下的力
			}
		}

	}

}

对于剩下的SolveMPC文件没有太大修改,采用原始程序去除类即可。

5 实物部署与优化

在完成仿真后目前最大的问题任然是将MPC部署到MOCO8实际机器人中,特别是对于MPC来说目前很多人在移植中遇到的最大问题是其计算量大导致运算不过来,在代码中MPC预测一般以设定的迭代次数来决定控制周期,其在不同平台上需要的计算耗时很不一样,下面给出了我测试过的几个算是高算力的平台在10和18预测周期时的首次计算耗时(这里面排除可能存在本身代码移植有问题)运行均为MIT原始代码:

可以看到采用upboard还是目前比较稳定的一个方案,毕竟MIT也是采用这个方案,但是之前也了解到有人在树莓派4B上运行过MPC步态100Hz当然是通过裁剪与优化的)因此后续会在我采用的Odroid C4主控制器上进行测试,这里也参考了之前加速MPC运算的帖子,目前里面效果最好的还是修改对角矩阵部分,其他的部分由于描述不是很详细导致编译存在问题:

注:另外,里面采用的静态变量在预测周期超过10后会出现Eigen大矩阵编译报错,这里需要将后面矩阵resize中Eigen::No change部分注释掉:

6 MPC存在的问题

MIT采用的MPC基于线性模型,其在不采用WBC的情况下可能会出现步频较快,而提高预测窗口数能增加支撑时间降低步态频率,这样有会导致计算量大大增加,因此可以看出要实现机器人高性能控制除了采用这样的前沿控制技术外,高效的优化求解库和数学软件工具才是核心,而这才是国外团队真正的核心技术,目前国内很多优化库都是基于国外免费或开源的资源,这样在未来非常受限制,而虽然机器人自由度变多非商用的求解器计算能力大大受限,我认为这在未来必然是制约中国智能机器人技术发展的卡脖子技术

对于MIT框架中实际也包含了基于视觉的落足处理VisionMPC,但通过我对比整个代码除了在选择落足点那不一样之外求解MPC的部分基本没变,并没有像ETH或IIT的论文中通过在MPC问题中引入ZMP或地形约束来实现控制,因此其性能必然不如Anymal这样的机器人,可见MIT这样的框架实际是需要深入的优化迭代而不是仅仅移植于提高计算预测窗口来实现的,这一块目前国外已经有很多改进的论文,值得深思。

在MIT目前框架中给出了基本的线性MPC控制方案,其唯一的不足是在原生逻辑中采用的是开环步态相序,MIT 在Cheetha3论文中实际介绍了他们基于惯量检测实现真实着地下的MPC调度方法,但是没有开源,可见这一块是MPC控制非常关键的部分,我对比了在上面的例子中采用真实着地处理和不采用时,后者仍然会出现越障后失稳的问题,关于这一块的内容希望大家关注我的知乎在后续帖子中介绍相关处理方法。