Files

CloudPlant Team 1c2d945a6d 重构项目：移除JY901传感器，添加CAN总线支持，集成电机/传感器模块库

2026-04-13 22:49:49 +08:00

14 KiB

Raw Blame History

电机控制方法指南

1. 系统概述

本电机控制系统基于 STM32F103 微控制器，采用 PID 控制算法 实现对 MF4010V2 无刷电机 的精确控制。系统通过 MPU6050 姿态传感器获取云台姿态，使用 CAN 总线 与电机通信。

1.1 系统架构

┌─────────────┐    ┌──────────┐    ┌─────────┐    ┌──────────┐
│  MPU6050    │───>│  PID 算法 │───>│ CAN 总线 │───>│MF4010V2  │
│ 姿态传感器  │    │ 控制器   │    │ 通信    │    │ 无刷电机 │
└─────────────┘    └──────────┘    └─────────┘    └──────────┘
     I2C              计算            CAN            执行

1.2 硬件组成

组件	型号	接口	说明
MCU	STM32F103xC/xE	-	主控制器，72MHz
电机	MF4010V2	CAN	无刷力矩电机，ID: 0x141/0x142
姿态传感器	MPU6050	I2C	6 轴 IMU（加速度计 + 陀螺仪）
CAN 收发器	TJA1050	CAN	CAN 总线物理层

2. 控制方法

2.1 控制模式

MF4010V2 电机支持多种控制模式：

2.1.1 速度闭环控制（使用）

命令格式：

#define COMMAND_SPEED_CONTROL(speedControl)

参数说明：

speedControl: int32_t，速度控制量
分辨率：0.01 DPS/LSB
范围：-204800 ~ +204800 (对应 -2048 ~ +2048 DPS)

使用示例：

// 设置电机速度为 100 DPS
int32_t speed = 10000;  // 100 * 100
uint8_t cmd[8] = COMMAND_SPEED_CONTROL(speed);
mf4010v2_send_command(&motor, cmd, 0);

2.1.2 转矩闭环控制

命令格式：

#define COMMAND_TORQUE_CONTROL(iqControl)

参数说明：

iqControl: int16_t，转矩控制量
范围：-2048 ~ +2048

2.1.3 位置闭环控制

单圈位置控制：

#define COMMAND_SINGLERING_CONTROL_1(spinDirection, angleControl)

增量位置控制：

#define COMMAND_POSITION_ADD_CONTROL(angleIncrement)

参数说明：

angleControl: uint32_t，目标角度
分辨率：0.01°/LSB (36000 = 360°)
spinDirection: 旋转方向 (0x00 顺时针，0x01 逆时针)

2.2 PID 控制算法

系统使用 增量式 PID 控制算法，具有抗积分饱和和微分滤波功能。

2.2.1 PID 结构定义

typedef struct {
    float Kp;           // 比例增益
    float Ki;           // 积分增益
    float Kd;           // 微分增益
    float tau;          // 微分低通滤波器时间常数
    float limMin;       // 输出最小值
    float limMax;       // 输出最大值
    float limMinInt;    // 积分最小值
    float limMaxInt;    // 积分最大值
    float T;            // 采样时间 (秒)
    
    // 控制器状态
    float integrator;   // 积分项
    float prevError;    // 上一次误差
    float differentiator; // 微分项
    float prevMeasurement; // 上一次测量值
    float out;          // 控制器输出
} PIDController;

2.2.2 PID 参数整定

默认参数（针对 MF4010V2 电机）：

#define PID_KP1  3.5f    // 比例增益
#define PID_KI1  1.2f    // 积分增益
#define PID_KD1  0.0f    // 微分增益（暂不使用）
#define PID_TAU1 0.02f   // 微分滤波时间常数

#define PID_LIM_MIN1    -500.0f   // 输出下限
#define PID_LIM_MAX1     500.0f   // 输出上限
#define PID_LIM_MIN_INT1 -100.0f  // 积分下限
#define PID_LIM_MAX_INT1  100.0f  // 积分上限

#define SAMPLE_TIME_S    0.1f     // 采样时间 100ms

2.2.3 PID 计算流程

