电机速度控制的爬坡算法
本帖最后由 keer_zu 于 2025-2-25 14:48 编辑本帖最后由 keer_zu 于 2025-2-25 14:46 编辑
```
// 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 jmaxKinematic 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时，可能需要先减速再加速，这在代码中已经处理了。此外，当位置不足时，可能会使用三角形轨迹来缩短时间。
总结一下，梯形轨迹规划和评估函数为电机控制的爬坡算法提供了基础。通过合理规划加速、匀速和减速阶段，可以实现平滑的启动和停止，减少振动和过载，提高系统的效率和稳定性。
接下来，我需要将这些思考整理成结构化的笔记，确保内容清晰易懂。可能需要分阶段解释，包括规划和评估部分，以及它们在爬坡过程中的应用。
## 电机控制的爬坡算法笔记
1. #### 梯形轨迹规划的基本概念
梯形轨迹是一种常用的运动控制策略，广泛应用于电机控制中。它将运动分为三个阶段：
加速阶段：从初始速度逐渐增加到目标速度。
匀速阶段：以目标速度保持恒定。
减速阶段：从目标速度逐渐减少到停止速度。
这种控制策略可以平滑地启动和停止电机，减少振动并提高运动的稳定性。
2. #### 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. 爬坡算法的应用
在电机控制的爬坡过程中，梯形轨迹规划和评估函数可以帮助实现平滑的启动和停止：
加速阶段：电机从静止开始加速到目标速度，使用最大加速度。
匀速阶段：电机以目标速度运行，保持恒定速度。
减速阶段：电机从目标速度减速到停止，使用最大减速。
通过合理规划各阶段的参数，可以有效减少振动和过载，提高系统的效率和稳定性。
6. #### 总结
梯形轨迹规划和评估函数为电机控制的爬坡算法提供了基础。通过合理规划加速、匀速和减速阶段，可以实现平滑的启动和停止，减少对系统的影响并提高控制精度。
这两个函数确实为电机速度控制提供了一个很好的梯形轨迹规划和评估框架。planTrapezoidal函数通过计算加速、匀速和减速阶段的时间和参数，为电机提供了平滑的启动和停止过程。eval函数则实时反馈电机状态，为控制策略的调整提供了依据。
页:
[1]