分类： ROS小车

Tackling the Poor Assumptions of Naive Bayes Text Classifiers

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: Tackling the Poor Assumptions of Naive Bayes Text Classifiers

下载链接
继续阅读“Tackling the Poor Assumptions of Naive Bayes Text Classifiers”本作品采用知识共享署名 4.0 国际许可协议进行许可。

小车电机和舵机控制

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: 小车电机和舵机控制

在STM32的代码中，通过运动学方程，从上位机得到的目标速度：Move_X, Move_Y, Move_Z中求解出阿克曼小车电机MOTOR_A，MOTOR_B的目标转速和舵机Servo的PWM值。从而控制驱动轮的电机电压来通过PID达到目标速度，控制舵机的角度来控制方向轮的角度从而控制小车方向。

void Drive_Motor(float Vx,float Vy,float Vz)
{
		float amplitude=3.5; //Wheel target speed limit //车轮目标速度限幅
 
		//Ackermann structure car
		//阿克曼小车
		if (Car_Mode==Akm_Car) 
		{
			//Ackerman car specific related variables //阿克曼小车专用相关变量
			int K=1000;
			float Ratio=1, Angle;
 
			// For Ackerman small car, Vz represents the front wheel steering Angle
			//对于阿克曼小车Vz代表前轮转向角度
			Angle=Vz;
 
			// Front wheel steering Angle limit (front wheel steering Angle controlled by steering engine), unit: rad
			//前轮转向角度限幅(舵机控制前轮转向角度)，单位：rad
			Angle=target_limit_float(Angle,-0.35f,0.35f);
 
			// The software compensates for the front wheel steering Angle due to mechanical structure limitations
			//机械结构限制，软件对前轮转向角度进行补偿
			if(Angle<0)Ratio=1.054;
			else if(Angle>0)Ratio=0.838;
			else Ratio=0;
 
			//Inverse kinematics //运动学逆解
			MOTOR_A.Target   = Vx*(1-Wheel_spacing*tan(Angle)/2/Axle_spacing);
			MOTOR_B.Target   = Vx*(1+Wheel_spacing*tan(Angle)/2/Axle_spacing);
			// The PWM value of the servo controls the steering Angle of the front wheel
			//舵机PWM值，舵机控制前轮转向角度
			Servo=(SERVO_INIT-Angle*K*Ratio); 
 
			//Wheel (motor) target speed limit //车轮(电机)目标速度限幅
			MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude); 
			MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude); 
			MOTOR_C.Target=0; //Out of use //没有使用到
			MOTOR_D.Target=0; //Out of use //没有使用到
			Servo=target_limit_int(Servo,900,2000);	//Servo PWM value limit //舵机PWM值限幅
		}		
}

STM32系统中的定时任务Balance_task负责把收集到的电机速度通过PID算法，不停的转换成对应的PWM值，直到达到目标速度。

