【智能车学习】全向行进控制算法总结——驱动部分

最新推荐文章于 2025-11-13 10:31:03 发布

原创

最新推荐文章于 2025-11-13 10:31:03 发布 · 5.2k 阅读

177 ·

CC 4.0 BY-SA版权

文章标签：

#c语言 #自动驾驶 #单片机

本文详细介绍了基于麦轮的全方位移动机器人控制系统的实现，包括麦轮基本原理、速度解算、PID控制算法以及陀螺仪的循迹算法。通过速度解算将导航速度转换为轮子转速，利用PID算法进行精确控制，并结合陀螺仪数据实现稳定循迹。此外，还探讨了变速控制策略，以优化机器人的行驶性能。附录中提供了关键代码段，如速度解算和电机控制。

前言

两年的竞赛生涯一晃而过，不知不觉我也升入了大三，这两天翻出了十六届比赛时写的代码，打算做一下归纳整理，也算是为实验室生活画个句号。

麦轮基本原理

麦克纳姆轮（Mecanum wheel），这种全方位移动方式是基于一个有许多位于机轮周边的轮轴的中心轮的原理上，这些成角度的周边轮轴把一部分的机轮转向力转化到一个机轮法向力上面。
基于麦克纳姆轮技术的全方位运动设备可以实现前行、横移、斜行、旋转及其组合等运动方式。

基本控制算法

速度解算

我们可以将麦轮车的部分分为前进方向（X或Y）和转向方向（Z），在摄像头判断出前方路况后得到X,Y,Z三轴的速度后，需要将车身的X,Y,Z三轴速度分配给四个轮子，以便于后续的控制，这个过程就是速度解算。
速度解算的核心是要得到逆运动学方程矩阵，以便于将导航速度向量映射到四轮角速度向量上。即模型输入为导航向量（车身X,Y,Z三轴速度），输出量为小车四个轮子的转动速度。
麦轮物理模型
（此处推导参考论文《Mecanum轮逆矩阵研究》）
如上图为麦轮运动方程建模，L为车长，W为车宽，车轮顺序为左上，右上，左下，右下。
逆运动学方程如下：

将上式列成表达式为：
速度解算（简化前）
由于在程序控制过程中可以根据PID控制系数来改变speed大小，所以我们可以简化上图公式中的系数，并自己合理调整，便于后续参数调节。改动后的程序如下。

/**
 * @file        速度解算，由逆矩阵公式解算
 * @note        参照论文《Mecanum轮逆矩阵研究》
 * @date        2021
 */
void Speed_Calculation(int16 X_speed, int16 Y_speed, int16 Z_speed)
{
   
   
    if(Z_speed>100) Z_speed = 100;
    else if(Z_speed < -100) Z_speed = -100;   // 对转向速度限幅

    SpeedSet1= X_speed - 0.3*Z_speed- Y_speed;
    SpeedSet2= X_speed + 0.3*Z_speed+ Y_speed;
    SpeedSet3= X_speed - 0.3*Z_speed+ Y_speed;
    SpeedSet4= X_speed + 0.3*Z_speed- Y_speed;
}

经典pid算法

//PID 参数初始化
void PID_Init(PID *SPID)
{
   
   
   SPID->SumError = 0;
   SPID->LastError = 0;   //Error[k-1]
   SPID->PrevError = 0;   //Error[k-2]
   SPID->Proportion = 0;  //比例常数 Proportional Const
   SPID->Integral = 0;    //积分常数 Integral Const
   SPID->Derivative = 0;  //微分常数 Derivative Const
}

int16 PID_Increase(PID *SPID,int16 NextPoint,int16 NowData)
{
   
   
    int16 iError,iIncpid;                                                          //当前误差、累计增量
  iError = NextPoint - NowData;                                                         //目标速度-当前速度
  iIncpid = (int16)( (iError-SPID->LastError)* (SPID->Proportion)
                   +  iError*(SPID->Integral)
                   + (iError-2*SPID->LastError+SPID->PrevError) * (SPID->Derivative));
  SPID->PrevError = SPID->LastError;                                                    //E(k-2)
  SPID->LastError = iError;                                                             //E(k-1)
  return(iIncpid);                                                                      //返回增量值
}


