描述

我們構(gòu)建了這個(gè)自穩(wěn)定機(jī)器人作為基礎(chǔ)設(shè)置，以展示機(jī)器人如何根據(jù)不同的傳感器類型與其周圍環(huán)境進(jìn)行交互。

在此示例中，您可以看到配備了 24GHz 雷達(dá)和超聲波傳感器的機(jī)器人：

XMC4700 提供了幾個(gè)未使用的通信接口，可以將不同類型的傳感器添加到您的項(xiàng)目中。

有興趣建造自己的機(jī)器人嗎？在此處查找如何構(gòu)建它的說(shuō)明。

1) 力學(xué)

使用“ Top Plate XMC4700 ”或“ Top Plate XMC4700 電線”開始構(gòu)建。電線版本為電致發(fā)光線提供了一個(gè)環(huán)繞的凹口。

該部件適合從 12V 電池電壓到 5V 的 DCDC 電壓轉(zhuǎn)換器 (WE 173010578)，用于我們的控制器和可能的電致發(fā)光逆變器。此外，它可以將您的電纜固定到位并固定機(jī)器人的主開關(guān)。

將 XMC4700 Relax Kit 擰在上面，并將 Motorshield 堆疊在微控制器頂部。Motorshield 提供了一個(gè)開放區(qū)域，非常適合在頂部焊接 IMU（慣性測(cè)量單元）傳感器。

繼續(xù)打印和組裝下部——底盤。它裝有兩個(gè)電機(jī)以及可選的電致發(fā)光逆變器。

使用與電池高度相匹配的 M3 墊片將頂板安裝到底盤上。

為了將車輪連接到電機(jī)，我們使用了 M5 墊片，我們鉆出并添加了一個(gè)螺紋以將輪軸固定到電機(jī)軸上。

為了將車輪連接到輪軸，我們打印了另一個(gè)適配器：

就是這樣 - 這些都是組裝自穩(wěn)定機(jī)器人所需的所有部件

2) 電子產(chǎn)品

本節(jié)簡(jiǎn)要介紹所使用的電子元件。如需更詳細(xì)的信息，請(qǐng)查看供應(yīng)商網(wǎng)頁(yè)。

步進(jìn)電機(jī)(Nema17) 當(dāng)您需要設(shè)置精確位置時(shí)，步進(jìn)電機(jī)很重要。對(duì)于這種不是必須的機(jī)器人，但我們用它們來(lái)驅(qū)動(dòng)機(jī)器人回到它的“原位”，這是你推開它后的起點(diǎn)。

Arduino v2 的 Adafruit Motor/Stepper/Servo Shield這是驅(qū)動(dòng)電機(jī)的功率級(jí)。它使用 TB6612 MOSFET 驅(qū)動(dòng)器，每個(gè)通道具有 1.2A 的電流能力，并帶有一個(gè)方便的 Arduino 庫(kù)來(lái)驅(qū)動(dòng)步進(jìn)電機(jī)。

El線我們使用電致發(fā)光 (EL) 線來(lái)顯示機(jī)器人的模式。這是一根用大約 100V 和 2500Hz 的交流電壓供電的電線。這將使它在整個(gè)表面發(fā)光。

EL逆變器 要為我們的 EL 線產(chǎn)生交流電壓，需要一個(gè) EL 逆變器：輸入 5V DC -> 輸出 100V AC

IMU BNO055該傳感器將幫助我們了解機(jī)器人的位置以及它是否即將倒向一側(cè)。為此，我們使用：絕對(duì)方向（歐拉矢量，100Hz）基于 360° 球體的三軸方向數(shù)據(jù)和角速度矢量（100Hz）三軸“旋轉(zhuǎn)速度”，單位為 rad/s。

XMC4700 Relax Kit是主控制器，因此也是我們機(jī)器人的大腦。它具有 Arduino Uno 引出線，可以在 Arduino IDE 中使用：

3）電機(jī)控制

您可以在四種模式下運(yùn)行電機(jī)：Microstep (1/8)、Interleave、Single 和 Double

Microstep (1/8)執(zhí)行從分離器的一個(gè)線圈到另一個(gè)線圈的平滑切換。這是最流暢但也是最慢的模式：

在從一個(gè)線圈切換到另一個(gè)線圈期間，Interleave僅引入半步(1:2) 。這種模式比微步更快，但不是那么平滑。

雙步和單步(1:1)正在完成一個(gè)完整的步驟。“單”模式僅使用一個(gè)線圈作為有源線圈，而“雙”模式使用兩個(gè)有源線圈，因此具有更大的扭矩。這些是最快的模式，但在低速時(shí)也有點(diǎn)粗糙。

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

為了充分利用步進(jìn)器在不同模式下的功能，我們通過從一個(gè)模式移交給另一個(gè)模式來(lái)使用所有這些功能：

不幸的是，對(duì)于任何后續(xù)控制算法來(lái)說(shuō)，這都是一場(chǎng)噩夢(mèng)，因?yàn)閷?duì)于控制信號(hào)（x 軸）的不同變化，您會(huì)看到速度值（y 軸）的不同增加。這就是為什么我們將電機(jī)控制線性化，以獲得控制信號(hào)與速度增加的線性關(guān)系：

