码盘融合传感器定位系统技术文档
一、 系统概述
本系统基于STM32F4微控制器,通过融合双编码器(码盘)和XV7011陀螺仪的数据,实现机器人在二维平面上的实时定位。系统采用轮式里程计原理,结合陀螺仪姿态测量,通过坐标变换解算出机器人在世界坐标系中的位置(x, y)和姿态角(yaw)。
核心传感器:
- 2个增量式编码器:测量定位轮转动角度,计算位移
- 1个XV7011陀螺仪:测量角速度,积分获得姿态角
输出结果:
- 实时位置坐标(pos_x, pos_y)
- 实时姿态角(yaw)
- 通过串口USART1/USART3发送数据
代码执行流程图
二、 项目文件结构（只列出重要文件）
d:\..\locter\
│
├── 修改记录2022_10_29.md // 项目修改记录
├── keilkill.bat // Keil项目清理脚本
├── USER/ // 用户代码目录
│ ├── main.c 📌 // 主程序入口
│ ├── stm32f4xx.h // STM32F4系列芯片头文件
│ ├── stm32f4xx_conf.h // 标准库配置文件
│ ├── stm32f4xx_it.c // 中断服务程序实现
│ ├── stm32f4xx_it.h // 中断服务程序头文件
│ ├── system_stm32f4xx.c // 系统时钟配置
│ ├── system_stm32f4xx.h // 系统配置头文件
│ └── JLinkSettings.ini // J-Link调试器配置
│
├── HARDWARE/ // 核心功能文件
│ ├── LOCATOR/ 📌 // 定位算法核心模块
│ │ ├── locator.c // 定位算法实现（核心文件）
│ │ └── locator.h // 定位数据结构和接口定义
│ ├── ENCODER/ 📌 // 编码器模块
│ │ ├── encoder.c // 编码器驱动实现
│ │ └── encoder.h // 编码器接口定义
│ ├── XV7011/ 📌 // 陀螺仪模块
│ │ ├── xv7011.c // XV7011陀螺仪驱动实现
│ │ └── xv7011.h // XV7011接口定义
│ ├── FILTER/ 📌 // 数据滤波模块
│ │ ├── filter.c // 窗口滤波器实现
│ │ └── filter.h // 滤波器接口定义
│ ├── KALMAN/ // 卡尔曼滤波模块
│ │ └── (卡尔曼滤波相关文件，暂无）
│ ├── TIM/ 📌 // 定时器模块
│ │ ├── tim.c // 定时器配置实现
│ │ └── tim.h // 定时器接口定义
│ ├── SPI/ 📌 // SPI通信模块
│ │ ├── bsp_spi.c // SPI底层驱动实现
│ │ └── bsp_spi.h // SPI接口定义
│ ├── CAN/ 📌 // CAN总线通信模块
│ │ ├── can.c // CAN通信协议实现
│ │ └── can.h // CAN接口定义
│ └── LED/ // LED指示灯模块
│ ├── led.c // LED控制实现
│ └── led.h // LED接口定义
│
├── SYSTEM/ 📌 // 系统抽象层
│ ├── delay/ // 延时功能模块
│ │ ├── delay.c // 延时函数实现
│ │ └── delay.h // 延时函数接口
│ ├── sys/ // 系统功能模块
│ │ ├── sys.c // 系统相关函数实现
│ │ └── sys.h // 系统函数接口定义
│ └── usart/ // 串口通信模块
│ ├── usart.c // 串口驱动实现
│ └── usart.h // 串口接口定义
│
├── FWLIB/ // STM32标准外设库
│ └── STM32F4xx_StdPeriph_Driver/ // STM32F4标准驱动
│ ├── inc/ // 头文件目录
│ │ ├── stm32f4xx_rcc.h // 时钟配置（所有模块必需）
│ │ ├── stm32f4xx_gpio.h // GPIO配置（所有硬件接口）
│ │ ├── stm32f4xx_tim.h // 定时器（编码器+系统定时）
│ │ ├── stm32f4xx_spi.h // SPI通信（XV7011陀螺仪）
│ │ ├── stm32f4xx_usart.h // 串口通信（数据输出）
│ │ └── misc.h // 中断控制（定时器中断）
| │ ------------------将来可能会使用的------------------
│ │ ├── stm32f4xx_can.h // CAN通信（如果使用CAN模块）
│ │ ├── stm32f4xx_exti.h // 外部中断（编码器中断模式）
│ │ └── stm32f4xx_dma.h // DMA传输（高速数据传输）
| │
│ └── src/
| ├── stm32f4xx_rcc.c // 时钟配置实现
| ├── stm32f4xx_gpio.c // GPIO配置实现
| ├── stm32f4xx_tim.c // 定时器实现
| ├── stm32f4xx_spi.c // SPI通信实现
| ├── stm32f4xx_usart.c // 串口通信实现
| └── misc.c // 中断控制实现
| ------------------将来可能会使用的------------------
│ ├── stm32f4xx_can.c // CAN通信实现
| ├── stm32f4xx_exti.c // 外部中断实现
| └── stm32f4xx_dma.c // DMA传输实现
│
├── CORE/.. // ARM Cortex-M4内核相关
├── DSP/ // DSP文件夹是官方提供的数字信号处理数学库（Digital Signal Processing），用于优化计算
│ ├── arm_cortexM4lf_math.lib // Cortex-M4浮点数学库
│ ├── inc/ // DSP头文件目录
│ │ ├── arm_common_tables.h // 通用数据表
│ │ ├── arm_const_structs.h // 常量结构体
│ │ └── arm_math.h // DSP数学库主头文件
│ └── src/ // DSP源文件目录
│ └── (DSP算法实现文件)
├── OBJ/.. // 编译输出目录
└── RTE/.. // 运行时环境
三、 核心代码逻辑
3.1 整体架构
系统采用中断驱动 + 主循环轮询的架构:
[TIM3定时中断100μs] → 读取陀螺仪 → 滑动窗口滤波 → 积分姿态角
↓
设置标志位(每10次/20次)
↓
[主循环轮询标志位] → 读取编码器 → 坐标变换计算 → 串口发送结果
模块分工:
- 传感器驱动层:
encoder.c,xv7011.c- 硬件数据采集 - 数据处理层:
filter.c- 滑动窗口滤波消除噪声 - 融合算法层:
locator.c- 核心定位算法,坐标变换 - 任务调度层:
tim.c- 定时器中断控制任务节奏 - 通信层:
usart.c- 数据输出
3.2关键函数解析
3.2.1. 主程序入口
函数: main() (源文件: USER/main.c)
int main(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 中断优先级分组：2位抢占，2位响应
delay_init(168); // SysTick延时初始化（168MHz）
USART3_Init(115200); // 调试串口初始化
Locator_Init(); // 定位系统初始化（核心）
while (1) {
if (locatorCalflag == 1) { // 计算标志（2ms周期）
Locator_Calculate(&locator); // 执行位置解算
locatorCalflag = 0;
}
if (locatorSendflag == 1) { // 发送标志（5ms周期）
Locator_USART_SendData(&locator); // DMA发送定位数据
UsartPrintf(USART3, "%.3f,%.3f,%.3f\n", // 调试输出
locator.pos_x, locator.pos_y, locator.yaw);
locatorSendflag = 0;
}
if (locatorResetflag == 1) { // 复位请求
__set_FAULTMASK(1); // 关闭所有中断
NVIC_SystemReset(); // 软件复位
}
}
}
功能: 系统初始化后进入主循环，轮询标志位执行定位计算和数据发送任务。
3.2.2. 定位系统初始化
函数: Locator_Init() (源文件: HARDWARE/LOCATOR/locator.c)
void Locator_Init(void)
{
Encoder_Init_TIM2(); // 初始化编码器1（TIM2，PA0/PA1）
Encoder_Init_TIM4(); // 初始化编码器2（TIM4，PB6/PB7）
XV7011_Init(); // 初始化陀螺仪（含SPI1配置）
UASRT1_Init(1152000); // 初始化数据串口（含DMA配置）
TIMER_TASK_TIM3_Init(); // 启动10kHz定时器中断
while(locatorInitflag) {} // 阻塞等待零偏标定完成（约1.5秒）
}
功能: 按序初始化所有硬件模块，最后等待陀螺仪自动完成零偏标定。
3.2.3. 编码器接口初始化
函数: Encoder_Init_TIM2() (源文件: HARDWARE/ENCODER/encoder.c)
void Encoder_Init_TIM2(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
// 使能时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
// GPIO复用配置（PA0-CH1, PA1-CH2）
GPIO_PinAFConfig(GPIOA, GPIO_PinSource0, GPIO_AF_TIM2);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource1, GPIO_AF_TIM2);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; // 复用功能
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; // 上拉
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// 定时器基本配置
TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 不分频
TIM_TimeBaseStructure.TIM_Period = 0xFFFF; // 16位最大值
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
// 编码器接口模式：TI12四倍频
TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12,
TIM_ICPolarity_Rising, TIM_ICPolarity_Falling);
// 输入捕获滤波器（消除机械抖动）
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_ICFilter = 10; // 10个时钟周期滤波
TIM_ICInit(TIM2, &TIM_ICInitStructure);
// 设置计数器初值为中点，启动定时器
TIM_SetCounter(TIM2, 0x8000); // 32768
TIM_Cmd(TIM2, ENABLE);
}
工作原理:
- 四倍频模式: AB相上升沿和下降沿都计数，分辨率提升4倍
- 中点计数: 初值0x8000避免溢出时的正负判断问题
- 硬件滤波: 消除编码器机械抖动带来的误计数
函数: TIM2_Encoder_Read() (源文件: HARDWARE/ENCODER/encoder.c)
int TIM2_Encoder_Read(void)
{
int cnt = 0;
cnt = TIM_GetCounter(TIM2) - 0x8000; // 读取相对增量
TIM_SetCounter(TIM2, 0x8000); // 重置到中点
return cnt; // 返回脉冲增量（有符号）
}
功能: 读取编码器自上次调用以来的脉冲增量，正值表示正转，负值表示反转。
3.2.4. SPI底层驱动初始化
函数: Init_SPI1() (源文件: HARDWARE/SPI/bsp_spi.c)
void Init_SPI1(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
// 使能时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
// GPIO复用映射（PA5-SCK, PA6-MISO, PA7-MOSI）
GPIO_PinAFConfig(GPIOA, GPIO_PinSource5, GPIO_AF_SPI1);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource6, GPIO_AF_SPI1);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource7, GPIO_AF_SPI1);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; // 推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOA, &GPIO_InitStructure);
SPI_ModeSet(SPI1, 0); // 配置为Mode 0（CPOL=0, CPHA=0）
SPI_Cmd(SPI1, ENABLE);
}
功能: 配置SPI1为主机模式，波特率5.25MHz（84MHz/16），用于XV7011陀螺仪通信。
函数: SPI1_RW_Byte() (源文件: HARDWARE/SPI/bsp_spi.c)
uint8_t SPI1_RW_Byte(uint8_t byte)
{
while (SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_TXE) == RESET); // 等待发送缓冲区空
SPI_I2S_SendData(SPI1, byte);
while (SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_RXNE) == RESET); // 等待接收完成
return SPI_I2S_ReceiveData(SPI1);
}
功能: SPI全双工收发一个字节，发送的同时接收数据。
3.2.5. 陀螺仪初始化与数据读取
函数: XV7011_Init() (源文件: HARDWARE/XV7011/xv7011.c)
void XV7011_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
// 配置CS片选引脚（PA4，推挽输出）
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_SetBits(GPIOA, GPIO_Pin_4); // CS默认高电平
Init_SPI1(); // 初始化SPI接口
// 软件复位陀螺仪
XV7011_Delay_ms(1);
XV7011_SPI_RW(GYRO_SW_RST); // 0x09寄存器
XV7011_Delay_ms(10);
// 配置工作模式
XV7011_WriteReg(GYRO_IF_CTL, 0); // 0x1F：使能SPI，禁用I2C
XV7011_WriteReg(GYRO_DSP_CTL2, 0x01); // 0x02：2阶低通滤波，35Hz截止频率
XV7011_WriteReg(GYRO_DSP_CTL3, 0x01); // 0x03：禁用零速率校准，FS/2量程
XV7011_WriteReg(GYRO_OUT_CTL1, 0x05); // 0x0B：24-bit连续输出模式
XV7011_SPI_RW(GYRO_SLP_OUT); // 0x06：退出睡眠模式
XV7011_Delay_ms(1);
}
功能: 配置XV7011陀螺仪工作参数，设置为24位高精度连续输出模式。
函数: XV7011_ReadData32() (源文件: HARDWARE/XV7011/xv7011.c)
void XV7011_ReadData32(int32_t *data)
{
static int16_t temp;
uint8_t buffer[3];
// 从0x0A寄存器连续读取3字节
XV7011_ReadBytes(GYRO_DAT_ACCON, 3, buffer);
// 组装24位有符号数（高字节在前）
temp = (int16_t)((buffer[0] << 8) | buffer[1]); // 先组装高16位
*data = (int32_t)((temp << 8) | buffer[2]); // 再拼接低8位
}
功能: 读取陀螺仪Z轴角速度原始数据（24位有符号整数）。
数据范围: -8388608 ~ +8388607，对应±300°/s。
3.2.6. 串口DMA初始化与发送
函数: UASRT1_Init() (源文件: SYSTEM/usart/usart.c)
void UASRT1_Init(u32 bound)
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
DMA_InitTypeDef DMA_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
// 使能时钟
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE);
// GPIO复用配置（PA9-TX, PA10-RX）
GPIO_PinAFConfig(GPIOA, GPIO_PinSource9, GPIO_AF_USART1);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource10, GPIO_AF_USART1);
// DMA2 Stream7配置（USART1_TX通道）
DMA_DeInit(DMA2_Stream7);
while (DMA_GetCmdStatus(DMA2_Stream7) != DISABLE);
DMA_InitStructure.DMA_Channel = DMA_Channel_4;
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)(&(USART1->DR));
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)USART1_DMA_TX_BUFFER;
DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; // 内存到外设
DMA_InitStructure.DMA_BufferSize = USART1_DMA_SEND_LEN; // 15字节
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; // 内存地址递增
DMA_InitStructure.DMA_PeripheralDataSize = DMA_MemoryDataSize_Byte;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; // 单次传输模式
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
DMA_Init(DMA2_Stream7, &DMA_InitStructure);
// 配置DMA中断
NVIC_InitStructure.NVIC_IRQChannel = DMA2_Stream7_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
DMA_ITConfig(DMA2_Stream7, DMA_IT_TC | DMA_IT_TE, ENABLE); // 使能传输完成中断
USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);
// USART1配置（波特率1152000，8N1）
USART_InitStructure.USART_BaudRate = bound;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART1, &USART_InitStructure);
USART_Cmd(USART1, ENABLE);
}
功能: 配置USART1用DMA方式发送定位数据，实现CPU零拷贝高效传输。
函数: UASRT1_DMA_TX() (源文件: SYSTEM/usart/usart.c)
void UASRT1_DMA_TX(void)
{
if (UART1_Use_DMA_Tx_Flag == 0) { // 检查忙标志
DMA_ClearFlag(DMA2_Stream7, DMA_FLAG_TCIF7);
DMA_SetCurrDataCounter(DMA2_Stream7, USART1_DMA_SEND_LEN);
DMA_Cmd(DMA2_Stream7, ENABLE); // 启动传输
UART1_Use_DMA_Tx_Flag = 1; // 设置忙标志
}
}
功能: 触发DMA传输，传输完成后由中断自动清除忙标志。
3.2.7. USART1中断服务（软件复位）函数
函数: USART1_IRQHandler() (源文件: SYSTEM/usart/usart.c)
void USART1_IRQHandler(void) //串口1中断服务程序
{
u8 Res = Res;
#if SYSTEM_SUPPORT_OS //使用UCOS操作系统
OSIntEnter();
#endif
{
// 1. 发送完成中断处理
if (USART_GetITStatus(USART1, USART_IT_TXE) == RESET)
{
USART_ITConfig(USART1, USART_IT_TC, DISABLE); // 关闭发送完成中断
UART1_Use_DMA_Tx_Flag = 0; // 清除DMA忙标志，允许下次DMA发送
}
// 2. 接收中断处理
if (USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
{
Res = USART_ReceiveData(USART1); // 读取接收到的数据
if(Res=='A') // 如果收到字符'A'
{
__set_FAULTMASK(1); // 关闭所有中断
NVIC_SystemReset(); // 软件复位MCU
}
USART_ClearFlag(USART1, USART_IT_RXNE); // 清除接收中断标志
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);// 重新使能接收中断
}
}
#if SYSTEM_SUPPORT_OS
OSIntExit(); //退出中断
#endif
}
功能说明:
- 发送完成中断：当DMA发送完成后，关闭发送完成中断，并清除DMA忙标志，允许下一次DMA发送。
- 接收中断：每当USART1收到新数据时触发。如果收到字符'A'，则立即关闭所有中断并执行系统复位（远程复位功能）。无论收到什么数据，均清除接收中断标志并重新使能接收中断，保证后续数据能继续接收。
- 操作系统兼容：如使用UCOS操作系统，则在中断入口和出口分别调用
OSIntEnter()和OSIntExit()，保证系统调度正常。
3.2.8. 定时器中断任务调度
函数: TIMER_TASK_TIM3_Init() (源文件: HARDWARE/TIM/tim.c)
void TIMER_TASK_TIM3_Init(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
// 定时器配置：产生10kHz中断
TIM_TimeBaseStructure.TIM_Period = 99; // 自动重装值（0-99共100个计数）
TIM_TimeBaseStructure.TIM_Prescaler = 83; // 预分频值（84分频）
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
/*
* 中断频率计算：
* TIM3时钟源 = APB1时钟 × 2 = 42MHz × 2 = 84MHz
* 中断频率 = 84MHz / ((83+1) × (99+1)) = 84MHz / 8400 = 10kHz
* 中断周期 = 100μs
*/
TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE); // 使能更新中断
// NVIC配置
NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; // 抢占优先级1
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
TIM_Cmd(TIM3, ENABLE);
}
功能: 配置TIM3产生100μs周期的定时中断，用于高频采集陀螺仪数据。
函数: TIM3_IRQHandler() (源文件: HARDWARE/TIM/tim.c)
void TIM3_IRQHandler(void)
{
static u16 count;
static int32_t gyro;
static int32_t sumGyro;
if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) {
if(locatorInitflag) { // 初始化模式：零偏标定
XV7011_ReadData32(&gyro);
sumGyro += gyro;
count++;
if (count == INITCOUNT) { // 采集15000次（1.5秒）
GYRO_OFFSET = (float)sumGyro / (float)INITCOUNT; // 计算平均值
WindowFilterStructInit(&Gyro_Window, 20, (int32_t)GYRO_OFFSET);
count = 0;
locatorInitflag = 0; // 切换到正常模式
}
}
else { // 正常工作模式
Locator_Filter_ReadYaw(&locator); // 每100μs读取陀螺仪并积分
count = (count + 1) % 1000; // 0-999循环计数
if (!(count % SENDCOUNT)) // 每50次（5ms）设置发送标志
locatorSendflag = 1;
if (!(count % CALCOUNT)) // 每20次（2ms）设置计算标志
locatorCalflag = 1;
}
TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
}
}
功能:
- 初始化阶段: 采集15000个陀螺仪数据求平均值作为零偏，同时初始化滤波器
- 正常工作: 每100μs读取陀螺仪并积分，每2ms设置计算标志，每5ms设置发送标志
时序分析:
下面以数据发送为例计算：
#define SENDCOUNT 50 // 发送计数值，在tim.h中配置
// STM32F405系统时钟为168MHz
// APB1时钟 = 168MHz / 4 = 42MHz
// TIM3时钟 = APB1时钟 × 2 = 84MHz (因为APB1预分频不为1时，定时器时钟×2)
中断频率 = TIM3时钟频率 / ((预分频值 + 1) × (重装载值 + 1))
解得：中断频率 = 84MHz / ((83 + 1) × (99 + 1)) = 10kHz = 10kHz
if (!(count % SENDCOUNT)) // 当count能被50整除时
{
locatorSendflag = 1; // 设置发送标志
}
数据发送频率 = 中断频率 / SENDCOUNT
解得：数据发送频率 = 10kHz / 50 = 200Hz（每5ms）
同理得：位置计算频率 = 10kHz / CALCOUNT = 10kHz / 20 = 500Hz（每2ms）
3.2.9. 滑动窗口滤波算法
函数: WindowFilterStructInit() (源文件: HARDWARE/FILTER/filter.c)
void WindowFilterStructInit(WindowFilterStruct *Window, uint8_t len, int32_t offset)
{
uint8_t i;
Window->len = len; // 设置窗口宽度
Window->index = 0; // 重置写指针
Window->sum = Window->len * offset; // 初始总和
for(i = 0; i < Window->len; i++) {
Window->buf[i] = offset; // 用offset值预填充缓冲区
}
}
功能: 初始化滑动窗口结构体，用陀螺仪零偏值预填充，避免启动瞬态。
函数: WindowFilter() (源文件: HARDWARE/FILTER/filter.c)
float WindowFilter(WindowFilterStruct *Window, int32_t inPut)
{
Window->sum -= Window->buf[Window->index]; // 减去最老的数据
Window->sum += inPut; // 加上新数据
Window->buf[Window->index++] = inPut; // 更新缓冲区并移动指针
if (Window->index >= Window->len) // 循环指针
Window->index = 0;
return (float)Window->sum / (float)Window->len; // 返回平均值
}
功能: O(1)复杂度的滑动平均滤波，窗口宽度20点（2ms历史数据）。
等效滤波: 500Hz低通滤波器，有效抑制陀螺仪高频噪声。
3.2.10. 陀螺仪数据处理与姿态积分
函数: Locator_Filter_ReadYaw() (源文件: HARDWARE/LOCATOR/locator.c)
void Locator_Filter_ReadYaw(LocatorDataTypedef *locator)
{
int32_t gyro; // 24位原始角速度
float yaw;
XV7011_ReadData32(&gyro); // 1. 读取原始数据
yaw = WindowFilter(&Gyro_Window, gyro); // 2. 滑动窗口滤波
yaw = -(yaw - GYRO_OFFSET); // 3. 零偏补偿，逆时针为正
// 4. 噪声门限过滤（|yaw| > 2600才积分）
if (!(yaw > -NOISE_FILTERDATA && yaw < NOISE_FILTERDATA)) {
// 5. 非对称积分（补偿硬件非线性）
if(yaw < 0)
locator->yaw += yaw * GYRO_SCALE_FACTOR_Z; // 负向系数
else
locator->yaw += yaw * GYRO_SCALE_FACTOR_F; // 正向系数
}
}
执行频率: 10kHz（100μs调用一次）
滑动窗口滤波: 20点平均，消除高频噪声
零偏补偿: 消除陀螺仪静态漂移
噪声门限: 2600阈值过滤静止时的抖动
非对称积分系数:
```
// locator.h中定义
#define GYRO_SCALE_FACTOR_Z 0.00000000559902458f // 负向
#define GYRO_SCALE_FACTOR_F 0.00000000559358637f // 正向
// 积分系数 = LSB灵敏度 × 采样周期 = (300°/s / 8388608) × 0.0001s
```
3.2.11. 坐标变换与位置解算
函数: Locator_Calculate() (源文件: HARDWARE/LOCATOR/locator.c)
void Locator_Calculate(LocatorDataTypedef *locator)
{
// 1. 计算定位仪相对世界坐标系的姿态角
float angleCal = locator->yaw + OFFSET_ANGLE; // 加45°安装偏角
AngleNormal(&angleCal); // 归一化到(-180°, 180°]
// 2. 读取编码器增量并转换为距离（mm）
locator->dis_p = (float)TIM2_Encoder_Read() * ENCODER_SCALE_FACTOR;
locator->dis_q = -(float)TIM4_Encoder_Read() * ENCODER_SCALE_FACTOR;
// 3. 坐标旋转变换：定位仪坐标系(p,q) → 世界坐标系(x,y)
locator->pos_x += locator->dis_p * arm_cos_f32(Angle2Rad(angleCal))
- locator->dis_q * arm_sin_f32(Angle2Rad(angleCal));
locator->pos_y += locator->dis_p * arm_sin_f32(Angle2Rad(angleCal))
+ locator->dis_q * arm_cos_f32(Angle2Rad(angleCal));
}
调用频率: 500Hz（每2ms计算一次）
数学原理: 二维旋转矩阵变换
其中θ = locator->yaw + 45°（安装偏角）
辅助函数: AngleNormal() (源文件: HARDWARE/LOCATOR/locator.c)
static void AngleNormal(float *angle)
{
while(*angle > 180) *angle -= 360; // 大于180°减360°
while(*angle <= -180) *angle += 360; // 小于-180°加360°
}
功能: 将角度归一化到(-180°, 180°]区间。
辅助函数: Angle2Rad() (源文件: HARDWARE/LOCATOR/locator.c)
static float Angle2Rad(float angle)
{
return angle * 0.01745329252f; // π/180
}
功能: 角度转弧度，供DSP库三角函数使用。
3.2.12. 定位数据打包与发送
函数: Locator_USART_SendData() (源文件: HARDWARE/LOCATOR/locator.c)
void Locator_USART_SendData(LocatorDataTypedef *locator)
{
uint8_t i;
uint8_t dataSum = 0;
AngleNormal(&(locator->yaw)); // 归一化姿态角
// 帧头
USART1_DMA_TX_BUFFER[0] = 'P';
USART1_DMA_TX_BUFFER[1] = 'G';
// X坐标（4字节小端序浮点数）
USART1_DMA_TX_BUFFER[2] = BYTE0(locator->pos_x);
USART1_DMA_TX_BUFFER[3] = BYTE1(locator->pos_x);
USART1_DMA_TX_BUFFER[4] = BYTE2(locator->pos_x);
USART1_DMA_TX_BUFFER[5] = BYTE3(locator->pos_x);
// Y坐标（4字节）
USART1_DMA_TX_BUFFER[6] = BYTE0(locator->pos_y);
USART1_DMA_TX_BUFFER[7] = BYTE1(locator->pos_y);
USART1_DMA_TX_BUFFER[8] = BYTE2(locator->pos_y);
USART1_DMA_TX_BUFFER[9] = BYTE3(locator->pos_y);
// 姿态角（4字节）
USART1_DMA_TX_BUFFER[10] = BYTE0(locator->yaw);
USART1_DMA_TX_BUFFER[11] = BYTE1(locator->yaw);
USART1_DMA_TX_BUFFER[12] = BYTE2(locator->yaw);
USART1_DMA_TX_BUFFER[13] = BYTE3(locator->yaw);
// 校验和
for (i = 0; i < 14; i++)
dataSum += USART1_DMA_TX_BUFFER[i];
USART1_DMA_TX_BUFFER[14] = dataSum;
UASRT1_DMA_TX(); // 启动DMA传输
}
功能: 将定位数据打包成15字节二进制帧，通过DMA发送。
数据帧格式:
- 用vofa的RawData协议显示如下图
四、重要参数
4.1 硬件配置参数
4.1.1 传感器参数
| 传感器 | 参数项 | 数值 | 说明 | 源文件 |
|---|---|---|---|---|
| 编码器1/2 | 分辨率 | 16384 ppr | 四倍频模式，每圈16384脉冲 | encoder.c |
| 工作模式 | TI12编码器接口 | AB相上升沿和下降沿计数 | encoder.c | |
| 数字滤波 | 10个时钟周期 | 消除机械抖动 | encoder.c | |
| 引脚配置 | TIM2: PA0/PA1 | 编码器1信号输入 | encoder.c | |
| TIM4: PB6/PB7 | 编码器2信号输入 | encoder.c | ||
| 陀螺仪XV7011 | 数据位宽 | 24-bit | 有符号整数，±8388608 | xv7011.c |
| 量程 | ±300°/s | 角速度测量范围 | xv7011.c | |
| LSB灵敏度 | 0.0000358°/s/LSB | 300/8388608 | 计算值 | |
| 通信接口 | SPI Mode 0 | CPOL=0, CPHA=0 | bsp_spi.c | |
| 时钟频率 | 5.25 MHz | 84MHz/16分频 | bsp_spi.c | |
| 低通滤波 | 35 Hz | 2阶硬件滤波器 | xv7011.c | |
| 引脚配置 | PA5/PA6/PA7/PA4 | SCK/MISO/MOSI/CS | bsp_spi.c |
4.1.2 机械参数
| 参数项 | 数值 | 定义位置 | 说明 |
|---|---|---|---|
| 定位轮直径 | 47.3 mm | locator.h: LOCATOR_D | 用于编码器脉冲转换为线性位移 |
| 安装偏角 | 45° | locator.h: OFFSET_ANGLE | 定位仪坐标系相对机器人坐标系旋转角 |
| 初始位置 | (0, 0) mm | locator.h: ORIGINAL_X/Y | 开机时世界坐标系原点 |
| 初始姿态 | 0° | locator.h: ORIGINAL_ANGLE | 开机时初始朝向角 |
4.1.3 通信接口参数
| 接口 | 参数项 | 配置值 | 用途 | 源文件 |
|---|---|---|---|---|
| USART1 | 波特率 | 1152000 bps | 定位数据发送 | usart.c |
| 数据格式 | 8N1 | 8位数据，无校验，1位停止位 | usart.c | |
| 传输方式 | DMA2 Stream7 | 零拷贝高效传输 | usart.c | |
| 帧长度 | 15 bytes | 2字节帧头+12字节数据+1字节校验 | locator.c | |
| 引脚 | PA9/PA10 | TX/RX | usart.c | |
| USART3 | 波特率 | 115200 bps | 调试输出 | usart.c |
| 引脚 | PC10/PC11 | TX/RX | usart.c |
4.2 算法核心参数
4.2.1 数据滤波参数
| 参数名 | 数值 | 定义位置 | 作用 | 调整建议 |
|---|---|---|---|---|
| 窗口长度 | 20 | tim.c: WindowFilterStructInit() | 滑动平均滤波点数 | 增大提高平滑度但增加延迟 |
| 窗口时间跨度 | 2 ms | 计算值: 20×100μs | 历史数据覆盖范围 | - |
| 等效截止频率 | 500 Hz | 理论值: 10kHz/20 | 低通滤波特性 | - |
| 噪声门限 | ±2600 | locator.h: NOISE_FILTERDATA | 低于此值不参与积分 | 静止漂移严重时增大 |
4.2.2 标度转换系数
| 系数名称 | 数值 | 定义位置 | 物理意义 | 推导公式 |
|---|---|---|---|---|
| 编码器标度 | 0.00771069429 | locator.h: ENCODER_SCALE_FACTOR | 脉冲转换为mm | π×D/16384 = π×47.3/16384 |
| 陀螺仪积分系数(负向) | 5.599024×10⁻⁹ | locator.h: GYRO_SCALE_FACTOR_Z | 角速度积分为角度 | LSB×Δt = 0.0000358×0.0001 |
| 陀螺仪积分系数(正向) | 5.593586×10⁻⁹ | locator.h: GYRO_SCALE_FACTOR_F | 正向旋转非线性补偿 | 经验标定值 |
系数标定方法:
1. 编码器标度验证:
- 推动机器人直线移动1米，记录编码器累计脉冲数
- 实际距离 / 脉冲数 = 实测标度系数
2. 陀螺仪积分系数标定:
- 使机器人原地旋转N圈(如10圈)
- 实际角度(N×360°) / 读数角度 = 校正倍数
- 用校正倍数乘以现有系数得到新系数
4.3 任务调度参数
4.3.1 定时器配置
| 参数项 | 数值 | 计算过程 | 源文件 |
|---|---|---|---|
| TIM3时钟源 | 84 MHz | APB1(42MHz)×2 | tim.c |
| 预分频值 | 83 | 84分频 | tim.c: TIM_Prescaler |
| 自动重装值 | 99 | 100计数 | tim.c: TIM_Period |
| 中断频率 | 10 kHz | 84MHz/84/100 = 10000Hz | 计算值 |
| 中断周期 | 100 μs | 1/10000Hz | 计算值 |
4.3.2 任务周期配置
| 任务 | 计数器阈值 | 触发周期 | 触发频率 | 宏定义 | 功能 |
|---|---|---|---|---|---|
| 陀螺仪采样 | 每次中断 | 100 μs | 10 kHz | - | Locator_Filter_ReadYaw() |
| 位置计算 | CALCOUNT=20 | 2 ms | 500 Hz | tim.h: CALCOUNT | Locator_Calculate() |
| 数据发送 | SENDCOUNT=50 | 5 ms | 200 Hz | tim.h: SENDCOUNT | Locator_USART_SendData() |
| 零偏标定 | INITCOUNT=15000 | 1.5 s | 一次性 | tim.h: INITCOUNT | 启动时执行 |
周期关系:
基准周期: 100μs (中断周期)
计算周期 = 基准周期 × CALCOUNT = 100μs × 20 = 2ms
发送周期 = 基准周期 × SENDCOUNT = 100μs × 50 = 5ms
标定时间 = 基准周期 × INITCOUNT = 100μs × 15000 = 1.5s
调整原则:
- 提高定位精度: 减小CALCOUNT（但增加CPU负载）
- 降低通信负载: 增大SENDCOUNT（但降低数据刷新率）
- 加快启动速度: 减小INITCOUNT（但降低零偏标定精度）
4.4 中断优先级配置
| 中断源 | 抢占优先级 | 响应优先级 | 用途 | 源文件 |
|---|---|---|---|---|
| TIM3 | 1 | 0 | 陀螺仪高频采样，最高优先级 | tim.c |
| DMA2_Stream7 | 1 | 1 | 串口DMA传输完成 | usart.c |
| CAN1_RX0 | 2 | 0 | CAN总线接收（预留） | can.c |
| USART1 | 0 | 3 | 串口接收复位命令，最低优先级 | usart.c |
优先级分组: NVIC_PriorityGroup_2 (2位抢占，2位响应)
4.5 性能指标
4.5.1 理论性能
| 指标 | 数值 | 说明 |
|---|---|---|
| 姿态更新率 | 10 kHz | 陀螺仪采样频率 |
| 位置更新率 | 500 Hz | 坐标计算频率 |
| 数据输出率 | 200 Hz | 串口发送频率 |
| 编码器单脉冲分辨率 | 0.0077 mm | π×47.3/16384 |
| 陀螺仪角速度分辨率 | 0.000036 °/s | 300/8388608 |
| 最大可测角速度 | 300 °/s | 传感器量程限制 |
| 最大可测线速度 | ~7 m/s | 编码器最大计数频率限制 |
| 启动时间 | 1.5 s | 零偏标定用时 |
4.5.2 实际精度
| 指标 | 典型值 | 影响因素 |
|---|---|---|
| 静止时姿态角漂移 | ±0.5°/min | 陀螺仪零偏温漂 |
| 短距离位置误差 | ±2 mm | 编码器分辨率、轮径测量误差 |
| 长距离位置误差 | 1-5 cm/m | 轮子打滑、地面不平、标度系数误差 |
| 姿态角累积误差 | ±1°/min | 陀螺仪漂移、积分误差累积 |
误差来源分析:
-
系统误差:
- 编码器安装偏心
- 轮径测量不准确
- 陀螺仪安装不水平
-
随机误差:
- 地面颗粒物导致轮子打滑
- 地面不平引起定位轮悬空
- 陀螺仪零偏随温度变化
-
算法误差:
- 滤波延迟
- 数值积分截断误差
- 浮点运算精度损失
4.6 关键宏定义汇总
文件: HARDWARE/LOCATOR/locator.h
#define LOCATOR_D 47.3f // 定位轮直径(mm)
#define OFFSET_ANGLE 45 // 安装偏角(°)
#define ORIGINAL_X 0 // 初始X坐标(mm)
#define ORIGINAL_Y 0 // 初始Y坐标(mm)
#define ORIGINAL_ANGLE 0 // 初始姿态角(°)
#define NOISE_FILTERDATA 2600 // 噪声门限(原始数据单位)
#define GYRO_SCALE_FACTOR_Z 0.00000000559902458f // 负向积分系数
#define GYRO_SCALE_FACTOR_F 0.00000000559358637f // 正向积分系数
#define ENCODER_SCALE_FACTOR 0.00771069429f // 编码器标度(mm/脉冲)
文件: HARDWARE/TIM/tim.h
#define SENDCOUNT 50 // 发送周期计数阈值 (5ms)
#define CALCOUNT 20 // 计算周期计数阈值 (2ms)
#define INITCOUNT 15000 // 零偏标定采样次数 (1.5s)
文件: HARDWARE/FILTER/filter.h
#define WINDOW_SIZE 50 // 滤波器缓冲区最大长度
五、项目常用参数
5.1 陀螺仪参数
5.1.1 NOISE_FILTERDATA (源文件: HARDWARE/LOCATOR/locator.h)
yaw噪声滤波,决定yaw数据平换程度
- NOISE_FILTERDATA=0时
无外界人工干扰，仍会产生明显零漂
- NOISE_FILTERDATA=6000时
无外界人工干扰，不会产生零漂
- NOISE_FILTERDATA=6000 VS NOISE_FILTERDATA=60000
有外界人工敲击干扰时，NOISE_FILTERDATA=6000会产生明显零漂，NOISE_FILTERDATA=60000不会产生零漂
5.2 编码器滤波
5.2.1 ENCODER_STATIC_THRESHOLD (源文件: HARDWARE/LOCATOR/locator.h)
- 无静止阈值检测时
- 加入禁止阈值检测时
- 禁止阈值过高时，数据平滑性减弱
5.2.2ENCODER_NOISE_THRESHOLD (源文件: HARDWARE/LOCATOR/locator.h)
- 加入死区处理前 vs 加入死区处理后
- ENCODER_NOISE_THRESHOLD 不同参数效果
5.2.3 ENCODER_KALMAN_Q 和 ENCODER_KALMAN_R (源文件: HARDWARE/KALMAN/kalman.h, 在文件: HARDWARE/LOCATOR/locator.h中宏定义)
专为编码器优化的轻量级卡尔曼滤波器
参数调优指南
| 参数 | 推荐范围 | 效果 |
|---|---|---|
| Q (过程噪声) | 0.001-0.1 | Q↓ 更平滑但响应慢 Q↑ 响应快但滤波弱 |
| R (测量噪声) | 0.5-5 | R↓ 更信任传感器 R↑ 滤波更强 |
快速调试步骤：
- 先用默认值：
Q=0.01,R=2.0 - 如果波形还有毛刺 → 增大
R到 3-5 - 如果响应太慢 → 增大
Q到 0.05-0.1 - 如果飘逸严重 → 减小
Q到 0.001-0.005
- Q=0.1时
