【正文】 分段比例控制能使小車更好的通過(guò)彎道，更好的進(jìn)行加速減速，以提高小車的運(yùn)行狀態(tài)。 注意事項(xiàng) 由于車模直立運(yùn)行，在制作電路板的時(shí)候盡可能減少電路板的尺寸，一方面便于固定，另一方面可以減少車模的慣量。 電磁傳感器安裝 電磁傳感器為兩個(gè)工字型的 10 毫亨電感。 圖 拆掉后輪之后的車模 傳感器安裝 車模中的傳感器包括有：速度傳感器，車模姿態(tài)傳感器（陀螺儀、加速度計(jì)）以及電磁檢測(cè)感應(yīng)線圈。 （ 2） 固定車模底盤與后輪支架 原有車模為了減輕后輪振動(dòng)對(duì)于車體的影響，后輪的支架與底盤之間采用了活動(dòng)連接方式?？梢酝ㄟ^(guò)微調(diào)加速度計(jì)的零偏值，減小車模在直立的時(shí)候朝一個(gè)方向的加速度 。保持車模直立靜止。 CarMagicAdjust()。 if(g_nCarMotionCount = CAR_MOTION_PERIOD) { g_nCarMotionCount = 0。 g_lnCarLeftPosition += g_nLeftMotorSpeed。 GetMotorSpeed(amp。 lnDelta = g_nCarMagicRightAverage g_nCarMagicLeftAverage。 以下算法中，采用了比例控制算法。 if(g_nMotorOutSpeedKeep MOTOR_OUT_MIN) g_nMotorOutSpeedKeep = MOTOR_OUT_MIN。 nSpeed = (nLeftSpeed + nRightSpeed) / 2。所以程序并沒(méi)有直接更新 g_nLeftMotorSpeedOut ， g_nRightMotorSpeedOut 的數(shù)值， 而是通過(guò)調(diào)用函數(shù) CalculateMotorSpeedOut()更新上面的數(shù)值。 if(g_nRightMotorOut MOTOR_OUT_MIN) g_nRightMotorOut = MOTOR_OUT_MIN。 else if(nSpeed MOTOR_SPEED_SET_MIN) nSpeed = MOTOR_SPEED_SET_MIN。 int nSpeed。其中涉及到兩個(gè)關(guān)鍵控制系數(shù)： k1:CAR_AA_P_INT： 傾角比例， （ *0x7fff） k2:CAR_AA_D_INT：角速度比例，（ *0x7fff） 這兩個(gè)系數(shù)是通過(guò)實(shí)驗(yàn)調(diào)試逐步確定的。 g_nCarGyroVal = (int)CV_GYRO_VAL。因此前面的 CAR_GYRO_RATIO_INT 也是需要考慮到這個(gè)比例的。 對(duì)于加速度計(jì)所得到的數(shù)值，通過(guò)比例系數(shù) (CV_ACCE_ANGLE_RATIO)將它歸一化到 1000 至 1000 之間。 MOTOR_SET(nLeftVal, nRightVal)。 if(nRightVal 0) nRightVal += MOTOR_OUT_DEAD_VAL。死區(qū)補(bǔ)償關(guān)系如圖 所示。 第二個(gè)子程序： MotorSpeedOut() 它的輸入?yún)?shù)為兩個(gè)全局變量： g_nLeftMotorOut, g_nRightMotorOut。} if(nRightVol 0) {setReg(PWM_PWMVAL3, 0)。 nPeriod = (short)getReg(PWM_PWMCM)。 （ 5）車模方向控制：根據(jù)前面讀取的電磁場(chǎng)檢波數(shù)值，計(jì)算偏差數(shù)值。 （ 3）讀取 AD 轉(zhuǎn)換值。中斷服務(wù)程序的任務(wù)被均勻分配在 04 的中斷片段中。跌倒判斷可以通過(guò)車模傾角是否超過(guò)一定范圍進(jìn)行確定，或者通過(guò)安裝在車模前后防撞支架上的微動(dòng)開(kāi)關(guān)來(lái)判斷。初始化的工作包括有兩部分，一部分是對(duì)于單片機(jī)各個(gè)應(yīng)用到的模塊進(jìn)行初始化。軟件編寫與調(diào)試主要任務(wù)包括： （ 1）建立軟件工程，配置 單片機(jī) 資源； （ 2）編寫單片機(jī)軟件程序框架，編寫上位機(jī)監(jiān)控軟件，建立軟件編譯、下載、調(diào)試的環(huán)境； （ 3）實(shí)現(xiàn)并測(cè)試各個(gè)子模塊的功能正確性； （ 3）逐步完成車模閉環(huán)控制，整定各個(gè)待定 參數(shù)； （ 4）進(jìn)行車模整體運(yùn)行性能測(cè)試與提高。；如果電機(jī)反轉(zhuǎn)，第二個(gè)脈沖超前 90176。 安徽理工大學(xué)畢業(yè)設(shè)計(jì) 21 圖 單極性 PWM、雙極性 PWM 因此每一路電機(jī)為了能夠?qū)崿F(xiàn)正反轉(zhuǎn)，都需要兩個(gè) PWM 信號(hào)。 圖 陀螺計(jì)、加速度計(jì)電路 圖 中，將陀螺儀的輸出信號(hào)放大了 10 倍左右，并將零點(diǎn)偏置電壓調(diào)整到工作電源的一半（ ）左右。 （ 2） 電機(jī)轉(zhuǎn)速脈沖接口 a) T0（ PIN19）：右側(cè)電機(jī)光電碼盤脈沖； b) T2（ PIN22）：左側(cè)電機(jī)光電碼盤脈沖。 （ 3） 定時(shí)器接口（ 2 路） a) 測(cè)量?jī)蓚€(gè)電機(jī)轉(zhuǎn)速，需要兩個(gè)定時(shí)器脈沖輸入端口。直立控制算法會(huì)自動(dòng) 調(diào)整車模往后傾斜，使得車模減速。直立控制是基礎(chǔ)，它的調(diào)整速度非?？?，速度和方向控制相對(duì)調(diào)整速度慢。由于存在積分環(huán)節(jié)，所以無(wú)論比例 Tg多么小，最終輸出角度 θ必然與加速度計(jì)測(cè)量的角度θg相等，只是這個(gè)調(diào)節(jié)過(guò)程會(huì)隨著 Tg的減小而延 長(zhǎng)。這個(gè)誤差會(huì)隨著時(shí)間延長(zhǎng)逐步增加，最終導(dǎo)致電路飽和，無(wú)法形成正確的角度信號(hào)，如圖 所示。 安徽理工大學(xué)畢業(yè)設(shè)計(jì) 13 在車模上安裝陀螺儀，可以測(cè)量車模傾斜的角速度，將角速度信號(hào)進(jìn)行積分便可以得到車模的傾角。但在實(shí)際車模運(yùn)行過(guò)程中，由于車模本身的運(yùn)動(dòng)所產(chǎn)生的加速度會(huì)產(chǎn)生很大的干擾信號(hào)疊加在上述測(cè)量信號(hào)上，使得輸出信號(hào)無(wú)法準(zhǔn)確 反映車模的傾角，如圖 。 MMA7260 是一款三軸低 g 半導(dǎo)體加速度計(jì)，可以同時(shí)輸出三個(gè)方向上的加速度模擬信號(hào)，如圖 所示。測(cè)量車模傾角和傾角加速度可以通過(guò)加速度傳感器和陀螺儀實(shí)現(xiàn)。 車模方向控制 道路電磁中心線的偏差檢測(cè) 道路電磁中心線檢測(cè)簡(jiǎn)單的方法可以通過(guò) 安裝在車模前方的兩個(gè)電磁感應(yīng)線圈實(shí)現(xiàn)。由上一節(jié)式（ 23）計(jì)算所得到的加速度控制量 a再乘以一個(gè)比例系數(shù)，即為施加在電機(jī)上的控制電壓，這樣便可以控制車模保持直立狀態(tài)。 （ 3）通過(guò)電機(jī)差速控制，可以實(shí)現(xiàn)車模方向控制。因此式（ 21）可變?yōu)? F = mgθ mk1θ mk2θ’ (22) 按照上面的控制方法，可把倒立擺模型變?yōu)閱螖[模型，能夠穩(wěn)定在垂直位置。 圖 在車輪上參照系中車體受力分析 倒立擺之所以不能象單擺一樣可以穩(wěn)定在垂直 位置，就是因?yàn)樵谒x平衡位置的時(shí)候，所受到的回復(fù)力與位移方向相同，而不是相反！因此，倒立擺便會(huì)加速偏離垂直位置，直到倒下。阻尼力越大，單擺越會(huì)盡快在垂直位置穩(wěn)定下來(lái)。下面通過(guò)對(duì)比單擺模型來(lái)說(shuō)明保持車模穩(wěn)定的控制規(guī)律。 世界上還沒(méi)有任何一個(gè)天才雜技演員可以蒙著眼睛使得木棒在自己手指上直立，因?yàn)闆](méi)有了負(fù)反饋。比 如速度控制時(shí)，假設(shè)車模已經(jīng)在直立控制下保持了直立穩(wěn)定，通過(guò)改變電機(jī)的電壓控制 車模加速和減速?？梢约僭O(shè)車模的電機(jī)可以虛擬地被拆解成三個(gè)不同功能的驅(qū)動(dòng)電機(jī)，它們同軸相連， 分別控制車模的直立平衡、 前進(jìn)行走、左右轉(zhuǎn)向，如圖 所示。 （ 3）控制系統(tǒng)的硬件設(shè)計(jì)與完成，從電源模塊設(shè)計(jì)開(kāi)始，完成道路信息 采集模塊、速度測(cè)量模塊、驅(qū)動(dòng)控制模塊、故障診斷模塊及串口模塊等的設(shè)計(jì)。1974 年，瑞典的 Volvo Kalmar 轎車裝配工廠與 Schiinder－ Digitron 公司合作，研制出一種可裝載轎車車體的 AGVS，并由多臺(tái)該種 AGVS 組成了汽車裝配線，從而取消了傳統(tǒng)應(yīng)用的拖車及叉車等運(yùn)輸工具。該系統(tǒng)通過(guò)光電傳感器獲取當(dāng)前道路信息，通過(guò)有效的數(shù)字圖像處理算法對(duì)原始圖像進(jìn)行處理，對(duì)主要的有用信息進(jìn)行分析和提取。在智能小車運(yùn)動(dòng)的控制中 ,對(duì)小車的轉(zhuǎn)向和速度采用 PID 控制算法 ,控制表來(lái)對(duì)智能小車進(jìn)行轉(zhuǎn)向和速度控制。 由于 Kalmar 工廠采用 AGVS 獲得了明顯的經(jīng)濟(jì)效益，許多西歐國(guó)家紛紛效仿 Volvo 公司，并逐步使 AGVS 在裝配作業(yè)中成為一種流行的運(yùn)輸手段。 （ 4）控制系統(tǒng)軟件的設(shè)計(jì)及實(shí)現(xiàn)，從單片機(jī)的功能入手完成系統(tǒng)各個(gè)模塊的設(shè)計(jì)，根據(jù)所選用的硬件，完成賽車信息的算法和車體的控制算法。 圖 車模運(yùn)動(dòng)控制分解示意圖 直流電機(jī)的力矩最終來(lái)自于電機(jī)驅(qū)動(dòng)電壓產(chǎn)生的電流。車模在加速和減速的時(shí)候，直立控制一直在起作用，它會(huì)自動(dòng)改變車 模的傾角，移動(dòng)車模的重心，使得車模實(shí)現(xiàn)加速和減速。 安徽理工大學(xué)畢業(yè)設(shè)計(jì) 5 圖 通過(guò)反饋保持木棒的直立 車模直立也是通過(guò)負(fù)反饋實(shí)現(xiàn)的。 重力場(chǎng)中使用細(xì)線懸掛著重物經(jīng)過(guò)簡(jiǎn)化便形成理想化的單擺模型。圖 顯示出不同阻尼系數(shù)下，單擺的運(yùn)動(dòng)曲線。 如何通過(guò)控制使得倒立擺能夠像單擺一樣，穩(wěn)定在垂直位置呢？要達(dá)到這一目的，只有兩個(gè)辦法：一個(gè)是改變重力的方向；另一個(gè)是增加額外的受力，使得恢復(fù)力與位移方向相反才行。因此，可得控制車輪加速度的控制 算法 a=k1θ +k2θ’ (23) 式中， θ 為車模傾角； θ’為角速度； k k2均為比例系數(shù)；兩項(xiàng)相加后作為車輪加速度的控制量。 電機(jī)運(yùn)動(dòng)控制是通過(guò)改變施加在其上的驅(qū)動(dòng)電壓實(shí)現(xiàn)的。 車模運(yùn)行速度調(diào)整時(shí)間相對(duì)很長(zhǎng)，此時(shí)，電機(jī)速度與施加在其上的電壓成正比。線圈一般采用 10mH 的工字型電感。 加速度傳感器 加速度傳感器可以測(cè)量由地球引力作用或者物體運(yùn)動(dòng)所產(chǎn)生的加速度。 圖 三軸加速度傳感器 安徽理工大學(xué)畢業(yè)設(shè)計(jì) 12 通過(guò)設(shè)置可以使得 MMA7260 最大輸出靈敏度為 800mV/g。 圖 車模運(yùn)動(dòng)引起加速度計(jì)信號(hào)波動(dòng) 車模運(yùn)動(dòng)產(chǎn)生的振動(dòng)加速度使得輸出電壓在實(shí)際傾角電壓附近波動(dòng)，可以通過(guò)數(shù)據(jù)平滑濾波將其濾除。如圖 。 圖 角度積分漂移 對(duì)于積累誤差的消除，可以通過(guò)上面的加速度傳感器獲得的角度信息對(duì)此進(jìn)行校正，如圖 。 為了避免輸出角度 θ跟著 θg過(guò)長(zhǎng)，可以采取以下兩個(gè)方面的措施： （ 1）仔細(xì)調(diào)整陀螺儀的放大電路，使得它的零點(diǎn)偏置盡量接近于設(shè)定值，并且穩(wěn)定。速度 和方向控制的輸出量是直接疊加在電機(jī)控制電壓上。車模轉(zhuǎn)向控制是在車速控制基礎(chǔ)之上，調(diào)節(jié)兩個(gè)電機(jī)驅(qū)動(dòng)電壓差使得電機(jī)運(yùn)行速度出現(xiàn)差動(dòng)，進(jìn)而調(diào)整車模的方向。 （ 4） 通訊接口（備用） a) SCI（ UART）：一路，用于程序下載和調(diào)試接口； b) I2C：（備用）如果選擇飛思卡爾公司的數(shù)字加速度 計(jì)，可以通過(guò) I2C 接口直接讀取加速度值。 （ 3） 電機(jī) PWM 驅(qū)動(dòng)接口 a) PWM03（ PIN23,24,28,29）：電機(jī)驅(qū)動(dòng)。放大倍數(shù)需要根據(jù)選取的傳感器輸出靈敏度設(shè)計(jì)。兩個(gè)電機(jī)總共需 4路 PWM 信號(hào)?？梢酝ㄟ^(guò)這個(gè)關(guān)系判斷電機(jī)是否正反轉(zhuǎn)。 軟件功能與框架 軟件的主要功能包括有： （ 1）車模運(yùn)行狀態(tài)檢測(cè)； （ 2）電機(jī) PWM 輸出； （ 3）車模運(yùn)行控制：直立控制、速度控制、方向控制； （ 4）車模運(yùn)行流程控制：程序初始化、車模啟動(dòng)與結(jié)束； （ 5）系統(tǒng)界面：狀態(tài)顯示、上位機(jī)監(jiān)控、參數(shù)設(shè)定等。這部分的代碼由CodeWarrior 集成環(huán)境的 ProcessorExpert 工具生成。一定車模跌倒，則停止車模運(yùn)行。因此每個(gè)中斷片段中的任務(wù)執(zhí)行的頻率為200Hz。這些值包括有陀螺儀、加速度計(jì)數(shù)值、電磁場(chǎng)檢測(cè)電壓值等。然后計(jì)算電機(jī)差?？刂齐妷簲?shù)值。 if(nLeftVol 0) {setReg(PWM_PWMVAL0, 0)。 nRightVol = mult(nRightVol, nPeriod)。 分別表示左右兩個(gè)電機(jī)輸出電壓。 圖 電機(jī)死區(qū)補(bǔ)償 （ 2） 進(jìn)行輸出數(shù)值的飽和判斷，確保在調(diào)用 MOTOR_SET 的時(shí)候，輸入?yún)⒘吭?0x8000 至 0x7fff 之間。 安徽理工大學(xué)畢業(yè)設(shè)計(jì) 30 else if(nRightVal 0) nRightVal = MOTOR_OUT_DEAD_VAL。 } 程序 電機(jī)速度輸出子程序 這個(gè)子程序由車模直立控制子程序調(diào)用，每 5ms 調(diào)用一次。同樣，后面的陀螺儀的讀出的數(shù)據(jù)也需要通過(guò)一個(gè)比例系數(shù)進(jìn)行歸一化。 AD 配置的時(shí)候，將所有 AD 通道的輸入數(shù)據(jù)都?xì)w一化到 0至 0x7ff0 之間。 g_nCarGyroVal = (int)(g_nCarGyroVal CV_GYRO_ZERO)。具體確定的方式如下： 首先將 CAR_AA_D_INT 置為 0。 int nP, nD。 安徽理工大學(xué)畢業(yè)設(shè)計(jì) 32 nLeft = nSpeed + g_nLeftMotorOutSpeed g_nMotorLeftRightDiff。 MotorSpeedOut()。函數(shù) CalculateMotorSpeedOut()是每 5 毫秒被調(diào)用一次，所以程序是將速度控制的變化量平均到 20 次進(jìn)行更新。 安徽理工大學(xué)畢業(yè)設(shè)計(jì) 33 nDeltaValue = g_nMotorSpeedSet nSpeed。 } void CalculateMotorOut