---

前言

本文介绍了基于全国大学生工程训练综合能力竞赛的代码和设计，具体聚焦于基于Arduino的机械臂物流麦克纳姆轮小车项目。在文章中，我们将探讨如何利用电机编码器、PID控制、IMU901与MPU6050等设备实现高效的角度与位置控制，同时还会涵盖机器视觉与神经网络在该项目中的应用。完整代码请参考以下GitHub仓库：

https://github.com/Pr0ximah/logisticCar_prp43

https://github.com/Pr0ximah/LogisticCar_NEW

---

一、基于PID的电机编码器定位

1.MG513电机

该项目中使用的电机为MG513电机，其配备的霍尔编码器通过霍尔码盘和霍尔元件来感应旋转角度。霍尔码盘与电机同轴，当电机转动时，霍尔元件会根据码盘上的磁极检测旋转情况，并输出若干脉冲信号。为了判断旋转方向，霍尔编码器提供两段具有固定相位差的方波信号：A组和B组，输出信号如图所示。

通过Arduino采集A组和B组的信号，可以实时监测电机的旋转状态。当A组信号出现上升沿或下降沿时，B组信号的状态可用于判断电机的旋转方向与圈数。具体实现时，我们使用Arduino的attachInterrupt函数，将A组信号的变化与读取B组状态的函数绑定，以此实现对电机的精确控制。实验数据显示，A组每转动一圈会产生约1595个脉冲（对于左前轮电机，右前轮电机为1599个），这为实现麦克纳姆轮的定量控制奠定了基础。

Encoder EncoderSet::encoderFL(port_Encoder_FL_A, port_Encoder_FL_B, Encoder_FL_Coefficient);
Encoder EncoderSet::encoderFR(port_Encoder_FR_A, port_Encoder_FR_B, Encoder_FR_Coefficient);
Encoder EncoderSet::encoderBL(port_Encoder_BL_A, port_Encoder_BL_B, Encoder_BL_Coefficient);
Encoder EncoderSet::encoderBR(port_Encoder_BR_A, port_Encoder_BR_B, Encoder_BR_Coefficient);
Encoder *EncoderSet::PtEncoderFL = nullptr;
Encoder *EncoderSet::PtEncoderFR = nullptr;
Encoder *EncoderSet::PtEncoderBL = nullptr;
Encoder *EncoderSet::PtEncoderBR = nullptr;

EncoderSet::EncoderSet() {
    PtEncoderFL = &encoderFL;
    PtEncoderFR = &encoderFR;
    PtEncoderBL = &encoderBL;
    PtEncoderBR = &encoderBR;
    attachInterrupt(encoderFL.ISR_Port, updateFL, CHANGE);
    attachInterrupt(encoderFR.ISR_Port, updateFR, CHANGE);
    attachInterrupt(encoderBL.ISR_Port, updateBL, CHANGE);
    attachInterrupt(encoderBR.ISR_Port, updateBR, CHANGE);
}

void EncoderSet::updateFL() { PtEncoderFL->updateCount(false); }

void EncoderSet::updateFR() { PtEncoderFR->updateCount(true); }

void EncoderSet::updateBL() { PtEncoderBL->updateCount(false); }

void EncoderSet::updateBR() { PtEncoderBR->updateCount(true); }

int Encoder::port_to_ISR(int port) {
    switch (port) {
        case 2:
            return 0;
        case 3:
            return 1;
        case 21:
            return 2;
        case 20:
            return 3;
        case 19:
            return 4;
        case 18:
            return 5;
    }
}

Encoder::Encoder(int _portA, int _portB, int _coefficient) : COEFFICIENT_PER_ROUND(_coefficient) {
    portA = _portA;
    portB = _portB;

    ISR_Port = port_to_ISR(portA);
    pulseCount = 0;
    numRound = 0;
    angleLast = 0;
    angleCur = 0;
    countOfUpdate = 0;
    for (int i = 0; i < 3; i++) {
        angleVel[i] = 0;
    }

    // 引脚模式设置
    pinMode(portA, INPUT);
    pinMode(portB, INPUT);
}

void Encoder::reset() {
    pulseCount = 0;
    numRound = 0;
    countOfUpdate = 0;
    for (int i = 0; i < 3; i++) {
        angleVel[i] = 0;
    }
    angleLast = 0;
    angleCur = 0;
}

float Encoder::getAngle() const { return double(pulseCount) / COEFFICIENT_PER_ROUND * 360; }

float Encoder::getAbsoluteAngle() const { return numRound * 360 + getAngle(); }

int Encoder::getRound() const { return numRound; }

