。
[0119] Bernsen算法描述如下：
[0121] 其中，f(i，j)為圖像在像素點(diǎn)（i，j)處的灰度值，T(i，j)為圖像中各個(gè)像素點(diǎn) (i，j)的二值化閾值；
[0122] 對(duì)圖像中各像素點(diǎn)（i，j)用二值化函數(shù)b(i，j)逐點(diǎn)進(jìn)行二值化，b(i，j)函數(shù)如 下：
[0124] Bernsen二值化流程圖如圖5所示；Bernsen二值化算法是一種經(jīng)典的局部二值化 方法，具體算法如上式（1)所示。
[0125] 經(jīng)二值化的跑道圖像效果對(duì)比如圖6、圖7所示；從圖7中可以看到，經(jīng)二值化后 的圖像，其跑道邊緣將會(huì)有一個(gè)強(qiáng)烈的上升沿，通過嵌入式計(jì)算機(jī)識(shí)別該上升沿的跳變即 識(shí)別出跑道邊緣。
[0126] 3)跑道曲率半徑計(jì)算。
[0127] 設(shè)識(shí)別出的跑道邊緣為f(i，j)，則在中心線上圖像曲率半徑為：
[0129] 設(shè)曲率與差速成線性變化，則曲率到左右電機(jī)PWM差速計(jì)算公式如下：
[0130] PWMDifference-KDiff*k
[0131] 其中，KDiff為差速增益。
[0132] 綜上所述，本發(fā)明的自動(dòng)導(dǎo)航系統(tǒng)采用攝像頭進(jìn)行平面式的導(dǎo)航，能夠有效預(yù)測(cè) 前方路況，進(jìn)行更加有效的預(yù)測(cè)控制，提高了控制精度；此外，得益于頂U(kuò)傳感器的采樣速 率及嵌入式計(jì)算機(jī)的處理速率，控制精度也大幅提高。
[0133] 實(shí)施例2:
[0134] 本實(shí)施例是一個(gè)應(yīng)用實(shí)例，將上述實(shí)施例1的自動(dòng)導(dǎo)航系統(tǒng)應(yīng)用在直立平衡車 中。
[0135] 如圖8所示，所述直立平衡車包括車體1、左車輪2、右車輪3、電機(jī)（圖中未示出） 和自動(dòng)導(dǎo)航系統(tǒng)，所述左車輪2和右車輪3設(shè)置在車體1的底部?jī)蛇叄鲭姍C(jī)帶動(dòng)左車輪 2和右車輪3運(yùn)動(dòng)；其中，所述IMU傳感器4和嵌入式計(jì)算機(jī)5設(shè)置在車體1內(nèi)，所述攝像 頭傳感器6設(shè)置在車體1頂部，所述嵌入式計(jì)算機(jī)5與電機(jī)連接。
[0136] 以上所述，僅為本發(fā)明專利優(yōu)選的實(shí)施例，但本發(fā)明專利的保護(hù)范圍并不局限于 此，任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明專利所公開的范圍內(nèi)，根據(jù)本發(fā)明專利的技 術(shù)方案及其發(fā)明專利構(gòu)思加以等同替換或改變，都屬于本發(fā)明專利的保護(hù)范圍。
【主權(quán)項(xiàng)】
1. 基于攝像頭檢測(cè)的自動(dòng)導(dǎo)航系統(tǒng)，其特征在于；所述系統(tǒng)包括直立平衡數(shù)據(jù)采集模 塊、攝像頭數(shù)據(jù)采集模塊和信號(hào)處理及控制模塊，所述直立平衡數(shù)據(jù)采集模塊、攝像頭數(shù)據(jù) 采集模塊分別與信號(hào)處理及控制模塊連接；其中： 所述直立平衡數(shù)據(jù)采集模塊，用于通過IMU傳感器采集的數(shù)據(jù)獲取直立平衡車的當(dāng)前 姿態(tài)角數(shù)據(jù)； 所述攝像頭數(shù)據(jù)采集模塊，用于通過攝像頭傳感器采集跑道的灰度圖像數(shù)據(jù)； 所述信號(hào)處理及控制模塊，用于通過嵌入式計(jì)算機(jī)收集、存儲(chǔ)和處理跑道的灰度圖像 數(shù)據(jù)、直立平衡車的當(dāng)前姿態(tài)角數(shù)據(jù)、直立平衡車的當(dāng)前車速及差速數(shù)據(jù)，W及根據(jù)直立平 衡車的當(dāng)前姿態(tài)角數(shù)據(jù)，對(duì)直立平衡車進(jìn)行平衡控制。2. 根據(jù)權(quán)利要求1所述的基于攝像頭檢測(cè)的自動(dòng)導(dǎo)航系統(tǒng)，其特征在于：所述信號(hào)處 理及控制模塊包括： 平衡控制單元，用于根據(jù)獲取的當(dāng)前姿態(tài)角數(shù)據(jù)，對(duì)直立平衡車進(jìn)行平衡控制； 圖像處理單元，用于對(duì)采集的跑道灰度圖像數(shù)據(jù)進(jìn)行計(jì)算處理。3. 根據(jù)權(quán)利要求1或2所述的基于攝像頭檢測(cè)的自動(dòng)導(dǎo)航系統(tǒng)，其特征在于：所述IMU 傳感器由L3G4200巧螺儀和MMA8451加速度計(jì)組成，所述巧螺儀和加速度計(jì)通過IIC總線 與嵌入式計(jì)算機(jī)連接。4. 根據(jù)權(quán)利要求1或2所述的基于攝像頭檢測(cè)的自動(dòng)導(dǎo)航系統(tǒng)，其特征在于；所述攝 像頭傳感器采用圖像傳感器0V7620,并通過DMA通道與嵌入式計(jì)算機(jī)連接。5. 根據(jù)權(quán)利要求1或2所述的基于攝像頭檢測(cè)的自動(dòng)導(dǎo)航系統(tǒng)，其特征在于；所述嵌 入式計(jì)算機(jī)采用飛思卡爾公司KinetisMK60DN512系列MCU構(gòu)成。6. 基于權(quán)利要求3所述自動(dòng)導(dǎo)航系統(tǒng)的控制方法，其特征在于所述方法包括： 51、IMU傳感器中巧螺儀和加速度計(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)前姿態(tài)角數(shù)據(jù)；同時(shí)，攝像頭傳感器的數(shù)據(jù)輸入嵌入式計(jì)算 機(jī)，獲取跑道的灰度圖像數(shù)據(jù)； 52、 在嵌入式計(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í)別出特定的使用W黑線為邊 緣的跑道，通過對(duì)跑道的邊界進(jìn)行邊緣特征計(jì)算，得到跑道的路況，并通過嵌入式計(jì)算機(jī)向 直立平衡車的電機(jī)發(fā)出差速信號(hào)，從而實(shí)時(shí)跟蹤軌道。7. 根據(jù)權(quán)利要求6所述的自動(dòng)導(dǎo)航系統(tǒng)的控制方法，其特征在于：步驟S1所述嵌入式 計(jì)算機(jī)對(duì)姿態(tài)角數(shù)據(jù)的解算，具體如下： 靜態(tài)姿態(tài)角解算按下列公式計(jì)算，解算出的姿態(tài)角為橫滾角、俯仰角兩個(gè)姿態(tài)：其中，a,，ay，屯為正交的=軸加速度數(shù)據(jù)，中o為靜態(tài)俯仰角，P。為靜態(tài)橫滾角，設(shè)置ay與電機(jī)軸正交，則有效靜態(tài)姿態(tài)角數(shù)據(jù)為中。角； 加入巧螺儀數(shù)據(jù)《做動(dòng)態(tài)數(shù)據(jù)融合，取與電機(jī)軸平行，則融合后的動(dòng)態(tài)俯仰角為：其中，Confidencegyt。為巧螺儀的置信度，Confidence。。。。為加速度計(jì)的置信度。8. 根據(jù)權(quán)利要求6所述的自動(dòng)導(dǎo)航系統(tǒng)的控制方法，其特征在于：步驟S2所述PID控 制器按下列公式計(jì)算：其中，Kp為控制器比例增益系數(shù)，K巧控制器積分系數(shù)，Kd為控制器微分系數(shù)，e為誤 差； 使用嵌入式計(jì)算機(jī)控制時(shí)作離散化：其中，e為姿態(tài)角偏離平衡角度的數(shù)值，U為直接輸出到電機(jī)的占空比，6k和Gk_i分別 表示k和k-1時(shí)刻的偏差。9. 根據(jù)權(quán)利要求6所述的自動(dòng)導(dǎo)航系統(tǒng)的控制方法，其特征在于：步驟S2所述動(dòng)態(tài)二 值化采用Bernsen算法，該Bemsen算法描述如下；其中，f(i，j)為圖像在像素點(diǎn)（i，j)處的灰度值，T(i，j)為圖像中各個(gè)像素點(diǎn)（i，j) 的二值化闊值； 對(duì)圖像中各像素點(diǎn)（i，j)用二值化函數(shù)b(i，j)逐點(diǎn)進(jìn)行二值化，b(i，j)函數(shù)如下：經(jīng)二值化后的圖像，其跑道邊緣將會(huì)有一個(gè)強(qiáng)烈的上升沿，通過嵌入式計(jì)算機(jī)識(shí)別該 上升沿的跳變即識(shí)別出跑道邊緣。10. 直立平衡車，包括車體、左車輪、右車輪W及電機(jī)，所述左車輪和右車輪設(shè)置在車體 的底部?jī)蛇?，所述電機(jī)帶動(dòng)左車輪和右車輪運(yùn)動(dòng)，其特征在于：還包括權(quán)利要求1所述的自 動(dòng)導(dǎo)航系統(tǒng)；其中，所述IMU傳感器和嵌入式計(jì)算機(jī)設(shè)置在車體內(nèi)，所述攝像頭傳感器設(shè)置 在車體頂部，所述嵌入式計(jì)算機(jī)與電機(jī)連接。
【專利摘要】本發(fā)明公開了一種基于攝像頭檢測(cè)的自動(dòng)導(dǎo)航系統(tǒng)、控制方法及直立平衡車，所述系統(tǒng)包括直立平衡數(shù)據(jù)采集模塊、攝像頭數(shù)據(jù)采集模塊和信號(hào)處理及控制模塊，所述直立平衡數(shù)據(jù)采集模塊、攝像頭數(shù)據(jù)采集模塊分別與信號(hào)處理及控制模塊連接；所述直立平衡數(shù)據(jù)采集模塊，用于獲取直立平衡車的當(dāng)前姿態(tài)角數(shù)據(jù)；所述攝像頭數(shù)據(jù)采集模塊，用于采集跑道的灰度圖像數(shù)據(jù)；所述信號(hào)處理及控制模塊，用于收集、存儲(chǔ)和處理跑道的灰度圖像數(shù)據(jù)、直立平衡車的當(dāng)前姿態(tài)角數(shù)據(jù)、直立平衡車的當(dāng)前車速及差速數(shù)據(jù)，以及根據(jù)直立平衡車的當(dāng)前姿態(tài)角數(shù)據(jù)，對(duì)直立平衡車進(jìn)行平衡控制。本發(fā)明能夠有效預(yù)測(cè)前方路況，進(jìn)行更加有效的預(yù)測(cè)控制，提高了控制精度。
【IPC分類】G05D1/02
【公開號(hào)】CN104932505
【申請(qǐng)?zhí)枴緾N201510308759
【發(fā)明人】劉富春, 梁偉鵬
【申請(qǐng)人】華南理工大學(xué)
【公開日】2015年9月23日
【申請(qǐng)日】2015年6月8日