AGV_little_C1.zip 22.31MB ZIP 50次下载

由于整个小车就做了三天所以结构不是太好。。。勿喷低技术力。。。。

基于ROS melodic，底盘控制器STM32的一个2D激光SLAM自主导小车，通过激光雷达扫描在未知环境中建立地图自主规划路径。

平台：Intel_up_core,底盘控制器：STM32F103ZETA6，STM32F103C8T6,传感器：USB2.0_720PCamera 思岚A1 激光雷达

ROS：

ROS是用于编写机器人软件程序的一种具有高度灵活性的软件架构。它包含了大量工具软件、库代码和约定协议，旨在简化跨机器人平台创建复杂、鲁棒的机器人行为这一过程的难度与复杂度。

ROS设计者将ROS表述为“ROS = Plumbing + Tools + Capabilities + Ecosystem”，即ROS是通讯机制、工具软件包、机器人高层技能以及机器人生态系统的集合体 [2]  。

ROS Package ：navigation/rbx1_nav simple_navigation_goals

Overview：

整个小车分为三个部分 最上层是intel up core开发平台 使用ubuntu18.04操作系统搭载ROS melodic（旋律）

由于外设分配问题下层分为了两个控制器 。

底盘使用一整块铝合金板材 打孔之金额固定电机和电池，采用独立悬挂，

电池是rc 12.6V 6000mAh 6块为了长续航

日本Namiki空心杯减速电机带编码器22CL-3501PG 12VDC 120转 手里有现成的就直接用了

控制器由于是直接开发板链接的

使用了正点原子的无线调试器，当时买这个调试器花了将近三百块，因为需要做编码器里程计 所以需要四个轮子接触点的线速度矢量合成 所以四个编码器使用了四个定时器 外设稍微有点不够 就分成了两个。

主控制器使用的是STM32F103ZET6 从控制器使用的是 STM32F103C8T6 是两款最常见的核心板，电机驱动使用的是 L298n电源直接就用LM2956了 因为想最快的时间把车搭出来就都用的现成的模块 没有去打板子 （主要验证软件 硬件凑合凑合就行）。

关于定时器编码器：如果计数器只在TI2的边沿计数，则置TIMx_SMCR寄存器中的SMS=001；如果只在TI1边沿计数，则置SMS=010；如果计数器同时在TI1和TI2边沿计数，则置SMS=011。通过设置TIMx_CCER寄存器中的CC1P和CC2P位可以选择TI1和TI2极性；如果需要，还可以对输入滤波器编程。两个输入TI1和TI2被用来作为增量编码器的接口。参看表77，假定计数器已经启动(TIMx_CR1寄存器中的CEN=’1’)，计数器由每次在TI1FP1或TI2FP2上的有效跳变驱动。 TI1FP1和TI2FP2是TI1和TI2在通过输入滤波器和极性控制后的信号；如果没有滤波和变相，则TI1FP1=TI1，TI2FP2=TI2。根据两个输入信号的跳变顺序，产生了计数脉冲和方向信号。依据两个输入信号的跳变顺序，计数器向上或向下计数，同时硬件对TIMx_CR1寄存器的DIR位进行相应的设置。不管计数器是依靠TI1计数、依靠TI2计数或者同时依靠TI1和TI2计数。在任一输入端(TI1或者TI2)的跳变都会重新计算DIR位。编码器接口模式基本上相当于使用了一个带有方向选择的外部时钟。这意味着计数器只在0到TIMx_ARR寄存器的自动装载值之间连续计数(根据方向，或是0到ARR计数，或是ARR到0计数)。所以在开始计数之前必须配置TIMx_ARR；同样，捕获器、比较器、预分频器、触发输出特性等仍工作如常。在这个模式下，计数器依照增量编码器的速度和方向被自动的修改，因此计数器的内容始终指示着编码器的位置。计数方向与相连的传感器旋转的方向对应。下表列出了所有可能的组合，假设TI1和TI2不同时变换。

电源使用锂电池组6个单元并联，每单元由三个锂电池串联构成，由于单电池带有保护板所以可以直接串联充放电。电池组输出电压为12V，经过DCDC降压模块降压到5V提供给控制器和其他单元。

L298N是ST公司生产的一种高电压，大电流的电机驱动芯片。该芯片采用15脚封装。主要特点是：工作电压高，最高工作电压可达46V，输出电流大，瞬间峰值可达3A，持续工作电流为2A；额定功率为25W。

