前言

??這是我第一次參加智能車競賽，也是我第一次接觸直立車，開始的時候真的是個啥也不懂的小白，隨著時間的推移，對直立車的理解和認知也慢慢地由淺入深，逐漸學到了很多關于PID的理論知識和直立車的特性，這段備賽和參賽的經歷非常寶貴，所以我想總結一下這段時光識訓的點點滴滴，同時也希望能幫助到大家，這篇博客是我的處女作，有些地方的排版和表達可能不是很合理，也可能存在表述錯誤的問題，請大家見諒，接下來讓我們直入主題吧！

直立環核心控制演算法—串級PID

??串級PID演算法是我這次比賽最大的識訓，在寫出這個演算法之前我嘗試過各種各樣的直立環控制演算法：最簡單的PD控制，清華方案等等，在第一次接觸到串級PID時，很不能理解它的控制思路，而且還看不懂流程框圖，沒法將演算法用代碼實作，但隨著時間的推移和閱歷的增加，我在一兩個月之后再次看到它時，居然看懂了，并且很快地用代碼實作了，但對于這個演算法為什么這樣進行計算還是不解，在之后調車的程序中才慢慢地心領神會，逐漸理解了這個控制演算法的每一步，
??串級PID的核心思想是讓環跟環之間形成倍訓，也就是級間反饋，這套演算法的反饋量和輸出量很關鍵，因為各環的反饋輸入量不僅包括傳感器的反饋量，還包括各環的輸出，下面用流程框圖進行詳細講解：

??串級直立PID的最外環是速度環，外環角度，內環角速度，而角速度環是這套演算法的核心，作為最外環的速度環不同于并行PID（最簡單的三環PID直接相加）的速度環，并行PID的速度環對與直立環屬于干擾因素，兩個環的輸出直接耦合作用于電機會嚴重影響系統的穩定性，同時也會使PID的引數變得更難整定，而串行PID的速度環屬于直立環的一部分，即速度環輸出作為角度環輸入，角度環輸出作為角速度環的輸入，最后由角速度環的輸出作為直立環的總輸出作用于電機，而且由于角速度環的原因，直立環的直立效果極好，調整好的直立環會給你一種車咬住電機的感覺，車的直立環會非常的硬，凡事有利有弊，雖然串級直立環的直立效果極好，但隨之而來的問題就是作用于電機的電流很大，但電機運轉的幅度很小，也就是電機堵轉，導致電機發熱現象嚴重，長時間除錯甚至會啥訓電機，所以調車時注意時刻關注電機溫度，適當地給電機降降溫，串級PID中各環的嵌套其實是各個量之間的量綱轉換，速度環的輸出去作為角度環的輸入參量，其實是把速度變化轉換為角度變化，而角度環的輸出去作為角速度環的輸入，是把角度變化轉換為角速度的變化，最后由角速度環將所有的變化進行統一計算，然后將輸出作用于電機，這樣就不存在速度環與直立環的輸出互相干擾的問題，
注：在這里對上圖中的參量做一些說明，以使大家能順暢地寫出這個控制演算法，俯仰角度是由兩個軸的加速度資料通過反正切函式計算而得，角速度就是陀螺儀的資料，

轉向環控制演算法

??轉向環的控制我沒有找到非常好的控制演算法，因為轉向環的控制演算法歷來都是各大強校代碼的核心部分，所以我就用了最簡單的PD控制，但在比賽前的一段時間聽說過串級的轉向環，串級的直立環非常穩定，可想而知，串級轉向環的效果肯定也會讓人大吃一驚，可惜臨近比賽，我沒有多余的時間去嘗試這個方案，在這里說一下大概的思路（不一定全對，我也是聽別人隨口一說，自己想了想感徑訓是有道理的）：車模繞車軸的旋轉運動其實是在垂直于車軸的平面內（這里稱為垂直面）的運動，在這個平面內車模進行旋轉運動時會產生角速度變化和加速度變化，同理，車模跑動時的轉向也可以近似地看作繞一根軸線的旋轉運動，只不過此時的旋轉運動是在平行于地面的平面（這里稱為水平面）上發生的，既然車模在垂直面上做旋轉運動時會產生加速度和角速度變化，那么在水平面上做旋轉運動時也會產生加速度和角速度的變化，我的想法是內環和外環還是像串級直立環那樣嵌套，最外環為普通的偏差轉向環，還有一種就是動態的轉向環，將轉向環的P和D跟速度環進行關聯，讓P和D隨速度的變化而變化，可以去嘗試一下，

演算法框架

