移动机器人下位机软件

---

前言

移动机器人的控制系统软件部分分为上位机软件与下位机软件两部分，二者之间通过串口进行通信，上位机主机树莓派中完成上位机软件设计，下位机STM32单片机中完成下位机软件设计。移动机器人下位机软件设计基于Keil5进行开发，主要完成与上位机树莓派之间的串口通信部分、PWM调速、PID速度控制、Encoder编码器数据获取以及里程计数据计算五部分。

---

一、串口通信部分

下位机STM32单片机与上位机树莓派之间主要通过串口进行通信，完成左右轮速度以及里程计数据的交互过程，通信时最大接受字节数为200。在该项目中，主要是利用rosserial实现STM32下位机与ROS上位机之间的通信功能。rosserial是一种用于ROS设备与非ROS设备之间进行通信的协议，分为客户端和服务器两部分。rosserial为非ROS设备提供了ROS节点的功能，可以使非ROS设备通过串口能够与ROS设备进行数据交互。rosserial_python是一个基于python的rosserial协议实现，在上位机中安装rosserial_python功能包，启动功能包内的serial_node.py节点，并且指定波特率与串口设备名与STM32进行连接。
串口发送数据与接受数据都是以一个字节为单位进行传输，传统的串口通信方法采用数据分离技术，比较复杂，这里在串口数据交互过程中采用共用体进行交互。共用体具有以下属性：
1.结构体内不同成员共享内存的机制；
2. 同一时刻，只能访问其中的一个成员；
3. 不同成员，按照成员类型的性质进行内存访问（利用的特性）。

通信双方均定义相同数据结构的共用体，一个float型的变量与一个unsigned char型的数组，数组的大小与float字节大小对应，因此在收发数据时，只发送或者接受共用体中unsigned char数组的元素，使用时即可使用float型的变量。
STM32串口发送数据内容与格式如下：
内容：里程计（x，y坐标、线速度、角速度和方向角度，单位为：mm，mm，mm/s，rad/s，rad，所有的数据都为float型，float型占4个字节）
格式：21个字节[x坐标4个字节][y坐标4个字节][方向角4个字节][线速度4个字节][角速度4个字节][结束符“\n”1个字节]

STM32串口接收数据内容与格式如下：
内容：机器人左右轮速度，单位mm/s，所有的数据均为float型，float型占4个字节
格式：10个字节[右轮速度4个字节][左轮速度4个字节][结束符“\r\n”2个字节]

本文采用串口1（USART1）来进行通信，单片机的PA9端口连接USB转TTL模块的RX端，而PA10接口连接USB转TTL模块的TX端。
串口发送：

// 底盘向串口发送里程计
      if(main_sta&0x01)
      {
          // 里程计数据
          x_data.odoemtry_float=position_x;
          y_data.odoemtry_float=position_y;
          theta_data.odoemtry_float=oriention;
          vel_linear.odoemtry_float=velocity_linear;
          vel_angular.odoemtry_float=velocity_angular;

          // 将里程计以字符的方式存在数组中
          for(j=0;j<4;j++)
          {
              odometry_data[j]=x_data.odometry_char[j];
              odometry_data[j+4]=y_data.odometry_char[j];
              odometry_data[j+8]=theta_data.odometry_char[j];
              odometry_data[j+12]=vel_linear.odometry_char[j];
              odometry_data[j+16]=vel_angular.odometry_char[j];
          }

          // 数组后加入结束符
          odometry_data[20]='\n';

          // 将数组中的数据发送给串口
          for(i=0;i<21;i++)
          {
              USART_ClearFlag(USART1,USART_FLAG_TC);     
              USART_SendData(USART1,odometry_data[i]);
              while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET);  
          }
          // 执行里程计计算步骤
          main_sta&=0xFE;
      }

串口接收：
串口接收时会执行串口中断服务函数USART1_IRQHandler，串口接收到数据之后，会执行主函数里的一个分支语句，读取上位机发送的左右轮的速度，然后控制小车运动。