上中下一共三层 ，最上层放的是传感器因为需要净空所以把激光雷达架了起来为了更好的扫描周围环境减少死区，中间层放着的是所有的控制板，上下两面放置上面是主控制器和Intel平台下面是电机驱动和从控制器还有电源管理。

RPLIDAR A1作为核心传感器，可快速获得环境轮廓信息，配合SLAMWARE使用，可以帮助机器人实现自主构建地图、实时路径规划与自动避开障碍物。

速度闭环控制

在过程控制中，按偏差的比例（P）、积分（I）和微分（D）进行控制的PID控制器（亦称PID调节器）是应用最为广泛的一种自动控制器。它具有原理简单，易于实现，适用面广，控制参数相互独立，参数的选定比较简单等优点；而且在理论上可以证明，对于过程控制的典型对象──“一阶滞后+纯滞后”与“二阶滞后+纯滞后”的控制对象，PID控制器是一种最优控制。PID调节规律是连续系统动态品质校正的一种有效方法，它的参数整定方式简便，结构改变灵活

参数调整一般步骤：

1、确定比例增益。

P确定比例增益P时，首先去掉PID的积分项和微分项，一般是令Ti=0、Td=0，PID为纯比例调节。输入设定为系统允许的最大值的60%~70%，由0逐渐加大比例增益P，直至系统出现振荡。再反过来，从此时的比例增益P逐渐减小，直至系统振荡消失，记录此时的比例增益P，设定PID的比例增益P为当前值的60%~70%。比例增益P调试完成。

2、确定积分时间常数Ti。比例增益P确定后，设定一个较大的积分时间常数Ti的初值，然后逐渐减小Ti，直至系统出现振荡，之后在反过来，逐渐加大Ti，直至系统振荡消失。记录此时的Ti，设定PID的积分时间常数Ti为当前值的150%~180%。积分时间常数Ti调试完成。

3、确定微分时间常数Td。微分时间常数Td一般不用设定，为0即可。若要设定，与确定P和Ti的方法相同，取不振荡时的30%。

4、系统空载、带载联调，再对PID参数进行微调，直至满足要求。

PID控制器参数整定的方法：

1、理论计算整定法。它主要是依据系统的数学模型，经过理论计算确定控制器参数。这种方法所得到的计算数据未必可以直接用，还必须通过工程实际进行调整和修改。

2、工程整定方法，它主要依赖工程经验，直接在控制系统的试验中进行，且方法简单、易于掌握，在工程实际中被广泛采用。PID控制器参数的工程整定方法，主要有临界比例法、反应曲线法和衰减法。三种方法各有其特点，其共同点都是通过试验，然后按照工程经验公式对控制器参数进行整定。现在一般采用的是临界比例法。利用该方法进行PID控制器参数的整定步骤如下：

(1)首先预选择一个足够短的采样周期让系统工作。

(2)仅加入比例控制环节，直到系统对输入的阶跃响应出现临界振荡，记下这时的比例放大系数和临界振荡周期。

(3)在一定的控制度下通过公式计算得到PID控制器的参数。

为了方便调试在后面加了一个显示屏能实时看见整车的运行情况。

gmapping 扫描环境建立地图 通过编码器里程计和激光雷达蒙特卡洛定位，

远程操作与建立地图演示 ROS分布式部署通过TCPIP链接远程监视建图过程

建立好的2D地图

ROS部分参考了 ROS wiki navigation rbx1_nav fgithub

STM32主控制器速度闭环部分代码：

#define MOTOR_OUTPUT_MAX				999
#define MOTOR_SPEED_PID_KP				2.400
#define MOTOR_SPEED_PID_KI				1.000
#define MOTOR_SPEED_PID_KD				0.000
#define PI						3.1415926535897932384626433832795

const PIDParamBaseTypedef PARAM_PID = {//电机pid速度控制参数

	.kp = MOTOR_SPEED_PID_KP,				//比例权重
	.ki = MOTOR_SPEED_PID_KI,				//积分权重
	.kd = MOTOR_SPEED_PID_KD,				//微分权重
};

float g_TargetSpeed[4]	 = {0,0,0,0};//目的速度
float g_MotorSpeed[4]	 = {0,0,0,0};//当前速度