??主要的計算控制都放在一個2ms的定時器中斷函式里，也可以采用5ms的定時中斷，中斷觸發時間取決于代碼的計算量和芯片的處理速度，在進行演算法計算前需要采集俯仰角、角速度、車輪轉速（編碼器值）這三個引數，采集函式都放在同一個定時器中斷里，各環的PID計算可以封裝成函式，以外調函式的方法來進行各環輸出的計算，各環的計算順序如下：
??首先進行速度環的計算，10ms計算一次（官方給的思路是100ms計算一次，個人感覺控制間隔太長了），輸入參量為左右輪兩編碼器的值，將兩編碼器的值取絕對值再相加，然后再取平均，即為車模當前的運行速度，速度環采用P控制就足夠了，理論上應該采用PI控制，但積分項I需要限幅和定時清零操作，用起來比較困難，而且如果處理不好，車模在運行程序中會出現不可預測的情況，
??其次計算角度環，10ms計算一次，輸入參量為車模當前的俯仰角和速度環的輸出量，角度環采用PD控制，P影響直立控制的回應速度，D可以彌補P的超調現象，
??最后計算角速度環，作為輸出直接作用于電機的最內環，角速度環需要高頻控制，2ms計算一次，輸入參量為角度環的輸出量和角速度，角速度環采用PI控制，I可以消除靜差，并影響電機的控制力度，即輸出給電機的電流，P可以消除I產生的低頻大幅抖動，

??以上是直立環各環的計算順序，另外，速度環的輸出需要限幅后再傳給角度環，角度環的輸出也需要限幅后再傳給角速度環，

??方向環的計算放在直立環計算前或者計算后都可以，因為方向環和直立環是獨立開的，兩環的輸出都會在最后共同作用于電機，方向環采用PD控制，P影響車模轉向的靈敏度，D可以彌補P的超調現象，有效改善車模連續轉向時的晃動情況，

/*速度環&角度環計算&方向控制-10ms一次*/
	if(TIM_COUNT_10ms==5){
		TIM_COUNT_10ms=0;
		
		/*速度環*/
		Speed_PWM = Speed(Encoder_Left,Encoder_Right);
		/*角度環*/
		Angle_PWM = Angle(angle_out,Speed_PWM);
		/*方向環*/			
		ImgDirec_PWM = ImgDirec(XK.error);
	}
	/*角速度環計算-2ms一次*/
	Angspeed_PWM = Angspeed(icm_gyro_y*0.005,Angle_PWM);

	MotorOutput(Angspeed_PWM,ImgDirec_PWM);	//電機控制

這里推薦給大家一篇具有啟蒙意義的技術報告：中南民族大學技術報告

搭車方法

??對于直立車而言，好的控制演算法固然重要，但機械結構才是重中之重，演算法和機械至少“三七分”，七分機械，三分演算法，這一點是我的親身經歷，備賽程序中就是在車的結構上走了不少彎路，直立車的機械結構應該遵循“質量集中、重心低、質量輕”這十字箴言，質量集中是為了減少轉動慣量，電池作為主要重物應盡量離車軸近；重心低是為了防止過彎側滑，能夠使過彎更平滑順暢；質量輕是為了便于控制，減少整車慣性，搭車時應盡量使重心靠近車軸，最好壓在車軸上，建議采用V字形車體結構，前后的臂展要適中，質量分布盡量均勻，就是無論車在哪個角度位置都不會出現明顯的加速傾倒，車體的自然直立姿態最好“抬頭壓尾”，就是車傾倒時會自然地往后倒，車體的前端留出較大的空余角度空間，因為前端留出的角度空間越大提速空間越多，上坡時越不容易卡坡，最后也是最重要的一點，車體結構一定要有較好的機械零點，車要能在機械零點位置立住一段時間，立住的時間越久越好，當然，如果能夠一直立住那再好不過了，

波形擬合

??在整定各環引數之前需要做一個準備作業—濾波擬合，陀螺儀傳回來的原始資料抖動很大，計算出的俯仰角資料抖動自然也大，波形上的體現就是有許多小幅度的尖峰跳變，濾波演算法有很多選擇，最經典的應屬互補濾波和卡爾曼濾波了，我用的是卡爾曼濾波，將初步計算的俯仰角和Y軸陀螺儀資料（具體哪個軸取決于你陀螺儀怎么安放）進行擬合從而得出濾波后的俯仰角，濾波擬合出的俯仰角波形要圓滑，并且波形的跟隨性（與原始資料波形的重合度）要好，

??可以用各種不同的擺動方式擺動車模以觀測波形是否正確，
??如果波形不夠圓滑，那么俯仰角資料就會存在突變現象，如果跟隨性不好就會導致俯仰角資料反饋滯后，車模控制就會有遲滯性，

具體怎么擬合我就不細說了，卡爾曼濾波還是挺復雜的，直接看下面吧：

