电机速度控制的爬坡算法

[i=s] 本帖最后由 keer_zu 于 2025-2-25 14:48 编辑 [/i]<br /> <br />

[i=s] 本帖最后由 keer_zu 于 2025-2-25 14:46 编辑 [/i]

// 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 velocity

bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
                                            float Vmax, float Amax, float Dmax) {
    float dX = Xf - Xi;  // Distance to travel
    float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance
    float dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacement
    float 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 paper
    if ((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 speed
    float 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-time
    Tf_ = Ta_ + Tv_ + Td_;
    Xi_ = Xi;
    Xf_ = Xf;
    Vi_ = Vi;
    yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); // pos at end of accel phase

    return true;
}

TrapezoidalTrajectory::Step_t TrapezoidalTrajectory::eval(float t) {
    Step_t trajStep;
    if (t < 0.0f) {  // Initial Condition
        trajStep.Y   = Xi_;
        trajStep.Yd  = Vi_;
        trajStep.Ydd = 0.0f;
    } else if (t < Ta_) {  // Accelerating
        trajStep.Y   = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t);
        trajStep.Yd  = Vi_ + Ar_*t;
        trajStep.Ydd = Ar_;
    } else if (t < Ta_ + Tv_) {  // Coasting
        trajStep.Y   = yAccel_ + Vr_*(t - Ta_);
        trajStep.Yd  = Vr_;
        trajStep.Ydd = 0.0f;
    } else if (t < Tf_) {  // Deceleration
        float td     = t - Tf_;
        trajStep.Y   = Xf_ + 0.5f*Dr_*SQ(td);
        trajStep.Yd  = Dr_*td;
        trajStep.Ydd = Dr_;
    } else if (t >= Tf_) {  // Final Condition
        trajStep.Y   = Xf_;
        trajStep.Yd  = 0.0f;
        trajStep.Ydd = 0.0f;
    } else {
        // TODO: report error here
    }

    return trajStep;
}

根据提供的两个C++函数来分析电机控制的爬坡算法，并整理成容易理解的笔记。首先，我需要理解这两个函数的作用和它们之间的关系。

首先，planTrapezoidal函数用于规划梯形轨迹。它计算了加速、匀速、减速阶段的时间和参数。这在电机控制中，特别是用于运动控制时，非常常见。梯形轨迹可以平滑地控制机械臂或电机的运动，避免了突然的加速或减速带来的冲击，从而提高系统的稳定性。

接下来，eval函数用于评估轨迹的执行情况，给定时间t，返回位置、速度和加速度。这在实际控制中，可以用来反馈系统的状态，以判断是否需要调整后续的控制策略。

现在，要考虑的是电机控制的爬坡算法。爬坡通常指的是电机从停机到满负荷运行的过程，这需要平滑的加速以避免振动或过载。梯形轨迹在这里应该是有用的，因为它允许电机先加速到目标速度，然后匀速运行，最后减速停止，这样可以减少振动和过载。

我需要分析这两个函数如何支持爬坡算法。首先，planTrapezoidal函数中的参数Vi是初始速度，通常为0，因为 startup 时电机可能处于静止状态。Vmax是最大速度，Amax是最大加速度，Dmax是最大减速，这些参数都会影响加速和减速的阶段。

在planTrapezoidal中，计算了加速时间Ta、匀速时间Tv、减速时间Td_，以及这些阶段的位移。这可以帮助确定整个爬坡过程所需的时间和能量消耗，从而优化控制策略。

eval函数则在运行过程中提供实时的状态信息，这对于实时控制和反馈调节非常重要。例如，如果电机在加速阶段遇到负载变化，实时评估可以帮助调整后续的控制参数，保持平滑运行。

我还需要考虑一些特殊情况，比如当初始速度Vi大于巡航速度Vr时，可能需要先减速再加速，这在代码中已经处理了。此外，当位置不足时，可能会使用三角形轨迹来缩短时间。

总结一下，梯形轨迹规划和评估函数为电机控制的爬坡算法提供了基础。通过合理规划加速、匀速和减速阶段，可以实现平滑的启动和停止，减少振动和过载，提高系统的效率和稳定性。

接下来，我需要将这些思考整理成结构化的笔记，确保内容清晰易懂。可能需要分阶段解释，包括规划和评估部分，以及它们在爬坡过程中的应用。

电机控制的爬坡算法笔记

0. 梯形轨迹规划的基本概念

梯形轨迹是一种常用的运动控制策略，广泛应用于电机控制中。它将运动分为三个阶段：

加速阶段：从初始速度逐渐增加到目标速度。

匀速阶段：以目标速度保持恒定。

减速阶段：从目标速度逐渐减少到停止速度。

这种控制策略可以平滑地启动和停止电机，减少振动并提高运动的稳定性。

0. planTrapezoidal函数的作用

该函数用于规划梯形轨迹的参数，包括各阶段的时间和加速度、减速等参数。

输入参数：

Xf：目标位置。

Xi：初始位置。

Vi：初始速度。

Vmax：最大速度。

Amax：最大加速度。

Dmax：最大减速。

输出参数：

Ta_：加速时间。

Tv_：匀速时间。

Td_：减速时间。

Ar_：最大加速度。

Dr_：最大减速。

Vr_：巡航速度。

3. 计算过程

位置差：dX = Xf - Xi，计算需要移动的总距离。

最小停止距离：stop_dist = (Vi Vi) / (2 Dmax)，计算在减速到静止所需的最小距离。

方向判断：s = sign_hard(dX - dXstop)，确定运动方向。

加速度和减速计算：Ar = s * Amax 和 Dr = -s * Dmax，分别表示最大加速度和最大减速。

速度调整：如果初始速度大于巡航速度，则调整加速度方向。

时间计算：

加速时间：Ta = (Vr - Vi) / Ar_

减速时间：Td = -Vr / Dr_

匀速时间：Tv = (dX - dXmin) / Vr，其中 dXmin 是加速和减速所需的最小距离。

位移计算：dXmin = 0.5 Ta_ (Vr + Vi) + 0.5 * Tv * Vr_，表示在加速和减速阶段的总位移。

4. eval函数的作用

该函数用于评估轨迹的执行情况，返回在给定时间 t 时的位移、速度和加速度。

初始条件：t < 0 时，位置、速度和加速度均为初始值。

加速阶段：0 ≤ t < Ta_，位置、速度和加速度分别由二次函数、线性函数和常数表示。

匀速阶段：Ta ≤ t < Ta + Tv_，位置、速度和加速度分别为线性函数、常数和零。

减速阶段：Ta + Tv ≤ t < Tf_，位置、速度和加速度分别由二次函数、线性函数和常数表示。

停止条件：t ≥ Tf_，位置、速度和加速度均为最终值。

5. 爬坡算法的应用

在电机控制的爬坡过程中，梯形轨迹规划和评估函数可以帮助实现平滑的启动和停止：

加速阶段：电机从静止开始加速到目标速度，使用最大加速度。

匀速阶段：电机以目标速度运行，保持恒定速度。

减速阶段：电机从目标速度减速到停止，使用最大减速。

通过合理规划各阶段的参数，可以有效减少振动和过载，提高系统的效率和稳定性。

0. 总结

梯形轨迹规划和评估函数为电机控制的爬坡算法提供了基础。通过合理规划加速、匀速和减速阶段，可以实现平滑的启动和停止，减少对系统的影响并提高控制精度。