自平衡机器人的构建

描述

我们构建了这个自稳定机器人作为基础设置，以展示机器人如何根据不同的传感器类型与其周围环境进行交互。

在此示例中，您可以看到配备了 24GHz 雷达和超声波传感器的机器人：

XMC4700 提供了几个未使用的通信接口，可以将不同类型的传感器添加到您的项目中。

有兴趣建造自己的机器人吗？在此处查找如何构建它的说明。

1) 力学

使用“ Top Plate XMC4700 ”或“ Top Plate XMC4700 电线”开始构建。电线版本为电致发光线提供了一个环绕的凹口。

该部件适合从 12V 电池电压到 5V 的 DCDC 电压转换器 (WE 173010578)，用于我们的控制器和可能的电致发光逆变器。此外，它可以将您的电缆固定到位并固定机器人的主开关。

将 XMC4700 Relax Kit 拧在上面，并将 Motorshield 堆叠在微控制器顶部。Motorshield 提供了一个开放区域，非常适合在顶部焊接 IMU（惯性测量单元）传感器。

继续打印和组装下部——底盘。它装有两个电机以及可选的电致发光逆变器。

使用与电池高度相匹配的 M3 垫片将顶板安装到底盘上。

为了将车轮连接到电机，我们使用了 M5 垫片，我们钻出并添加了一个螺纹以将轮轴固定到电机轴上。

为了将车轮连接到轮轴，我们打印了另一个适配器：

就是这样 - 这些都是组装自稳定机器人所需的所有部件

2) 电子产品

本节简要介绍所使用的电子元件。如需更详细的信息，请查看供应商网页。

步进电机(Nema17) 当您需要设置精确位置时，步进电机很重要。对于这种不是必须的机器人，但我们用它们来驱动机器人回到它的“原位”，这是你推开它后的起点。

Arduino v2 的 Adafruit Motor/Stepper/Servo Shield这是驱动电机的功率级。它使用 TB6612 MOSFET 驱动器，每个通道具有 1.2A 的电流能力，并带有一个方便的 Arduino 库来驱动步进电机。

El线我们使用电致发光 (EL) 线来显示机器人的模式。这是一根用大约 100V 和 2500Hz 的交流电压供电的电线。这将使它在整个表面发光。

EL逆变器 要为我们的 EL 线产生交流电压，需要一个 EL 逆变器：输入 5V DC -> 输出 100V AC

IMU BNO055该传感器将帮助我们了解机器人的位置以及它是否即将倒向一侧。为此，我们使用：绝对方向（欧拉矢量，100Hz）基于 360° 球体的三轴方向数据和角速度矢量（100Hz）三轴“旋转速度”，单位为 rad/s。

XMC4700 Relax Kit是主控制器，因此也是我们机器人的大脑。它具有 Arduino Uno 引出线，可以在 Arduino IDE 中使用：

3）电机控制

您可以在四种模式下运行电机：Microstep (1/8)、Interleave、Single 和 Double

Microstep (1/8)执行从分离器的一个线圈到另一个线圈的平滑切换。这是最流畅但也是最慢的模式：

在从一个线圈切换到另一个线圈期间，Interleave仅引入半步(1:2) 。这种模式比微步更快，但不是那么平滑。

双步和单步(1:1)正在完成一个完整的步骤。“单”模式仅使用一个线圈作为有源线圈，而“双”模式使用两个有源线圈，因此具有更大的扭矩。这些是最快的模式，但在低速时也有点粗糙。

在此图中可以更好地看出不同的速度范围：

为了充分利用步进器在不同模式下的功能，我们通过从一个模式移交给另一个模式来使用所有这些功能：

不幸的是，对于任何后续控制算法来说，这都是一场噩梦，因为对于控制信号（x 轴）的不同变化，您会看到速度值（y 轴）的不同增加。这就是为什么我们将电机控制线性化，以获得控制信号与速度增加的线性关系：

在 Arduino 中，我们可以使用这个映射函数来做到这一点：

