运动控制--小车的启动和停止算法

一、现实问题

小车在启动时由于受到惯性，后轮和前轮速度不一致，会引起车身不稳。

如小车上面装的是水，会出现倾洒，体验差。

二、数学研究

启动时 $V(t)=\frac{V_max}{(1+exp(-k*(t-t0)))}$

停止时 $V(t)=\frac{V0}{(1+exp(k*(t-t0)))}$

急动度（jerk)

三、BLDC控制与S型曲线的融合逻辑

BLDC电机的速度控制通常采用双闭环结构：外层为速度环（PID控制），内层为电流环（或直接转矩控制）。S型曲线的核心是为速度环提供平滑的目标速度参考，确保电机启动/停止时加速度渐变，避免转矩突变（导致机械冲击或电流过冲）。

关键步骤：

S型曲线生成：根据目标速度（如用户设定的车速）、最大允许加速度/急动度，生成连续的速度参考曲线（如前所述的Sigmoid模型）。
速度环PID控制：实时计算当前速度与S型曲线参考速度的误差，通过PID调节器输出转矩参考（或电流参考）。
电流环控制：将转矩参考转换为三相电流参考，通过PWM调制驱动电机，确保电流平滑跟随，避免转矩脉动。

四、BLDC系统的S型曲线控制架构

典型的BLDC控制系统（结合S型曲线）架构如下：

输入：

目标速度（用户设定）→ S型曲线生成器 → 速度参考v_ref(t)↑当前速度v_measured（编码器/Hall传感器反馈）→ 速度环PID → 转矩参考T_ref↑电流环控制器（FOC/PWM）→ 三相PWM驱动 → BLDC电机

五、代码实现（基于STM32的BLDC S型曲线控制）

以下代码基于STM32F4系列微控制器（支持硬件浮点运算），实现S型曲线生成、速度环PID控制及BLDC换相逻辑。代码包含关键模块的伪代码和实际寄存器操作。

1. 系统参数定义

#include "stm32f4xx_hal.h"
#include <math.h>// BLDC电机参数
#define POLE_PAIRS       4       // 极对数（影响转速计算）
#define ROTOR_RES        0.5f    // 转子电阻（Ohm）
#define L                0.001f  // 电感（H）
#define MAX_TORQUE       2.5f    // 最大转矩（Nm）
#define MAX_CURRENT      10.0f   // 最大相电流（A）// S型曲线参数
#define V_MAX            10.0f   // 目标最大速度（m/s）
#define K_S              0.3f    // S型曲线陡峭系数
#define TOTAL_TIME       10.0f   // 总加速时间（s）
#define T0               (TOTAL_TIME/2.0f)  // S型曲线中心点时间// 控制周期（PWM频率10kHz → 周期100μs）
#define CONTROL_PERIOD   0.0001f  // 秒// 全局变量
float current_time = 0.0f;       // 当前时间（s）
float ref_speed = 0.0f;          // S型曲线参考速度（m/s）
float measured_speed = 0.0f;     // 编码器测量速度（m/s）
float pid_output = 0.0f;         // PID输出转矩参考（Nm）

2. S型曲线参考速度生成

在每个控制周期更新参考速度，确保平滑过渡。

// S型曲线速度计算函数（与Python/C版本逻辑一致）
float sigmoid_ref_speed(float t) {return V_MAX / (1.0f + expf(-K_S * (t - T0)));
}// 每个控制周期调用（更新参考速度）
void update_ref_speed(void) {current_time += CONTROL_PERIOD;  // 累加控制周期时间if (current_time > TOTAL_TIME) {current_time = TOTAL_TIME;   // 限制最大时间（避免超调）}ref_speed = sigmoid_ref_speed(current_time);  // 计算S型曲线速度
}

3. 速度环PID控制器

通过PID调节将当前速度误差转换为转矩参考，需考虑电机动力学特性（如转动惯量）。

