机器人运动规划方法动态运动基元(dynamic movement primitive)应如何理解实现?
【导读】开源MIT Mini cheetah机械狗设计第39篇，番外篇第15篇，Quad-SDK设计之局部规划（local_planner）。
上一篇讲解了路径的全局规划，得到了一条由很多节点组成的轨迹，得到轨迹后接着就进入局部规划。
轨迹规划得到的主要信息都封装在robot plan里面，包括：
接着我们先看看局部规划都需要输入哪些参数：
上述4个参数的输入通过ros消息进行订阅，具体的转换不作介绍，和全局规划的差不多。
根据输入的参数会提取一个参考的轨迹，再通过NMPC进行跟随。
局部规划的入口函数为：LocalPlanner::spin()
实现的功能如下：
1）判断是否收到有效数据；
2）获取参考状态信息（需要跟踪的状态）相关信息：getReference();
3）进行局部规划：computeLocalPlan()；
4）发布局部规划结果，也即NMPC计算得到的机器人相关状态信息以及足底反作用力信息。
代码如下：
接下来对上述功能进行一一解析。
一、获取参考状态
实现函数为：getReference();
如果是第一次规划，记录上一次的足底状态和索引，当使用的是遥控命令，则对初始时间戳进行初始化。
如果使用的不是遥控器，而是全局规划，则采用全局规划的时间戳
跟踪轨迹，进入运行模式
获取轨迹索引，确保不会重复计算
其中轨迹的索引计算函数为：getPlanIndex（）
索引计算的方法如下图：
获得当前机器人质心和足底的状态并转换成矩阵形式，足底状态是基于世界系的，还需要获得在本体系中足端的状态。
初始化参考状态和基元状态，所谓基元状态就是参考状态的下个状态该执行什么动作，是跳跃，踏步还是小跑等。
接着就是分两种情况，一种是使用遥控器的情况，一种是使用全局规划的情况
1）使用遥控器
确保遥控器数据是最新的：
设置机器人当前位置的地面高度：
机器人站立的时候的位置 如果没有初始化，则初始化为当前位置。
设置四足狗的支撑中心：
判断当前速度是否超过阈值，或者质心离支撑点太远，如果满足其中一个，则说明机器人有可能不稳，会跌倒，则需要调整机器人模式，比如通过踏步来调整重心位置离支持点不要太远。
如果超出阈值，则进行踏步调整，如果没有超出阈值，则尽力稳定住。
对状态参数进行赋值：
状态包括 ，共12个参数
获得地形的roll和pitch：
预测N_个状态，也即ref_body_plan_为一个N_行，12列的矩阵。
for (int i = 1; i < N_; i++) {
Eigen::VectorXd current_cmd_vel = cmd_vel_;
double yaw = ref_body_plan_(i - 1, 5);
//速度分解，将X,Y速度分解yaw方向上，其中yaw角度为机器人方向和y轴的夹角
current_cmd_vel[0] = cmd_vel_[0] * cos(yaw) - cmd_vel_[1] * sin(yaw);
current_cmd_vel[1] = cmd_vel_[0] * sin(yaw) + cmd_vel_[1] * cos(yaw);
for (int j = 0; j < 6; j++) {
if (i == 1) {//均速模型
ref_body_plan_(i, j) = ref_body_plan_(i - 1, j) +
current_cmd_vel[j] * first_element_duration_;
} else {
ref_body_plan_(i, j) =
ref_body_plan_(i - 1, j) + current_cmd_vel[j] * dt_;
}
ref_body_plan_(i, j + 6) = (current_cmd_vel[j]);
}
ref_ground_height_(i) = local_footstep_planner_->getTerrainHeight(
ref_body_plan_(i, 0), ref_body_plan_(i, 1));
ref_body_plan_(i, 2) = z_des_ + ref_ground_height_(i);
// Adaptive roll and pitch
local_footstep_planner_->getTerrainSlope(
ref_body_plan_(i, 0), ref_body_plan_(i, 1), ref_body_plan_(i, 5),
ref_body_plan_(i, 3), ref_body_plan_(i, 4));
}
2）使用全局规划
直接使用全局规划出来的状态对ref_body_plan_进行赋值：
// Use global plan
for (int i = 0; i < N_; i++) {
// If the horizon extends past the reference trajectory, just hold the
// last state
//如果超出预测范围了，则保持为上一个状态
if (i + current_plan_index_ > body_plan_msg_->plan_indices.back()) {
ref_body_plan_.row(i) =
quad_utils::bodyStateMsgToEigen(body_plan_msg_->states.back().body);
if (i < N_) {
ref_primitive_plan_(i) = body_plan_msg_->primitive_ids.back();
}
} else {
ref_body_plan_.row(i) = quad_utils::bodyStateMsgToEigen(
body_plan_msg_->states[i + current_plan_index_].body);
if (i < N_) {
ref_primitive_plan_(i) =
body_plan_msg_->primitive_ids[i + current_plan_index_];
}
}
ref_ground_height_(i) = local_footstep_planner_->getTerrainHeight(
ref_body_plan_(i, 0), ref_body_plan_(i, 1));
}
ref_ground_height_(0) = local_footstep_planner_->getTerrainHeight(
current_state_(0), current_state_(1));
// Stand if the plan has been tracked
//如果本体位置和参考位置小于某个阈值，则将控制模式设为站立模式
if ((current_state_ - ref_body_plan_.bottomRows(1).transpose()).norm() <=
stand_pos_error_threshold_) {
control_mode_ = STAND;
}
对足底进行规划：
// Update the body plan to use for foot planning
//本体状态的行数
int N_current_plan = body_plan_.rows();
//对body_plan_进行初始化，初始化成N_*12的矩阵
if (N_current_plan < N_) {
// Cold start with reference plan
body_plan_.conservativeResize(N_, 12);
// Initialize with the current foot positions
for (int i = N_current_plan; i < N_; i++) {
//用ref_body_plan_的值来初始化body_plan_
body_plan_.row(i) = ref_body_plan_.row(i);
//用current_foot_positions_body_初始化foot_positions_body_
foot_positions_body_.row(i) = current_foot_positions_body_;
//用current_foot_positions_world_初始化foot_positions_world_
foot_positions_world_.row(i) = current_foot_positions_world_;
}
} else {
// Only shift the foot position if it's a solve for a new plan index
//如果是一个新的索引，则只需要移动足端的位置
if (plan_index_diff_ > 0) {// plan_index_diff_ = current_plan_index_ - previous_plan_index;
//整体往上移动一行
body_plan_.topRows(N_ - 1) = body_plan_.bottomRows(N_ - 1);
grf_plan_.topRows(N_ - 2) = grf_plan_.bottomRows(N_ - 2);
foot_positions_body_.topRows(N_ - 1) =
foot_positions_body_.bottomRows(N_ - 1);
foot_positions_world_.topRows(N_ - 1) =
foot_positions_world_.bottomRows(N_ - 1);
}
}
初始化第一行：
二、局部规划
实现函数为：computeLocalPlan();
判断数据是否为空：
开启定时器：
1、判断足底接触状态
根据ref_primitive_plan_记录的动作状态，判断足底是接触状态还是离地状态，比如LEAP_STANCE（起跳）则是接触状态，FLIGHT（飞行）则是离地状态，LAND_STANCE（着陆）也是接触状态。
具体实现：
void LocalFootstepPlanner::computeContactSchedule(
int current_plan_index, const Eigen::MatrixXd &body_plan,
const Eigen::VectorXi &ref_primitive_plan, int control_mode,
std::vector<std::vector<bool>> &contact_schedule) {
// Compute the current phase in the nominal contact schedule
int phase = current_plan_index % period_;//period_ 以时间步为单位的步态周期
// Compute the current contact schedule by looping through the nominal
// starting at the phase
contact_schedule.resize(horizon_length_);
for (int i = 0; i < horizon_length_; i++) {
contact_schedule[i].resize(num_feet_);
if (control_mode == LocalPlannerMode::STAND) {
for (int j = 0; j < contact_schedule[i].size(); j++) {
contact_schedule[i][j] = true;
}
} else {//根据占空比和偏移量可知，只有trot对角步态
contact_schedule[i] = nominal_contact_schedule_[(i + phase) % period_];
}
}
// Check the primitive plan to see if there's standing or flight phase
for (int i = 0; i < horizon_length_; i++) {
// Leaping and landing
if (ref_primitive_plan(i) == LEAP_STANCE) {//起跳
contact_schedule.at(i) = {true, true, true, true};
} else if (ref_primitive_plan(i) == FLIGHT) {//飞行
// Flight, check that min landing height is exceeded
double min_landing_height = 0.3;
double current_height =
body_plan(i, 2) - terrain_grid_.atPosition(
"z_inpainted", body_plan.row(i).head<2>(),
grid_map::InterpolationMethods::INTER_NEAREST);
if (current_height < min_landing_height && body_plan(i, 8) < 0) {//不高就直接迈过去
contact_schedule.at(i) = {true, true, true, true};
} else {//否则就要跳过去
contact_schedule.at(i) = {false, false, false, false};
}
} else if (ref_primitive_plan(i) == LAND_STANCE) {//着陆
contact_schedule.at(i) = {true, true, true, true};
}
}
}
2、足底规划
足底规划包括落足点规划和摆动轨迹规划，这部分是整个过程非常重要的部分。
1）落足点规划
落足点的规划就是规划摆动相的起点到终点的距离，Quad-SDK的落足点规划其实是和MIT Cheetah 3是一样的，如下图所示，落足点是由 共同得到的，而且得到的是一个相对于世界坐标系的位置。
另外，落足点是相对于对称点的，其中 是由以下3部分组成的：
1、abad关节为0时，hip关节的坐标原点在本体系下的水平坐标，然后再换算到世界系下，也即程序中的hip_position_midstance。
2、跟踪速度修正补偿的位移，采用的是启发式算法（来自于Raibert的理论），代码中为vel_tracking
3、离心力产生的落足点偏移，代码中为centrifugal
对应论文中的公式：
所以最终的名义落足点 为：
然后根据上一个落足点优化这次落足点，得到最终落足点：
2）摆动相的轨迹规划
有了足底的初始位置和结束位置，以及对称点，就可以分别对 三个方向分别进行插值。
这个就没什么好讲的，MIT Mini cheetah已经有所介绍。
最终得到足端的位置，速度和加速度数据。
将新的足端位置转换到本体系。
考虑足端半径，计算支撑力位置：
3、NMPC规划
接着使用NMPC来确定地面反作用力，以更好地跟踪期望轨迹。该NMPC将机器人建模为单个刚体，但保持SE(3)动力学的非线性。给定期望的body状态轨迹 ，地面反力
，足端位置
，初始条件
，离散时间MPC的解可表示为以下优化问题:
代码为：
之后就是将NMPC计算得到的地面反力，body状态轨迹
，足端位置
发布出去，进行下一步的Robot Driver 规划。这部分下一节再讲
三、结束语
局部规划的实现已经已经讲完，下一篇就是底层的Robot Driver 规划，包括：
1）Leg Controller：腿部控制器
腿部控制器将机器人姿态和摆动腿的轨迹从局部规划转换为关节空间命令。
对局部规划进行插值并进行运动学逆解，得到每条腿的广义坐标参考位置 和速度
，以及每条腿的反作用力
。然后通过逆动力学将这些映射到关节(广义)空间中。这是通过求解前馈支撑力矩
和摆动力矩
来实现的
2）State Estimator：状态估计器
状态估计器负责解析传感器数据集并维护机器人完整状态的估计。
传感器融合由运动，IMU，和编码器数据进行融合估计。
身体的位置和方向直接从运动中获得，而角速度和关节信息则从IMU和电机编码器读取。线速度的计算采用了一种互补滤波器，该滤波器融合了微分位置和积分IMU线加速度。Quad-SDK并未使用EKF算法进行状态估计。
新公网安备 65010402001845号