物流机器人小车的运动控制与定位

   本题项目基于2019年上海市高校智能物流机器人选拔赛,题目为:自主设计并制作一款能执行物料搬运任务的智能物流机器人（简称：机器人）。该机器人能够在规定场地内自主行走、寻找等，通过扫描二维码领取任务，自主按任务要求将其物料搬运至指定地点，并按照要求的位置和方向精准摆放。机器人应具有自主定位、自主移动、自主避障、二维码读取、物料位置、颜色及形状识别、物料抓取与搬运、路径规划等功能；竞赛过程由机器人自主运行，不允许使用遥控等人机交互手段及除机器人本体之外的任何辅助装置。

就本文来说,我做的是规划好路径并能快速,稳定,精准的定位到原料区,加工区,成品区.

我们的小车(布线过于凌乱):

    对于程序来说,核心代码有三个点:坐标建立更新,PID寻迹,终点事件处理.下面分别开始介绍.

坐标系

小车运动和定位用到的传感器是寻迹传感器,对于寻迹方案来说,最好是四个方向都用上八路传感器.但是我们这辆小车设计时出了点问题,所以用的是三个八路和两个四路寻迹传感器, 对各个传感器命名为ABCDE,并对各传感器的各个传感器进行标号.如下图所示:

    我们可以对上图所示的比赛场地进行坐标化,以左上角为坐标原点,横轴为X轴,纵轴为Y轴.下面定义数据结构和变量:  

//小车的方向
typedef enum CoordinateDir
{
  DIR_NONE=0,
  DIR_X_P=1, //X正方向
  DIR_X_N,
  DIR_Y_P, //Y正方向
  DIR_Y_N
} Dir;

//坐标点
typedef struct Point{
  u8 x;
  u8 y;
} Point;
 
//定义各个区域的点
//原料区
Point Point_RawMaterialArea;
Point Point_RawMaterialAreaFar;
Point Point_RawMaterialAreaStartMV;
Point Point_RawMaterialAreaEndMV;
//完成区
Point Point_FinishedAeraRed;
Point Point_FinishedAeraGreen;
Point Point_FinishedAeraBlue;
//加工区
Point Point_ProcessingAeraRed;
Point Point_ProcessingAeraGreen;
Point Point_ProcessingAeraBlue;
//二维码区
Point Point_QRCodeAera={2,1};
Point Point_QRCodeAeraFar={2,2};
//出发区
Point Point_InitAera={8,1};
Point Point_InitAeraEnd={7,1};
Point Point_InitAeraStart={7,0};

 下面根据寻迹传感器实现坐标更新的算法:

//更新坐标
void UpdateCoordinatePointNoRotation(void)
{
	switch (car.dir)
	{
	case DIR_X_N:
	    UpdatePoint_Forward_X_N();
	break;
	case DIR_X_P:
	    UpdatePoint_Backward_X_P();
	break;
	case DIR_Y_P:
	    UpdatePoint_Left_Y_P();
	break;
	case DIR_Y_N:
	    UpdatePoint_Right_Y_N();
	break;
	default:
		break;
	}
}

//X正方向更新坐标
void UpdatePoint_Backward_X_P(void)
{
	if(car.flagReadyUpdatePoint==1 && car.flagUpdatePoint==0)
	{
		car.flagUpdatePoint=20;
		if(!TSC1&&!TSC4&&!TSC2&&!TSC3)//当小车位置两个坐标点之间时,进入准备更新坐标的状态
			car.flagReadyUpdatePoint=0;
	}
	//车前面的传感器先经过黑线
	if(car.flagReadyUpdatePoint==0&&car.flagUpdatePoint==0&&((TSA8)||(TSA1)))
		car.flagUpdatePoint=1;
	//车子中间的传感器经过黑线,坐标更新
	if(car.flagUpdatePoint==1 &&(TSC3||TSC2||TSD2||TSD3)){
		car.flagUpdatePoint=2;
		car.currentPoint.x++;
		LED2_Toggle;
		car.flagReadyUpdatePoint=1;
	}
	
}

PID寻迹

  关于STM32的电机控制,可参考前面的博文:http://www.chenjianqu.com/show-26.html.关于麦克纳姆轮如何进行寻迹,参考:http://www.chenjianqu.com/show-27.html. 

终点事件处理

  当小车到达终点后,需要执行一些事件,如与OpenMV交互等,下面为我写的事件处理函数:

