PMSM FOC位置环S曲线规划：从急动度约束到代码实现

1. 为什么需要S曲线规划

做电机控制的朋友应该都遇到过这样的场景：当你给电机发送一个位置指令时，如果直接让电机从当前位置跳到目标位置，电机就会像被踹了一脚似的突然启动，到达目标位置时又来个急刹车。这种粗暴的控制方式不仅会让电机产生剧烈震动，长期使用还会缩短电机寿命。

我在实际项目中就吃过这个亏。当时用普通梯形速度曲线控制伺服电机，电机运转时总是发出"咯吱咯吱"的异响，机械臂末端还会轻微抖动。后来改用S曲线规划后，这些问题都迎刃而解了。

S曲线规划的核心思想是让速度变化更平滑。想象一下老司机开车：起步时缓慢踩油门，速度逐渐提升；快到目的地时提前松油门，让车自然减速。这种控制方式既舒适又省油，S曲线规划就是电机控制界的"老司机技巧"。

2. 从急动度到S曲线的数学原理

2.1 什么是急动度

急动度(Jerk)这个概念可能有些朋友不太熟悉。简单来说，急动度就是加速度的变化率。就像加速度是速度的一阶导数，急动度就是加速度的一阶导数，或者说速度的二阶导数。

用数学公式表示就是： J = da/dt = d²v/dt²

为什么要关心急动度呢？因为在实际物理系统中，加速度不可能瞬间改变。比如汽车加速时，油门踩下去后加速度是逐渐增加的，这个变化过程就由急动度决定。

2.2 恒定急动度的S曲线生成

恒定急动度的S曲线规划分为三个阶段：

1. 加速阶段：急动度保持恒定正值，加速度线性增加
2. 匀速阶段：急动度为零，加速度保持恒定
3. 减速阶段：急动度保持恒定负值，加速度线性减小

我用Matlab做了个仿真，设置J=1、A=1（单位控制时间），得到的速度和位置曲线如下：

% 参数设置 J = 1; % 急动度 A = 1; % 时间参数 t = 0:0.01:12*A; % 时间向量 % 初始化变量 a = zeros(size(t)); v = zeros(size(t)); s = zeros(size(t)); % 分段计算 for i = 1:length(t) if t(i) < A % 第一阶段：加速度增加 a(i) = J*t(i); v(i) = 0.5*J*t(i)^2; s(i) = (1/6)*J*t(i)^3; elseif t(i) < 2*A % 第二阶段：加速度恒定 a(i) = J*A; v(i) = 0.5*J*A^2 + J*A*(t(i)-A); s(i) = (1/6)*J*A^3 + 0.5*J*A^2*(t(i)-A) + 0.5*J*A*(t(i)-A)^2; elseif t(i) < 3*A % 第三阶段：加速度减小 a(i) = J*A - J*(t(i)-2*A); v(i) = 1.5*J*A^2 + J*A*(t(i)-2*A) - 0.5*J*(t(i)-2*A)^2; s(i) = (7/6)*J*A^3 + 1.5*J*A^2*(t(i)-2*A) + 0.5*J*A*(t(i)-2*A)^2 - (1/6)*J*(t(i)-2*A)^3; else % 匀速阶段 a(i) = 0; v(i) = 2*J*A^2; s(i) = 3*J*A^3 + 2*J*A^2*(t(i)-3*A); end end

从仿真结果可以看到，位置曲线呈现出完美的S形，这正是我们想要的平滑运动轨迹。

3. 嵌入式C代码实现

3.1 数据结构设计

在实际嵌入式系统中，我们需要设计合适的数据结构来存储S曲线规划的各种参数。参考ST的X-CUBE-MCSDK库，我设计了如下结构体：