float Encoder::getDisOfWheel() const { return PI * WHEEL_DIAMETER * getAbsoluteAngle() / 360; }

void Encoder::update() {
    if (pulseCount <= -COEFFICIENT_PER_ROUND) {
        pulseCount += COEFFICIENT_PER_ROUND;
        numRound--;
    }
    if (pulseCount >= COEFFICIENT_PER_ROUND) {
        pulseCount -= COEFFICIENT_PER_ROUND;
        numRound++;
    }

    if (pulseCount <= -COEFFICIENT_PER_ROUND) {
        pulseCount += COEFFICIENT_PER_ROUND;
        numRound--;
    }
    if (pulseCount >= COEFFICIENT_PER_ROUND) {
        pulseCount -= COEFFICIENT_PER_ROUND;
        numRound++;
    }
    countOfUpdate++;

    if (countOfUpdate % 3 == 0) {
        timeCur = millis();
        angleCur = getAbsoluteAngle();
        if (firstTimeFlag) {
            timeLast = timeCur;
            angleLast = angleCur;
            firstTimeFlag = false;
            return;
        } else {
            int timeInterval = timeCur - timeLast;
            double angleDiff = angleCur - angleLast;
            double angleVelTemp;
            if (timeInterval != 0) {
                angleVelTemp = angleDiff * 1000 / timeInterval;
            } else {
                angleVelTemp = 0;
            }
            angleVel[0] = angleVel[1];
            angleVel[1] = angleVel[2];
            angleVel[2] = angleVelTemp;
            timeLast = timeCur;
            angleLast = angleCur;
        }
    }
}

void Encoder::updateCount(bool R) {
    cli();
    bool flag;
    flag = ((digitalRead(portA) == digitalRead(portB)) && R) || ((digitalRead(portA) != digitalRead(portB) && !R));
    if (flag) {
        pulseCount++;
    } else {
        pulseCount--;
    }
    sei();
}

void Encoder::testCoefficient() {
    Serial.begin(9600);

    while (true) {
        String cmd = Serial.readString();
        if (cmd == "STOP") {
            break;
        } else if (cmd == "RESET") {
            pulseCount = 0;
            Serial.println("reset done");
        }
        Serial.println(pulseCount);
        delay(10);
    }
}

double Encoder::getAngleVel() const {
    double sum = 0;
    int num = 0;
    for (int i = 0; i < 3; i++) {
        if (angleVel[i] != 0) {
            sum += angleVel[i];
            num++;
        }
    }
    if (num == 0) {
        num = 1;
    }
    return sum / num;
}

2.麦克纳姆轮

在收集到电机旋转的具体数据之后,通过解算得到底盘相对于初始位置的相对位置:

   float disWheel[4] = {encoders.encoderFL.getDisOfWheel(), encoders.encoderFR.getDisOfWheel(),
                         encoders.encoderBL.getDisOfWheel(), encoders.encoderBR.getDisOfWheel()};
    posCur.setXY((disWheel[0] - disWheel[1] - disWheel[2] + disWheel[3]) / 4,
                 (disWheel[0] + disWheel[1] + disWheel[2] + disWheel[3]) / 4);

3.PID

PID控制是一种在工业过程控制中应用最为广泛的控制系统.它基于控制系统输出和输入的偏差进行调节.以比例,积分和微分三个环节的加和来控制被控量.具体来说,PID控制器会根据测试出的比例,积分和微分系数，通过测量,比较和执行三个步骤来纠正调节系统实际运行与期望的偏差,从而使得被控系统的输出尽可能接近期望的输出.
在PID控制器的三个组成部分中,比例控制主要通过对误差信号进行放大或缩小来调整系统的输出与输入偏差的比例关系,也就是期望值与实际值的差的放大或缩小反馈到控制阶段,从而对控制阶段进行改进.它能够快速地响应系统中的变化并进行快速的调整,从而加快调节过程并减小误差.积分环节则通过对误差进行积分运算,以消除系统中的稳态误差即因为场地原因导致的阻力.微分环节则通过对误差进行微分运算,预测系统中的未来变化,从而提前进行调节,以减小未来可能产生的误差.同时也减小因为过度比例调控导致的过充和振荡问题,也就使得系统工作更为稳定.