// 串口中断函数
void USART1_IRQHandler(void)
{
  if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)   
  {
    // 接受串口数据
    serial_rec =USART_ReceiveData(USART1);

    if((USART_RX_STA&0x8000)==0)  // 接受未完成
        {
            if(USART_RX_STA&0x4000)  // 接收到了0x0d
            {
                if(serial_rec==0x0a)
                {
                    if((USART_RX_STA&0x3f)==8)
                    {              
                        USART_RX_STA|=0x8000;  // 接收完成
                        main_sta|=0x04;
                        main_sta&=0xF7;
                    }
                    else
                    {
                        main_sta|=0x08;
                        main_sta&=0xFB;
                        USART_RX_STA=0;  // 接受错误，重新开始
                    }
                }
                else 
                {
                    main_sta|=0x08;
                    USART_RX_STA=0;  // 接受错误，重新开始
                }
            }
            else  // 还未收到0x0d
            {  
                if(serial_rec==0x0d)USART_RX_STA|=0x4000;
                else
                {
                    USART_RX_BUF[USART_RX_STA&0X3FFF]=serial_rec ;
                    USART_RX_STA++;
                    if(USART_RX_STA>(USART_REC_LEN-1))
                    {
                        main_sta|=0x08;
                        USART_RX_STA=0;  // 接受错误，重新开始
                    }              
                }     
            }
        }        
  }
}

rosserial官网：http://wiki.ros.org/rosserial

二、PWM调速

脉冲宽度调制（Pulse Width Modulation，PWM）利用微处理器的数字输出来对模拟电路进行控制的一种非常有效的技术，即通过数字信号模拟输出了模拟信号，这里采用PWM来对电机的转速进行控制。STM32除去TIM6、TIM7外的所有定时器都可以产生PWM输出，高级定时器TIM1和TIM8可以同时产生多达7路的PWM输出，而通用定时器也能同时产生多达4路的PWM输出，这样，STM32最多可以同时产生30路。

脉冲宽度调制模式可以产生一个由ARR重装载寄存器确定输出频率、由捕获/比较寄存器CCRx确定占空比的信号。如上图所示，定时器自动重装载值是ARR，比较值是CCRx，在t时刻使用计数器CNT与捕获/比较寄存器CCRx进行比较，如果计数器CNT的值大于捕获/比较寄存器CCRx，那么该时刻输出高电平，如t1_{t2时刻，反之，则输出低电平，如0}t1时刻。当计数器CNT数值达到重装载值ARR时，定时器溢出，重新向上计数，一个PWM周期完成，占空比为高电平时间与整个周期时间的比例，如此往复循环，即可产生PWM波。

占空比是指一个脉冲周期内，高电平的时间与整个周期的时间的比值。可以通过改变占空比来改变IO口输出的有效电压。如果信号始终为高电平，则它处于100％占空比，如果它始终处于低电平，则占空比为0％。

这里初始化TIM2来作为PWM输出，通过设置定时器的period与prescaler来设置定时器的周期。需要初始化定时器的OC通道为输出比较模式，OCMode设置为PWM1模式。本文使用定时器2的OC2与OC3通道，将经过PID闭环控制输出的PWM分别赋给TIM2的CCR2和CCR3，使电机按照设定的速度进行转动。单片机与驱动器之间的连接方式如下：
左轮：PA1 -> PWM PC6 -> IN1 PC7 -> IN2
右轮：PA2 -> PWM PC11 -> IN3 PC10 -> IN4

三、编码器数据获取

由于地面存在摩擦力会导致左右两轮的实际速度并不相同，因此需要利用PID来对速度进行闭环控制。PID闭环控制以及里程计数据的计算都需要用到当前时刻电机的转速，因此需要使用编码器来采集单位时间内的脉冲数，并且将其转换为当前电机的速度。

编码器是一种将角位移或者角速度转换成一串电数字脉冲的旋转式传感器，编码器一般有光电编码器与霍尔编码器。这里我们采用霍尔编码器，霍尔编码器由霍尔码盘和霍尔元件组成。霍尔编码器的工作原理如下：霍尔码盘与电机连接，电机的转动带动霍尔码盘转动，霍尔码盘上均匀分布着不同的磁极，当霍尔码盘上边的磁极经过霍尔元件时，霍尔元件能够检测出脉冲信号，这里输出两组存在相位差的方波信号，可以根据方波信号来判断电机的转向。

在本文中，利用TIM3和TIM4来作为编码器采集定时器，采集编码器数据为输入捕获，使用定时器的IC通道。首先需要给TIM3与TIM4进行初始化处理，设置为IC输入捕获模式，并且使用编码器配置函数来设置为编码器模式，这里采用四倍频来提高测量精度。当定时器的自动重装载寄存器ARR计数溢出之后会执行TIM3与TIM4的中断函数，采集单位时间内的脉冲数据，然后执行计算左右电机的编码数，并判断其正反转。

