3D打印的兩輪自平衡機器人的模糊自適應控制策略

張俊杰，周惠興*，王衍學，劉天宇

(1.北京建筑大學車輛與機電工程學院，北京 100044；2.北京建筑大學北京市建筑安全監測工程技術研究中心，北京 100044；3.中國農業大學工學院，北京 100083)

兩輪自平衡機器人是一種集機械構件，微型控制器，傳感器和自動控制技術于一體的輪式移動機器人[1]。其運動模型來源于倒立擺，具有非線性、不確定性和強耦合性的特點[2]。它可依據自身的姿態變化做出相應的動態決策，在運動的同時，保持自身的平衡狀態[3]。

3D打印是一種快速成型技術，是基于數字模型文件[4]，使用金屬粉末或塑料等可黏合材料，依靠逐層打印來構建對象的增材制造的方法[5]。相比于傳統的機加工，使用3D打印技術來設計和制造兩輪自平衡機器人，易于實現機器人各結構零件的個性化和多樣化設計，可以節省材料和加工成本，縮短制造周期，節省組裝時間[6]。

現對于自平衡機器人的常用控制算法有線性二次型調節器(linear quadratic regulator，LQR)和PID(proportion integral differential)。LQR算法需要針對自平衡機器人建立準確的數學模型[7]，而3D打印的機器人存在構型多樣化和運動模型的多樣化的情況。PID算法不需要建立準確的數學模型，但參數調節將耗費大量的人力和時間[8]。因此，旨在研究一種不依賴于機器人的精確數學模型，可對不同構型的自平衡機器人的運動進行自適應調節的運動控制策略。

將從自平衡機器人軟硬件的系統設計，運動控制策略的研究和程序的編寫，以及檢測控制策略可行性及有效性相關試驗的設計與施行等方面進行闡述。

1 系統設計

兩輪自平衡機器人的系統框圖如圖1所示。系統的初始電源為充電電池，電池為驅動板供電，驅動板為控制器和左右輪電機供電，控制器為各傳感器和通信模塊供電。通信方面，各傳感器信號通過線路連接至控制器的信號輸入口，遙控信號則通過藍牙模塊傳輸給控制器，從而實現無線通信。控制器通過算法對所有信號進行處理，產生輸出信號給驅動器，驅動器調節對左右輪電機的供電幅值與頻率，從而實現自平衡機器人系統的閉環控制。

圖1 兩輪自平衡機器人系統框圖

下面介紹各部分模塊的具體選型與相關參數。

1.1 3D打印構件

3D打印構件由聚乳酸(PLA)材料打印而成，構件板厚約為5 mm，即可以滿足裝載各模塊的強度要求，又使得系統整體質量較小。板上設計了多種形狀和大小的孔，可用于穿線，固定螺釘螺母，或者直接嵌入小型的電路板模塊，使各模塊的拆卸與安裝十分方便。圖2為安裝完成的3D打印的兩輪自平衡機器人的實物圖。

圖2 3D打印的兩輪自平衡機器人實物圖

在該3D打印構件上，通過調整不同模塊的安裝位置可以輕松改變系統的重心位置和整體重心分布，便于實現控制算法對于非特定模型的控制效果的驗證。

1.2 電源和電機模塊

整個機器人系統的供電來源于一個12 V的可充電鋰電池，系統的輪電機選用兩個帶編碼器的直流減速電機，電機的額定電壓為7.4～12 V，最高轉速為500 r/min，最大扭矩為0.4 kg·cm。

1.3 傳感器模塊

系統的傳感器模塊是為了即時反映系統當前的姿態及狀態量，其中包含機器人的姿態角、姿態角速度及車輪的轉速，用到的傳感器有MPU6050和磁性編碼器。

