536 lines
14 KiB
Markdown
536 lines
14 KiB
Markdown
# 电机控制方法指南
|
||
|
||
## 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 速度闭环控制（使用）
|
||
|
||
**命令格式**：
|
||
```c
|
||
#define COMMAND_SPEED_CONTROL(speedControl)
|
||
```
|
||
|
||
**参数说明**：
|
||
- `speedControl`: int32_t，速度控制量
|
||
- 分辨率：0.01 DPS/LSB
|
||
- 范围：-204800 ~ +204800 (对应 -2048 ~ +2048 DPS)
|
||
|
||
**使用示例**：
|
||
```c
|
||
// 设置电机速度为 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 转矩闭环控制
|
||
|
||
**命令格式**：
|
||
```c
|
||
#define COMMAND_TORQUE_CONTROL(iqControl)
|
||
```
|
||
|
||
**参数说明**：
|
||
- `iqControl`: int16_t，转矩控制量
|
||
- 范围：-2048 ~ +2048
|
||
|
||
#### 2.1.3 位置闭环控制
|
||
|
||
**单圈位置控制**：
|
||
```c
|
||
#define COMMAND_SINGLERING_CONTROL_1(spinDirection, angleControl)
|
||
```
|
||
|
||
**增量位置控制**：
|
||
```c
|
||
#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 结构定义
|
||
|
||
```c
|
||
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 电机）：
|
||
```c
|
||
#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 计算流程
|
||
|
||
```c
|
||
// 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 更新算法**：
|
||
```c
|
||
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 (根据配置)
|
||
|
||
**初始化**：
|
||
```c
|
||
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);
|
||
}
|
||
```
|
||
|
||
**发送函数**：
|
||
```c
|
||
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);
|
||
}
|
||
```
|
||
|
||
**接收函数**：
|
||
```c
|
||
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 电机驱动
|
||
|
||
**电机初始化**：
|
||
```c
|
||
mf4010v2_t motor1 = mf4010v2_init(0x141); // 电机 ID: 0x141
|
||
mf4010v2_t motor2 = mf4010v2_init(0x142); // 电机 ID: 0x142
|
||
```
|
||
|
||
**发送命令并接收响应**：
|
||
```c
|
||
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);
|
||
}
|
||
```
|
||
|
||
**常用命令宏**：
|
||
```c
|
||
// 电机启动
|
||
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 数据格式**：
|
||
```c
|
||
// 转换为 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 (软件控制，用于切换地址)
|
||
|
||
**初始化**：
|
||
```c
|
||
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;
|
||
}
|
||
```
|
||
|
||
**获取姿态数据**：
|
||
```c
|
||
// 获取加速度
|
||
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 控制流程
|
||
|
||
```c
|
||
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 控制循环
|
||
|
||
**主循环结构**：
|
||
```c
|
||
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 参数整定步骤
|
||
|
||
1. **初始化设置**
|
||
- 设置 Ki = 0, Kd = 0
|
||
- 设置较小的 Kp (如 1.0)
|
||
|
||
2. **调整比例增益 Kp**
|
||
- 逐渐增大 Kp
|
||
- 观察系统响应
|
||
- 当出现轻微振荡时，减小 20%
|
||
|
||
3. **调整积分增益 Ki**
|
||
- 如果有稳态误差，逐渐增大 Ki
|
||
- 注意防止积分饱和
|
||
|
||
4. **调整微分增益 Kd**（可选）
|
||
- 如果系统振荡，可以加入 Kd
|
||
- 设置合适的滤波时间常数 tau
|
||
|
||
5. **验证与微调**
|
||
- 测试不同工况
|
||
- 微调参数获得最佳性能
|
||
|
||
### 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. 参考资料
|
||
|
||
- [MF4010V2 电机数据手册](#)
|
||
- [MPU6050 数据手册](https://invensense.tdk.com/)
|
||
- [STM32F103 参考手册](https://www.st.com/)
|
||
- [CAN 总线协议规范](https://www.can-cia.org/)
|
||
|
||
---
|
||
|
||
**文档版本**: v1.0
|
||
**创建日期**: 2026-03-30
|
||
**作者**: CloudPlant Team
|
||
**基于**: MotorController 项目代码
|