---

文章目錄

• 00-靈感篇
• 01-設計篇
• 02-硬體篇
• 03-原理篇
• 04-軟體篇
• 05-3D列印篇
• 06-測試篇
• 網盤連結

---

  對某個篇章感興趣的，請直接跳轉

---

00-靈感篇

  小型自平衡機器人EVA的製作靈感，來源於2018年的STM峰會的一次work shop活動。

  work shop上的這位大神，格子（別稱，因為他穿著高深程式設計師的標誌-格子襯衫）被邀請到峰會上介紹STM的有關產品，格子當時大四，能被邀請到STM32峰會的大學生想必不用多做介紹。

  在演講中可知，格子是個綜合能力極強的人，只要有想法，就能從0做起，構思-設計-原理圖-電路圖-外觀-組裝-測試等全部自己完成，而且十分痴迷“小”的東西，自制了非常多有趣的物品。小白也鍾愛“小”的玩物，被其中一個吸引，就是他自制的迷你自平衡機器人nano。

  第一次接觸Arduino、第一次接觸PID、第一次接觸姿態解算……，很是憂愁，何以解憂，唯有動手。

01-設計篇

   首先是確定功能，核心功能是自平衡，為了比較好玩，外加的功能有：OLED（顯示）、藍芽（手機控制）、超聲波（跟隨物體）、紅外陣列感測器（識別感應火焰），以下是思維導圖（紅外陣列刪了的原因是不小心用烙鐵頭把鏡片焊壞了…

）：

  然後是外觀，看到nano後，馬上想到了電影《機器人總動員》中的瓦力。

  既然想到了瓦力，就順便模仿一下EVA，但是要給EVA加上輪子，於是最初的外觀圖紙如下

  瓦力是方形的，想到三維畫圖可能比較容易，於是外觀選擇是瓦力，那麼名字就取EVA，兩者結合就是我最終的小玩物。

---

02-硬體篇

  根據思維導圖，可以得出材料清單：

名稱	型號/驅動	用途	簡述
Arduino MCU	nano	主控晶片	建議買貴一點的，有質量保證
6軸陀螺儀	GY-521 MPU6050	姿態解算，獲得x軸偏移角度（pitch）	需要用到加速度計和角速度計
減速電機	GA12N20	帶動輪子轉動	可以不用減速，但是一定要自帶碼盤進行測速
電機驅動	TB6612	驅動電機，控制正反轉和速度	TB6612比較小型，推薦
OLED液晶屏	SSD1306	自檢資訊顯示、圖片顯示	IIC或者SPI都可以，最好IIC，能節省MCU埠
藍芽	HC05-V13	手機端控制	主從一體或者只有從模式都可以
超聲波	HC-SR04	跟隨物品移動	只能直線行駛，超聲波不能判斷方向
紅外陣列感測器	GY-AMG8833	在一個矩陣範圍內識別火焰位置	這個感測器非常貴，注意資金

  還有一些硬體比如蜂鳴器、LED和按鍵等等，就隨意購買了。

---