/*Q值為程序噪聲，越小系統越容易收斂，我們對模型預測的值信任度越高；
但是太小則容易發散，如果Q為零，那么我們只相信預測值；
Q值越大我們對于預測的信任度就越低，而對測量值的信任度就變高；
如果Q值無窮大，那么我們只信任測量值；

R值為測量噪聲，太小太大都不一定合適，
R太大，卡爾曼濾波回應會變慢，因為它對新測量的值的信任度降低；
越小系統收斂越快，但過小則容易出現震蕩；
測驗時可以保持陀螺儀不動，記錄一段時間內陀螺儀的輸出資料，
這個資料近似正態分布，按3σ原則，取正態分布的(3σ)^2作為R的初始化值，

測驗時可以先將Q從小往大調整，將R從大往小調整；先固定一個值去調整另外一個值，看收斂速度與波形輸出，

系統中還有一個關鍵值P，它是誤差協方差初始值，表示我們對當前預測狀態的信任度，
它越小說明我們越相信當前預測狀態；
它的值決定了初始收斂速度，一般開始設一個較小的值以便于獲取較快的收斂速度，
隨著卡爾曼濾波的迭代，P的值會不斷的改變，當系統進入穩態之后P值會收斂成一個最小的估計方差矩陣，
這個時候的卡爾曼增益也是最優的，所以這個值只是影響初始收斂速度，*/

float LowPassFilter_kalman(float datan, int w){
	static int Xlast=0;
	static int P=5;
	float Q=0.3;	//過大噪聲嚴重，越小越平滑但跟隨性會下降
	int R=40;
	float S=0.045;	//角速度積分時間，影響跟隨性
	float Kg, PP;
	float Xnow,x;
	x=datan*100;
	PP=P+Q;
	Kg=PP/(PP+R);
	Xnow=Xlast+Kg*(x-Xlast);
	P=(1-Kg)*PP;
	Xlast=Xnow+w*S;
	return Xnow;
}

另外推薦給大家兩篇關于卡爾曼濾波的博客：
卡爾曼濾波演算法原理、C語言實作及實際應用
卡爾曼濾波中關鍵引數的調整

??這里給大家提供一個方便的波形除錯工具——山外上位機，這個上位機不僅僅可以觀測波形還有很多其他輔助調車的工具，與上位機的通信代碼如下：

/*本檔案函式用于與山外上位機通信*/

//串口發送一個位元組
//void UART_send_byte(uint8 byte) 
//{
//    while(!((USART1->ISR)&(1<<7)));
//    USART1->TDR=byte;    
//}

void uart4_putbuff(uint8 *data,uint32 size)
{
    uint8 i=0;
    for(;i<size;i++)
    {
        uart_putchar(UART_4,*(data+i));
    }
}

void vcan_sendware(uint8 *wareaddr, uint32 waresize)
{
    uint8_t cmdf[2] = {0x03, 0xfc};    //串口除錯 資料包幀頭協議
    uint8_t cmdr[2] = {0xfc, 0x03};    //串口除錯 資料包幀尾協議

    uart4_putbuff(cmdf, sizeof(cmdf));    //先發送前命令
    uart4_putbuff(wareaddr, waresize);    //發送資料
    uart4_putbuff(cmdr, sizeof(cmdr));    //發送后命令
}

下載鏈接：山外上位機

調車方法

??直立車的調車思路都是一樣的，直立環是基礎，車要先能立住才能跑，但不同于并級PID的直立環，串級直立環包含了速度環，速度環是直立環的一部分，而并級PID的速度環和直立環是相互獨立的，串級直立環應先整定角速度環，再整定角度環，最后整定速度環，即整定的順序自內向外，未整定的各環引數全部置零，

角速度環整定方法

??角速度環采用PI控制，增量式和位置式的PID都可以，我用的是位置式，角速度環的整定方式有點特殊，需要先調I再調P，因為陀螺儀的角速度資料抖動非常大，十分容易出現靜差，需要加上i來消除那部分靜差，調整的現象是加上i車就能大幅的低頻來回晃，能勉強立住，立住的效果越好i的引數越好，i過大也會導致高頻抖動；在大幅度低頻抖動的情況下逐漸增加P直至低頻抖動消失，然后再緩慢增加P直至出現輕微的高頻抖動，適當減小一點P或I至高頻抖動消失，至此角速度環整定完畢，過大的P和I都會引起高頻抖動，整定好的角速度環已經基本可以直立了，但還不具備直立姿態的自動調節能力，車體姿態在機械零點附近時車就能保持基本直立，而且還很硬，整定好的角速度環當雙手抓住輪子時給車體一個傾角，車就會緩慢地向一側傾倒，可以用這種抓住車輪讓車體動的方法檢驗角速度環是否已整定好，

角度環整定方法

