四足机器人MPC控制算法编写仿真

发表于 分类于 阅读次数： 本文字数： 16k 阅读时长 ≈ 15 分钟

开源地址：https://gitee.com/momaoto/mpc_webots_emulation

原理介绍

机器人控制系统，就是要解决机器人在哪儿、去哪儿、怎么去的问题，分别对应了机器人控制系统的状态估计器、轨迹规划器和控制器。

原论文中第二章介绍了控制系统中的2.3步态调度、2.4状态估计、2.5质心轨迹生成、2.6摆动相规划，分别解决了在哪儿（状态估计）与去哪儿（轨迹规划）的问题。第三章使用MPC对四足机器人控制量进行优化计算，解决机器人怎么去（模型预测控制）的问题。现对以上内容尝试c++面向对象的实现。

狗腿说明：

狗头

2 1

4 3

框架设计

  • GaitPlanner类：步态调度器，提供机器人步态变量输出，以供摆动相规划与支撑相控制。
  • StateEstimator类：状态估计器，收集IMU与编码器返回的原始状态信息，加工并输出世界系下质心与四足的位置速度信息。
  • TrajectoryGenerator类：质心轨迹生成器，接收遥控器的指令期望速度，生成期望轨迹。
  • MPCsolver类：凸优化求解器，根据生成的估计状态与期望轨迹创建qp问题并求解，生成所需的足底反力。

模块实现

以下为提供给用户使用的接口说明。

params.h

这里定义了四足机器人全局静态参数，使用时预先在这里设置好：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
#ifndef _PARAMS_H_
#define _PARAMS_H_

#include <Eigen/Dense>

// 机器狗参数
extern double H_X; // (2.2)
extern double H_Y; // (2.2)
extern double L_1; // (2.2)
extern double L_2; // (2.2)
extern double L_3; // (2.2)
extern double Mass ; //质量

// 控制周期
extern double DELTA_T_MPC;
extern double DELTA_T_OTHERS;

#define PREDICTION_HORIZON 10 // 预测区间h

extern double I_B_array[9]; // 机器狗惯性张量
extern double kp;

extern double p_x_offset; // x方向落足点偏移量

// MPC限制条件
extern double INF;
extern double f_max;
extern double miu;
#endif

GaitPlanner.cpp

步态调度器，负责根据用户选择的步态模式计算输出每个控制周期四足的摆动相、支撑相相位，反映步态信息。

类定义

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
#ifndef _GAITPLANNER_H_
#define _GAITPLANNER_H_

#include "params.h"
#include "global.h"
#include <string>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

/**
 * @brief The GaitPlanner class represents a gait planner for a quadruped robot.
 *
 * This class is responsible for calculating the gait of the robot based on the
 * given parameters and current state.
 * It provides methods to set the gait parameters, set the robot's state, set
 * the desired motion, and calculate the gait.
 * The calculated gait can be accessed through various getter methods.
 */
class GaitPlanner
{
private:
    double t_; // 当前时间
    int n_;    // 当前迈步周期数

    // Gait scheduler
    int setflag_; // 调用函数前先设置步态
    double t_sw_; // 名义摆动相时间
    double t_st_; // 名义支撑相时间

    Vector4d Gd_;      // 占空比
    Vector4d Go_;      // 偏移量
    double T_gait_;    // = t_sw + t_st; //完整迈步周期,=t_sw_+t_st_
    double t_ng_;      // 当前周期的时间进度
    double t_ng_norm_; // = t_ng / T_gait; //归一化当前周期时间进度

    Vector4d t_fai_norm_; // 相位进度,i=0,1,2,3
    Vector4d s_fai_;      // 是否处于支撑相,i=0,1,2,3
    Vector4d t_st_norm_;  // 支撑相相位进度,i=0,1,2,3
    Vector4d t_sw_norm_;  // 摆动相相位进度,i=0,1,2,3

