Hector双足机器人MPC控制

文章arxiv.org/pdf/2104.00065.pdf

Force-and-moment-based Model Predictive Control for Achieving Highly Dynamic Locomotion on Bipedal Robots

一、任务说明

机器人模型：宇树机器人改成垂直结构

控制目标：双足机器人按照一定速度行走

二、控制原理

控制方法：基于力和力矩的MPC，站立阶段mpc，摆动阶段pd

动力学模型

动力学方程

状态方程

输入U

状态变量

不等式约束（15）是跟摩擦系数有关，

等式约束（16）是摆动腿阶段，应该采用另外的PD控制

站立状态：MPC控制

摆动状态：PD控制

MPC问题的求解使用第三方库

力矩控制的转换

三、工程文件说明：

hector_control是控制代码，

包含ConvexMPC是双足机器人的基于力和力矩的控制器MPC；src中main.cpp、行走和被动状态切换的有限状态机FSM、interface提供用户键盘控制接口、common是底层电机控制指令；third_party是求解MPC 问题的第三方库qpOASES。

hector_description是hector机器人描述文件，包含stl文件，xacro建模文件。

unitree_ros主要关注包含gazebo的仿真元素。

四、代码实现：

main函数

FSM类有限状态机切换，FSMState_Passive和FSMState_Walking状态切换，若高度太低就转化成passive状态被动不可用户控制，通常是walking状态

FSM* _FSMController = new FSM(_controlData);

FSMState_Walking类，注意初始化了MPC对象，函数run()调用MPC类的run函数

ConvexMPCLocomotion类

MPC类run函数

首先根据当前机器人状态选择合适的步态模式（站立or行走），

然后根据步态模式生成相应的脚部期望轨迹，

设置控制参数，

MPC问题的设置和求解，

最后更新机器人的控制命令。

五、效果