Odrive源码分析(四) 位置爬坡算法

Odrive中自带一个简单的梯形速度爬坡算法，本文分析下这部分代码。

代码如下：

#include <cmath>
#include "odrive_main.h"
#include "utils.hpp"// A sign function where input 0 has positive sign (not 0)
float sign_hard(float val) {return (std::signbit(val)) ? -1.0f : 1.0f;
}// Symbol                     Description
// Ta, Tv and Td              Duration of the stages of the AL profile
// Xi and Vi                  Adapted initial conditions for the AL profile
// Xf                         Position set-point
// s                          Direction (sign) of the trajectory
// Vmax, Amax, Dmax and jmax  Kinematic bounds
// Ar, Dr and Vr              Reached values of acceleration and velocitybool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,float Vmax, float Amax, float Dmax) {float dX = Xf - Xi;  // Distance to travelfloat stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distancefloat dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacementfloat s = sign_hard(dX - dXstop); // Sign of coast velocity (if any)Ar_ = s * Amax;  // Maximum Acceleration (signed)Dr_ = -s * Dmax; // Maximum Deceleration (signed)Vr_ = s * Vmax;  // Maximum Velocity (signed)// If we start with a speed faster than cruising, then we need to decel instead of accel// aka "double deceleration move" in the paperif ((s * Vi) > (s * Vr_)) {Ar_ = -s * Amax;}// Time to accel/decel to/from Vr (cruise speed)Ta_ = (Vr_ - Vi) / Ar_;Td_ = -Vr_ / Dr_;// Integral of velocity ramps over the full accel and decel times to get// minimum displacement required to reach cuising speedfloat dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_;// Are we displacing enough to reach cruising speed?if (s*dX < s*dXmin) {// Short move (triangle profile)Vr_ = s * std::sqrt(std::max((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);Td_ = std::max(0.0f, -Vr_ / Dr_);Tv_ = 0.0f;} else {// Long move (trapezoidal profile)Tv_ = (dX - dXmin) / Vr_;}// Fill in the rest of the values used at evaluation-timeTf_ = Ta_ + Tv_ + Td_;Xi_ = Xi;Xf_ = Xf;Vi_ = Vi;yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); // pos at end of accel phasereturn true;
}TrapezoidalTrajectory::Step_t TrapezoidalTrajectory::eval(float t) {Step_t trajStep;if (t < 0.0f) {  // Initial ConditiontrajStep.Y   = Xi_;trajStep.Yd  = Vi_;trajStep.Ydd = 0.0f;} else if (t < Ta_) {  // AcceleratingtrajStep.Y   = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t);trajStep.Yd  = Vi_ + Ar_*t;trajStep.Ydd = Ar_;} else if (t < Ta_ + Tv_) {  // CoastingtrajStep.Y   = yAccel_ + Vr_*(t - Ta_);trajStep.Yd  = Vr_;trajStep.Ydd = 0.0f;} else if (t < Tf_) {  // Decelerationfloat td     = t - Tf_;trajStep.Y   = Xf_ + 0.5f*Dr_*SQ(td);trajStep.Yd  = Dr_*td;trajStep.Ydd = Dr_;} else if (t >= Tf_) {  // Final ConditiontrajStep.Y   = Xf_;trajStep.Yd  = 0.0f;trajStep.Ydd = 0.0f;} else {// TODO: report error here}return trajStep;
}

首先当需要控制电机运动到某个位置时，会调用函数，该函数会调用上面的函数planTrapezoidal。

void Controller::move_to_pos(float goal_point) {axis_->trap_traj_.planTrapezoidal(goal_point, pos_setpoint_, vel_setpoint_,axis_->trap_traj_.config_.vel_limit,axis_->trap_traj_.config_.accel_limit,axis_->trap_traj_.config_.decel_limit);axis_->trap_traj_.t_ = 0.0f;trajectory_done_ = false;
}

然后会在control对象中调用eval函数不断的计算出下一时刻的目标位置和速度。

        case INPUT_MODE_TRAP_TRAJ: {if(input_pos_updated_){move_to_pos(input_pos_);input_pos_updated_ = false;}// Avoid updating uninitialized trajectoryif (trajectory_done_)break;if (axis_->trap_traj_.t_ > axis_->trap_traj_.Tf_) {// Drop into position control mode when done to avoid problems on loop counter delta overflowconfig_.control_mode = CONTROL_MODE_POSITION_CONTROL;pos_setpoint_ = axis_->trap_traj_.Xf_;vel_setpoint_ = 0.0f;torque_setpoint_ = 0.0f;trajectory_done_ = true;} else {TrapezoidalTrajectory::Step_t traj_step = axis_->trap_traj_.eval(axis_->trap_traj_.t_);pos_setpoint_ = traj_step.Y;vel_setpoint_ = traj_step.Yd;torque_setpoint_ = traj_step.Ydd * config_.inertia;axis_->trap_traj_.t_ += current_meas_period;}

那么关键就是两个函数planTrapezoidal和函数eval，当位置更新时调用前者，周期性调用后者，后者的输出更新到位置闭环和速度闭环中实现轨迹跟随。

planTrapezoidal代码分析如下：

    //计算出加速阶段和减速阶段需要的事件Ta_ = (Vr_ - Vi) / Ar_;Td_ = -Vr_ / Dr_;//如果能跑到最大速度，那么计算加速阶段和减速阶段运行的位移float dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_;if (s*dX < s*dXmin) {//如果是短位移，这里算出三角规划的速度，这里看下面的公式推导Vr_ = s * std::sqrt(std::max((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));//重新计算加速时间和减速时间，匀速阶段为0Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);Td_ = std::max(0.0f, -Vr_ / Dr_);Tv_ = 0.0f;} else {//如果是长位移，那么走梯形速度，这里得出匀速阶段的时间Tv_ = (dX - dXmin) / Vr_;}//计算出本次规划需要的总时间Tf_ = Ta_ + Tv_ + Td_;Xi_ = Xi;Xf_ = Xf;Vi_ = Vi;//计算加速阶段结束时的位置，即加速阶段的位移。yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); 

• 三角规划公式推导如下：
• 

eval代码分析如下：

    if (t < 0.0f) {  //初始条件，不会进入trajStep.Y   = Xi_;trajStep.Yd  = Vi_;trajStep.Ydd = 0.0f;} else if (t < Ta_) {  //加速阶段trajStep.Y   = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t);  //按照加速阶段计算当前时刻的位置trajStep.Yd  = Vi_ + Ar_*t;   //一阶导数，即当前时刻的速度trajStep.Ydd = Ar_;           //二阶导数即当前时刻的加速度} else if (t < Ta_ + Tv_) {  //匀速阶段trajStep.Y   = yAccel_ + Vr_*(t - Ta_);  //按照匀速阶段计算当前时刻的位置trajStep.Yd  = Vr_;     //一阶导数，即当前时刻的速度trajStep.Ydd = 0.0f;    //二阶导数即当前时刻的加速度为0} else if (t < Tf_) {  //减速阶段float td     = t - Tf_;trajStep.Y   = Xf_ + 0.5f*Dr_*SQ(td);  //按照减速阶段计算当前时刻的位置trajStep.Yd  = Dr_*td;   //一阶导数，即当前时刻的速度trajStep.Ydd = Dr_;      //二阶导数即当前时刻的减速度} else if (t >= Tf_) {  //规划完成trajStep.Y   = Xf_;    trajStep.Yd  = 0.0f; trajStep.Ydd = 0.0f;} else {// TODO: report error here}