    // Swing phase planner
    Vector3d p_touch_com_O_;
    double psi_touch_;

    Vector3d p_t_com_O_;  // position of com in O, 来自状态估计器
    Vector3d v_t_com_O_;  // velocity of com in O, 来自状态估计器
    Matrix3d R_t_B_O_;    // rotation matrix from B to O, 来自状态估计器
    Vector3d omega_OB_O_; // angular velocity of the robot, 来自状态估计器
    double psi_t_;        // yaw angle of the robot, 来自状态估计器

    Vector3d v_t_com_d_B_; // desired velocity of com in B, 来自轨迹规划器
    double omega_t_z_d_;   // desired yaw rate of the robot, 来自轨迹规划器

    Vector3d getp_hip_i_B_(int i);
    Vector3d getp_touch_hip_i_O(int i); // 获取第i腿触地点在O系下的坐标,i=1,2,3,4

public:
    GaitPlanner();
    ~GaitPlanner();

    void setGait(double swingTime, double standTime, string mode); // 设置步态
    void calculateGait(double currentTime);                        // 计算步态

    void setState(Vector3d p_t_com_O, Vector3d v_t_com_O, Matrix3d R_t_B_O,
                  Vector3d omega_OB_O, double psi_t);          // 设置状态
    void setDesired(Vector3d v_t_com_d_B, double omega_t_z_d); // 设置期望

    Vector3d getp_swend_i_O(int i); // 获取第i腿摆动末端在O系下的坐标,i=1,2,3,4
    Vector4d gets_fai();            // 获取当前各腿支撑相状态
    // Vector3d getp_stend_i_O(int i); // 获取四足最近一次支撑相足底坐标
};

#endif

使用方法

1
2
3
// 未使用单例模式，因此写在main里，向线程函数传入引用
 GaitPlanner *gp = new GaitPlanner(); 
 

StateEstimator.cpp

状态估计器，负责估计四足机器人运动过程中难以直接测量的状态信息，对IMU返回的原始旋转矩阵、电机编码器返回的角度与转速进行转换，反映四足机器人在世界系下的质心位置、质心速度、四足位置等状态信息。其中包括一个使用卡尔曼滤波实现的足底里程计，帮助MPC算法更加精确地收集状态信息。

类定义

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104

#ifndef _STATEESTIMATOR_H_
#define _STATEESTIMATOR_H_

#include "params.h"
#include "global.h"
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

/*
    状态估计器
*/

class StateEstimator {
private:
  double t_; // 当前时间
  int n_;    // 当前为第n个周期

  bool IMUsetflag_;
  bool Encoderflag_;
  bool Calculatedflag_;
  // IMU
  Vector3d a_com_B_;     // B系质心加速度
  Vector3d a_com_O_;     // O系质心加速度
  Vector3d omega_OB_B_;  // B系躯干角速度
  Vector3d omega_OB_O_;  // O系躯干角速度
  Matrix3d omega_OBX_B_; // B系躯干角速度张量(2.51)

  Vector3d p_i_B_;     // 本体系下i足足底位置(2.47)
  Vector3d p_dot_i_B_; // 本体系下i足足底速度(2.49)

  Matrix3d R_imu_imu0_;        // imu->imu0旋转矩阵(IMU原始旋转矩阵)
  Matrix<double, 4, 3> q_j_i_; // 12个关节电机角度,电机编号见顶部
  Matrix<double, 4, 3> q_dot_j_i_; // 12个关节电机角速度,电机编号见顶部
  Vector3d a_original_;            // 原始IMU加速度
  Vector3d omega_original_;        // 原始IMU角速度

  Matrix3d R_B_O_; // B->O躯干旋转矩阵(通过IMU和世界系的定义计算得出)
  Matrix3d R_B_imu_; // B->imu旋转矩阵(常值)

