# Logistics_trolley

**Repository Path**: Zrx_Ttkx/Logistics_trolley

## Basic Information

- **Project Name**: Logistics_trolley
- **Description**: 工训赛物流搬运小车，基于stm32f407的物流小车设计
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 2
- **Forks**: 0
- **Created**: 2025-06-27
- **Last Updated**: 2025-09-23

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

#tics_trolley

#### 硬件介绍
开发板：RoBoMaster C型开发板
轮子电机：RoboMaster M2006（搭配C610电调）
云台电机：达妙DM-G6220
舵机：串行总线舵机（搭配TTL转串口调试板）
摄像头：USB工业模块相机摄像头GF100
显示屏：串口屏
上位机：miniPC
#### 软件设计介绍：
运动任务负责了整体的运动逻辑，也是本系统的主要线程，通过电机编码器传回
的电机参数，来对电机转过总圈数进行闭环控制，从而实现固定的路径长度。同
时，该任务还包括在粗加工区以及暂存区的车辆位置校准，根据陀螺仪以及上位机坐标数据进行闭环控制，
从而消除在行驶过程中由于电机编码器产生的累计误差。抓取与放置任务中主要 
负责使用串口与串行总线舵机进行通信，并根据其协议开发对应的数据包，并以 
固定的频率进行发送，这样一来只需在主要线程对舵机的角度、速度、加速度变 
量进行调用与修改即可实时对舵机进行控制。除了多线程的任务进行，在对数据 
实时性较高的部分我们采用了优先级较高的中断进行处理，从而保证闭环控制的 
准确与高效。中断方面主要包含Can中断、串口中断以及定时器中断。Can中断 
主要用于获取电机编码器传回的数据并对该数据进行校验与处理。串口中断共两 
个，分别对应陀螺仪、上位机。用于进行数据接收并根据帧头帧尾进行数据的校 
验，降低丢包率。定时器中断的作用在于控制PID的计算频率，从而保证控制的 
稳定性。

#### 软件架构
软件架构说明
工作流程图(https://foruda.gitee.com/images/1751092302769378797/83401b64_13552594.png "屏幕截图")

#### 部分算法具体介绍

1.  麦克纳姆轮运动学解算及漂移算法
根据麦克纳姆轮的原理，对四个电机的速度进行统一解算，拆分成Vx、Vy、Vz。据此，只需对Vx、Vy、Vz进行赋值即可控制车辆的前进速度、横移速度以及自旋速度。
解算如图：
![麦克纳姆轮解算](https://foruda.gitee.com/images/1751093085962462270/cac8d928_13552594.png "屏幕截图")
由于比赛地图弯道较多，如果单纯的用上下左右平移及自旋，车体容易超出边界，故利用麦克纳姆轮的特性可以内外侧轮子差速来实现绕一个圆心做圆周运动，内外圈轮子转速计算公式如下图。

![公式](https://foruda.gitee.com/images/1751093138948122112/75867760_13552594.png "屏幕截图")

![C语言实现](https://foruda.gitee.com/images/1751093165994000093/b490eabf_13552594.png "屏幕截图")

2.  闭环路径算法
路径闭环采用了串级PID的思想，通过编码器传回的总圈数数据来让电机旋转指定的圈数。将期望转过的圈数加上当前记录的总圈数，得到目标总圈数。并在圈数PID循环中实时计算当前圈数与目标圈数的差值，再将其差值代入到速度环PID中，作为速度的目标值，来驱动电机进行运动。

![实现流程图](https://foruda.gitee.com/images/1751093260560602876/6c5b90e2_13552594.png "屏幕截图")

![C语言实现](https://foruda.gitee.com/images/1751093289715963480/89450e4d_13552594.png "屏幕截图")

3.  姿态校准算法
在小车执行任务时，由于机械误差，编码器误差，或多或少都会影响到小车姿态，因此在小车执行所有任务时，我们通过外置陀螺仪在串口中断中获取数据，处理数据后。植入了姿态矫正算法。整体思路如下：在运作控制线程中利用总角度和目标角度做PID计算出速度环的目标值，后在定时器中断中利用yaw轴的角速度和总角度环的输出计算出gyro_out，再换为上图麦克纳姆轮运动学解算代码的Vz，实现自旋实时控制姿态为陀螺仪读取到的值，从而保证姿态始终不变。并且由于陀螺仪可能yaw飘，所以分阶段重新读取目标值，减少误差，通过标识位gyro_temp控制是否利用陀螺仪进行姿态矫正。下图分别为角度校准算法的流程图与代码部分。

![流程图](https://foruda.gitee.com/images/1751093468363315876/84b84019_13552594.png "屏幕截图")

![代码实现](https://foruda.gitee.com/images/1751093509180070055/e0822a10_13552594.png "屏幕截图")

4.  视觉定位算法
在修正完角度误差后，还需修正X轴方向与Y轴方向上的误差，那么此时就依靠OpenCV传回的圆环坐标数据进行矫正。具体的思路如下：通过上位机传回的坐标，在一个for循环中先后矫正Y轴与X轴，当误差达到理想值时跳出循环。

![实现代码](https://foruda.gitee.com/images/1751093647981872002/9a46a17a_13552594.png "屏幕截图")


#### b站实物视频链接
https://b23.tv/H2vuHDJ