if(Regelausgang_Lage >= -160 && Regelausgang_Lage <= 160){
        Schrittart=1; // Microschritt (beide Richtungen)
        Speed = map (Regelausgang_Lage, -160,160, -400,400);
      }
      else if (Regelausgang_Lage >=161 && Regelausgang_Lage <= 650)  {
        Schrittart=2; //Interleave (positiv)
        Speed = map (Regelausgang_Lage, 161,650, 90,400);
      }
        else if ( Regelausgang_Lage >= -650 && Regelausgang_Lage <=-161) {
        Schrittart=2; //Interleave (negativ)
        Speed = map (Regelausgang_Lage, -161,-650, -90,-400); 
       }
      if (Regelausgang_Lage >=651)  {
        Schrittart=3; //Double  (positiv)
        Speed = map (Regelausgang_Lage, 651,1300, 200,400);
       }                    
       if ( Regelausgang_Lage <= -651) {
        Schrittart=3; //Double (negativ)
        Speed = map (Regelausgang_Lage, -651,-1300, -200,-400); 
       }                    
    }

4) 控制算法

级联控制用于控制系统。这意味着我们使用四个从属控制回路。这种拓扑结构的优点是可以使用多个子系统来简化复杂系统。每个子系统都由特定的控制器调节。作用于内部子系统的干扰会在影响外部控制回路之前进行调整。由于每个子系统都需要一定的时间来执行操纵变量，因此更高级别的控制系统必须慢 4 倍。该系统可分为四个子系统：

• Lageregelung调整机器人的角度并防止机器人跌倒。为此，我们使用 PID 控制器来控制轮速。
• Geschwindigkeitsregelung控制机器人的速度。为此，我们使用另一个 PID 控制器来控制机器人的角度。
• Positionsregelung控制机器人相对于其起点的位置。我们使用 P 控制器来操纵机器人的速度。
• Orientierungsregelung控制机器人移动的方向。我们为此使用另一个 P 控制器，它作用于轮速。

由于运行这些控制算法的时间至关重要，因此它们由定时器中断触发

// Interrupt Setup
//------------------------------------------------------------------------------------------------
// Setup Postition Interrupt
//------------------------------------------------------------------------------------------------
  XMC_CCU4_SLICE_COMPARE_CONFIG_t pwm_config = {0};
            pwm_config.passive_level = XMC_CCU4_SLICE_OUTPUT_PASSIVE_LEVEL_HIGH;
            pwm_config.prescaler_initval = XMC_CCU4_SLICE_PRESCALER_32768;
            XMC_CCU4_Init(CCU41, XMC_CCU4_SLICE_MCMS_ACTION_TRANSFER_PR_CR);
            XMC_CCU4_SLICE_CompareInit(CCU41_CC43, &pwm_config);
            XMC_CCU4_EnableClock(CCU41, 3);
            XMC_CCU4_SLICE_SetTimerPeriodMatch(CCU41_CC43, 704); // Adjust last Value or Prescaler
 /* Enable compare match and period match events */
  XMC_CCU4_SLICE_EnableEvent(CCU41_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH);
/* Connect period match event to SR0 */
  XMC_CCU4_SLICE_SetInterruptNode(CCU41_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH, XMC_CCU4_SLICE_SR_ID_0);
  /* Configure NVIC */
  /* Set priority */
  NVIC_SetPriority(CCU41_0_IRQn, 10U);
  /* Enable IRQ */
  NVIC_EnableIRQ(CCU41_0_IRQn); 
        XMC_CCU4_EnableShadowTransfer(CCU41, (CCU4_GCSS_S0SE_Msk << (4 * 3)));
        XMC_CCU4_SLICE_StartTimer(CCU41_CC43);

//------------------------------------------------------------------------------------------------
// Setup Geschwindigkeit Interrupt
//------------------------------------------------------------------------------------------------
XMC_CCU4_Init(CCU42, XMC_CCU4_SLICE_MCMS_ACTION_TRANSFER_PR_CR);
            XMC_CCU4_SLICE_CompareInit(CCU42_CC43, &pwm_config);
            XMC_CCU4_EnableClock(CCU42, 3);
            XMC_CCU4_SLICE_SetTimerPeriodMatch(CCU42_CC43, 176); // Adjust last Value or Prescaler 176->25Hz 220->20Hz
 /* Enable compare match and period match events */
  XMC_CCU4_SLICE_EnableEvent(CCU42_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH);