  Matrix3d R_imu0_O_;      // imu0->O旋转矩阵(常值)
  Matrix3d R_t0_imu_imu0_; // 初始时刻IMU旋转矩阵(IMU启动时读出)

  Matrix<double, 4, 3> p_stend_O_last_; // 上一控制周期求得有效支撑相足底坐标

  // Kalman filter
  Matrix<double, 18, 18> A_;
  Matrix<double, 18, 3> B_;
  Matrix<double, 18, 18> Q_;
  Matrix<double, 28, 28> R_;
  Matrix<double, 28, 18> H_;
  Matrix<double, 18, 18> P_;
  Matrix<double, 18, 28> K_;
  Vector3d u_;
  Matrix<double, 28, 1> z_k_;
  Matrix<double, 18, 1> x_k_; // k时刻状态变量[p_com_O, v_com_O, p_foot_O](18*1)

  Vector3d geta_com_B(); // 计算并返回质心加速度，参数为原始IMU加速度
  Vector3d geta_com_O(); // 经过R_B_O_变换后的质心加速度(2.42)
  Vector3d getomega_OB_B(); // 计算并返回躯干角速度，参数为原始IMU角速度
  Vector3d getomega_OB_O(); // 经过R_B_O_变换后的躯干角速度(2.44)

  Matrix3d getomega_OBX_B();

  // 计算本体系下i足足底位置(2.47)，参数q_j_i为第i条腿上的3个关节电机角度，0-髋电机，1-大腿电机，2-小腿电机
  Vector3d getp_i_B(int i, const Vector3d &q_j_i);
  // 计算本体系下i足足底速度(2.49)，参数q_dot_j_i为第i条腿上的3个关节电机速度，0-髋电机，1-大腿电机，2-小腿电机
  Vector3d getp_dot_i_B(int i, const Vector3d &q_j_i,
                        const Vector3d &q_dot_j_i);

public:
  StateEstimator();
  ~StateEstimator();

  // void setIMUdata(const Matrix3d &R_imu_imu0, const Vector3d &a_original,
  // const Vector3d &omega_original);

  // 设置InertialUnit输出的欧拉角(原始旋转矩阵)，Gyro输出的角速度，Accelerometer输出的加速度
  void setIMUdata(const Vector3d &Eulerangles, const Vector3d &a_original,
                  const Vector3d &omega_original);
  // 设置12个关节电机角度和角速度
  void setEncoderdata(const Matrix<double, 4, 3> &q_j_i,
                      const Matrix<double, 4, 3> &q_dot_j_i);
  // 计算所有过程变量
  void calculateAll();

  // 卡尔曼滤波计算x_k
  void kalman_update(int times);

  // 计算并返回躯干旋转矩阵，对于spot来说即为原始IMU旋转矩阵
  Matrix3d getR_B_O();

  Matrix<double, 4, 3> getp_stend_O(const Vector4d &s_fai);

  // 返回18*1向量，为[p_com_O(3*1), v_com_O(3*1), p_foot_O(12*1)]
  Matrix<double, 18, 1> getx_k();

  Matrix<double, 4, 3> getp_foot();
};

#endif

使用方法

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
// 未使用单例模式，因此写在main里，向线程函数传入引用
StateEstimator *se = new StateEstimator();

// 用于计算估计状态
   se->setIMUdata(Eulerangles, a_original, omega_original);
   se->setEncoderdata(q_j_i, q_dot_j_i);
   se->calculateAll();
   se->kalman_update();
   
*x_k = se->getx_k();

// 用于读取中间状态
se->getp_stend_O(gp->gets_fai());
se->getR_B_O();

TrajectoryGenerator.cpp

质心轨迹生成器，负责接受手柄传来的控制命令，转化为MPC算法需要的未来h周期地机器人期望轨迹（状态），作为实际轨迹优化问题的目标。

  • 接受：期望速度，期望角速度
  • 返回：期望速度，期望角速度，期望位置，期望角位置（欧拉角）

