用了自動(dòng)導(dǎo)航系統(tǒng)，能夠?qū)崟r(shí)檢 測跑道，具有識(shí)別效果好、跟蹤速度高及實(shí)時(shí)性強(qiáng)的優(yōu)點(diǎn)，且采用的平衡直立算法及圖像處 理算法高效便捷、實(shí)用有效。
【附圖說明】
[0050] 圖1為本發(fā)明實(shí)施例1的自動(dòng)導(dǎo)航系統(tǒng)結(jié)構(gòu)框圖。
[0051] 圖2為本發(fā)明實(shí)施例1的傾角融合結(jié)果圖。
[0052] 圖3為本發(fā)明實(shí)施例1的倒立擺模型圖。
[0053] 圖4為本發(fā)明實(shí)施例1的中值濾波流程圖。
[0054] 圖5為本發(fā)明實(shí)施例1的Bernsen算法流程圖。
[0055] 圖6為本發(fā)明實(shí)施例1所獲取的跑道圖像原圖。
[0056] 圖7為本發(fā)明實(shí)施例1經(jīng)過Bernsen算法處理后的跑道圖像。
[0057] 圖8為本發(fā)明實(shí)施例2的直立平衡車結(jié)構(gòu)示意圖。
【具體實(shí)施方式】
[0058] 下面結(jié)合實(shí)施例及附圖對(duì)本發(fā)明作進(jìn)一步詳細(xì)的描述，但本發(fā)明的實(shí)施方式不限 于此。
[0059] 實(shí)施例1 :
[0060] 如圖1所示，本實(shí)施例的自動(dòng)導(dǎo)航系統(tǒng)包括直立平衡數(shù)據(jù)采集模塊、攝像頭數(shù)據(jù) 采集模塊和信號(hào)處理及控制模塊，所述直立平衡數(shù)據(jù)采集模塊、攝像頭數(shù)據(jù)采集模塊分別 與信號(hào)處理及控制模塊連接；
[0061] 所述直立平衡數(shù)據(jù)采集模塊，用于通過MU傳感器采集的數(shù)據(jù)獲取直立平衡車的 當(dāng)前姿態(tài)角數(shù)據(jù)；
[0062] 所述攝像頭數(shù)據(jù)采集模塊，用于通過攝像頭傳感器采集跑道的灰度圖像數(shù)據(jù)；
[0063] 所述信號(hào)處理及控制模塊，用于通過嵌入式計(jì)算機(jī)收集、存儲(chǔ)和處理跑道的灰度 圖像數(shù)據(jù)、直立平衡車的當(dāng)前姿態(tài)角數(shù)據(jù)、直立平衡車的當(dāng)前車速及差速數(shù)據(jù)，以及根據(jù)直 立平衡車的當(dāng)前姿態(tài)角數(shù)據(jù)，對(duì)直立平衡車進(jìn)行平衡控制；它可以包括：
[0064] 平衡控制單元，用于根據(jù)獲取的當(dāng)前姿態(tài)角數(shù)據(jù)，對(duì)直立平衡車進(jìn)行平衡控制；
[0065] 圖像處理單元，用于對(duì)采集的跑道灰度圖像數(shù)據(jù)進(jìn)行計(jì)算處理。
[0066] 所述嵌入式計(jì)算機(jī)采用飛思卡爾公司Kinetis MK60DN512系列MCU構(gòu)成；上述MU 傳感器由L3G4200陀螺儀和MMA8451加速度計(jì)組成，所述陀螺儀和加速度計(jì)通過IIC總線 與嵌入式計(jì)算機(jī)連接；所述攝像頭傳感器采用圖像傳感器0V7620。
[0067] 本實(shí)施例的自動(dòng)導(dǎo)航系統(tǒng)的控制過程如下：
[0068] S1、MU傳感器中陀螺儀和加速度計(jì)的數(shù)據(jù)通過IIC總線輸入嵌入式計(jì)算機(jī)，嵌入 式計(jì)算機(jī)通過對(duì)加速度計(jì)數(shù)據(jù)的反三角函數(shù)運(yùn)算，結(jié)合陀螺儀數(shù)據(jù)的積分?jǐn)?shù)據(jù)，進(jìn)行一階 低通濾波，得到當(dāng)前置信度，解算出直立平衡車的當(dāng)前姿態(tài)角數(shù)據(jù)；同時(shí)，攝像頭傳感器的 數(shù)據(jù)輸入嵌入式計(jì)算機(jī)，獲取跑道的灰度圖像數(shù)據(jù)；
[0069] S11、嵌入式計(jì)算機(jī)對(duì)姿態(tài)角數(shù)據(jù)的解算，具體如下：
[0070] 1)靜態(tài)姿態(tài)角解算按下列公式計(jì)算，解算出的姿態(tài)角為橫滾角、俯仰角兩個(gè)姿 態(tài)：