MPU6050電路板結合了陀螺儀和加速度計的功能，集成了姿態求解器，并內置了動態卡爾曼濾波算法，可以在動態環境中精確輸出三個維度的加速度，角速度以及姿態。其可測量的加速度范圍為±16g，分辨率為6.1×10-5g，角速度范圍為±2 000(°)/s；分辨率為7.6×10-3(°)/s。其數據輸出頻率最高為100 Hz，即115 200 bps的波特率。

左右輪電機上配有高精度的磁性編碼器，相較于光學編碼器，磁性編碼器具有更強的抗干擾能力。每當車輪旋轉一圈時，電機編碼器產生260個單位脈沖，可得編碼器輸出頻率為260×4=1 040 Hz，編碼器精度為360(°)/1 040=0.35°。

1.4 控制驅動模塊

系統的控制驅動模塊包含一個Arduino Mega2560控制器和一塊雙L298N驅動板。

Arduino Mega2560控制器的核心處理器為ATmega2560，具有54個數字輸入輸出端口，其中的16個端口可直接用于脈沖寬度調制(PWM)信號的輸出。還具有16個模擬信號輸入端口，4個通用異步收發傳輸器(UART)接口，一個16 MHz的晶體振蕩器，一個通用串行總線(USB)接口等。該控制器可適用于針對多傳感器與執行器的實時運動控制[9]。

雙L298N驅動板集成了最小輸入為6 V的LM2596穩壓器芯片和ULN2003驅動器芯片。該驅動板的最大輸出電流為2 A，可以支持4個3～24 V的直流電機的運轉，或者相同數量、相同功率的兩相步進電機和四相步進電機的工作，同時還能夠輸出穩定的5 V電壓，可以作為其他模塊的電源。

2 模糊自適應PID控制器設計

PID控制器是目前應用最為廣泛的一種自動控制器，它不依賴于受控對象的精確數學模型，只針對實際與理想輸出間的偏差值來進行控制，其中比例(P)、積分(I)、微分(D)3個參數的設定是該控制器的核心[10]。模糊控制是基于專家的經驗，制定用于推理控制的模糊規則。通過模糊控制對PID控制器進行參數調整，可以省去繁雜的人力參數調整步驟，使控制系統具有自適應性和魯棒性[11]。

圖3為兩輪自平衡機器人的控制系統框圖，該系統使用MPU6050傳感器中的X軸角度，Y軸角速度，以及車輪電機編碼器脈沖作為系統輸入，將左右兩輪電機所接驅動板的脈寬調制(pulse width modulation，PWM)信號作為系統輸出。

圖3 兩輪自平衡機器人左右車輪電機的控制系統框圖

從系統輸入到系統輸出之間是一個模糊自適應PID控制器，如圖4所示，其包括平衡環模糊PD控制器、速度環模糊PI控制器及轉動環模糊PD控制器。

圖4 左右車輪的模糊自適應PID控制器系統框圖

2.1 模糊邏輯控制器

在模糊邏輯控制系統中，隸屬函數的形狀對控制性能有很大的影響。當隸屬函數越尖銳時，模糊子集的分辨率越高，控制靈敏度也越高；否則，當隸屬函數越平坦時，控制特性越平滑，系統穩定性越好。

基于上述原因，在設計模糊控制器時，應在較小的誤差區間內使用更尖銳的隸屬函數，而在較大的誤差區間內使用更平坦的隸屬函數。這樣可以有效改善系統的動態和靜態特性，增強抗干擾能力。因此，結合經驗，自平衡機器人控制系統的誤差e和誤差變化率eec的隸屬函數設計如圖5所示。

Z表示零值(zero)；NS表示較小的負值；NM表示中等大小的負值；NB表示較大的負值；PS、PM、PB分別表示較小的正值、中等大小的正值、較大的正值

