ROS自动导航小车
1.材料清单
项目 | Value |
---|---|
Jeston nano(套餐一) | ¥899 |
GM37-545直流减速电机带编码器 DC24V 1:90 | ¥138 |
板子自己画(附带电机接口以及IMU) | ¥200~300 |
电池·12V 容量2250ma | ¥80~100 |
A1雷达 | ¥550 |
小车底盘(默认两轮差速的模型) | 可以自己选择 |
2.概述
(1)首先先了解ROS中的作用,如果仅仅只是做到避障,使用ros系统,使用传感器超声波也是绰绰有余。但如果你想在自动导航的过程中清晰的知道小车自己的位置以及到达指定的目的地,ROS便可以使用相对较简单的操作的来实现。
(2)ROS在机器人自动导航的过程中,想让机器人比较准确的知道自己在运动过程中的位置,一般都需要三个东西:里程计、陀螺仪、激光雷达的数据,之后会逐步介绍以及如何得到。ros得到这三个数据,可以通过amcl定位算法来得到机器人在运动过程中的位姿。简单来说就是让小车知道自己在哪。机器人首先需要知道自己的位置才能准确到达目的地。
3.里程计如何得到并传输到ROS上
(1)这里里程计不包括视觉里程计。是由上述材料清单中的电机中的编码器得到。首先通过底层驱动板通过编码器的传感器得到小车两轮子的速度。
。。。所使用的底层驱动是STM32,首先初始化电机编码器。
void RightMoto_Encoder_Input_init(void)
{
GPIO_InitTypeDef gpio;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3 ,ENABLE);
gpio.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
gpio.GPIO_Mode = GPIO_Mode_AF;
gpio.GPIO_OType = GPIO_OType_PP;
gpio.GPIO_PuPd = GPIO_PuPd_UP;
gpio.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_Init(GPIOA,&gpio);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource6, GPIO_AF_TIM3);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource7, GPIO_AF_TIM3);
TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_Cmd(TIM3, ENABLE);
TIM3->CNT = 0x7fff;
}
RCC_AHB1Periph_GPIOA, GPIO_Pin_6 | GPIO_Pin_7 所对应的PA6与PA7是单片机上插入的编码器的A相B相的端口。这只是让单片机能够读入编码器。如果你不规定一个周期来结合编码器所获得的脉冲也无法得到轮子转速。
为了获得轮子的转速你还得设置一个定时器中断,我这里设置的是TIM7中断,代码如下
void BaseBoard_TIM7_Init(u16 arr,u16 psc)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7,ENABLE);
TIM_TimeBaseInitStructure.TIM_Period = arr;
TIM_TimeBaseInitStructure.TIM_Prescaler=psc;
TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM7,&TIM_TimeBaseInitStructure);
TIM_ITConfig(TIM7,TIM_IT_Update,ENABLE);
TIM_Cmd(TIM7,ENABLE);
NVIC_InitStructure.NVIC_IRQChannel=TIM7_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01;
NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03;
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
main函数中的初始化为
BaseBoard_TIM7_Init(999,8400);
所采取的STM32型号为F405,主频为84M,TIM7通用计时器,延迟时间大概为100ms。所以便可以在TIM7的终端程序中每100ms得到当前小车左右轮子的速度,代码如下
void Robot_Encoder_Get_CNT(void)
{
Left_moto.Encoder_Value = (TIM3->CNT)-0x7fff;
Right_moto.Encoder_Value = -((TIM4->CNT)-0x7fff);
Left_moto.Current_Speed \
= -((ROBOT_INITIATIVE_DIAMETER *Pi_v * (Left_moto.Encoder_Value / ENCODER_TTL_COUNT_VALUE))/CONTROL_TIMER_CYCLE);
Right_moto.Current_Speed\
= ((ROBOT_INITIATIVE_DIAMETER *Pi_v * (Right_moto.Encoder_Value / ENCODER_TTL_COUNT_VALUE))/CONTROL_TIMER_CYCLE);
TIM3->CNT = 0x7fff;
TIM4->CNT = 0x7fff;
}
void TIM7_IRQHandler(void)
{
if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET)
{
RUN_LED = ~RUN_LED;
Robot_Encoder_Get_CNT();
}
TIM_ClearITPendingBit(TIM7,TIM_IT_Update);
}
之所以设置TIM3和TIM4初始为0x7fff因为寄存器为16位,轮子有前后的旋转,选取中间的值更容易去计算正速度和负速度,其中Left_moto为共同体如下:
typedef struct _Moto_
{
int Encoder_Value;
float Current_Speed;
float Target_Speed;
short ESC_Output_PWM;
float L_Error;
float LL_Error;
}_Moto_Str;
_Moto_Str Left_moto;
_Moto_Str Right_moto;
轮子左右轮子的速度得到之后,下一步便是将这个数据传输到jeston nano的ros里面去。主要方式便是通过串口通信的模式,将底层驱动般的方式传输到jeston nano的ros里面去,首先第一步先初始化底层驱动板的串口,代码如下:
void Usart1_Init(u32 bound)
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);
GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1);
GPIO_PinAFConfig(GPIOA,GPIO_PinSource9 ,GPIO_AF_USART1);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_9;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOA,&GPIO_InitStructure);
USART_InitStructure.USART_BaudRate = bound;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART1, &USART_InitStructure);
USART_Cmd(USART1, ENABLE);
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority =0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
接下来就是发送数据,代码如下:
void USART1_SendChar(unsigned char b)
{
while (USART_GetFlagStatus(USART1,USART_FLAG_TC) == RESET);
USART_SendData(USART1,b);
}
void SendTo_UbuntuPC()
{
unsigned char i = 0;
Send_Data.Sensor_Str.Header = PROTOCOL_HEADER;
Send_Data.Sensor_Str.End_flag = PROTOCOL_END;
Send_Data.Sensor_Str.X_speed = (Left_moto.Current_Speed + Right_moto.Current_Speed)/2.0f;
Send_Data.Sensor_Str.Y_speed = 0.0;
Send_Data.Sensor_Str.Z_speed = (Left_moto.Current_Speed - Right_moto.Current_Speed)/(Base_Width);
Send_Data.Sensor_Str.Source_Voltage = Source_Valtage;
for(i=0; i<PROTOCL_DATA_SIZE; i++)
{
USART1_SendChar(Send_Data.buffer[i]);
}
}
通过这个SendTo_UbuntuPC函数便可以将你所得到的左右轮速度传到ros系统上去!
后续IMU也是一样,主要是通过IIC的方式让驱动板得到数据,同样也是用串口通信这个方法传上去。所以接下来的问题便是如何让上位机的ros系统接收到这个写信息,并读取出来并且发布odom的信息。
下位机的程序连接在下
https://download.csdn.net/download/xiaomaotian/12665331