构造时默认当前周期为0， 接收到手柄传来的本体系下的期望速度与角速度后生成未来h周期的、世界系下的期望速度，角速度，位置，欧拉角。

每次生成期望轨迹后，预测区间右移一格，重新生成未来h周期期望轨迹。

类定义

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
#ifndef _TRAJECTORYGENERATOR_H_
#define _TRAJECTORYGENERATOR_H_

#include "params.h"
#include "global.h"
#include <string>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;

/*
    质心轨迹生成器
        角标命名规则：右上角开始顺时针命名
*/

class TrajectoryGenerator
{
private:
    double t_;          // 当前时间
    int n_;             // 当前为第n个周期

    int setflag_;       // 调用函数前先设置期望值

    double block_para_; // 堵转保护系数

    double v_x_d_B_;     // 期望x速度(B系)
    double v_y_d_B_;     // 期望y速度(B系)
    double omega_z_d_B_; // 期望绕z角速度(B系)

    Vector3d p_0_com_d_O_; // 上一控制周期求得此刻期望位置
    Vector3d v_0_com_d_O_; // 上一控制周期求得此刻期望速度
    Vector3d p_0_com_O_;   // 此刻实际位置 // BUG

    Vector3d v_k_com_d_O_; // 期望质心速度(2.21)
    Vector3d p_k_com_d_O_; // 期望质心位置(2.63)
    Vector3d omega_k_d_O_; // 期望角速度(2.65)
    Vector3d THETA_k_d_;   // 期望欧拉角(2.76)

    Matrix<double, 13 * PREDICTION_HORIZON, 1> D_;
    // slope estimate
    Matrix<double, 4, 3> p_stend_O_; // 4条腿的最近一次支撑相足底坐标
    Vector3d A_pla_hat_;                // 估计的地面平面系数

public:
    TrajectoryGenerator();
    ~TrajectoryGenerator();

    // 设置四足最近一次支撑相足底坐标，来自步态规划器
    void setState(const Matrix<double, 4, 3> &p_stend_O);
    // 设置期望值，来自上位机或手柄
    void setDesired(const double &v_x, const double &v_y, const double &omega_z);
    // 计算期望轨迹向量:[THETA_k_d,p_k_com_d_O,v_k_com_d_O,omega_k_d_O]*PREDICTION_HORIZON，其中包括坡度估计
    void calculateAll(const Matrix3d &Rot);
    // 返回期望轨迹向量(13h*1)
    void getD(Matrix<double, 13 * PREDICTION_HORIZON, 1> &vectorForD);

    void getpsi_k_d(Matrix<double, PREDICTION_HORIZON, 1> &vectorForpsi_k_d);

    // Vector3d *getv_k_com_d_O(int k, Matrix3d *Rot);
    // Vector3d *getp_k_com_d_O(int k, Vector3d *p_0_com_O, Matrix3d *Rot);
    // Vector3d *getomega_k_d_O();
    // Vector3d *getTHETA_k_d();
};

#endif

使用方法

1
2
3
4
5
6
7
8
// 未使用单例模式，因此写在main里，向线程函数传入引用
TrajectoryGenerator *tg = new TrajectoryGenerator(); // 

   tg->setState(se->getp_stend_O(gp->gets_fai()));
   tg->setDesired(v_x, v_y, omega_z);
   tg->calculateAll(se->getR_B_O());
   tg->getD(*vecD);
   tg->getpsi_k_d(*psi_k_d);

MPCsolver.cpp

  • 目的：计算出当前时刻（t时刻）应当输入的控制量UU
  • 约束：
    1. 输入UU后系统状态与预期状态尽可能接近(误差最小)
    2. 所输入的UU本身尽可能小(输入最小)

控制以上两个约束的权重后，求UU为一个二次凸优化问题，以下为MPC具体实现时所需要的变量推导：