typedef enum { TC_READY_FOR_COMMAND = 0, TC_MOVEMENT_ON_GOING = 1, TC_TARGET_POSITION_REACHED = 2, } PosCtrlStatus_t; typedef struct { float SamplingTime; // 采样时间 float MovementDuration; // 总运动时间 float SubStep[6]; // 各阶段时间节点 float SubStepDuration; // 子阶段持续时间 float ElapseTime; // 已运行时间 float Jerk; // 急动度 float CruiseSpeed; // 巡航速度 float Acceleration; // 当前加速度 float Omega; // 当前速度 float OmegaPrev; // 前一次速度 float ThetaPrev; // 前一次角度 int16_t StartingAngle; // 起始角度 int16_t FinalAngle; // 目标角度 int16_t AngleStep; // 角度变化量 int16_t Theta; // 当前角度 int16_t hMaxTorque; // 最大扭矩 int16_t flag; // 标志位 PosCtrlStatus_t PositionCtrlStatus; // 状态 PID_Handle_t *PIPos; // PID控制器 } PosControl_Handle_t;

这个结构体包含了S曲线规划所需的所有参数，从急动度到各个阶段的时间节点，再到当前的运动状态。

3.2 参数初始化函数

在开始运动前，我们需要计算各种参数：

void TC_MoveCommand(PosControl_Handle_t *pHandle, int16_t startingAngle, int16_t angleStep, float movementDuration) { float fMinimumStepDuration; if ((pHandle->PositionCtrlStatus == TC_READY_FOR_COMMAND) && (movementDuration > 0)) { // 计算最小时间单位 fMinimumStepDuration = (9.0f * pHandle->SamplingTime); // 调整运动时间为最小时间单位的整数倍 pHandle->MovementDuration = (float)((int)(movementDuration / fMinimumStepDuration)) * fMinimumStepDuration; // 设置角度参数 pHandle->StartingAngle = startingAngle; pHandle->AngleStep = angleStep; pHandle->FinalAngle = startingAngle + angleStep; // 将总时间分为9段 pHandle->SubStepDuration = pHandle->MovementDuration / 9.0f; // 设置各阶段时间节点 pHandle->SubStep[0] = 1 * pHandle->SubStepDuration; // 加速阶段1 pHandle->SubStep[1] = 2 * pHandle->SubStepDuration; // 加速阶段2 pHandle->SubStep[2] = 3 * pHandle->SubStepDuration; // 加速阶段3 pHandle->SubStep[3] = 6 * pHandle->SubStepDuration; // 减速阶段1 pHandle->SubStep[4] = 7 * pHandle->SubStepDuration; // 减速阶段2 pHandle->SubStep[5] = 8 * pHandle->SubStepDuration; // 减速阶段3 // 计算急动度 J = Δθ/(12*A³) pHandle->Jerk = pHandle->AngleStep / (12 * pHandle->SubStepDuration * pHandle->SubStepDuration * pHandle->SubStepDuration); // 计算巡航速度 v = 2*J*A² pHandle->CruiseSpeed = 2 * pHandle->Jerk * pHandle->SubStepDuration * pHandle->SubStepDuration; // 初始化状态变量 pHandle->ElapseTime = 0.0f; pHandle->Omega = 0.0f; pHandle->Acceleration = 0.0f; pHandle->Theta = startingAngle; pHandle->PositionCtrlStatus = TC_MOVEMENT_ON_GOING; } }

这个函数完成了所有前期计算工作，包括急动度、巡航速度的计算，以及各阶段时间节点的设置。

3.3 实时更新函数

在电机运行过程中，我们需要在每个控制周期更新电机的位置指令：

void TC_MoveExecution(PosControl_Handle_t *pHandle) { float jerkApplied = 0; if(pHandle->PositionCtrlStatus != TC_MOVEMENT_ON_GOING) { return; } // 判断当前处于哪个阶段 if (pHandle->ElapseTime < pHandle->SubStep[0]) { // 加速阶段1：急动度为正 jerkApplied = pHandle->Jerk; } else if (pHandle->ElapseTime < pHandle->SubStep[1]) { // 加速阶段2：急动度为零 } else if (pHandle->ElapseTime < pHandle->SubStep[2]) { // 加速阶段3：急动度为负 jerkApplied = -(pHandle->Jerk); } else if (pHandle->ElapseTime < pHandle->SubStep[3]) { // 匀速阶段 pHandle->Acceleration = 0.0f; pHandle->Omega = pHandle->CruiseSpeed; } else if (pHandle->ElapseTime < pHandle->SubStep[4]) { // 减速阶段1：急动度为负 jerkApplied = -(pHandle->Jerk); } else if (pHandle->ElapseTime < pHandle->SubStep[5]) { // 减速阶段2：急动度为零 } else if (pHandle->ElapseTime < pHandle->MovementDuration) { // 减速阶段3：急动度为正 jerkApplied = pHandle->Jerk; } else { // 运动结束 jerkApplied = pHandle->Jerk; } // 更新运动状态 pHandle->Acceleration += jerkApplied * pHandle->SamplingTime; pHandle->Omega += pHandle->Acceleration * pHandle->SamplingTime; pHandle->Theta += pHandle->Omega * pHandle->SamplingTime; // 检查是否到达目标位置 int16_t delta = pHandle->Theta - pHandle->FinalAngle; if((delta < 0 && (pHandle->Theta + pHandle->Omega * pHandle->SamplingTime) >= pHandle->FinalAngle) || (delta > 0 && (pHandle->Theta + pHandle->Omega * pHandle->SamplingTime) <= pHandle->FinalAngle) || (delta == 0)) { pHandle->Theta = pHandle->FinalAngle; pHandle->PositionCtrlStatus = TC_TARGET_POSITION_REACHED; return; } pHandle->ElapseTime += pHandle->SamplingTime; }

这个函数根据当前时间判断处于哪个运动阶段，然后应用相应的急动度，更新加速度、速度和位置。

4. 实际应用中的问题与解决方案

4.1 浮点运算误差问题

在嵌入式系统中，浮点运算可能会引入误差。特别是在长时间运行或多圈旋转时，这些误差会累积，导致最终位置与目标位置存在偏差。

我在项目中遇到过这样的情况：电机在接近目标位置时会出现"犹豫"现象，先减速到几乎停止，然后又突然加速一小段。经过分析发现是浮点运算误差导致的。

解决方案有两种：

1. 使用定点数运算代替浮点数运算。比如将角度值放大1000倍，用int32_t类型存储，计算完成后再缩小。

2. 调整时间参数，使计算出的急动度尽可能简单。比如让急动度是2的负整数幂，这样浮点运算可以精确表示。

4.2 实时性优化

S曲线规划需要较多的计算，在资源有限的MCU上可能会影响实时性。我总结了几点优化经验：

1. 预先计算所有常数项，避免运行时重复计算。比如1/6、1/2这样的系数可以预先计算好。

2. 使用查表法替代实时计算。对于固定的S曲线，可以预先计算好位置-时间对照表，运行时直接查表。

3. 合理选择控制周期。太短的周期会增加计算负担，太长的周期会影响控制精度。通常选择1ms左右的周期比较合适。

4.3 多圈旋转处理

前面的代码假设电机只在一圈内旋转。如果需要支持多圈旋转，需要做以下修改：

1. 将角度变量改为32位整数或浮点数。

2. 修改角度差计算逻辑，考虑跨圈情况。

3. 在位置环PID中增加对多圈的处理。

// 多圈角度差计算 int32_t CalculateAngleDifference(int32_t current, int32_t target) { int32_t diff = target - current; // 处理跨圈情况 if(diff > 32768) { diff -= 65536; } else if(diff < -32768) { diff += 65536; } return diff; }

5. 调试技巧与经验分享

在实际调试S曲线位置环时，我总结了一些实用技巧：

1. 先仿真后实机：先用Matlab或Python仿真S曲线，确认算法正确性，再移植到嵌入式系统。

2. 分阶段调试：

   • 先调试加速阶段，确保加速度曲线符合预期
   • 再调试匀速阶段，确认速度稳定
   • 最后调试减速阶段，检查能否平滑停止

3. 关键参数监控：实时监控以下参数：

   • 当前急动度
   • 当前加速度
   • 当前速度
   • 当前位置

4. 使用示波器抓取波形：通过DAC输出关键参数到示波器，直观观察曲线形状。

5. 从简单到复杂：

   • 先实现单圈小角度运动
   • 再实现单圈大角度运动
   • 最后实现多圈运动

我在调试过程中发现，急动度的选择对系统性能影响很大。急动度太小会导致加速过程太长，影响响应速度；急动度太大会导致机械振动。经过多次试验，我找到了一个平衡点：让急动度产生的最大加速度略小于电机和负载能承受的最大加速度。