[0073] 其中，ax，ay，az為正交的三軸加速度數(shù)據(jù)，9〇為靜態(tài)俯仰角，P。為靜態(tài)橫滾角，設(shè) 置ay與電機(jī)軸正交，則有效靜態(tài)姿態(tài)角數(shù)據(jù)為角；
[0074] 2)靜態(tài)姿態(tài)角數(shù)據(jù)的在動(dòng)態(tài)時(shí)諧波噪聲較大，由此應(yīng)加入陀螺儀數(shù)據(jù)《做動(dòng)態(tài) 數(shù)據(jù)融合；取與電機(jī)軸平行，則融合后的動(dòng)態(tài)俯仰角為：
[0076] 其中，C〇nfidenCegyM為陀螺儀的置信度，Confidence aeee為加速度計(jì)的置信度，通 常取加速度計(jì)的置信度為5%，陀螺儀置信度為95%，置信度是指被測量參數(shù)的測量值的 可信程度；
[0077] 按上述方法得到的數(shù)據(jù)波形如圖2所示，輸入信號(hào)為+-90°的轉(zhuǎn)角信號(hào)，從波形 可以看到，按照上述方法能得到比較好的效果。
[0078] S2、在嵌入式計(jì)算機(jī)中，平衡控制單元將嵌入式計(jì)算機(jī)解算出的姿態(tài)角數(shù)據(jù)代入 PID控制器，控制直立平衡車的電機(jī)運(yùn)轉(zhuǎn)實(shí)現(xiàn)直立平衡車的平衡控制；同時(shí)，圖像處理單元 對(duì)嵌入式計(jì)算機(jī)獲取的跑道灰度圖像數(shù)據(jù)進(jìn)行動(dòng)態(tài)二值化處理，識(shí)別出特定的使用以黑線 為邊緣的跑道，通過對(duì)跑道的邊界進(jìn)行邊緣特征計(jì)算，得到跑道的轉(zhuǎn)向特征、直線特征、十 字特征等路況，并通過嵌入式計(jì)算機(jī)向直立平衡車的電機(jī)發(fā)出差速信號(hào)，從而實(shí)時(shí)跟蹤軌 道。
[0079] S21、平衡控制單元將嵌入式計(jì)算機(jī)解算出的姿態(tài)角數(shù)據(jù)代入PID控制器，具體 為：
[0080] 1)對(duì)直立平衡車建模為倒立擺系統(tǒng)，如圖3所示。
[0081] 根據(jù)受力平衡，倒立擺所受的回復(fù)力為：
[0082] F=mg*sin9 _ma*cos9
[0083]在工作點(diǎn)附近，0很?。▇ 0)，因此在對(duì)上述方程在工作點(diǎn)附近做線性化處理，則 上述方程化為：
[0084] F = mg 9-mb9
[0085] 考慮空氣阻力等因素，則必須加入一個(gè)阻尼力，該阻尼力方向應(yīng)與倒立擺角速度 方向一致，則考慮阻尼力因素后回復(fù)力為：
[0086] F= mg 0 -mki 0 -mk2 0'
[0087] 假設(shè)電機(jī)功率足夠大，則電機(jī)輸入電壓與輸出轉(zhuǎn)矩力近似成線性關(guān)系：
[0088] F=k3*u
[0089] 聯(lián)合上式得
[0090] u= 0 +k2* 0'
[0091] 因此得出控制電機(jī)扭矩與角度之間的關(guān)系近似成ro控制關(guān)系。
[0092] 2)對(duì)系統(tǒng)建立運(yùn)動(dòng)方程。
[0093] 設(shè)系統(tǒng)重心與系統(tǒng)底部距離為L，a(t)為系統(tǒng)運(yùn)動(dòng)加速度，x(t)為擾動(dòng)加速度，則 系統(tǒng)的運(yùn)動(dòng)方程為：
[0095] 在工作點(diǎn)附近，由于0很小，因此在工作點(diǎn)附近對(duì)上述運(yùn)動(dòng)方程作線性化處理：
[0097] 系統(tǒng)平衡時(shí)，a(t) = = 0 ;
[0098] 此時(shí)：
[0100] 系統(tǒng)的傳遞函數(shù)為：
[0102] 顯然此時(shí)系統(tǒng)有兩個(gè)極點(diǎn)，其中一個(gè)極點(diǎn)在S平面的右半面，此時(shí)系統(tǒng)不穩(wěn)定；
[0103] 加入比例微分控制后系統(tǒng)的傳遞函數(shù)變?yōu)椋?br>[0105] 系統(tǒng)的兩個(gè)極點(diǎn)為：
[0107] 要使兩極點(diǎn)都位于s平面的左半平面，則必須g，k2> 0 ;
[0108] 據(jù)此可得出使用ro控制器使系統(tǒng)穩(wěn)定的參數(shù)。
[0109] 3)為提高穩(wěn)態(tài)精度，加入積分控制，對(duì)該系統(tǒng)采用PID控制器進(jìn)行控制。
[0110] 所述PID控制器按下列公式計(jì)算：
[0112] 其中，Kp為控制器比例增益系數(shù)，K i為控制器積分系數(shù)，K d為控制器微分系數(shù)，e為 誤差；
[0113] 使用嵌入式計(jì)算機(jī)控制時(shí)作離散化：
[0115] 其中，e為姿態(tài)角偏離平衡角度的數(shù)值，u為直接輸出到電機(jī)的占空比，ek和ek_# 別表示k和k-1時(shí)刻的偏差。
[0116] S22、圖像處理單元對(duì)嵌入式計(jì)算機(jī)獲取的跑道灰度圖像數(shù)據(jù)進(jìn)行動(dòng)態(tài)二值化處 理，識(shí)別出特定的使用以黑線為邊緣的跑道，具體為：
[0117] 1)由于采集到的圖像數(shù)據(jù)具有一定的噪聲，使用中值濾波算法對(duì)信號(hào)進(jìn)行濾波如 圖4所示；中值濾波法是一種非線性平滑技術(shù)，它將每一像素點(diǎn)的灰度值設(shè)置為該點(diǎn)某鄰 域窗口內(nèi)的所有像素點(diǎn)灰度值的中值，其原理是把數(shù)字圖像或數(shù)字序列中一點(diǎn)的值用該點(diǎn) 的一個(gè)鄰域中各點(diǎn)值的中值代替，讓周圍的像素值接近的真實(shí)值，從而消除孤立的噪聲點(diǎn)。
[0118] 2)為適應(yīng)不同的光照條件，對(duì)圖像的二值化處理應(yīng)使二值化閾值可變，閾值可變 的動(dòng)態(tài)二值化算法采用Bernsen算法