errorNow = target - current;
    P = kp * errorNow;
    if (initFlag) {
        errorLast = errorNow;
        initFlag = false;
        errorInt = 0;
    }
    errorDiff = errorNow - errorLast;
    D = kd * errorDiff;
    if (fabs(P) >= IRangeLocal) {
        errorInt = 0;
    } else {
        errorInt += errorNow;
        if (fabs(errorInt) * ki > IMax) {
            errorInt = sign(errorInt) * IMax / ki;
        }
    }
    I = ki * errorInt;
    if (fabs(errorNow) <= errorTol) {
        if (outSetZeroWhenArrive) {
            if (secStable >= 2) {
                outVal = 0;
                arriveFlag = true;
            } else {
                outVal = P + I + D;
                arriveFlag = false;
            }
        } else {
            outVal = P + I + D;
            arriveFlag = false;
        }
    } else {
        secStable = 0;
        outVal = P + I + D;
    }
    secNow = millis() / 1000;
    if (initFlag) {
        secLast = secNow;
    }
    if (fabs(errorNow <= errorTol)) {
        secStable += secNow - secLast;
    }
    secLast = secNow;
    errorLast = errorNow;

    Serial.println("P I D: " + String(P) + " " + String(I) + " " + String(D));

    return outVal;

二、IMU901和MPU6050的角度控制

1.IMU901

通过使用USB串口模块直接读取IMU901的串口信息(16进制),通过观察总结规律写出适用于Arduino的串口读写程序.但是IMU901稳定性很低,多次测量后会有很大的角度误差出现,对于瞬时旋转测出的值响应速度很慢,无法满足项目需求.

// 软串口 (rx, tx)
SoftwareSerial SoftSerial(tx_IMU, rx_IMU);
static double heading_original = 401;  // heading初始值，缺省401

void IMU_Init() { SoftSerial.begin(9600); }

double IMU_Heading() {
    int16_t buffer[9];
    unsigned long long time_start = millis();
    while (true) {
        if (millis() - time_start > IMU_JUMP_TIME) {
            return 0;
        }
        if (SoftSerial.find(0x55)) {
            for (int i = 0; i < 9; i++) {
                while (!SoftSerial.available()) {
                    delayMicroseconds(1);
                }
                buffer[i] = SoftSerial.read();
            }
            if (buffer[0] == 0x55 && buffer[1] == 0x01 && buffer[2] == 0x06) {
                for (int i = 0; i < 9; i++) {
                    Serial.print(buffer[i], HEX);
                    Serial.print(" ");
                }
                Serial.print("\n");

                double heading = (float((int16_t)(buffer[8] << 8) | buffer[7])) / 32768.0 * 180;
                if (heading_original == 401) {
                    heading_original = heading;
                }
                heading -= heading_original;
                // 归一化到 (-180 deg ~ 180 deg]
                while (heading > 180) {
                    heading -= 360;
                }
                while (heading <= -180) {
                    heading += 360;
                }
                return heading;
            }
        }
        delayMicroseconds(1);
    }
}

2.MPU6050

直接调库使用

#include "MPU6050_6Axis_MotionApps20.h"
#include <Arduino.h>
#include <SoftwareSerial.h>

#include "ConstDef.h"
#include "Geometry.h"
#include "PinDef.h"

IMU *IMU::instance_ptr_ = nullptr;

void IMU::InitIMU() {
    Wire.begin();
    Wire.setClock(400000); 
    while (!is_dmp_ready_) {
        mpu_.initialize();
        dev_status_ = mpu_.dmpInitialize();
        delay(1000);
        if (dev_status_ == 0) {
            mpu_.CalibrateAccel(6);
            mpu_.CalibrateGyro(6);
            mpu_.PrintActiveOffsets();
            mpu_.setDMPEnabled(true);
            mpu_int_status_ = mpu_.getIntStatus();
            is_dmp_ready_ = true;
            packet_size_ = mpu_.dmpGetFIFOPacketSize();
        }
    }
    delay(1000);
    SetBias();
}

double IMU::GetAngle() {
    if (mpu_.dmpGetCurrentFIFOPacket(fifo_buffer_)) {
        mpu_.dmpGetQuaternion(&q_, fifo_buffer_);
        mpu_.dmpGetGravity(&gravity_, &q_);
        mpu_.dmpGetAccel(&aa_, fifo_buffer_);
        mpu_.dmpConvertToWorldFrame(&aa_world_, &aa_, &q_);
        mpu_.dmpGetGyro(&gg_, fifo_buffer_);
        mpu_.dmpConvertToWorldFrame(&gg_world_, &gg_, &q_);
        mpu_.dmpGetYawPitchRoll(ypr_, &q_, &gravity_);
        angle_ = ypr_[0] * RAD_TO_DEG;

        /// @note 修正为逆时针旋转角度增加
        angle_ = -angle_;
    }
    double angle_ret_ = AngleNormalize(angle_ - angle_origin_);
    return angle_ret_;
}