根据前文实现的四个计算类编写代码，求出当前时刻最优输入控制量UU，其中第一列为所需足底反力。

类定义

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
#ifndef _MPCSOLVER_H_
#define _MPCSOLVER_H_

#include <iostream>
#include <string>
#include "all.h"
#include <qpOASES.hpp>

// using namespace qpOASES; // contain class "Matrix",ambigurous with Eigen;
using namespace std;
using namespace Eigen;

class MPCsolver
{
private:
    qpOASES::QProblem *qproblem;

    Matrix<double, 13 * PREDICTION_HORIZON, 1> D_;                                 // D(13h*1)
    Matrix<double, 13, 1> x_0_;                                                    // x_0(13*1)
    DiagonalMatrix<double, 13 * PREDICTION_HORIZON, 13 * PREDICTION_HORIZON> Q_;   // Q(13h*13h),  NOTE: Q_ is diagonal matrix
    DiagonalMatrix<double, 12 * PREDICTION_HORIZON, 12 * PREDICTION_HORIZON> R_;   // R(12h*12h),  NOTE: R_ is diagonal matrix
    Matrix<double, 20 * PREDICTION_HORIZON, 12 * PREDICTION_HORIZON, RowMajor> C_; // C(20h*12h),  NOTE: row major storage
    Matrix<double, 20 * PREDICTION_HORIZON, 1> c_lb_;                              // clb(20h*1)
    Matrix<double, 20 * PREDICTION_HORIZON, 1> c_ub_;                              // cub(20h*1)

    Matrix<double, 13 * PREDICTION_HORIZON, 13> A_qp_;                             // Aqp(13h*13)
    Matrix<double, 13 * PREDICTION_HORIZON, 12 * PREDICTION_HORIZON> B_qp_;        // Bqp(13h*12h)
    Matrix<double, 12 * PREDICTION_HORIZON, 12 * PREDICTION_HORIZON, RowMajor> H_; // H(12h*12h),  NOTE: row major storage
    Matrix<double, 12 * PREDICTION_HORIZON, 1> g_;                                 // g(13h*1)
    Matrix<double, 12 * PREDICTION_HORIZON, 1> U_;                                 // U(12h*1)

public:
    MPCsolver(/* args */);
    ~MPCsolver();

    // 初始化qp问题
    void setD_and_x0(const Matrix<double, 13 * PREDICTION_HORIZON, 1> &vecD,
                     const Matrix<double, 13, 1> &x_0);

    // 计算A_qp_、B_qp_、H_、g_、C_
    void calculateAll(const Matrix<double, PREDICTION_HORIZON, 1> &psi_k_d,
                      const Matrix<double, 3 * PREDICTION_HORIZON, 4> &r_k_ix);
    
    // 卡尔曼滤波首次求解
    void qpSolveU_first();

    // 卡尔曼滤波后续求解
    void qpSolveU_next(Matrix<double, 12,1> &U_optimal);

};

#endif

使用方法

1
2
3
4
5
6
7
8
9
10
11
12
// 未使用单例模式，因此写在main里，向线程函数传入引用
MPCsolver *mpc = new MPCsolver();

mpc->setD_and_x0(*vecD, *x_0);
mpc->calculateAll(*psi_k_d, *r_k_ix);
mpc->qpSolveU_first();

while (robot->step(4) != -1) {
    mpc->setD_and_x0(*vecD, *x_0);
    mpc->calculateAll(*psi_k_d, *r_k_ix);
    mpc->qpSolveU_next(*f_MPC);
 }

控制器

电机编号说明

1
2
3
4
5
6
7
8
9
10
/*  电机编号
            头
     9  5       4 8
        1       0
          |dog|
          |dog|
          |dog|
        3       2
     11 7       6 10
*/