// PID参数（需现场调试）
#define KP 2.5f   // 比例系数
#define KI 0.1f   // 积分系数
#define KD 0.5f   // 微分系数float pid_error = 0.0f;      // 速度误差
float pid_integral = 0.0f;   // 积分项
float prev_pid_error = 0.0f; // 上一周期误差// 速度环PID计算
void speed_pid_controller(void) {pid_error = ref_speed - measured_speed;  // 计算误差// 积分项（抗饱和）pid_integral += pid_error * CONTROL_PERIOD;if (pid_integral > 10.0f) pid_integral = 10.0f;  // 积分限幅if (pid_integral < -10.0f) pid_integral = -10.0f;// 微分项（误差变化率）float error_deriv = (pid_error - prev_pid_error) / CONTROL_PERIOD;prev_pid_error = pid_error;// PID输出（转矩参考）pid_output = KP * pid_error + KI * pid_integral + KD * error_deriv;// 转矩限幅（避免过流）if (pid_output > MAX_TORQUE) pid_output = MAX_TORQUE;if (pid_output < -MAX_TORQUE) pid_output = -MAX_TORQUE;
}

4. FOC电流控制（将转矩转换为PWM）

FOC（磁场定向控制）通过坐标变换将三相电流转换为d-q轴电流，直接控制转矩和磁链。此处简化为电流环输出PWM占空比。

// 简化的FOC电流控制（示例）
void foc_current_control(float torque_ref) {// 1. 转矩与d-q轴电流的关系（简化模型）float i_q_ref = torque_ref / (POLE_PAIRS * L * 0.1f);  // 近似计算q轴电流// 2. 电流环PI控制（假设已采样三相电流并转换为i_d, i_q）float i_q_error = i_q_ref - current_i_q;  // 实际i_q与参考的误差float v_q = Kp_current * i_q_error + Ki_current * integral_i_q;  // q轴电压// 3. 逆Park变换（d-q → α-β）float v_alpha = v_d * cos_theta - v_q * sin_theta;float v_beta = v_d * sin_theta + v_q * cos_theta;// 4. 空间矢量PWM（SVPWM）生成svpwm_generate(v_alpha, v_beta);  // 硬件实现SVPWM调制
}

5. BLDC换相逻辑（六步换相模式）

对于低成本BLDC电机，也可采用六步换相控制（无需霍尔传感器时需反电动势检测）。S型曲线的加速度变化需匹配换相频率，避免转矩脉动。

// 六步换相状态机（示例）
typedef enum {STEP_1, STEP_2, STEP_3, STEP_4, STEP_5, STEP_6
} commutation_step_t;commutation_step_t current_step = STEP_1;
uint8_t hall_sensor = 0;  // 霍尔传感器输入（3位）// 根据霍尔信号更新换相状态
void update_commutation(void) {hall_sensor = HAL_GPIO_ReadPin(HALL_PORT, HALL_PINS);  // 读取霍尔信号switch (hall_sensor) {case 0b001: current_step = STEP_1; break;case 0b011: current_step = STEP_2; break;case 0b010: current_step = STEP_3; break;case 0b110: current_step = STEP_4; break;case 0b100: current_step = STEP_5; break;case 0b101: current_step = STEP_6; break;default: current_step = STEP_1;}// 根据当前步设置三相PWM导通（示例：高电平有效）switch (current_step) {case STEP_1: HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_2); break;  // 相A导通case STEP_2: HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2); HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3); break;  // 相B导通// ... 其他步骤类似}
}

6. 主控制循环

int main(void) {HAL_Init();SystemClock_Config();MX_GPIO_Init();MX_TIM1_Init();   // PWM定时器（驱动BLDC）MX_ADC_Init();    // 电流/速度采样ADCwhile (1) {// 1. 采样传感器数据（霍尔/编码器/电流）measured_speed = read_encoder_speed();  // 编码器测速（rpm→m/s）measured_current = read_phase_current();// ADC采样相电流// 2. 更新S型曲线参考速度update_ref_speed();// 3. 速度环PID控制speed_pid_controller();// 4. FOC电流控制（或六步换相）#ifdef USE_FOCfoc_current_control(pid_output);#elseupdate_commutation();  // 六步换相模式#endif// 5. 周期延时（匹配控制周期）HAL_Delay(CONTROL_PERIOD * 1000);  // 转换为ms}
}

六、关键注意事项

动态响应匹配：S型曲线的k参数需与电机的转动惯量（J）匹配。转动惯量大时，需减小k以避免加速度突变导致的超调。
公式：kopt≈Tsample4ζ2J（ζ为阻尼比，Tsample为控制周期）。
电流限制：S型曲线的最大加速度对应最大转矩，需确保电机相电流不超过额定值（通过PID限幅实现）。
无传感器优化：若使用霍尔传感器或无传感器算法（如反电动势观测器），需在S型曲线启动阶段抑制初始转矩脉动（可通过调整初始k值实现）。