// 1. 初始化
PIDController pid;
pid.Kp = 3.5f;
pid.Ki = 1.2f;
pid.Kd = 0.0f;
pid.T = 0.1f;
// ... 其他参数
PIDController_Init(&pid);

// 2. 控制循环中更新
float output = PIDController_Update(&pid, setpoint, measurement);

PID 更新算法：

float PIDController_Update(PIDController *pid, float setpoint, float measurement)
{
    // 计算误差
    float error = setpoint - measurement;
    
    // 比例项
    float pTerm = pid->Kp * error;
    
    // 积分项（带抗饱和）
    pid->integrator += error * pid->T;
    if(pid->integrator > pid->limMaxInt)
        pid->integrator = pid->limMaxInt;
    else if(pid->integrator < pid->limMinInt)
        pid->integrator = pid->limMinInt;
    
    float iTerm = pid->Ki * pid->integrator;
    
    // 微分项（带低通滤波）
    float dTerm = (pid->Kd * (measurement - pid->prevMeasurement)) / 
                  (pid->T + pid->tau);
    
    // 总输出
    pid->out = pTerm + iTerm + dTerm;
    
    // 输出限幅
    if(pid->out > pid->limMax)
        pid->out = pid->limMax;
    else if(pid->out < pid->limMin)
        pid->out = pid->limMin;
    
    // 保存状态
    pid->prevError = error;
    pid->prevMeasurement = measurement;
    
    return pid->out;
}

3. 硬件驱动层

3.1 CAN 总线驱动

硬件连接：

CAN_TX: PA12
CAN_RX: PA11
波特率：125kbps (根据配置)

初始化：

void can1_init()
{
    // GPIO 配置
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN1, ENABLE);
    
    // CAN 配置
    CAN_InitTypeDef can_init_struct;
    can_init_struct.CAN_Mode = CAN_Mode_Normal;
    can_init_struct.CAN_SJW  = CAN_SJW_1tq;
    can_init_struct.CAN_BS1  = CAN_BS1_2tq;
    can_init_struct.CAN_BS2  = CAN_BS2_3tq;
    can_init_struct.CAN_Prescaler = 6;  // 波特率配置
    CAN_Init(CAN1, &can_init_struct);
    
    // 过滤器配置（接收所有帧）
    CAN_FilterInitTypeDef can_filter_init_struct;
    can_filter_init_struct.CAN_FilterNumber = 0;
    can_filter_init_struct.CAN_FilterMode   = CAN_FilterMode_IdMask;
    can_filter_init_struct.CAN_FilterScale  = CAN_FilterScale_32bit;
    can_filter_init_struct.CAN_FilterIdHigh = 0x0000;
    can_filter_init_struct.CAN_FilterMaskIdHigh = 0x0000;
    can_filter_init_struct.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&can_filter_init_struct);
}

发送函数：

void can1_send_message(uint16_t id, uint8_t* data, uint8_t len)
{
    CanTxMsg TxMessage;
    TxMessage.StdId = id;
    TxMessage.IDE   = CAN_Id_Standard;
    TxMessage.RTR   = CAN_RTR_Data;
    TxMessage.DLC   = len;
    
    for(int i = 0; i < len; i++)
        TxMessage.Data[i] = data[i];
    
    CAN_Transmit(CAN1, &TxMessage);
}

接收函数：

CanRxMsg can1_receive_message()
{
    CanRxMsg rx;
    if(CAN_MessagePending(CAN1)) {
        CAN_Receive(CAN1, CAN_FIFO0, &rx);
        return rx;
    }
    // 返回错误标识
    rx.StdId = 0xFFFF;
    return rx;
}

3.2 MF4010V2 电机驱动

电机初始化：

mf4010v2_t motor1 = mf4010v2_init(0x141);  // 电机 ID: 0x141
mf4010v2_t motor2 = mf4010v2_init(0x142);  // 电机 ID: 0x142

发送命令并接收响应：