03-原理篇

  兩輪自平衡的數學模型是倒立擺，與單擺相似的運動。

   對於倒立擺（軸是可轉動的），在不施加外力的情況下，由於重力作用，上方的物體會向左或者向右擺，而我們的期望是讓上方的物體保持在垂直位置（平衡位置），所以需要新增外力讓倒立擺保持平衡。

   新增外力的控制演算法一般是PID（比例-積分-微分），一個短小精悍的演算法，要使用PID，系統中需要有迴環，迴環簡單理解為一個單元發出的訊號需要被反饋回來。
   舉個例子，拿出自己的手機，一邊說話一邊測分貝，你發出的聲音被手機上的分貝反饋回來讓自己知道，這個過程就是迴環，然後用反饋回來的分貝調整自己的聲音停留在某一個分貝值附近，這個過程就是PID。

  兩輪平衡的迴環由電機的轉速和重心的偏移角度構成，PID演算法使用3個引數去控制平衡，分別是P（比例）、I（積分）和D（微分），假設感測器對重心偏移角度的歷史取樣序列為：X₁，X₂， ······ ，X_k-1，X_k，而平衡位置的目標值為S_v（一般為0，意思是沒有偏移）

  比例P控制，基本思想是關心當前偏差，用目標值減去最近的一次測量，得到P_k = S_v - X_k
  P_k>0; 當前控制未達標(輸出訊號要升高)
  P_k=0; 當前控制達標(不控制)
  P_k<0; 當前控制超標(輸出訊號要降低)
  得到 P_out = P * P_k + P_m（P_m是常數，用於抵消機器阻力）

  積分I控制，基本思想是關心歷史偏差， 把歷史取樣點資料序列逐個與Sv比較，得到歷史偏差序列：P₁，P₂， ······ P_k-1，P_k，然後對歷史偏差序列積分：I_k = P₁+P₂+······+P_k-1+P_k (每一項都可正可負)
  I_k>0; 所有偏差之和為正，控制總體偏低，未達標(輸出訊號要升高)
  I_k=0; 所有偏差之和為零，控制總體正常，達標(不輸出訊號)
  I_k<0; 所有偏差之和為負，控制總體偏高，超標(輸出訊號要降低)
  得到 I_out = I * I_k + I_m（I_m是常數，用於抵消機器阻力）

  微分D控制，基本思想是關心近期偏差， 把最近的兩次偏差相減得到D_k = P_k - P_k-1
  D_k>0; 這一次的偏差值大於上一次，越來越偏離我們的目標，偏差有增大趨勢
  D_k=0; 這一次取樣和後一次取樣之間的變化沒有產生變化
  D_k<0; 這一次的偏差值小於上一次，越來越偏離我們的目標，偏差有減小趨勢
  得到 D_out = D * D_k + D_m（D_m是常數，用於抵消機器阻力）

  最後的PID控制輸出 OUT = P_out + I_out + D_out，可以看出，如果要編寫PID，只有幾行的程式碼，就可以控制平衡。

  以上是MCU的運算，直觀而言，可以簡單粗暴理解為，當檢測到機器人身體向前傾，就讓輪子猛一下往前轉，利用慣性讓身體往後擺動，同理，當檢測機器人身體往後傾，就讓輪子猛一下往後轉，由此不斷重複，進而保持平衡。

---

04-軟體篇

  接下來就是每個模組的驅動程式編寫，MCU平臺為Arduino，開發非常快捷，為了方便移植，在程式設計中小白儘量自己寫庫或者引用一些已經非常完善的平臺無關庫。

  MPU6050姿態結算後使用一階互補濾波演算法得到pitch的偏移角，因為Arduino計算處理能力並不是很強，也不需要太精準的濾波，用一階互補濾波演算法足矣，但是小白在原始碼中還是編寫了卡爾曼濾波，可以比較精準得到當前角度，移植到STM的時候可以使用

/**
  * @brief	一階互補濾波函式，得到偏移角度
  */
float MPU6050::one_filter(float angle_m,float gyro_m,float dt)
{
	float K1 =0.07; //Weight of accelerometer 

	float one_filter_angle = K1 * angle_m+ (1-K1) * (one_filter_angle + gyro_m * dt);
	return one_filter_angle;
}

/**
  * @brief	卡爾曼濾波函式，得到具體角度
  */