void Balance_task(void *pvParameters)
{ 
	  u32 lastWakeTime = getSysTickCnt();
    while(1)
    {	
			// This task runs at a frequency of 100Hz (10ms control once)
			//此任务以100Hz的频率运行（10ms控制一次）
			vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ)); 
 
			//Time count is no longer needed after 30 seconds
			//时间计数，30秒后不再需要
			if(Time_count<3000)Time_count++;
 
			//Get the encoder data, that is, the real time wheel speed, 
			//and convert to transposition international units
			//获取编码器数据，即车轮实时速度，并转换位国际单位
			Get_Velocity_Form_Encoder();   
 
			if(Check==0) //If self-check mode is not enabled //如果没有启动自检模式
			{
				if      (APP_ON_Flag)      Get_RC();         //Handle the APP remote commands //处理APP遥控命令
				else if (Remote_ON_Flag)   Remote_Control(); //Handle model aircraft remote commands //处理航模遥控命令
				else if (PS2_ON_Flag)      PS2_control();    //Handle PS2 controller commands //处理PS2手柄控制命令
 
				//CAN, Usart 1, Usart 3 control can directly get the three axis target speed, 
				//without additional processing
				//CAN、串口1、串口3(ROS)控制直接得到三轴目标速度，无须额外处理
				else                      Drive_Motor(Move_X, Move_Y, Move_Z);
 
				//Click the user button to update the gyroscope zero
				//单击用户按键更新陀螺仪零点
				Key(); 
 
				//If there is no abnormity in the battery voltage, and the enable switch is in the ON position,
        //and the software failure flag is 0
				//如果电池电压不存在异常，而且使能开关在ON档位，而且软件失能标志位为0
				if(Turn_Off(Voltage)==0) 
				 { 			
           //Speed closed-loop control to calculate the PWM value of each motor, 
					 //PWM represents the actual wheel speed					 
					 //速度闭环控制计算各电机PWM值，PWM代表车轮实际转速
					 MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, MOTOR_A.Target);
					 MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, MOTOR_B.Target);
					 MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, MOTOR_C.Target);
					 MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, MOTOR_D.Target);
 
					 //Set different PWM control polarity according to different car models
					 //根据不同小车型号设置不同的PWM控制极性
					 switch(Car_Mode)
					 {
							case Akm_Car:       Set_Pwm(-MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,  MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, Servo); break; //Ackermann structure car //阿克曼小车
					 }
				 }
				 //If Turn_Off(Voltage) returns to 1, the car is not allowed to move, and the PWM value is set to 0
				 //如果Turn_Off(Voltage)返回值为1，不允许控制小车进行运动，PWM值设置为0
				 else	Set_Pwm(0,0,0,0,0); 
			 }	
			 //If self-check mode is enabled, the run self-check function is executed
			 //如果开启了自检模式，则执行运行自检函数
       else CheckTask(); 
		 }  
}
函数功能：增量式PI控制器
入口参数：编码器测量值(实际速度)，目标速度
返回  值：电机PWM
根据增量式离散PID公式 
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差  以此类推 
pwm代表增量输出
在我们的速度控制闭环系统里面，只使用PI控制
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
**************************************************************************/
int Incremental_PI_A (float Encoder,float Target)
{ 	
	 static float Bias,Pwm,Last_bias;
	 Bias=Target-Encoder; //Calculate the deviation //计算偏差
	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; 
	 if(Pwm>7200)Pwm=7200;
	 if(Pwm<-7200)Pwm=-7200;
	 Last_bias=Bias; //Save the last deviation //保存上一次偏差 
	 return Pwm;    
}
void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d,int servo)
{
	//Forward and reverse control of motor
	//电机正反转控制
  if(motor_a<0)		AIN2=0,		AIN1=1;
	else				    AIN2=1,		AIN1=0;
	//Motor speed control 
	//电机转速控制
	PWMA=abs(motor_a);
 
	//Forward and reverse control of motor
	//电机正反转控制	
	if(motor_b<0)		BIN2=1,		BIN1=0;
	else 	          BIN2=0,		BIN1=1;
	//Motor speed control 
	//电机转速控制
	PWMB=abs(motor_b);
 
	//Forward and reverse control of motor
	//电机正反转控制	
	if(motor_c>0)		CIN2=0,		CIN1=1;
	else 	          CIN2=1,		CIN1=0;
	//Motor speed control 
	//电机转速控制
  PWMC=abs(motor_c);
 
	//Forward and reverse control of motor
	//电机正反转控制
	if(motor_d>0)		DIN2=0,		DIN1=1;
	else 	          DIN2=1,		DIN1=0;
	//Motor speed control 
	//电机转速控制
	PWMD=abs(motor_d);
 
	//Servo control
	//舵机控制
	Servo_PWM =servo;
}

获取4个车轮编码器的值，并计算出速度。

void Get_Velocity_Form_Encoder(void)
{
	  //Retrieves the original data of the encoder
	  //获取编码器的原始数据
		float Encoder_A_pr,Encoder_B_pr,Encoder_C_pr,Encoder_D_pr; 
		OriginalEncoder.A=Read_Encoder(2);	
		OriginalEncoder.B=Read_Encoder(3);	
		OriginalEncoder.C=Read_Encoder(4);	
		OriginalEncoder.D=Read_Encoder(5);	
 
	  //Decide the encoder numerical polarity according to different car models
		//根据不同小车型号决定编码器数值极性
		switch(Car_Mode)
		{
			case Akm_Car:       Encoder_A_pr=-OriginalEncoder.A; Encoder_B_pr= OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C;  Encoder_D_pr= OriginalEncoder.D; break;
		}
 
		//The encoder converts the raw data to wheel speed in m/s
		//编码器原始数据转换为车轮速度，单位m/s
		MOTOR_A.Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;  
		MOTOR_B.Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;  
		MOTOR_C.Encoder= Encoder_C_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; 
		MOTOR_D.Encoder= Encoder_D_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; 
}

本作品采用知识共享署名 4.0 国际许可协议进行许可。

上位机与下位机通信

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: 上位机与下位机通信

一般情况，上位机由ROS框架运行在Ubuntu的树莓派构成，下位机由STM32F103VET6芯片板载电机，舵机，陀螺仪，里程计等构成。里程计提供ROS需要的速度信息，陀螺仪提供加速度方向等信息给ROS，再加上连接到树莓派上的激光雷达，ROS就可以进行SLAM制图和导航了。下位机接收到ROS下发的速度信息后，转换成电机的PWM信号和舵机的PWM信号进行方向和速度控制。

Content:
  1. STM32串口发送与接收
  2. ROS串口发送与接收


本篇就介绍上位机和下位机进行通信的一种方式：串口通信。
继续阅读“上位机与下位机通信”本作品采用知识共享署名 4.0 国际许可协议进行许可。