int16 PID_Realize(PID *SPID,float iError)
{
   
   
    int16 place;//dError;
    //int16 iError;
    //iError = NextPoint - NowData;               //偏差
    SPID->SumError += iError;                     //积分
    //dError = iError - SPID->LastError;          //微分
    //SPID->LastError = iError;
    place = (int16)(SPID->Proportion * iError     //比例项
    + SPID->Integral * SPID->SumError);             //积分项
    //+ SPID->Derivative * icm_gyro_y);             //微分项
    return place;
}

控制算法pro

运用陀螺仪的循迹算法

首先要使用陀螺仪控制，这里可能会涉及到陀螺仪初始化过不了的问题，如果用的是逐飞科技的陀螺仪，可以尝试一下以下方法。

这里使用到了两层pid控制，首先用第一层pid算出的turn_R，作为前方道路曲率的倒数，再根据车速及曲率导出应有角速度，与陀螺仪测出的实时角速度比较，代入第二层pid，实现最终控制。代码如下。

void Dir_Speed_Control(float Calculate_Center)
{
   
   
	turn_R = PID_Realize(&PID1,g_fMid_AD);
	Car_speed = (encoder_data[0]+encoder_data[1]+encoder_data[2]+encoder_data[3])/4;
	w = turn_R * Car_speed;
	SpeedSet_Z = PID_Realize(&PID2,icm_gyro_z-w);
	if(SpeedSet_Z<-100)
		SpeedSet_Z = -100;
		
}

变速控制

为了最大程度发挥小车性能，更快完成赛道，可引入变速控制，具体方案如下。

SpeedSet_X = K_speed*(84/10 - Error/10) + PWM_default;

PWM_default为设定最低速度，error为摄像头算出的偏差（我的摄像头图像宽度为170，所以最大偏差为84），并通过系数K_speed调节速度上限。另外如果想要控制响应更迅速，也可以考虑使用平方项。

驱动部分的总结到此就告一段落了，相对而言这部分的变换还是相对较少的，主要还是要注意参数的调试，也可以尝试着把电机数据发送到上位机辅助调试。

附录：Motor.c代码

#include "headfile.h"

/*********速度测量相关初始化**********/
int32 speed1_power;
int32 speed2_power;
int32 speed3_power;
int32 speed4_power;

int16 encoder_data[4];

int16 Pulse_Filtered_data1=0;
int16 Pulse_Filtered_data2=0;
int16 Pulse_Filtered_data3=0;
int16 Pulse_Filtered_data4=0;

/*********速度解算相关初始化**********/
float L=0.186,W=0.183;    //车尺寸，单位m
//int R=28;

/*********速度控制相关初始化**********/
int16 SpeedSet1;
int16 SpeedSet2;
int16 SpeedSet3;
int16 SpeedSet4;

int16 SpeedSet_X;
int16 SpeedSet_Y;
int16 SpeedSet_Z;

PID Motor1={
   
   0,0,0,  16,  3,   3};
PID Motor2={
   
   0,0,0,  16,  3,   3};
PID Motor3={
   
   0,0,0,  16,  3,   3};
PID Motor4={
   
   0,0,0,  16,  3,   3};

PID Dir_Speedr1 ={
   
   0,0,0,  11,       0,  0.1};           //右转向
PID Dir_Speedr2 ={
   
   0,0,0,  0.002,    0,  0.0002};

PID Dir_Speedl1 ={
   
   0,0,0,  10,       0,  0.1};           //左转向
PID Dir_Speedl2 ={
   
   0,0,0,  0.002,    0,  0.0002};

PID   Roundr1   ={
   
   0,0,0,  10,       0,  0};           //右环
PID   Roundr2   ={
   
   0,0,0,  0.0029,   0,  0.0002 };

PID   Roundl1   ={
   
   0,0