键盘输入

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
switch (key) {
case Keyboard::UP:
  cout << "FORWARD" << endl;
  v_x = 0.1;
  v_y = 0;
  omega_z = 0;
  break;
case Keyboard::DOWN:
  cout << "BACKWARD" << endl;
  v_x = -0.1;
  v_y = 0;
  omega_z = 0;
  break;
case Keyboard::LEFT:
  cout << "LEFT" << endl;
  v_x = 0;
  v_y = 0;
  omega_z = 0.1;
  break;
case Keyboard::RIGHT:
  cout << "RIGHT" << endl;
  v_x = 0;
  v_y = 0;
  omega_z = -0.1;
  break;
default:
  v_x = 0;
  v_y = 0;
  omega_z = 0;
  break;
}

贝塞尔曲线

1
2
3
4
5
6
7
8
9
Vector3d cubicBezier3D(Vector3d &p0, Vector3d &p1, Vector3d &p2, Vector3d &p3,
                       float t) {
  Vector3d result;
  for (int i = 0; i < 3; i++)
    result[i] = (1 - t) * (1 - t) * (1 - t) * p0[i] +
                3 * (1 - t) * (1 - t) * t * p1[i] +
                3 * (1 - t) * t * t * p2[i] + t * t * t * p3[i];
  return result;
}

运动学逆解

位置控制的运动学逆解，将足端坐标、速度转化为电机转角、转速。

原位控版本：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
#ifndef DOG_SOLVE_H
#define DOG_SOLVE_H


#define M_PI 3.1415926
#define leg_length1  0.050//单位为m
#define leg_length2  0.340
#define leg_length3  0.400

//extern float kp;
//extern float kd;

struct dog_pd{
	float kp;
	float kd;
};
extern struct dog_pd dog_pd[12];//每条腿的pd参数

struct dog_motor{
	double sol_gamma;
	double sol_alfa;
	double sol_beta;
	double  a1, l4; // 计算的角度和长度(中间变量)
};
extern struct dog_motor leg_angle[4];

struct dog_foot{
	double x;
	double y;
	double z;
};
extern struct dog_foot foot_position[4];

struct dog_motor_ouput{
	double sol_gamma;
	double sol_alfa;
	double sol_beta;
};
extern struct dog_motor_ouput leg_angle_ouput[4];


void dog_solve(); // 运动学逆解
// void dog_solve_output();//把逆解计算出来的结果处理后输出给电机

#endif

推荐使用更精简版本：（更新于12.22）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
Vector3d inverseKinematics(int legNumber, Vector3d footPosition) {
  Vector3d motorAngles;

  double gamma, alfa, beta;

  double x = footPosition[0];
  double y = footPosition[1];
  double z = footPosition[2];

  double ksi = (legNumber == 1 || legNumber == 3) ? 1 : -1;

  gamma = MY_PI - acos(L1 / sqrt(pow(y, 2) + pow(z, 2))) - atan(-ksi * y / z);

  float L23 = sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2) - pow(L1, 2));

  beta = acos((pow(L2, 2) + pow(L3, 2) - pow(L23, 2)) / (2 * L2 * L3));

  alfa = acos(x / L23) +
         acos((pow(L2, 2) + pow(L23, 2) - pow(L3, 2)) / (2 * L2 * L23));

#ifdef IK_TEST
  cout << "x: " << x << " y: " << y << " z: " << z << endl;
  cout << atan((L1 * cos(gamma) - z) / x) << endl;
  cout << asin(sqrt(pow(L1 * sin(gamma) - y, 2) + pow(L1 * cos(gamma) - z, 2)) /
               L23)
       << endl;

  cout << "gamma: " << gamma << endl;
  cout << "L23: " << L23 << endl;
  cout << "beta: " << beta << endl;
  cout << "alfa: " << alfa << endl;
#endif

#ifdef ZERO_OFFSET_SPOT
  gamma = (legNumber == 1 || legNumber == 3) ? gamma - MY_PI / 2
                                             : MY_PI / 2 - gamma;
  alfa = -alfa + (2 * MY_PI / 3);
  beta = -beta + 1.6;