在 Arduino 中，我們可以使用這個(gè)映射函數(shù)來(lái)做到這一點(diǎn)：

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) 控制算法

級(jí)聯(lián)控制用于控制系統(tǒng)。這意味著我們使用四個(gè)從屬控制回路。這種拓?fù)浣Y(jié)構(gòu)的優(yōu)點(diǎn)是可以使用多個(gè)子系統(tǒng)來(lái)簡(jiǎn)化復(fù)雜系統(tǒng)。每個(gè)子系統(tǒng)都由特定的控制器調(diào)節(jié)。作用于內(nèi)部子系統(tǒng)的干擾會(huì)在影響外部控制回路之前進(jìn)行調(diào)整。由于每個(gè)子系統(tǒng)都需要一定的時(shí)間來(lái)執(zhí)行操縱變量，因此更高級(jí)別的控制系統(tǒng)必須慢 4 倍。該系統(tǒng)可分為四個(gè)子系統(tǒng)：

• Lageregelung調(diào)整機(jī)器人的角度并防止機(jī)器人跌倒。為此，我們使用 PID 控制器來(lái)控制輪速。
• Geschwindigkeitsregelung控制機(jī)器人的速度。為此，我們使用另一個(gè) PID 控制器來(lái)控制機(jī)器人的角度。
• Positionsregelung控制機(jī)器人相對(duì)于其起點(diǎn)的位置。我們使用 P 控制器來(lái)操縱機(jī)器人的速度。
• Orientierungsregelung控制機(jī)器人移動(dòng)的方向。我們?yōu)榇耸褂昧硪粋€(gè) P 控制器，它作用于輪速。

由于運(yùn)行這些控制算法的時(shí)間至關(guān)重要，因此它們由定時(shí)器中斷觸發(fā)

// 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”

請(qǐng)記住 - 我們調(diào)整機(jī)器人的角度并防止機(jī)器人跌倒。為了設(shè)置一定的角度，電機(jī)軸以低于重心（機(jī)器人站立）或通過加速力矩（機(jī)器人移動(dòng)）建立力矩平衡的方式移動(dòng)。位置控制回路通過 100 Hz 的定時(shí)器中斷調(diào)用。可以描述如下：

PID控制器：

P - 比例份額是將我們的角度偏移量乘以一個(gè)常數(shù)因子“Kp”你可以把它想象成一個(gè)杠桿的長(zhǎng)度來(lái)推動(dòng)偏移量

I - 積分部分術(shù)語(yǔ)“Ki”不僅增加了與錯(cuò)誤相關(guān)的動(dòng)作，而且還增加了錯(cuò)誤持續(xù)的時(shí)間。我們只需將當(dāng)前偏移量添加到“積分器”值。

D-微分項(xiàng)“Kd”不考慮誤差，而是誤差的變化率。幸運(yùn)的是，我們的傳感器已經(jīng)通過測(cè)量角速度矢量來(lái)測(cè)量這一點(diǎn)。D 部分對(duì)系統(tǒng)起到阻尼器的作用。

//------------------------------------------------------------------------------------------------
// 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 調(diào)整位置控制器

我們通過調(diào)整 PID 控制算法的參數(shù) Kp、Ki、Kd 來(lái)調(diào)整控制器。使用的方法稱為“齊格勒-尼科爾斯調(diào)諧法”。保持 Ki 和 Kd 為 0，同時(shí)增加 Kp，直到系統(tǒng)達(dá)到穩(wěn)定極限，從而進(jìn)入諧波振蕩。通過這種方式在 out case 中找到的值被稱為 Kp_krit=200 并顯示了 300ms 的周期。

使用下表計(jì)算 Kd = Kp*Tv 和 Ki = Kp/Tn

注意：在我們的例子中 Ki 為 800，但是當(dāng)我們每秒增加 100 次時(shí)，我們將它降低到 8。

4.2 速度控制 - “Geschwindigkeitsregelung”

速度控制器控制機(jī)器人的速度。為此，我們使用另一個(gè) PID 控制器來(lái)控制機(jī)器人的角度。位置控制回路通過 25Hz 的定時(shí)器中斷調(diào)用。可以描述如下：

機(jī)器人的輪速由我們的“Lageregelung”控制回路給出。我們還使用低通濾波器對(duì)該值進(jìn)行了平滑處理。然后我們還通過獲取實(shí)際速度值和上次速度值以及測(cè)量它們之間的 40ms (25Hz) 時(shí)間差來(lái)計(jì)算加速度。

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 調(diào)整速度控制器

速度控制器的調(diào)整與位置控制器相同。使用 PID，我們?nèi)匀粫?huì)看到低速偏移的振蕩。切換到用于低速偏移的 PI 控制器解決了這個(gè)問題。在此我們降低了 Kp 并稍微增加了 Ki。這是一個(gè)嘗試 ;)

4.3 位置控制——“Positionsregelung”

該控制器控制機(jī)器人相對(duì)于其初始位置的位置。通過計(jì)算步進(jìn)器的旋轉(zhuǎn)步數(shù)可以知道該位置。作用于機(jī)器人速度的簡(jiǎn)單 P 控制器就足夠了：

我們嘗試了不同的 Kp 值。在 Kp=4 時(shí)，顯示了最佳的控制器性能。

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);      

}