void mf4010v2_send_command(mf4010v2_t* mf4010v2, 
                           uint8_t command[8], 
                           uint8_t create_json)
{
    // 保存命令
    for(int i = 0; i < 8; i++)
        mf4010v2->command[i] = command[i];
    
    // 通过 CAN 发送
    can1_send_message(mf4010v2->id, command, 8);
    
    // 接收响应
    CanRxMsg rx = can1_receive_message();
    for(int i = 0; i < 8; i++)
        mf4010v2->return_code[i] = rx.Data[i];
    
    // 可选：创建 JSON 格式数据
    if(create_json)
        mf4010v2_to_json(mf4010v2);
}

常用命令宏：

// 电机启动
COMMAND_USEAGE(&motor1, COMMAND_MOTOR_RUNNING);

// 电机停止
COMMAND_USEAGE(&motor1, COMMAND_MOTOR_STOP);

// 电机关闭
COMMAND_USEAGE(&motor1, COMMAND_MOTOR_CLOSE);

// 速度控制
int32_t speed = 5000;  // 50 DPS
COMMAND_USEAGE(&motor1, COMMAND_SPEED_CONTROL(speed));

// 读取 PID 参数
COMMAND_USEAGE(&motor1, COMMAND_READ_PID_PARAMS);

// 写入 PID 参数到 RAM
COMMAND_USEAGE(&motor1, 
    COMMAND_WRITE_PID_PARAMS_RAM(20, 5, 60, 1, 60, 30));

JSON 数据格式：

// 转换为 JSON 格式
void mf4010v2_to_json(mf4010v2_t* mf4010v2)
{
    cJSON* json_root = cJSON_CreateObject();
    cJSON_AddNumberToObject(json_root, "id", mf4010v2->id);
    
    cJSON* json_commands = cJSON_CreateArray();
    for(int i = 0; i < 8; i++)
        cJSON_AddItemToArray(json_commands, 
            cJSON_CreateNumber(mf4010v2->command[i]));
    cJSON_AddItemToObject(json_root, "commands", json_commands);
    
    cJSON* json_return_codes = cJSON_CreateArray();
    for(int i = 0; i < 8; i++)
        cJSON_AddItemToArray(json_return_codes, 
            cJSON_CreateNumber(mf4010v2->return_code[i]));
    cJSON_AddItemToObject(json_root, "return_codes", json_return_codes);
    
    mf4010v2->json_data = json_root;
}

// 打印 JSON
char* str = mf4010v2_print_json(&motor1);
printf("%s\n", str);
cJSON_free(str);

3.3 MPU6050 姿态传感器

硬件连接：

SCL: PB6 (I2C1_SCL)
SDA: PB7 (I2C1_SDA)
AD0: PF12 (软件控制，用于切换地址)

初始化：

u8 MPU_Init(void)
{
    // GPIO 初始化
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOF, ENABLE);
    GPIO_InitTypeDef gpio;
    gpio.GPIO_Pin = GPIO_Pin_12;
    gpio.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_Init(GPIOF, &gpio);
    
    MPU_AD0_CTRL = 0;  // 设置 AD0 为低电平，地址 0x68
    
    MPU_IIC_Init();    // 初始化 I2C
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0x80);  // 复位
    mirco_delay_ms(100);
    
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0x00);  // 唤醒
    MPU_Set_Gyro_Fsr(3);   // 2000 DPS
    MPU_Set_Accel_Fsr(0);  // ±2g
    MPU_Set_Rate(50);      // 50Hz 采样率
    
    // 验证设备 ID
    u8 id = MPU_Read_Byte(MPU_DEVICE_ID_REG);
    if(id != MPU_ADDR)
        return 1;  // 设备不存在
    
    return 0;
}

获取姿态数据：

// 获取加速度
void MPU_Get_Accelerometer(short* ax, short* ay, short* az)
{
    *ax = MPU_Read_ACCEL_XOUT_H();
    *ay = MPU_Read_ACCEL_YOUT_H();
    *az = MPU_Read_ACCEL_ZOUT_H();
}

// 获取角速度
void MPU_Get_Gyroscope(short* gx, short* gy, short* gz)
{
    *gx = MPU_Read_GYRO_XOUT_H();
    *gy = MPU_Read_GYRO_YOUT_H();
    *gz = MPU_Read_GYRO_ZOUT_H();
}

4. 应用层控制逻辑

4.1 控制流程