float MPU6050::Kalman_filter(float newAngle, float newRate, float dt)
{
	rate = newRate - bias;
	angle += dt * rate;

	// Update estimation error covariance - Project the error covariance ahead
	/* Step 2 */
	P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
	P[0][1] -= dt * P[1][1];
	P[1][0] -= dt * P[1][1];
	P[1][1] += Q_bias * dt;

	// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
	// Calculate Kalman gain - Compute the Kalman gain
	/* Step 4 */
	float S = P[0][0] + R_measure; // Estimate error
	/* Step 5 */
	float K[2]; // Kalman gain - This is a 2x1 vector
	K[0] = P[0][0] / S;
	K[1] = P[1][0] / S;

	// Calculate angle and bias - Update estimate with measurement zk (newAngle)
	/* Step 3 */
	float y = newAngle - angle; // Angle difference
	/* Step 6 */
	angle += K[0] * y;
	bias += K[1] * y;

	// Calculate estimation error covariance - Update the error covariance
	/* Step 7 */
	float P00_temp = P[0][0];
	float P01_temp = P[0][1];

	P[0][0] -= K[0] * P00_temp;
	P[0][1] -= K[0] * P01_temp;
	P[1][0] -= K[1] * P00_temp;
	P[1][1] -= K[1] * P01_temp;

	return angle;
}

  開啟藍芽模式後，EVA一直保持平衡，同時解析手機發送的資料開始行走，手機連線藍芽的APP使用一個兩輪自平衡機器人開源網站提供的APP-Balanduino，本來小白想自己寫，但是發現了這個寶物後就懶得動手了，可以從Balanduino源程式中或者官方說明中得到資料協議。

  其中一個模擬搖桿的資料協議是：（CJ，x，y ），其中搖桿的座標是單位座標，上下左右座標軸長度都為1，只要根據x，y判斷哪個現象後，再賦予兩個輪子不同的速度，就可以進行轉彎、前進和後退，速度的大小需要自己去除錯，每個機器人都不一樣。

#ifdef HC05_DEBUG
	if((micros() - hc05_time) > 200000)
	{
		data = hc05.recv();
		char str[32];
		if(!data.equals(AT_ERROR))
		{
			/* CJ,x,y */
			data.toCharArray(str, data.length());
			strtok(str, ",");
			float x = atof(strtok(NULL, ","));
			float y = atof(strtok(NULL, ","));
			
			speed_setpoint = x * 15;
			
			if(x >0  && y >0)								//第一象限
			{
				int x_speed = x * 15;
				int y_speed = y * 30;
				tb6612_A.set_rotate_dir(TB6612_LEFT);  
				tb6612_A.set_speed(x_speed);
				tb6612_B.set_rotate_dir(TB6612_LEFT);  
				tb6612_B.set_speed(y_speed);
				
			}	
			if(x < 0 && y > 0)								//第二象限
			{		
				int x_speed = x * 15;
				int y_speed = y * 60;
				tb6612_A.set_rotate_dir(TB6612_LEFT);  
				tb6612_A.set_speed(x_speed);
				tb6612_B.set_rotate_dir(TB6612_LEFT);  
				tb6612_B.set_speed(y_speed);				
			}
			if(x < 0 && y < 0)								//第三象限
			{		
				int x_speed = x * 30;
				int y_speed = y * 15;
				tb6612_A.set_rotate_dir(TB6612_LEFT);  
				tb6612_A.set_speed(x_speed);
				tb6612_B.set_rotate_dir(TB6612_LEFT);  
				tb6612_B.set_speed(y_speed);					
			}
			if(x > 0 && y < 0)								//第四象限
			{
				int x_speed = x * 60;
				int y_speed = y * 15;
				tb6612_A.set_rotate_dir(TB6612_LEFT);  
				tb6612_A.set_speed(x_speed);
				tb6612_B.set_rotate_dir(TB6612_LEFT);  
				tb6612_B.set_speed(y_speed);					
			}
		}
		PID_speed_compute();
	}
#endif

 
  超聲波的跟隨控制就比較直觀了，只要檢測到物體在跟隨範圍內，就賦予一個特定的速度，讓EVA前進，同時保持與物體的距離，讓EVA後退，當物體不在跟隨範圍的時候，就停止。

#ifdef HC_SR04_DEBUG
	if((micros() - hcsr_time) > 200000)
	{
		hcsr_distance = hcsr.get_distance();
		if(hcsr_distance > 10.0 && hcsr_distance < 15.0)		//向前跟
		{
			speed_setpoint = 25;
		}
		else if(hcsr_distance > 2.0 && hcsr_distance < 10.0)	//往後退
		{
			speed_setpoint = -25;
		}
		else 
		{
			speed_setpoint = SPEED_DEFAULT;						//停止
		}
		hcsr_time = micros();
		
		PID_speed_compute();
	}