??角度環采用PD控制，將角度環的設定角度（期望角度）設為車的機械零點角度，在角速度環調好的基礎上加入角度環的P（數量級大概是角速度環P的一千分之一）后，車就具備了基本的自動調節能力，但會產生較大的振蕩，需要加入D（數量級跟P相同）來進行穩定，調整好角度環時，角速度環的P和I引數值大概是原來的60%~70%，所以在調整角度環前可以把角速度環的引數值先調低，也可以在調角度環的時候隨時伺機調低角速度環的引數，這么做是因為如果不調低角速度環的引數值，那么即使將角度環引數調節到高頻抖動，此時雖然車的直立姿態很硬，但是自我調節的回應速度不夠，也就是此時的角度環引數還偏小，可以先將角速度環的引數調低到原來的60%，然后調節角度環的P至車體出現輕微的高頻抖動，然后加入D，逐漸增加D至抖動消失，此時可以再微調一下P，然后在車體不抖動的情況下再慢慢增加角速度環的I和P至車體抖動，再降低至車體抖動消失，此時角度環就差不多整定完了，整定引數時，P和D過大都會引起車體的高頻抖動，整定時可以拍打車體的前端或者后端來觀察車體姿態自我調節的回應速度和振蕩幅度，回應速度越快，振蕩幅度越小，角速度環的引數越好，整定好角度環后，只要車的機械結構合理，車是不會倒的，好的直立環即使車在運行程序中撞到或者碾過賽道路肩車也不會倒，

??角速度環和角度環都整定完畢，那么基本的直立環也就整定完畢了，可以用檢驗角速度環的方式檢驗當前的直立環：抓住車的兩個輪子，將車體往前壓或者往后壓都會感覺到明顯的阻力，阻力越大說明直立環越硬，松開手后車體就會緩慢回到設定的角度位置；手抓住兩個輪子來回晃車體也不會傾倒，會回到設定的角度位置，

演示視頻：串級直立環直立效果

速度環整定方法

??速度環按照理論來說應該采用PI控制，但I比較難用，需要限幅和定時清零操作，否則車的運行會出現不可預料的錯誤，所以I用不好不如不用，純P控制也足夠了，速度環的P引數跟設定的速度有關，成反比關系，合適的P可以讓車在設定速度下穩定地運行，P過大時，車體姿態會先往前傾從而快速加速，然后車體姿態會往后傾倒從而進行減速，如此往復，也就是所謂的“點頭車”；P過小時，車從靜止開始運行提速會很慢甚至跑不起來，給車推一把有可能會導致車一直加速直至前傾倒地，這是P過小時的兩個常見現象，另外，速度環跟轉向環雖然沒有關聯，但是會有互相影響，如果想提速，需要增加設定速度，減小速度環的P，使加速平緩，而轉向環就需要更加靈敏，同時增加P和D，同時角度環的P和D也需要調小，使轉向更加穩定，否則車在轉向時會很僵硬，轉向遲緩，但過小會導致車在出彎的時候轉向過沖，會撞向賽道的一邊，

轉向環整定方法

??轉向環采用PD控制，先整定P引數，P過小時轉向的轉角不足，轉向時會撞向彎道外圈，P過大時轉向時會內切，當車能夠穩穩當當地跑完一個環形彎，P引數基本整定完畢，在只有P的情況下出彎時車體會有明顯的左右晃動，這是因為單純的P控制振蕩較大，加入D能使車的振蕩幅度減小，D的值大概是P的15~20倍，D引數可以調的粗一些，P調好后直接猛加D，D過大不會引起左右晃動，但會導致曲線預判超前，車還沒出彎可能就撞上了彎道外圈，給人一種出彎時轉不過來的感覺，

其他問題

??如果車在跑的程序中有抖動，可能是直立環調得太硬了，需要適當調小角度環的P，調大D，

END

??直立車是個很有意思的控制模型，對于PID的運用非常靈活，研究直立車能加深對PID演算法的理解和運用，也能間接鍛煉耐力和心態，因為后期的調參程序非常無聊乏味，而且直立車是個非常敏感的控制系統，車體結構一旦變動就會導致重心偏移，機械零點的位置就會改變，車體機械零點對應的俯仰角也就隨之改變了，這樣的話各環引數都需要重新整定，一次兩次還好，但是次數多了，心態崩了也是再所難免的（作為一個過來人，調參程序中的辛酸淚就不多說了T-T），所以建議大家在準備調車前就敲定車模的機械結構，并且把機械結構搭得牢固可靠一點，不會輕易被撞散架的那種（有個不成熟的想法，可以試著去跟麥輪或者越野碰一碰，看看誰的頭更硬^_），實踐出真知，越是敢于嘗試越能識訓意外驚喜，希望這篇文章能幫到看到這里的每一位朋友！