void app_control(uint8_t enable_motor1, float measurement1, float setpoint1,
                 uint8_t enable_motor2, float measurement2, float setpoint2)
{
    // 1. PID 计算
    float output_motor1 = PIDController_Update(&pidControllerMotor1, 
                                               setpoint1, measurement1);
    float output_motor2 = PIDController_Update(&pidControllerMotor2, 
                                               setpoint2, measurement2);
    
    // 2. 电机 1 控制
    if(enable_motor1) {
        int32_t speed1 = (int32_t)(output_motor1 * 100);  // 转换为 0.01 DPS 单位
        uint8_t cmd1[8] = COMMAND_SPEED_CONTROL(speed1);
        mf4010v2_send_command(&motor1, cmd1, 0);
    }
    
    // 3. 电机 2 控制（带死区补偿）
    if(enable_motor2) {
        int32_t speed2 = (int32_t)(output_motor2 * 100);
        // 死区补偿：当测量值接近 0 时停止电机
        if(measurement2 < 0.8 && measurement2 > -0.8)
            speed2 = 0;
        uint8_t cmd2[8] = COMMAND_SPEED_CONTROL(speed2);
        mf4010v2_send_command(&motor2, cmd2, 0);
    }
}

4.2 控制循环

主循环结构：

void main_loop()
{
    float target_angle_pitch = 0.0f;  // 目标俯仰角 0°
    float target_angle_roll  = 0.0f;  // 目标横滚角 0°
    
    while(1) {
        // 1. 读取当前姿态
        float current_pitch = get_pitch_angle();
        float current_roll  = get_roll_angle();
        
        // 2. PID 控制计算
        app_control(1, current_pitch, target_angle_pitch,
                    1, current_roll, target_angle_roll);
        
        // 3. 延时（保持控制周期）
        mirco_delay_ms(100);  // 10Hz 控制频率
    }
}

5. 调试与优化

5.1 PID 参数整定步骤

初始化设置
- 设置 Ki = 0, Kd = 0
- 设置较小的 Kp (如 1.0)
调整比例增益 Kp
- 逐渐增大 Kp
- 观察系统响应
- 当出现轻微振荡时，减小 20%
调整积分增益 Ki
- 如果有稳态误差，逐渐增大 Ki
- 注意防止积分饱和
调整微分增益 Kd（可选）
- 如果系统振荡，可以加入 Kd
- 设置合适的滤波时间常数 tau
验证与微调
- 测试不同工况
- 微调参数获得最佳性能

5.2 常见问题

Q1: 电机抖动

降低 Kp 值
增加速度反馈滤波
检查机械连接是否松动

Q2: 响应太慢

增加 Kp
提高控制频率（减小采样时间 T）
检查电机驱动电流是否足够

Q3: 稳态误差

增加 Ki（注意抗饱和）
检查传感器零点校准
检查机械结构是否水平

Q4: CAN 通信失败

检查 CAN 波特率配置
检查终端电阻（120Ω）
检查电机 ID 是否正确

6. 性能指标

指标	数值	说明
控制频率	10Hz	可调整（SAMPLE_TIME_S）
速度分辨率	0.01 DPS	MF4010V2 速度控制精度
角度分辨率	0.01°	位置控制精度
CAN 波特率	125kbps	可配置
PID 输出范围	±500	可配置（limMin/limMax）

7. 参考资料

文档版本: v1.0
创建日期: 2026-03-30
作者: CloudPlant Team
基于: MotorController 项目代码

14 KiB Raw Blame History Unescape Escape

电机控制方法指南

1. 系统概述

1.1 系统架构

1.2 硬件组成

2. 控制方法

2.1 控制模式

2.1.1 速度闭环控制（使用）

2.1.2 转矩闭环控制

2.1.3 位置闭环控制

2.2 PID 控制算法

2.2.1 PID 结构定义

2.2.2 PID 参数整定

2.2.3 PID 计算流程

3. 硬件驱动层

3.1 CAN 总线驱动

3.2 MF4010V2 电机驱动

3.3 MPU6050 姿态传感器

4. 应用层控制逻辑

4.1 控制流程

4.2 控制循环

5. 调试与优化

5.1 PID 参数整定步骤

5.2 常见问题

6. 性能指标

7. 参考资料

14 KiB

Raw Blame History