/* Connect period match event to SR0 */
  XMC_CCU4_SLICE_SetInterruptNode(CCU42_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH, XMC_CCU4_SLICE_SR_ID_0);
  /* Configure NVIC */
  /* Set priority */
  NVIC_SetPriority(CCU42_0_IRQn, 10U);
  /* Enable IRQ */
  NVIC_EnableIRQ(CCU42_0_IRQn); 
        XMC_CCU4_EnableShadowTransfer(CCU42, (CCU4_GCSS_S0SE_Msk << (4 * 3)));
        XMC_CCU4_SLICE_StartTimer(CCU42_CC43);
        
//------------------------------------------------------------------------------------------------
// Setup Sensor Interrupt
//------------------------------------------------------------------------------------------------
XMC_CCU4_Init(CCU43, XMC_CCU4_SLICE_MCMS_ACTION_TRANSFER_PR_CR);
            XMC_CCU4_SLICE_CompareInit(CCU43_CC43, &pwm_config);
            XMC_CCU4_EnableClock(CCU43, 3);
            XMC_CCU4_SLICE_SetTimerPeriodMatch(CCU43_CC43, 43); // Adjust last Value or Prescaler 
 /* Enable compare match and period match events */
  XMC_CCU4_SLICE_EnableEvent(CCU43_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH);
/* Connect period match event to SR0 */
  XMC_CCU4_SLICE_SetInterruptNode(CCU43_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH, XMC_CCU4_SLICE_SR_ID_0);
  /* Configure NVIC */
  /* Set priority */
  NVIC_SetPriority(CCU43_0_IRQn, 10U);
  /* Enable IRQ */
  NVIC_EnableIRQ(CCU43_0_IRQn); 
       XMC_CCU4_EnableShadowTransfer(CCU43, (CCU4_GCSS_S0SE_Msk << (4 * 3)));
       XMC_CCU4_SLICE_StartTimer(CCU43_CC43

4.1 位置控制 - “Lageregelung”

请记住 - 我们调整机器人的角度并防止机器人跌倒。为了设置一定的角度，电机轴以低于重心（机器人站立）或通过加速力矩（机器人移动）建立力矩平衡的方式移动。位置控制回路通过 100 Hz 的定时器中断调用。可以描述如下：

PID控制器：

P - 比例份额是将我们的角度偏移量乘以一个常数因子“Kp”你可以把它想象成一个杠杆的长度来推动偏移量

I - 积分部分术语“Ki”不仅增加了与错误相关的动作，而且还增加了错误持续的时间。我们只需将当前偏移量添加到“积分器”值。

D-微分项“Kd”不考虑误差，而是误差的变化率。幸运的是，我们的传感器已经通过测量角速度矢量来测量这一点。D 部分对系统起到阻尼器的作用。

//------------------------------------------------------------------------------------------------
// PID Lageregler 
//------------------------------------------------------------------------------------------------
    Winkelabweichung  = (double(euler.z())-sollWinkel);  
    integrator_Lage   = constrain (integrator_Lage + Winkelabweichung,-integrator_LageMAX, integrator_LageMAX); // I Anteil der Lageregelung wird begrenzt
    Regelausgang_Lage = constrain (KP_Lage*Winkelabweichung\
                                 + KI_Lage * integrator_Lage\
                                 + KD_Lage*double(gyroscope.x()),-1300, 1300);                                  // Begrenzung auf Schrittmotorkennlinie

4.1.1 调整位置控制器

我们通过调整 PID 控制算法的参数 Kp、Ki、Kd 来调整控制器。使用的方法称为“齐格勒-尼科尔斯调谐法”。保持 Ki 和 Kd 为 0，同时增加 Kp，直到系统达到稳定极限，从而进入谐波振荡。通过这种方式在 out case 中找到的值被称为 Kp_krit=200 并显示了 300ms 的周期。

使用下表计算 Kd = Kp*Tv 和 Ki = Kp/Tn

注意：在我们的例子中 Ki 为 800，但是当我们每秒增加 100 次时，我们将它降低到 8。

4.2 速度控制 - “Geschwindigkeitsregelung”

速度控制器控制机器人的速度。为此，我们使用另一个 PID 控制器来控制机器人的角度。位置控制回路通过 25Hz 的定时器中断调用。可以描述如下：

机器人的轮速由我们的“Lageregelung”控制回路给出。我们还使用低通滤波器对该值进行了平滑处理。然后我们还通过获取实际速度值和上次速度值以及测量它们之间的 40ms (25Hz) 时间差来计算加速度。

PID控制器：

void get_sollsollWinkel() {
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Tiefpassfilter
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
      Regelausgang_Lage_5 = Regelausgang_Lage_4;
      Regelausgang_Lage_4 = Regelausgang_Lage_3;
      Regelausgang_Lage_3 = Regelausgang_Lage_2;
      Regelausgang_Lage_2 = Regelausgang_Lage_1;
      Regelausgang_Lage_1 = Regelausgang_Lage;

      Regelausgang_Lage_filter = (Regelausgang_Lage_1 \
                                + Regelausgang_Lage_2 \
                                + Regelausgang_Lage_3 \
                                + Regelausgang_Lage_4 \
                                + Regelausgang_Lage_5)/5;   
        
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Bilden der Geschwindigkeitsableitung
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
    
      d_Geschwindigkeit = (Regelausgang_Lage_filter - Regelausgang_Lage_filter_alt)\
                          /Ta_Geschwindigkeitsregler;
      Regelausgang_Lage_filter_alt = Regelausgang_Lage_filter;
        
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Geschwindigkeitsregler -> Ausgang sollWinkel
//--------------------------------------------------------------------------------------------------------------------------------------------------------------

     Geschwindigkeitsabweichung = sollGeschwindigkeit - Regelausgang_Lage_filter;

     if (abs(Positionsabweichung) <= 15 ) {  //Regelparameter bei geringer Positionsabweichung
           KP_geschw = 0.0045;               // Proportionalanteil des Geschwindigkeitsreglers
           KI_geschw = 0.0009;               // Integralanteil des Geschwindigkeitsreglers  
           KD_geschw = 0.000;                // Differentianteil des Geschwindigkeitsreglers  
     }     
     else{                                   //Regelparameter bei größerer Positionsabweichung
           KP_geschw = 0.0090;               // Proportionalanteil des Geschwindigkeitsreglers
           KI_geschw = 0.0007;               // Integralanteil des Geschwindigkeitsreglers  
           KD_geschw = 0.0002;               // Differentianteil des Geschwindigkeitsreglers  
     }
           
           integrator_geschw = constrain(integrator_geschw + Geschwindigkeitsabweichung,\
                               -sollWinkelMAX/KI_geschw, sollWinkelMAX/KI_geschw);   //I Anteil wird begrenzt
           sollWinkel        = constrain(KP_geschw * Geschwindigkeitsabweichung \   
                                       + KI_geschw * integrator_geschw \                           
                                       + KD_geschw * d_Geschwindigkeit,\                                  
                                       -sollWinkelMAX, sollWinkelMAX);               //Winkelbegrenzung
  }

4.2.1 调整速度控制器

速度控制器的调整与位置控制器相同。使用 PID，我们仍然会看到低速偏移的振荡。切换到用于低速偏移的 PI 控制器解决了这个问题。在此我们降低了 Kp 并稍微增加了 Ki。这是一个尝试 ;)