单片机与编码器之间的连接方式如下：
左轮：PB6 -> C1 PB7 -> C2
右轮：PA6 -> C1 PA7 -> C2

// 右轮电机编码器初始化
void ENC_Init2(void)
{
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    TIM_ICInitTypeDef TIM_ICInitStructure;    
    GPIO_InitTypeDef GPIO_InitStructure;

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
    GPIO_StructInit(&GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOB, &GPIO_InitStructure);

    TIM_DeInit(ENCODER2_TIMER);
    TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);

    TIM_TimeBaseStructure.TIM_Prescaler = 0;                   
    TIM_TimeBaseStructure.TIM_Period = (4*ENCODER2_PPR)-1;     

    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;   
    TIM_TimeBaseInit(ENCODER2_TIMER, &TIM_TimeBaseStructure);

    TIM_EncoderInterfaceConfig(ENCODER2_TIMER, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
    TIM_ICStructInit(&TIM_ICInitStructure);
    TIM_ICInitStructure.TIM_ICFilter = ICx_FILTER;
    TIM_ICInit(ENCODER2_TIMER, &TIM_ICInitStructure);

    TIM_ClearFlag(ENCODER2_TIMER, TIM_FLAG_Update);
    TIM_ITConfig(ENCODER2_TIMER, TIM_IT_Update, ENABLE);  // 中断配置

    TIM_Cmd(ENCODER2_TIMER, ENABLE); 
}

// 计算右电机的编码数
s16 ENC_Calc_Rot_Speed2(void)
{   
    s32 wDelta_angle;
    u16 hEnc_Timer_Overflow_sample_one;
    u16 hCurrent_angle_sample_one;
    s32 temp;
    s16 haux;

    if (!bIs_First_Measurement2)
    {  
        hEnc_Timer_Overflow_sample_one = hEncoder_Timer_Overflow2;   
        hCurrent_angle_sample_one = ENCODER2_TIMER->CNT;
        hEncoder_Timer_Overflow2 = 0;
        haux = ENCODER2_TIMER->CNT;   

        if ( (ENCODER2_TIMER->CR1 & TIM_CounterMode_Down) == TIM_CounterMode_Down)  
        {
            wDelta_angle = (s32)((hEnc_Timer_Overflow_sample_one) * (4*ENCODER2_PPR) -(hCurrent_angle_sample_one - hPrevious_angle2));
        }
        else  
        {
            wDelta_angle = (s32)(hCurrent_angle_sample_one - hPrevious_angle2 + (hEnc_Timer_Overflow_sample_one) * (4*ENCODER2_PPR));
        }    
        temp=wDelta_angle;
    } 
    else
    {
        bIs_First_Measurement2 = false;   
        temp = 0;
        hEncoder_Timer_Overflow2 = 0;
    }
    hPrevious_angle2 = haux;  
    return((s16) temp);
}

// 执行右电机计数中断
void TIM4_IRQHandler (void)
{   
    TIM_ClearFlag(ENCODER2_TIMER, TIM_FLAG_Update);     
    if (hEncoder_Timer_Overflow2 != U16_MAX)            // 不超范围
    {
        hEncoder_Timer_Overflow2++;                     // 脉冲数累加
    }
}

四、PID速度控制

STM32单片机接收到上位机发送的速度指令之后，PWM通过控制电压的方式来控制电机的转速，所以当负载变化时，电机达到相同的转速所需的电压也是变化的。如果不采用闭环控制，那么电机的设定转速与实际转速之间会存在误差，最后会导致机器人偏离设定的路径。为了减少电机实际转速与设定转速之间的误差，本文采用PID闭环控制进行调速，可以避免因负载变化带来的电机转速的变化。PID调速原理图如下图所示，主要包括比例环节、积分环节和微分环节。比例环节主要是使控制量更快地接近目标值，P越大，响应速度越快，但容易产生震荡，破坏系统的稳定性；积分环节主要是消除系统的静态误差，积分虽然会消除静态误差，但是会拉低系统的响应速度，I对P会有抑制作用；微分环节的主要作用使维持系统的稳定性，通过提高D来减小震荡，D越大，抑制P的效果越强。

vi表示移动机器人车轮电机的设定值，v0表示编码器测量得到的车轮电机的实际值，系统的偏差e(t)为车轮电机的设定值与车轮电机的实际值二者之差，即：

PID控制公式如下：

计算机只能处理离散的数据，因此进行PID控制时需要对其进行离散化处理，利用差分代替微分，利用累加代替积分，因此，增量式PID控制公式如下：