ROS小车实现介绍

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: ROS小车实现介绍

ROS机器人操作系统是一个很有前景的框架。通过实现一个ROS小车来了解机器人自主导航的原理。

第一部分 ROS地图的创建与导航

机器人在一个陌生的环境自主导航，就得通过自己的传感器和环境交互学习环境，进而创建环境地图，然后在地图上实现导航。不管是SLAM制图还是Teb局部规划，其核心的作用就是：因为运动和观察都有误差，我们通过优化参数来使得运动和观察的误差最小。

同时定位与制图-SlamGMapping 通过小车自带的里程计和激光雷达创建地图。
ROS导航-MoveBase ROS导航节点的主程序。MoveBase包通过全局规划器和局部规划器，利用代价地图costmap来实现当前地点到目标地点的导航。其中，costmap由map和小车传感器共同决定。现在通过对其的源码解读来了解这个框架是怎么完成这个任务的。
全局规划器-NavfnROS 机器人导航路径的规划可以分为两种：全局规划和局部规划。对事先已经知道的静态信息进行的规划为全局规划，如根据高清地图进行的规划，全局规划器就如同平时生活中的地图导航一样，这是一个大尺度的规划。基于传感器信息进行的机器人控制为局部路径规划，这是一个小尺度的规划。大范围内的静态信息不易改变，只需要定期更新高清地图就可以保证规划有效性。局部范围的动态信息则需要摄像头，激光雷达等设备及时回馈，变化频繁。
局部规划器-TebLocalPlannerROS 由于Teb算法能很好的支持阿克曼小车，即模型类似于汽车一样的机器人，这儿使用TebLocalPlannerROS进行局部规划。Teb局部规划器包（teb_local_planner）是作为一个局部规划器（base_local_planner）插件的形式融入2D导航栈的。它基于时间的弹性算法来优化局部轨迹，满足：轨迹的时间尽量短即速度和加速度尽量大；和障碍物能明显分离；以及必须满足机器人动力学约束。
非线性优化 非线性优化
G2O优化解析-手动微分 TebLocalPlannerROS依赖的G2O优化解析。G2O是一个开源的C++框架，用于优化基于图形的非线性误差函数。g2o被设计为易于扩展到各种各样的问题，一个新的问题通常可以在几行代码中指定。当前的实现为SLAM和BA的几个变体提供了解决方案，机器人技术和计算机视觉中的一系列问题都涉及到最小化可以用图形表示的非线性误差函数，典型的例子是同步定位和映射（SLAM）或束调整（BA）。这些问题的总体目标是最大限度地找到能够解释受到高斯噪声影响后的一组测量数据的参数或状态变量的配置。通过把优化问题设计成图的模式：带优化的变量称为顶点，对于待优量的限制条件为边。限制条件可理解成损失函数，这个函数是优化的关键，我们要通过不断的迭代获取最小的损失，从而推断出优化后的顶点而求解。G2O是一个开源的C++框架，用于解决非线性最小二乘问题。G2O的性能可与针对特定问题的最先进方法的实现相媲美。本文通过曲线上的点受高斯噪声影响后，利用G2O设计图模型，然后经过手动计算微分实现来重新拟合曲线。
G2O优化解析-自动微分 TebLocalPlannerROS依赖的G2O优化解析。上文G2O优化解析-手动微分我们学过用G2O来实现拟合曲线，但是大多情况下微分并不好计算，因为我们可能根本就不知道目标函数是什么，而是只知道一些断断续续的约束函数。导数主要以梯度和黑森函数的形式存在于机器学习中，自动微分（AD），也称为算法微分或简称“自动微分”，是一系列技术，类似于反向传播，但比反向传播更为普遍，用于高效、准确地计算以计算机程序表示的数值函数的导数。

第二部分 小车的下位机控制

在得到上位机发出的速度命令后，控制小车运动。这儿的下位机有STM32系统支持。

上位机与下位机通信 一般情况，上位机由ROS框架运行在Ubuntu的树莓派构成，下位机由STM32F103VET6芯片板载电机，舵机，陀螺仪，里程计等构成。里程计提供ROS需要的速度信息，陀螺仪提供加速度方向等信息给ROS，再加上连接到树莓派上的激光雷达，ROS就可以进行SLAM制图和导航了。下位机接收到ROS下发的速度信息后，转换成电机的PWM信号和舵机的PWM信号进行方向和速度控制。
小车电机和舵机控制 在STM32的代码中，通过运动学方程，从上位机得到的目标速度：Move_X, Move_Y, Move_Z中求解出阿克曼小车电机MOTOR_A，MOTOR_B的目标转速和舵机Servo的PWM值。从而控制驱动轮的电机电压来通过PID达到目标速度，控制舵机的角度来控制方向轮的角度从而控制小车方向。

本作品采用知识共享署名 4.0 国际许可协议进行许可。