void IMU::SetBias() {
    double sum = 0;
    if (mpu_.dmpGetCurrentFIFOPacket(fifo_buffer_)) {
        Serial.println("get bias");
        for (int i = 0; i < 500; i++) {
            mpu_.dmpGetQuaternion(&q_, fifo_buffer_);
            mpu_.dmpGetGravity(&gravity_, &q_);
            mpu_.dmpGetAccel(&aa_, fifo_buffer_);
            mpu_.dmpConvertToWorldFrame(&aa_world_, &aa_, &q_);
            mpu_.dmpGetGyro(&gg_, fifo_buffer_);
            mpu_.dmpConvertToWorldFrame(&gg_world_, &gg_, &q_);
            mpu_.dmpGetYawPitchRoll(ypr_, &q_, &gravity_);
            sum += ypr_[0] * RAD_TO_DEG;
            delay(10);
        }
        angle_bias_ = sum * (-1) / 500;
    }
    angle_origin_ = angle_bias_;

}

具体的控制仍然使用PID完成.

三、神经网络和机器视觉

项目组使用的是OpenMV H7 Plus的摄像头,其自带了内部处理器.
将标记好的涵盖场地边线的图片输入在https://edgeimpulse.com/中,进行神经网络的训练,再将得到的CNN放置在摄像头的内部处理器中.

最终得到的正确率还是很可观的

在Arduino中获取数据即可:

#include "Vision.h"

#include <Arduino.h>
#include <string.h>

Vision::Vision(CamType type) { type_ = type; }

void CalculateDistance(double &dis, const String &str) {
    int y_coordinate[100];
    double sum = 0;
    int count = 0;
    char *data = const_cast<char *>(str.c_str());
    char *group = strtok(data, ";");

    while (group != nullptr) {
        char *coordinates = strrchr(group, ',');
        if (coordinates != nullptr) {
            coordinates++;
            y_coordinate[count++] = atoi(coordinates);
        }
        group = strtok(nullptr, ";");
    }

    for (int i = 0; i < count; i++) {
        sum += y_coordinate[i];
    }
    dis = 320 - sum / count;
}

void CalculateCenterDiff(Vector2D &diff, const String &str) {
    int center[2];
    char *data = const_cast<char *>(str.c_str());
    char *temp = strtok(data, ",");
    int cnt = 0;
    while (temp) {
        center[cnt++] = atoi(temp);
        temp = strtok(nullptr, ",");
    }
    int x = center[0];
    int y = center[1];
    Serial.print(x);
    Serial.print("  ,  ");
    Serial.println(y);
    diff.SetXY(x - 70, 45 - y);
}

void Vision::Init() {
    switch (type_) {
        case FRONT:
            Serial2.begin(4800);
            break;
        case RIGHT:
            Serial3.begin(4800);
            break;
    }
    found_mark_ = false;
}

void Vision::Update() {
    String str = "";
    switch (type_) {
        case FRONT:
            str = Serial2.readStringUntil('\n');
            break;
        case RIGHT:
            str = Serial3.readStringUntil('\n');
            break;
    }
    Serial.println(str);
    if (str.substring(0, 4) == "edge") {
        if (str.length() > 5) {
            CalculateDistance(edge_distance_, str.substring(5));
        }
    } else if (str.substring(0, 4) == "mark") {
        found_mark_ = true;
    } else if (str.substring(0, 4) == "circ") {
        CalculateCenterDiff(center_diff_, str.substring(5));
    } else if (str.substring(0, 2) == "fr") {
        found_edge_ = true;
    }
}

double Vision::GetEdgeDistance() { return edge_distance_; }

Vector2D Vision::GetCenterDiff() { return center_diff_; }

四.整体控制

1.基于编码器的控制

2.基于视觉完成的状态机控制

---

总结

在本次项目中，通过结合了多种控制技术和智能算法，包括电机编码器的PID控制、IMU角度检测、机器视觉和神经网络等，从而实现了机械臂物流小车的高效运动与智能决策。通过MG513电机和霍尔编码器，实现了精准的位置控制；通过IMU901和MPU6050，实现了小车在复杂地形下的稳定运行；而机器视觉和神经网络的结合，更使小车具备了一定的环境适应和智能决策能力。

这个项目展示了如何将硬件控制与智能算法结合起来，以实现复杂的机器人任务。希望本文能够为类似的项目提供一些启示和参考。如果您对本项目感兴趣或想要获取更多信息，请访问我们的GitHub仓库。感谢您的阅读！