//速度值更新
void updataSpeed(void)
{
	int16_t etr[4];
	
	//获得编码器值
	etr[0] = TIM1->CNT;//M1
	TIM1->CNT = 0;
	etr[1] = TIM4->CNT;//M2
	TIM4->CNT = 0;
	etr[2] = TIM3->CNT;//M3
	TIM3->CNT = 0;
	etr[3] = TIM2->CNT;//M4
	TIM2->CNT = 0;
	
	for(int i = 0;i < 4;i++)
	{
		float etrtemp = 0;
		
		//编码器脉冲计算轮角速度
		//360度常量 2*80 = 160t
		etrtemp = ((float)(etr[i] % 30000)) / (2.0 * 80.0);
		//角速度计算轮线速度
		etrtemp = etrtemp * (66 * PI);
		//写入线速度到全局
		g_MotorSpeed[i] = etrtemp * 10.0 * 0.25;
	}
}
//上传速度值
void sendUpMotorSpeed(void)
{
	uint8_t str[500];

	//分速度的合成
	float Vx = (((g_MotorSpeed[0] + g_MotorSpeed[2]) / 2) + 
			((g_MotorSpeed[1] + g_MotorSpeed[3]) / 2) ) / 2;
	
	float Vw = ((((g_MotorSpeed[0] - g_MotorSpeed[2]) / 2) * 0.891749586 / 187.833) + 
			(((g_MotorSpeed[1] - g_MotorSpeed[3]) / 2) * 0.891749586 / 187.833)) / 2;
	
	uint8_t Syx,Syw;
	
	Syx = (Vx >= 0) ? 'A':'B';
	Syw = (Vw >= 0) ? 'A':'B';
	
	Vx = (Vx >= 0) ? Vx : -Vx;
	Vw = (Vw >= 0) ? Vw : -Vw;
	
	
	uint16_t vxi = Vx ;
	uint16_t vwi = Vw * 1000;
	
	
	sprintf((char *)str,"<S%c%03d,%c%04d>\r\n",Syx,vxi,Syw,vwi);
	
	if(g_MotorSpeed[0] != 0)
	{
		USART_SendString(USART3,str);
	}
	USART_SendString(USART1,str);
}

/**
  * @brief  MotorSpeedPidCtrl.
  *         Out = Kp[e(k)]+ki∑e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
  * @retval None
  */
void MotorSpeedPidCtrl(void)
{
	static PIDDateBaseTypedef pid[4] = {
		[0].de	= 0,	[1].de	= 0,	[2].de	= 0,	[3].de	= 0,	
		[0].fe	= 0,  [1].fe	= 0,  [2].fe	= 0,  [3].fe	= 0, 
		[0].de1 = 0,  [1].de1 = 0,  [2].de1 = 0,  [3].de1 = 0, 
		[0].de2 = 0,  [1].de2 = 0,  [2].de2 = 0,  [3].de2 = 0, 
		[0].out = 0,  [1].out = 0,  [2].out = 0,  [3].out = 0, 
	};
	
	//进行PID控制计算
	for(int i = 0;i < 4;i++)
	{
		//计算当前误差并移动历史误差
		pid[i].de2	=  pid[i].de1;
		pid[i].de1	=  pid[i].de;
		pid[i].de		=  g_TargetSpeed[i] - g_MotorSpeed[i];
		pid[i].fe	 +=  pid[i].de;
		//pid[i]控制器核心方程
		pid[i].out = 	PARAM_PID.kp * pid[i].de 	+ 
									PARAM_PID.ki * pid[i].fe 	+ 
									PARAM_PID.kd * ( pid[i].de - 2 * pid[i].de1		+ pid[i].de2) * (pid[i].de < 100);
		//输出限制幅度
		pid[i].out = (pid[i].out > MOTOR_OUTPUT_MAX) ? MOTOR_OUTPUT_MAX : pid[i].out;
		pid[i].out = (pid[i].out <-MOTOR_OUTPUT_MAX) ?-MOTOR_OUTPUT_MAX : pid[i].out;
		
		//输出死区限制
		
		
	}
	//输出到电机控制
	setMotorPWM(pid[0].out,pid[1].out,pid[2].out,pid[3].out);
	
	
	//uint8_t str[50];
	//sprintf((char *)str,"%d,%d,%d,%d\r\n",pid[0].out,pid[1].out,pid[2].out,pid[3].out);
	//USART_SendString(USART1,str);
}

//pid控制器执行周期分频器
void PID_Divider(void)
{
	static uint32_t div = 0;
	div++;
	if(div >= 100)
	{
		div = 0;
		//更新当前速度
		updataSpeed();
		//上传速度信息
		sendUpMotorSpeed();
		//执行电机PID控制器
		MotorSpeedPidCtrl();
	}
}