描述

我們構建了這個自穩定機器人作為基礎設置，以展示機器人如何根據不同的傳感器類型與其周圍環境進行交互。

在此示例中，您可以看到配備了 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);      

}