不同取值的e和eec，比例系數變化量ΔKp，積分系數變化量ΔKi，微分系數變化量ΔKd的設計原則如下：①當e較大時，為了使系統跟蹤性能更好，應設置相對較大的比例系數Kp和相對較小的微分系數Kd，并且為了避免系統響應過大，通常應限制積分作用，將積分系數Ki設為零；②當e和eec大小適中時，為了減少系統過沖，Kp和Kd應該相對較小，Ki應當取適當值；③當e較小時，為了使系統具有更好的穩定性，應將Kp和Ki都設置為較大，同時，為了避免系統長時間振蕩并增強系統的抗干擾能力，當eec大時，Kd應該小，當eec小時，Kd應該大。

選擇誤差和誤差變化率作為模糊PID控制器的輸入，與方法設為最小值，反模糊化取質心，每個輸入選擇7個語言變量，因此總共可以提取49條模糊規則，如表1～表3所示。

表1 ΔKp的模糊規則

表2 ΔKi的模糊規則

表3 ΔKd的模糊規則

2.2 平衡環模糊PD控制器

P控制或PI控制可用于一般控制系統。而那些需要對干擾做出快速響應的控制過程，D是必不可少的[12]。在MATLAB中設計兩輪自平衡機器人的平衡環模糊PD控制器如圖6所示，其中MPU6050的X軸角度和X軸角速度是該控制器的兩個輸入。

圖6 平衡環模糊PD控制器框圖

基于平衡環P、D參數的初始值kp0B和kd0B及自動調整變量ΔkpB和ΔkdB，可以根據式(1)計算得出平衡環P、D參數的校正值：

(1)

2.3 速度環模糊PI控制器

對于兩輪自平衡機器人的速度環的控制，基于速度控制中最常用線性控制器PI控制器，設計相應的模糊邏輯控制器如圖7所示，其中左右輪電機的編碼器均值和編碼器均值的變化率作為該控制器的兩個輸入。

圖7 速度環模糊PI控制器框圖

基于速度環P、I參數的初始值kp0V和ki0V及自動調整變量ΔkpV和ΔkiV，可以根據式(2)計算得出速度環P、I參數的校正值：

(2)

2.4 轉動環模糊PD控制器

與平衡環類似，設計兩輪自平衡機器人的轉動環模糊PD控制器如圖8所示，其中MPU6050的Y軸角速度和Y軸角加速度是該控制器的兩個輸入。

同樣基于轉動環P、D參數的初始值kp0T和kd0T及自動調整變量ΔkpT和ΔkdT，可以根據式(3)計算得出轉動環P、D參數的校正值：

(3)

3 運動控制算法程序設計

Arduino控制板的程序編寫包含運動控制算法、傳感器串行通信、電機驅動和編碼器中斷計數四個子模塊，以下只對運動控制算法程序進行介紹，其包含平衡控制算法，速度控制算法，轉動控制算法，以及綜合控制算法四部分。

3.1 平衡控制算法

輸入：X軸偏轉角度Angle_x,

X軸偏轉速度w_x

初始化：平衡控制函數Balance，

平衡點偏差值 Bias,

平衡控制P系數Kp=20,

平衡控制D系數Kd=0.01，

平衡點Balance_point

電機平衡控制PWM值balance

Bias=Angle_x-Balance_point;//計算平衡點偏差值；

balance=Kp* Bias + w_x *Kd; ;//計算電機平衡控制PWM值；

return balance;//返還電機平衡控制PWM值；

3.2 速度控制算法

輸入：左輪電機編碼器encoder_left

右輪電機編碼器encoder_right

初始化：速度控制函數Velocity，

速度控制P系數Kp=2,

速度控制I系數Ki=Kp/200，

電機速度控制PWM值velocity，

最新速度偏差Encoder_Least，

編碼器濾波器Encoder，

編碼器積分Encoder_Integral

Encoder_Least=(encoder_left + encoder_right)-0;//獲取最新速度偏差等于測量速度(左右編碼器之和)減去目標速度(此處為零)。

Encoder*=0.7;

Encoder+=Encoder_Least * 0.3;//設計一階低通濾波器。