// 位置PID
float PID_calculate(struct PID *Control,float CurrentValue_left )
{
  
  float Value_Kp;
  float Value_Ki;
  float Value_Kd;
  
  
  Control->error_0 = Control->OwenValue - CurrentValue_left + 0*span;
  Value_Kp = Control->Kp * Control->error_0 ;
  Control->Sum_error += Control->error_0;     
  
  OutputValue = Control->OutputValue;
  if(OutputValue>5 || OutputValue<-5)  
  {
      Control->Ki = 0; 
  }
 
  Value_Ki = Control->Ki * Control->Sum_error;
  Value_Kd = Control->Kd * ( Control->error_0 - Control->error_1);
  Control->error_1 = Control->error_0;
  Control->OutputValue = Value_Kp  + Value_Ki + Value_Kd;
  
  if( Control->OutputValue > MaxValue)
    Control->OutputValue = MaxValue;
  if (Control->OutputValue < MinValue)
    Control->OutputValue = MinValue;
    
  return (Control->OutputValue) ;
}

五、里程计数据计算

里程计是机器人路径规划中非常重要的组成部分，是衡量移动机器人从起始位姿到终点位姿的一个重要标准。里程计可以通过直流电机上的霍尔编码器来获取电机在单位时间内的脉冲数进行计算，获得移动机器人在单位时间内左右轮的位移，进而可以获得机器人在该时刻的位姿。里程计计算如图所示，设车轮直径为D，轮间距为L，电机转动一圈编码器产生的脉冲数M为码盘线数、减速比与倍频数的乘积。在t单位时间内，左轮产生的脉冲数为Nl，右轮产生的脉冲数为Nr，那么左右轮移动的距离为dl、dr，机器人中心点移动的距离d和转过的角度theta的公式如下所示。




机器人单位时间内在世界坐标系X、Y轴上的变化量以及角度的变化量公式如下所示。



将单位时间的变化量与上一时刻机器人的位姿进行相加即可求得当前时刻机器人的位姿为(x,y,theta)。

计算机器人中心点的速度(v,w)，因此机器人的里程计数据为(x,y,z,v,w)。

// 里程计计算函数
void odometry(float right,float left)
{  
  if(flag)  
  {
    const_frame=wheel_diameter*pi/(line_number*multiplier*deceleration_ratio);      // 单位脉冲小车移动的距离
    const_angle=const_frame/wheel_interval;                                         // 单位脉冲小车转过的角度
    flag=0;
  }
    
  distance_sum = 0.5f*(right+left);                                                  // 小车移动的距离为两轮速度和的一半
  distance_diff = right-left;                                                        // 小车转过的角度为两轮速度差
  
  if((odometry_right>0)&&(odometry_left>0))           
  {
    delta_distance = distance_sum;
    delta_oriention = distance_diff;
  }
  else if((odometry_right<0)&&(odometry_left<0))      
  {
    delta_distance = -distance_sum;
    delta_oriention = -distance_diff;
  }
  else if((odometry_right<0)&&(odometry_left>0))      
  {
    delta_distance = -distance_diff;
    delta_oriention = -2.0f*distance_sum;    
  }
  else if((odometry_right>0)&&(odometry_left<0))      
  {
    delta_distance = distance_diff;
    delta_oriention = 2.0f*distance_sum;
  }
  else
  {
    
    delta_distance=0;
    delta_oriention=0;
  }
    
  oriention_interval = delta_oriention * const_angle;        // 采样时间内走过的角度
  oriention = oriention + oriention_interval;                // 里程计的方向角
  oriention_1 = oriention + 0.5f * oriention_interval;       // 里程计方向角数据位变化
  
  sin_ = sin(oriention_1); // 计算单位时间内x方向移动的距离
  cos_ = cos(oriention_1); // 计算单位时间内y方向移动的距离
  
  position_x = position_x + delta_distance * cos_ * const_frame; // 里程计的x
  position_y = position_y + delta_distance * sin_ * const_frame; //里程计的
    
  velocity_linear = delta_distance*const_frame / dt; // 线速度
  velocity_angular = oriention_interval / dt; // 角速度
  
  if(oriention > pi)
  {
    oriention -= pi_2_1;
  }
  else
  {
    if(oriention < -pi)
    {
      oriention += pi_2_1;
    }
  }
}

---

总结

更多内容欢迎关注微信公众号：深度学习与路径规划