#endif

#ifdef TO_DEGREE
  gamma = gamma * 180 / MY_PI;
  alfa = alfa * 180 / MY_PI;
  beta = beta * 180 / MY_PI;
#endif

  motorAngles[0] = gamma;
  motorAngles[1] = alfa;
  motorAngles[2] = beta;

  return motorAngles;
}

开始第一个demo

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
void trot_balanced() {

  Robot *robot = new Robot();
  GaitPlanner *gp = new GaitPlanner();
  TrajectoryGenerator *tg = new TrajectoryGenerator();
  StateEstimator *se = new StateEstimator();
  MPCsolver *mpc = new MPCsolver();

  init_global_variables();
  thread thread1(thread_trajectory_generate, robot, tg, se, gp);
  thread thread2(thread_state_estimate, robot, se, gp);
  thread thread3(thread_mpc_solver, robot, mpc);
  thread thread4(thread_send_command, robot);

  thread1.join();
  thread2.join();
  thread3.join();
  thread4.join();

  delete gp;
  delete tg;
  delete se;
  delete mpc;
  delete robot;
}

int main() {
  trot_balanced();
  return 0;
}

其他

Intel MKL加速

在#include之后的预处理区添加以下代码，可利用intel硬件加速Eigen矩阵的计算：

1
2
3
4
5
6
7
8
// 使用Intel MKL 线性代数库加速
#ifndef EIGEN_USE_MKL_ALL
#define EIGEN_USE_MKL_ALL
#endif

#ifndef EIGEN_VECTORIZE_SSE4_2
#define EIGEN_VECTORIZE_SSE4_2
#endif

代码性能测量

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
#define time_print  //需要打印系统时间时保留

/*
    时间测量函数
    用法：
        #include <timetest.cpp>

        float tStart = timetest();
        //待测量代码段
        float tEnd = timetest();
        cout<<tEnd-tStart;
*/

float timetest()
{
    auto now = std::chrono::system_clock::now();
    // 通过不同精度获取相差的毫秒数
    uint64_t dis_millseconds =
    std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count()
    -
    std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count()
    * 1000;
    time_t tt = std::chrono::system_clock::to_time_t(now);
    auto time_tm = localtime(&tt);
    char strTime[25] = {0};
    float ttt = time_tm->tm_hour * 3600 + time_tm->tm_min * 60 +
    time_tm->tm_sec + 0.001*(float)dis_millseconds;
    #ifdef time_print
    sprintf(strTime, "%d-%02d-%02d %02d:%02d:%02d %03d", time_tm->tm_year +
    1900, time_tm->tm_mon + 1, time_tm->tm_mday, time_tm->tm_hour,
    time_tm->tm_min, time_tm->tm_sec, (int)dis_millseconds);
    std::cout << strTime << std::endl;
    #else
    #endif

    return ttt;
}

Eigen库使用

1
matrix.block<p,q>(i,j);  //从坐标(i,j)提取大小为<p,q>的块，可读可写

qpOASES使用

Webots手册

IMU相关

卡尔曼滤波相关

日志

  • 11.19 构建第二章代码框架
  • 11.20 实现GaitScheduler，增加性能测试函数
  • 11.22 添加Eigen矩阵计算库，添加开源协议，开始实现StateEstimator
  • 11.23 实现StateEstimator卡尔曼滤波前的部分
  • 11.24 实现StateEstimator卡尔曼滤波状态方程部分，完善readmeGaitScheduler部分
  • 11.28 实现StateEstimator完成(含卡尔曼滤波)，通过初步测试
  • 11.30 实现TrajectoryGenerator至地形坡度估计，暂时跳过
  • 12.02 分析MPC算法具体实现逻辑
  • 12.06 搭建MPC算法框架
  • 12.10 实现MPC初版代码
  • 12.25 Webots仿真移植成功