Encoder_Integral +=Encoder;//通過積分得出位移，積分時間為40 ms。

if(Encoder_Integral > 21 000)

Encoder_Integral=21 000;

if(Encoder_Integral <-21 000)

Encoder_Integral=-21 000;//對積分做限幅處理。

velocity=Encoder *Kp+ Encoder_Integral *Ki;

return velocity;//通過計算返還速度控制中的電機PWM值。

3.3 轉動控制算法

輸入：Z軸偏轉速度w_z,

初始化：轉動控制函數Turn，

轉動目標值Turn_Target=0，

轉動控制P系數Kp=2,

轉動控制D系數Kd=0.001，

轉動幅值Turn_Amplitude=80,

電機轉動控制PWM值turn

if(Turn_Target > Turn_Amplitude)

Turn_Target=Turn_Amplitude;

if(Turn_Target <-Turn_Amplitude)

Turn_Target=-Turn_Amplitude;//對轉動速度做限幅處理。

turn=-Turn_Target *Kp+ w_z *Kd;

return turn;//通過計算返還轉動控制中的電機PWM值。

3.4 綜合控制算法

輸入：電機平衡控制PWM值balance，

電機速度控制PWM值velocity，

電機轉動控制PWM值turn

初始化：綜合控制函數PID_PWM，

轉動計數Turn_Count，

左輪速度Velocity_L，

右輪速度Velocity_R

平衡PWM容器Balance_Pwm，

速度PWM容器Velocity_Pwm，

轉動PWM容器Turn_Pwm;

Balance_Pwm=Balance(Angle[0], w[0]);

Velocity_L=Count_L;

Count_L=0;

Velocity_R=Count_R;

Count_R=0;//讀取左右輪編碼器數據并清零，由單位時間內的脈沖數得到速度。

Velocity_Pwm=Velocity(Velocity_L, Velocity_R);//將速度控制中的返還值裝入綜合控制中的速度變量。

if(++Turn_Count >=4)

{

Turn_Pwm=Turn(w[1]);

Turn_Count=0;//將轉動控制中的返還值裝入綜合控制中的轉動變量。

}

Motor_L=Balance_Pwm-Velocity_Pwm + Turn_Pwm;

Motor_R=Balance_Pwm-Velocity_Pwm-Turn_Pwm;//最后將左右輪各自的平衡、速度、轉向三環的PWM值疊加，即得到最終輸出。

4 實驗與結果分析

3D打印的兩輪自平衡機器人系統構建完成后，通過設計一個涵蓋直線運動與轉向運動的實驗來驗證該控制策略的效果。

運動實驗具體步驟為先使機器人繞Y軸旋轉90°，以查看其是否可以保持穩定的平衡狀態，然后再遙控機器人以一定速度沿直線移動0.2 m，最后使機器人停止，觀察趨于穩定的過程。

圖9為通過MPU6050采集到的機器人的角度隨時間變化的曲線。觀察可知，從開始錄制起的5 s內，偏航角、俯仰角和滾轉角均保持固定值不變，此時機器人尚未開始運動。之后，偏航角發生了90°的變化，這是由于機器人的轉動引起的。然后，機器人前進時滾轉角開始逐漸增大，并在機器人停止移動后，通過振蕩趨于穩定，此時機器人再次處于穩定平衡狀態。

圖9 兩輪自平衡機器人系統的角度-時間變化曲線

結果表明所設計的模糊自適應控制策略對于無精確數學模型的3D打印的兩輪自平衡機器人系統具有良好的控制能力和自適應性。該控制策略可以為兩輪自平衡機器人在轉彎模式，前進模式和停止模式下保持穩定平衡狀態。

5 結論

所設計的模糊自適應控制策略對于無精確數學模型的3D打印的兩輪自平衡機器人系統具有良好的控制能力和自適應性。該控制策略可以為兩輪自平衡機器人在轉彎模式，前進模式和停止模式下保持穩定平衡狀態。