void TernimalHandleNoRotationRPF(void)
{
//达到第一个终点,扫描物块颜色并扫描二维码
if(car.pos==1)
{
    SendAndWaitMV(ORDER_SCANOBJECT);//发送OpenMV,让它扫描物块颜色
    MoveOne_Forward_X_N();//往XN方向移动一格
    MoveOne_Forward_X_N();//往XN方向移动一格
    MoveOne_Right_Y_N();//往YN方向移动一格
    //小车的位置偏了,需要进行修正
    u8 t=GetTSC_Number();
    if(t<4)
    {
        CarTurnBackward();
        CarRunBackward(500);
        delay_ms((4-t)*60);
        CarStop();
        SendAndWaitMV(ORDER_QRCODE);
        CarTurnForward();
        CarRunForward(500);
        delay_ms((4-t)*60);
        CarStop();
    }
    else
    {
        SendAndWaitMV(ORDER_QRCODE);//扫描二维码
    }
    flagOrderPPC=car.order;
    car.terminalPoint=Point_ProcessingAeraA;
}
//终点2,到达加工区,由于小车设置的原因,故要先对加工区进行圆环颜色识别
else if(car.pos==2)
{
    u8 i=0;
    while(i<SCAN_TIME)
    {
        SendAndWaitMV(ORDER_SCANCOLOR);
        MoveOne_Backward_X_P();
        SendAndWaitMV(ORDER_SCANCOLOR);
        MoveOne_Backward_X_P();
        SendAndWaitMV(ORDER_SCANCOLOR);
        if(car.order>=ORDER_RGB && car.order<=ORDER_BGR){
            CarSetProcessPoint();//根据扫描到颜色设置加工区各个颜色的位置
            break;
        }
        if(i==SCAN_TIME){
            car.order=ORDER_RGB;
            CarSetProcessPoint();
        }
        MoveOne_Forward_X_N();
        MoveOne_Forward_X_N();
        i++;
    }
    //根据之前扫二维码得到的指令设置任务序列
    CarSetPipelinePoint_A();
    car.terminalPoint=Point_RawMaterialArea;
}
//终点3,到达原料区
else if(car.pos==3)
{
    Y_N_FixAngle();//位置修正
    
    SendAndWaitMV(ORDER_CARRYUP);
    if(car.order==ORDER_FALSE)
    {
        MoveOne_Backward_X_P();
        SendAndWaitMV(ORDER_CARRYUP);
    }
    car.terminalPoint=ppc[0];
    erminalPoint=ppc[1];
}
...篇幅所限,就不一一介绍

全局变量

这是本项目的核心代码之一,之所以把他们放进一个结构体中是为了方便全局变量在各个源代码文件中进行传递.

typedef struct{
    Point currentPoint;//小车当前坐标点
    Point terminalPoint;//小车目标坐标点
    Point lastPoint;
    Dir dir;//小车当前前进的方向
    Dir last_dir;
    u8 dir_m;//小车电机运行的方向
    R_Dir dir_r;
    bool isRotating;//小车是否正在旋转
    bool isRun;//是否运行
    u16 speedA;
    u16 speedB;
    u16 speedC;
    u16 speedBase;//小车PID的基本速度
    u8 flagBlockForward;//小车阻塞运行的标记位
    u16 delayBlockForward;//小车阻塞运行时间
    SpecialSetPosition flagSetPosition;//小车特俗位置标志位
    char order;//小车接收来自OpenMV的指令
    u8 pos;//小车目标点标志位
    //各个标志位
    u8 flagRotation;
    u16 flagUpdatePoint;
    u16 flagReadyUpdatePoint;
    u16 flagHighSpeedDelay;
    //小车位置方案    
    PositionMode pm;
    bool isSendScanObj;
    //小车路径
    CarPath cp;
} MyCar;

主函数

以下是本项目代码的主程序:

int main(void)
{
	u32 cycleTimer = 0;
	int deltaX = 0;
	int deltaY = 0;

	//系统初始化
	HAL_Init();

	SystemClock_Config();

        //自定义延迟和串口初始化
	delay_init(168);
	uart_init(115200);
	uart3_init(9600);
	uart6_init(115200);
        //cubemx初始化GPIO和定时器
	MX_GPIO_Init();

	MX_TIM2_Init();
	MX_TIM3_Init();
	MX_TIM6_Init();

	LCD();//LCD初始化
	
	//PID初始化
	PID_Init(&PID_Car);


	SendStringEndl("System Launch Succeed", 22);
	SendStringEndl_BL("Connected BL", 12);
	SendStringEndl("waitKey...", 11);

	delay_ms(1500);


	sysCounter = 0;

	LCDShowTaskNumber();

	InitCar();

	//有六种不同的区域设置方案
	//car.pm=PM_RPF;
	car.pm = PM_RFP;
	//car.pm=PM_PRF;
	//car.pm=PM_FRP;

	//car.pm=PM_PFR;
	//car.pm=PM_FPR;

    //根据不同的区域方案设置坐标点
	SetAeraCoordinate();
        
        //不同的方案初始终点不同
	if (car.pm == PM_RFP || car.pm == PM_RPF)
	  car.terminalPoint = Point_RawMaterialAreaFar;
	else
	  car.terminalPoint = Point_QRCodeAera;
	//发送给OPENMV方案信息
	switch (car.pm)
	{
	case PM_RPF:
		SendChar_MV(ORDER_RPF);
		break;
	case PM_RFP:
		SendChar_MV(ORDER_RFP);
		break;
	case PM_PRF:
		SendChar_MV(ORDER_PRF);
		break;
	case PM_PFR:
		SendChar_MV(ORDER_PFR);
		break;
	case PM_FPR:
		SendChar_MV(ORDER_FPR);
		break;
	case PM_FRP:
		SendChar_MV(ORDER_FRP);
		break;
	}

	SendStringEndl("Run Start", 9);

	CarStop();


	while (1)
	{
		delay_us(100);

		cycleTimer++;
                //按键和串口处理
		KeyHandle();
		Usart1Handle();//usart connect to computer
		Usart3Handle();//connect to BL


		//每隔一段时间发送输出调试信息
		if (cycleTimer == INTERVAL_SEND_INF) {
			cycleTimer = 0;
			//SendInformationToBL();
			SendInformationToPC();
			//LED1_Toggle;
			//delay_ms(50);
		}
                //到达某一时间后触发回到出发区的程序
		if (sysCounter > Limited_Time)//go back to init area
		{
			LED1_Toggle;
			LED2_Toggle;
			car.terminalPoint = Point_InitAera;
		}
                //小车停止
		if (car.isRun == false)
		{
			CarStop();
			LED1(true);
			LED2(true);
			if (cycleTimer == INTERVAL_SEND_INF - 1) {
				SendStringEndl("Wait", 4);
			}
			continue;
		}

                //特殊区域处理
		if (car.flagSetPosition != SSP_RegularMode)
		{
			switch (car.flagSetPosition)
			{
			case SSP_InitPosition:
				//InitCarPosition();
				//InitCarPosition2();	
				InitCarPosition3();//小车从出发区移到最近的坐标点
				break;
			case SSP_ResetPosition:
				//ResetCarPosition();
				break;
			case SSP_RawPosition:
				break;
			case SSP_MoveToRawPosition:
				//MoveToRawPosition3();
				break;
			default:
				break;
			}
			delay_ms(1);
			continue;
		}

		car.last_dir = car.dir;

		//坐标更新
		UpdateCoordinatePointNoRotation();

		//根据坐标设置方向
		deltaX = car.terminalPoint.x - car.currentPoint.x;
		deltaY = car.terminalPoint.y - car.currentPoint.y;

		if (deltaY > 0)
			car.dir = DIR_Y_P;
		else if (deltaY < 0)
			car.dir = DIR_Y_N;
		else if (deltaX > 0)
			car.dir = DIR_X_P;
		else if (deltaX < 0)
			car.dir = DIR_X_N;
		else
		{//当小车执行到坐标终点之后,阻塞执行与OPENMV的通信 并设置新的坐标点
			CarStop();
			car.pos++;
			car.lastPoint = car.terminalPoint;
			CarBrake();
                        //不同的区域设置方案小车执行的程序不同
			switch (car.pm)
			{
			case PM_RPF:
				TernimalHandleNoRotationRPF();
				break;
			case PM_RFP:
				TernimalHandleNoRotationRFP();
				break;
			case PM_FRP:
				TernimalHandleNoRotationFRP();
				break;
			case PM_FPR:
				TernimalHandleNoRotationFPR();
				break;
			case PM_PRF:
				TernimalHandleNoRotationPRF();
				break;
			case PM_PFR:
				TernimalHandleNoRotationPFR();
				break;
			default:
				break;
			}

                        //LCD显示任务信息
			if (equal(car.currentPoint, Point_QRCodeAera))
			{
				taskInf.list[taskInf.currentPoint] = car.order;
				taskInf.currentPoint++;
				LCDShowTaskNumber();
			}

			car.dir = DIR_NONE;
		}

		if (car.dir != car.last_dir)//方向改变时设置电机的方向
			CarSetMotor();

		//根据距离终点的远近设置小车的速度
		if (deltaY > 2 || deltaY < -2)
			car.speedBase = SPEED_LEVEL_LESS_N;
		else if (deltaY == 2 || deltaY == -2)
			car.speedBase = SPEED_LEVEL_LESS2;
		else if (deltaY == 1 || deltaY == -1)
		{
			if (car.flagUpdatePoint == 0)
				car.speedBase = SPEED_LEVEL_LESS1;
			if (car.flagUpdatePoint > 1)
				car.speedBase = SPEED_LEVEL_LESS0;
		}
		else if (deltaX > 2 || deltaX < -2)
			car.speedBase = SPEED_LEVEL_LESS_N;
		else if (deltaX == 2 || deltaX == -2)
			car.speedBase = SPEED_LEVEL_LESS2;
		else if (deltaX == 1 || deltaX == -1)
		{
			if (car.flagUpdatePoint == 0)
				car.speedBase = SPEED_LEVEL_LESS1;
			if (car.flagUpdatePoint > 1)
				car.speedBase = SPEED_LEVEL_LESS0;
		}
            
                //根据方向小车执行循迹PID
		switch (car.dir)
		{
		case DIR_X_N:
			CarForwardPID();
			break;
		case DIR_X_P:
			CarBackwardPID();
			break;
		case DIR_Y_N:
			CarRightPID();
			break;
		case DIR_Y_P:
			CarLeftPID();
			break;
		default:
			CarStop();
			break;
		}

	}

	/* USER CODE END 3 */
}

完整项目代码:https://github.com/chenjianqu/MHR_v1.0