# 惯导小车

**Repository Path**: my-yao-yao/guandao_car

## Basic Information

- **Project Name**: 惯导小车
- **Description**: 这是我的惯导小车代码仓库
- **Primary Language**: C
- **License**: GPL-3.0
- **Default Branch**: main
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 18
- **Forks**: 1
- **Created**: 2025-06-24
- **Last Updated**: 2025-08-30

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 逐飞 CYT4BB7 智能车全局航位推算惯性导航项目

[![Gitee star](https://gitee.com/my-yao-yao/guandao_car.git/stargazers)](https://gitee.com/my-yao-yao/guandao_car.git/stargazers)

> 本项目是基于 Cypress CYT4BB7 微控制器和逐飞科技开源库开发的智能车导航系统。核心目标是实现精确的室内全局航位推算（Dead Reckoning）与惯性导航，为智能车提供稳定可靠的定位与姿态信息。项目着重于代码架构的模块化设计，以便于理解、维护和功能扩展。
>
> 如果您觉得它还不错，欢迎点亮 Star ⭐ 支持一下！

---

## 项目概述

该项目旨在实现一个能够在室内环境中自主导航的小车。其核心功能包括路径记录、路径存储与加载、惯性测量单元（IMU）数据处理、轮式里程计以及基于 Pure Pursuit 算法的路径跟踪控制。

- **路径记录 (Path Recording)**：小车在人工控制下行驶时，记录其运动轨迹（X, Y 坐标和偏航角）。
- **路径存储与加载 (Path Storage & Loading)**：将记录的路径数据持久化存储到 Flash 非易失性存储器中，并在需要时加载回内存。
- **惯性导航 (Inertial Navigation)**：利用 IMU（陀螺仪和加速度计）数据进行姿态（偏航角）和速度估计。
- **轮式里程计 (Wheel Odometry)**：通过编码器测量车轮转动，估算小车行驶距离和速度。
- **路径跟踪控制 (Path Tracking Control)**：使用 Pure Pursuit 算法，根据当前位置和目标路径计算转向误差，并控制小车沿预设路径行驶。
- **实时控制 (Real-time Control)**：通过定时器中断（PIT ISR）实现高频率的传感器数据采集、IMU 数据处理和电机控制。

## 详细代码架构分析

### `main_cm7_0.c` (主程序文件)

- **职责**:
  - 系统初始化：包括时钟、调试接口、以及一个通用的 `CodeInit()` 函数（包含外设初始化）。
  - 主循环逻辑：协调各个模块的运行，处理高层逻辑，如路径记录、Flash 操作、路径回放和显示。
- **关键逻辑**:
  - **系统初始化**: `clock_init()`, `debug_init()`, `CodeInit()`.
  - **路径记录模式**:
    - 通过 `road_memery_start_Plus_flag` 和 `road_memery_finish_Plus_flag` 控制。
    - 当左轮前进 1cm (`left_cm_ticks > 0`) 时，调用 `Distance_Get_Plus()` 记录当前 `X`, `Y` 坐标到 `X_Memery_Plus` 和 `Y_Memery_Plus` 数组。
  - **Flash 操作**:
    - 根据 `flash_flag_Plus` 状态触发：
      - `flash_flag_Plus == 1`: 调用 `flash_road_memery_store_Plus()` 存储路径数据。
      - `flash_flag_Plus == 2`: 调用 `flash_road_memery_get_Plus()` 读取路径数据。
  - **路径回放模式**:
    - 通过 `road_recurrent_Plus_flag` 控制。
    - 当左轮前进时，根据 `left_cm_ticks` 和当前偏航角 `yaw_plus` 更新小车的实时 `X`, `Y` 坐标（航迹推算）。
    - 调用 `pure_pursuit_control()` 计算转向误差 `err_guandao_plus`。
  - **调试与显示**: `show_guandao()` 显示实时数据和调试信息。
  - **按键处理**: 检查 `Key_Flag` 并调用 `KEY_Scan()`。
- **依赖**: `zf_common_headfile.h`, `flash.h`, `Guandao_Plus.h`, `imu.h`。

### `cm7_0_isr.c` (中断服务例程文件)

- **职责**: 处理各种硬件中断，确保实时性任务的执行。
- **关键中断**:
  - **`pit0_ch0_isr()` (定时器 0 通道 0 中断)**:
    - **频率**: 可能是 5ms 一次（根据 `Guandao_Plus.h` 中的 `SAMPLING_INTERVAL_MS` 推断）。
    - **功能**:
      - 获取左右轮编码器当前计数。
      - 计算左右轮速度 (`speed_left`, `speed_right`)：调用 `Get_Wheel_Speed()`。
      - 执行电机控制：当 `Run_Flag == 1` 时，调用 `Motor_control()` 进行速度闭环控制。
  - **`pit0_ch1_isr()` (定时器 0 通道 1 中断)**:
    - **频率**: 可能是 5ms 一次，或比 `pit0_ch0_isr` 更快。
    - **功能**:
      - 读取 IMU 原始数据：`imu660ra_get_gyro()`, `imu660ra_get_acc()`。
      - 对 IMU 数据进行零偏校准和单位转换：`gyro_transform_value()`, `acc_transform_value()`。
      - 积分陀螺仪 Z 轴数据以获取偏航角：`gyro_yaw_integral()` (当 `gyro_zero_flag` 为 1 时)。
- **依赖**: `zf_common_headfile.h`, `Guandao_Plus.h`, `imu.h`。

### `imu.c` / `imu.h` (IMU 模块)

- **职责**: 处理 IMU 传感器数据，包括校准、单位转换和姿态（偏航角）及速度的估计。
- **数据结构**:
  - `gyro_zero_paramTypedef`, `acc_zero_paramTypedef`: 存储陀螺仪和加速度计的零偏值。
  - `gyro_paramTypedef`, `acc_paramTypedef`: 存储经过处理（去零偏、转换单位）的陀螺仪角速度和加速度计加速度。
- **关键变量**:
  - `gyro_zero_flag`, `acc_zero_flag`: 校准完成标志。
  - `yaw_plus`: 小车相对于全局坐标系 Y 轴的偏航角（弧度，范围 `[-pi, pi]`），顺时针为负，逆时针为正。这是导航的核心偏航角。
  - `acc_y_speed`: 通过 Y 轴加速度积分得到的 Y 轴速度。
  - `yaw_memery`, `yaw_store`: 用于 Flash 存储和读取的偏航角数组。
- **关键函数**:
  - `gyro_zero_param_init()`, `acc_zero_param_init()`: 上电时进行陀螺仪和加速度计的零偏校准（通过多次采样取平均）。
  - `gyro_transform_value()`, `acc_transform_value()`: 将原始 IMU 数据减去零偏，并转换为实际物理单位（例如，角速度为度/秒或弧度/秒，加速度为 m/s²）。
  - `gyro_yaw_integral()`: 使用梯形积分法 (`(yaw_last + yaw_now) / 2 * dt`) 积分 Z 轴角速度，计算 `yaw` 和 `yaw_plus`。`yaw_plus` 会被归一化到 `[-pi, pi]` 范围。
  - `acc_y_integral()`: 使用梯形积分法积分 Y 轴加速度，计算 `acc_y_speed`。并应用低通滤波器 (`LPF_Update`)。
- **依赖**: `zf_common_headfile.h` (提供 IMU 驱动接口如 `imu660ra_get_gyro` 等), `math.h`。

### `Guandao_Plus.c` / `Guandao_Plus.h` (惯导与路径跟踪模块)

- **职责**: 实现轮式里程计、路径记录、Pure Pursuit 路径跟踪算法以及滑移检测。
- **数据结构**:
  - `WheelData` 结构体: 封装了车轮编码器的数据，包括累计脉冲、上次累计脉冲、上次编码器值（用于速度计算）和目标脉冲数（用于 1cm 步进）。
- **关键变量**:
  - `X_Memery_Plus`, `Y_Memery_Plus`: 用于记录路径的 X、Y 坐标数组。
  - `X_Memery_Store_Plus`, `Y_Memery_Store_Plus`: 从 Flash 读取后存储路径的 X、Y 坐标数组。
  - `X`, `Y`: 小车当前在全局坐标系中的实时位置（厘米）。
  - `road_memery_finish_Plus_flag`, `road_memery_start_Plus_flag`, `road_recurrent_Plus_flag`: 路径记录/回放状态标志。
  - `NUM_L_Plus`: 记录路径点的索引，也表示当前行驶的距离（以 1cm 为单位）。
  - `road_destination`: 记录路径的终点索引。
  - `wheel_left`, `wheel_right`: `WheelData` 结构体实例，分别代表左右轮。
  - `speed_left`, `speed_right`: 左右轮速度（脉冲/5ms）。
  - `err_guandao_plus`: 路径跟踪的横向误差（转向误差）。
  - `e_lat`: 横向误差的浮点表示。
  - `locate_index`, `target_index`: Pure Pursuit 算法中用于查找路径点的索引。
  - `wheelSlipFlags`: 滑移状态标志。
- **关键函数**:
  - `calculate_delta()`: 辅助函数，处理 16 位编码器计数器的溢出问题，正确计算增量。
  - `Update_Wheel_Pulses()`: 更新车轮的累计脉冲数，并返回经过了多少个 `target_pulses`（例如，每 1cm）。
  - `Get_Wheel_Speed()`: 计算车轮在固定时间间隔内的速度（脉冲/时间单位）。
  - `Distance_Get_Plus()`: 在路径记录模式下，当小车每前进 1cm 时，记录当前的 X、Y 坐标。
  - `find_lookahead_index(float L)`: Pure Pursuit 算法的核心。根据当前小车位置和预瞄距离 L，在存储的路径中查找一个合适的预瞄点。它首先找到离小车最近的路径点，然后通过“跳跃式”搜索和精细搜索来确定预瞄点。
  - `calculate_e_lat(int lookahead_index)`: 计算小车当前位置与预瞄点之间的横向误差（即小车需要向左或向右转向多少才能对准预瞄点）。
  - `pure_pursuit_control()`: 根据 `e_lat` 和预瞄距离计算出小车所需的转向角或转向量。
  - `Slip_Check()`: (待完善)用于检测车轮滑移，可能通过比较左右轮速差、加速度与轮速的关系等。
- **依赖**: `zf_common_headfile.h`, `stdbool.h`, `math.h`, `float.h`, `imu.h` (获取 `yaw_plus`)。

### `flash.c` / `flash.h` (Flash 存储模块)

- **职责**: 管理 Flash 存储器，用于持久化存储路径数据。
- **关键定义**:
  - `FLASH_SECTION_INDEX`: Flash 存储区域的索引。
  - `road_memery_page_INDEX`: 存储 `yaw_memery` 的 Flash 页索引。
  - `X_memery_page_INDEX_1` 到 `X_memery_page_INDEX_13`: 存储 `X_Memery_Plus` 的多个 Flash 页索引。
  - `Y_memery_page_INDEX_2` 到 `Y_memery_page_INDEX_12`: 存储 `Y_Memery_Plus` 的多个 Flash 页索引。
  - `FLASH_PAGE_LENGTH`: 单个 Flash 页的长度（可存储的数据单元数量）。
- **关键变量**:
  - `flash_flag`, `flash_flag_Plus`: Flash 操作的状态标志（0:初始, 1:开始存储, 2:开始读取, 3:存储完成, 4:读取完成）。
- **关键函数**:
  - `flash_road_memery_store()`: 存储 `yaw_memery` 到单个 Flash 页。
  - `flash_road_memery_get()`: 从单个 Flash 页读取数据到 `yaw_store`。
  - `flash_road_memery_store_Plus()`: **核心存储函数**。将 `X_Memery_Plus` 和 `Y_Memery_Plus` 这两个大型数组分段存储到多个 Flash 页中。在最后一个 X 坐标页的末尾，还会额外存储 `NUM_L_Plus`（路径长度）和 `road_destination`（路径终点）。
  - `flash_road_memery_get_Plus()`: **核心读取函数**。从多个 Flash 页中读取数据，重新组合成 `X_Memery_Store_Plus` 和 `Y_Memery_Store_Plus`。同时读取 `road_destination`。
- **依赖**: `zf_common_headfile.h` (提供 Flash 操作的底层接口，如 `flash_buffer_clear`, `flash_check`, `flash_erase_page`, `flash_write_page_from_buffer`, `flash_read_page_to_buffer`, `flash_union_buffer`, `FLASH_PAGE_LENGTH`)。依赖 `imu.h` 和 `Guandao_Plus.h` 中定义的全局数组和变量。

## 模块间交互与数据流

1.  **传感器数据采集**:
    - 编码器数据 (`encoder_get_count`) 在 `pit0_ch0_isr` 中被 `Get_Wheel_Speed` 和 `Update_Wheel_Pulses` 使用。
    - IMU 原始数据 (`imu660ra_get_gyro`, `imu660ra_get_acc`) 在 `pit0_ch1_isr` 中被 `imu` 模块处理。
2.  **IMU 数据处理**:
    - `imu` 模块在 `pit0_ch1_isr` 中将原始 IMU 数据转换为物理量，并积分得到 `yaw_plus` (偏航角) 和 `acc_y_speed` (Y 轴速度)。
    - `yaw_plus` 是核心，被 `Guandao_Plus` 模块用于航迹推算和 Pure Pursuit 算法。
3.  **轮式里程计**:
    - `Guandao_Plus` 模块在 `pit0_ch0_isr` 中通过 `Get_Wheel_Speed` 计算轮速，并在 `main` 循环中通过 `Update_Wheel_Pulses` 计算 `left_cm_ticks` (行驶距离)。
4.  **路径记录**:
    - 在 `main` 循环的路径记录模式下，`Distance_Get_Plus` (属于 `Guandao_Plus` 模块) 使用 `X`, `Y` (当前航迹推算位置) 和 `yaw_plus` 来记录路径点到 `X_Memery_Plus` 和 `Y_Memery_Plus`。
5.  **路径存储与加载**:
    - `flash` 模块在 `main` 循环中根据 `flash_flag_Plus` 状态，将 `X_Memery_Plus`, `Y_Memery_Plus` (来自 `Guandao_Plus`) 和 `yaw_memery` (来自 `imu`) 存储到 Flash，或从 Flash 读取到 `X_Memery_Store_Plus`, `Y_Memery_Store_Plus` 和 `yaw_store`。
6.  **路径跟踪 (Pure Pursuit)**:
    - 在 `main` 循环的路径回放模式下，小车的实时位置 `X`, `Y` (通过航迹推算更新) 和 `yaw_plus` (来自 `imu`) 被用于 `find_lookahead_index` 和 `calculate_e_lat` (属于 `Guandao_Plus` 模块)。
    - `calculate_e_lat` 计算出横向误差 `e_lat` (即 `err_guandao_plus`)。
7.  **电机控制**:
    - `pit0_ch0_isr` 中的 `Motor_control` 函数使用目标速度和 `speed_left`, `speed_right` (来自 `Guandao_Plus` 模块) 进行速度闭环控制。
    - `err_guandao_plus` (转向误差) 最终会影响到左右轮的目标速度差，从而实现转向控制。

## 关键算法与技术

- **航迹推算 (Dead Reckoning)**: 基于轮式编码器和 IMU 偏航角估算小车的实时 X、Y 坐标。
- **梯形(中值)积分 (Trapezoidal Integration)**: 用于 IMU 数据（角速度和加速度）的积分，以获得角度和速度。
- **Pure Pursuit 算法**: 一种常用的路径跟踪算法，通过计算预瞄点与当前位置的横向误差来确定转向指令。
- **PID 控制**: 隐式存在于 `Motor_control` 函数中，用于实现电机的速度闭环控制。
- **Flash 存储管理**: 针对大型数组（X、Y 坐标）进行分段存储和读取，以适应 Flash 页的限制。
- **实时系统设计**: 利用定时器中断（PIT）实现高频率的传感器数据采集、处理和控制，确保系统的响应速度和稳定性。
- **数据类型转换与溢出处理**: `calculate_delta` 函数专门处理 16 位编码器计数器的溢出问题。

## 参考资料

- **IMU 姿态解算**: 从 IMU 数据中计算旋转、速度、位置，IMU 测量的原理: [https://blog.csdn.net/u011341856/article/details/132948044](https://blog.csdn.net/u011341856/article/details/132948044)
- **扩展卡尔曼滤波**: 实现 GPS+IMU 融合，EKF ESKF GPS+IMU: [https://blog.csdn.net/u011341856/article/details/114262451?spm=1001.2014.3001.5501](https://blog.csdn.net/u011341856/article/details/114262451?spm=1001.2014.3001.5501)
- **教材**: 严老师教材《捷联惯导算法与组合导航原理讲义》
- **开源惯导**: 王昱顺大佬的开源惯导: [https://www.stcaimcu.com/thread-9849-1-1.html](https://www.stcaimcu.com/thread-9849-1-1.html) (出处: 国芯技术交流网站)

## 优化方向建议

1.  **IMU 数据处理**: 目前对陀螺仪数据的获取与处理略微粗糙，可以尝试引入四元数解算与卡尔曼滤波，进一步抑制零飘，提高惯导精确度。
2.  **控制算法**: 缺乏与惯导契合的控制算法，目前仅应用简化版的纯跟踪控制算法，车模速度上限不高。可考虑建立更加精细的纯跟踪控制模型与设计配套的 PID 算法，实现精细化控制，提供稳定性与速度。
3.  **多传感器融合**: 考虑将摄像头数据与惯导数据进行融合，实现两者优势互补，提高车模整体上限。
4.  **打滑检测**: 完善打滑检测机制，提高算法整体鲁棒性。
5.  **陀螺仪零飘抑制与刚体力学建模**: 陀螺仪加热加上温度校准就可以很好的减少零飘。对车身进行刚体力学建模，列出旋转方程，然后通过车身的旋转和陀螺仪测得的角速度进行融合，写出状态空间方程用 EKF，还是可以比较稳的(参考西凉马戏团)。

## 鸣谢

- 86152
- drama

**作者**: 林林林

## 演示视频

- 【免费开源！缩微光电全局惯导抛砖引玉】: [https://www.bilibili.com/video/BV1jbKfzFECs/?share_source=copy_web&vd_source=557e7649774cc6652137c0e60b34161b](https://www.bilibili.com/video/BV1jbKfzFECs/?share_source=copy_web&vd_source=557e7649774cc6652137c0e60b34161b)

---

### 支持与贡献 ✨

如果您觉得这个项目对您有帮助，我们非常欢迎您以任何方式支持我们！

#### 1. 点亮 Star ⭐ (最简单的方式)

如果您喜欢这个项目，请在页面右上角帮我们点亮一颗 ⭐！
您的 Star 是对我们最大的认可和鼓励，也能让更多人发现这个项目。这只需要您一秒钟，但对我们意义重大。

#### 2. 提出建议或问题

如果您有任何想法或遇到了问题，欢迎通过 **[Issues](https://gitee.com/my-yao-yao/guandao_car.git/issues)** 告诉我们。

#### 3. 贡献代码

如果您想亲自修复问题或添加新功能，我们非常欢迎！

- **Fork 本项目** 到您自己的空间。
- 修改代码后，向我们提交 **Pull Request**。
- **小提示**：开始前，最好先让您的 Fork 与主仓库保持同步，可以参考这篇 [Gitee 官方教程](https://gitee.com/help/articles/4136)。

---

## 开发环境

- **IDE**: IAR Embedded Workbench for ARM
- **微控制器**: Cypress CYT4BB7 系列
- **逐飞开源库**: 针对 CYT4BB7 平台的逐飞科技开源库

## 许可证

本项目遵循 [GNU General Public License v3.0 (GPLv3)](https://www.gnu.org/licenses/gpl-3.0.en.html) 许可。请参阅项目根目录下的 `LICENSE` 文件和 `Seekfree_CYT4BB_Opensource_Library/libraries/doc/GPL3_permission_statement.txt` 获取详细信息。

## 注意事项

main 分支源代码打包文件过大，源代码下载请移步 download 分支