关于第十八届智能车独轮组自动返航功能的实现方法

关于第十八届智能车独轮组自动返航功能的实现方法

前几天无意刷到今年智能车独轮冠军的视频，其自动返航功能令人眼前一亮十分有趣。于是斥巨资买了一个两轮平衡车架连夜画板来验证一下我的想法。
我的思路很简单，当小车启动后不停地读取编码器数据然后根据当前的航向角计算出小车的坐标。

代码实现也非常简单。

void GetCoordinate(float Angle)
{
	static float AngleLast;
	float AngleFilter = (Angle+AngleLast)/2;
	
	AngleLast = Angle;
	
	CoordinateCar.SpeedR = Encoder1.GetCnt();
	CoordinateCar.SpeedL = Encoder2.GetCnt();
	Encoder1.ClearCnt();
	Encoder2.ClearCnt();
	CoordinateCar.Speed = (CoordinateCar.SpeedR+CoordinateCar.SpeedL)/2;
	
	CoordinateCar.Wheel_L_SUM += CoordinateCar.SpeedL;
	CoordinateCar.Wheel_R_SUM += CoordinateCar.SpeedR;
	
	CoordinateCar.PlaceX += (float)CoordinateCar.Speed*cos(AngleFilter*DEG2RAD);
	CoordinateCar.PlaceY += (float)CoordinateCar.Speed*sin(AngleFilter*DEG2RAD);
}

理论上讲这段代码执行频率越高越好，因为航向是不停在变的，且无法预测的。
当触发返航时，根据当前的小车坐标(X,Y)进行反正切运算就可以算出小车相对于原点的方位，进而可以计算出小车的目标航向角。当然需要对X=0进行特殊处理，并且计算目标航向角的时候需要判断小车坐标所在象限。

void AutoBackControl(void)
{
	uint8_t a=0,b=0,Q;
	
	if(AutoBack != 0)
	{
		if(CoordinateCar.PlaceX == 0)
		{
			if(CoordinateCar.PlaceY > 0)
			{
				ATT_Yaw.Target_EulerVal = -90;
			}
			else
			{
				ATT_Yaw.Target_EulerVal = 90;
			}
		}
		else
		{
			if(CoordinateCar.PlaceX > 0)
			{
				a = 1;
			}
			if(CoordinateCar.PlaceY >= 0)
			{
				b = 1;
			}
			Q = (a<<1) | b;
			switch(Q)
			{
				case 2:case 3:
				{
					ATT_Yaw.Target_EulerVal = atan(CoordinateCar.PlaceY/CoordinateCar.PlaceX)*RAD2DEG + 180;
				}break;
				default:
				{
					ATT_Yaw.Target_EulerVal = atan(CoordinateCar.PlaceY/CoordinateCar.PlaceX)*RAD2DEG;
				}break;
			}
			if(ATT_Yaw.Target_EulerVal>180)
			{
				ATT_Yaw.Target_EulerVal-=360;
			}
			else if(ATT_Yaw.Target_EulerVal<-180)
			{
				ATT_Yaw.Target_EulerVal+=360;
			}
		}
		Distance = CoordinateCar.PlaceX*CoordinateCar.PlaceX + CoordinateCar.PlaceY*CoordinateCar.PlaceY;
		AutoBackSpeed = 200;
		CoordinateCar.Backing = 1;
	}
}

经过两天的调试，测试效果还是非常令人满意的。首先遥控小车乱跑然后在随机位置触发返航，小车计算出的原点位置和实际的原点位置在最初的两分钟里误差很小，即使后期误差累积变大后放在比赛场景里也可以让起始点标记出现在摄像头视野里不影响小车入库。
最后附上小车实物图两张。