#endif

 
  其它模組的驅動都比較簡單，可以在檔案末尾連結得到，這裡貼出PID的控制演算法程式碼，PID應用在兩個環上，分別是速度環和角度環，角度環令EVA平衡，速度環令EVA行走

/* out = (P * P_err) + (I * I_err) */
static void PID_speed_compute(void)
{
	if(micros() - PID_speed_timer > 10000)
	{
		P_speed_err = (count_L + count_R ) * 1.25 - speed_setpoint ;
		I_speed_err += P_speed_err;
		pitch_setpoint = P_speed * P_speed_err + I_speed * I_speed_err;
		PID_speed_timer = micros();		
	}
}

/* out = (P * P_err) + (I * I_err) + (D * D_err) */
static void PID_angle_compute(void)
{
	if(micros() - PID_angle_timer > 10000)
	{
		float pitch_current = accelgyro.get_filter_pitch();			//當前的pitch
		float P_angle_err = pitch_setpoint - pitch_current;			//獲得偏差
		I_angle_err += P_angle_err;									//積分，累計微小變化量
		D_angle_err = P_angle_err - P_angle_err_last;				//微分，偏差的變化趨勢
		P_angle_err_last = P_angle_err;								//記錄當前誤差

		float angle_output = P_angle*P_angle_err + I_angle*I_angle_err + D_angle*D_angle_err;
		
		int speed;
		if(angle_output > 255.0)
			speed = 255;
		else if(angle_output < -255.0)
			speed = -255;
		else
			speed = (int)angle_output;
		
		do_motor(speed);

		PID_angle_timer = micros();
	}

}

---

05-3D列印篇

  三維建模工具是CAD，三維建模而言，CAD並沒有像Solidworks這樣方便快捷，不過頭一次建模也夠用了，到CAD官網認證學生後有3年的免費試用！！不用煩破解安裝。

  CAD只是建模工具，實際的模型還是需要圖紙的，和最初圖紙有一定的差別，因為小白不想把外殼做得複雜，然後用紙皮做了模具，頭、身體、連線、電池夾和底座：

  然後用CAD三維建模，適當渲染後得到如下模型：

  最後打印出來，實物圖如下（後面其實經過修改，把身體的兩個照明燈取消，因為電壓不夠）：

06-測試篇

  3D列印好零件後，進行連線和組裝：

  如果自己有輪子的，儘量不要用列印，因為打印出來的輪子很光滑，不能使用的，小白給它套了橡膠圈才能正常使用

  組裝完成後，首先進行平衡測試，第一次調PID，真的是非常難調，調了大概3天時間才調出比較好的引數。

  怕輪子打滑厲害，因為橡膠圈磨損得比較薄了，放了件衣服增加摩擦測試平衡，EVA平衡的擺動幅度不大，已經可以自平衡了。

   其實文章是EVA一邊開發一邊編寫的，本來這個時候要編寫藍芽連線控制的，但是由於引數調的過高，在測試藍芽控制的時候EVA突然過沖撞向對面的桌子，跌倒在地，本來焊接好的線很多都斷了，難以重新連線，還發現電機編碼器的電路板被刮破了，一個電機已經不能使用，下面是大型翻車現場…

  上圖還是小白補救後的圖，摔倒在地後其實身體和基座都斷開了，電機驅動的線基本全部斷，重新焊接後才得到上面的圖，非常心疼EVA，還有超聲波跟隨測試和藍芽控制測試沒有進行，只能重新買模組組裝，不過條件有限，EVA只能暫時到此，等小白有條件後再回來……

   核心功能已經完成，其實後面的超聲波和藍芽程式碼已經寫好，只欠測試，另外還有很多【買多/買錯/湊單】的零件，可能後面拿來做有趣的東西。

• 自平衡
• 超聲波跟隨
• 藍芽控制

網盤連結

最後分享所有的硬體資料+Arduino原始碼+3D檔案：百度網盤 密碼：zn9e