4.3 位置控制——“Positionsregelung”

该控制器控制机器人相对于其初始位置的位置。通过计算步进器的旋转步数可以知道该位置。作用于机器人速度的简单 P 控制器就足够了：

我们尝试了不同的 Kp 值。在 Kp=4 时，显示了最佳的控制器性能。

void get_sollGeschwindigkeit() {
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Positionsregler -> Ausgang sollGeschwindigkeit
//--------------------------------------------------------------------------------------------------------------------------------------------------------------      
 Positionsabweichung  = (sollPosition - pos_aktuell);
 
   //+++Ausweichen+++    
       if (Distanz_X1 <= 15 && Distanz_X1 > 0) {      //Einschaltschwelle "Hand folgen"
        KP_dist_X1 = 75;
        KP_pos = 0;
        EL_threshold = 1;                             //EL-Wire leuchtet
       }                                              
   //+++Positionsregler+++                            
       else if (Distanz_X1 > 30 || Distanz_X1 == 0) { //Ausschaltschwelle "Hand folgen"
         KP_dist_X1 = 0;
         KP_pos = 4;
         EL_threshold = constrain(abs(Positionsabweichung),0,300)* -0.15 + 50;
                                                     //EL-Wire blinkt(2-10Hz)              
       }
       else {/*Hysterese*/}

        sollGeschwindigkeit  =  constrain (KP_pos*Positionsabweichung  \
                                         + KP_dist_X1*(-15+Distanz_X1),\
                                -GeschwindigkeitMAX, GeschwindigkeitMAX);      

}