專利名稱：一種輪式倒立擺的傳感器處理與平衡控制算法的制作方法
技術(shù)領(lǐng)域：
本發(fā)明屬于自動控制技術(shù)領(lǐng)域，涉及一種輪式倒立擺的傳感器處理與平衡控制算 法。
背景技術(shù)：
輪式倒立擺的控制方法，一直是國內(nèi)外研究的熱點(diǎn)問題。1988年，日本專利說明書JP63-305082公開了一種只有左右兩輪、能自動保持動 態(tài)平衡的小車。該申請只是一種能演示一級倒立擺的實(shí)驗(yàn)方案，離實(shí)際使用在技術(shù)上尚有 一段較遠(yuǎn)的距離。2004年，國內(nèi)專利說明書CN1557668A公開了一種可以用于騎行的兩輪自動平衡 小車，其核心控制輸出為輪子的轉(zhuǎn)矩。2007年，國內(nèi)專利說明書CN1952996A公開了一種遙控型兩輪自平衡小車，其平衡 算法的控制輸出仍然是輪子的轉(zhuǎn)矩。由于不規(guī)則地面情況下，輪子的負(fù)載有較大變化，因此，單獨(dú)控制輪子的轉(zhuǎn)矩，將 不能有效地對車體的平衡進(jìn)行控制。因此，本發(fā)明提出了直接對車體的加速度進(jìn)行控制的 方法。上述各專利中，很少提到傳感器的融合方法以及對車體在控制策略上的保護(hù)上， 由于兩輪自平衡小車本身就是一個不穩(wěn)定系統(tǒng)，實(shí)際應(yīng)用中，不可避免地要遇到一些意外， 因此本發(fā)明從控制策略上提出了一些保護(hù)機(jī)制，有利于增強(qiáng)整個系統(tǒng)的穩(wěn)定性，有利于保 護(hù)所述輪式倒立擺上的設(shè)備與人員。

發(fā)明內(nèi)容
本發(fā)明的目的是提供一種輪式倒立擺的傳感器處理與平衡控制算法，它能提高倒 立擺的工作的穩(wěn)定性和可靠性，在保持倒立擺平衡的情況下，做了較多的保護(hù)措施，使得整 個倒立擺系統(tǒng)在復(fù)雜的外部環(huán)境中運(yùn)行的更加穩(wěn)定可靠。為了解決背景技術(shù)所存在的問題，本發(fā)明是采用以下技術(shù)方案它包括傳感器處 理算法，平衡控制算法，過速度保護(hù)算法，應(yīng)急處理算法。所述傳感器處理算法，包括加速度傳感器與陀螺儀數(shù)據(jù)融合算法以及濾波算法。加速度傳感器的輸出是當(dāng)前物體的加速度，當(dāng)?shù)沽[靜止時，其只收到重力加速 度的影響，因此可以在倒立擺靜止時通過公式9 =arCSin(aCC)計算出當(dāng)前倒立擺的偏轉(zhuǎn) 角；但是，當(dāng)?shù)沽[收到外力作用(如輪子)而具有X方向的加速度時，上述公式就不能計 算出當(dāng)前的偏轉(zhuǎn)角度，因此只使用加速度傳感器來測量當(dāng)前倒立擺的角度時不能滿足要求 的，但是其主要特點(diǎn)是低頻效果較為理想。陀螺儀的輸出是倒立擺偏轉(zhuǎn)的角速度信息，而對角速度值進(jìn)行積分，即可得到當(dāng) 前的角度值。但是由于零位誤差和積分誤差的影響，導(dǎo)致使用陀螺儀的積分值所獲得的角 度存在漂移問題。隨著時間的推移，漂移角度的擴(kuò)大，最終導(dǎo)致所測量的角度誤差無法接受，但其高頻效果比較理想。因此單獨(dú)使用陀螺儀或者加速度傳感器無法得到較為準(zhǔn)確的角度值，因此需要有 效結(jié)合兩者的優(yōu)點(diǎn)，對兩者進(jìn)行融合即可。所述加速度傳感器與陀螺儀的數(shù)據(jù)融合算法，旨在利用加速度傳感器和陀螺儀的 輸出通過一定的融合手段，用以測量出當(dāng)前倒立擺的偏轉(zhuǎn)角。本算法的特征在于利用加速 度傳感器的低頻特性，和陀螺儀輸出的高頻特性，用以得到一個在動態(tài)情況下較為穩(wěn)定的 倒立擺偏轉(zhuǎn)角。所述平衡控制算法，通過使用當(dāng)前倒立擺的偏轉(zhuǎn)角與倒立擺的偏轉(zhuǎn)角速度，來控 制當(dāng)前車體的加速度和角度?？刂七^程的輸入?yún)?shù)是當(dāng)其車體運(yùn)動的速度，當(dāng)其車體的偏轉(zhuǎn)角，以及偏轉(zhuǎn)角速 度。輸出為車體的加速度，計算公式如下Speed _ acc = kl* angle + k2* anglerate + A:3* | angle其中SpeecLacc為輪子的輸出加速度，angle為車體偏轉(zhuǎn)角度，anglerate是偏轉(zhuǎn) 的角速度，kl，k2，k3是相關(guān)參數(shù)，其具體的大小與實(shí)際的物理參數(shù)及其他因素。使用平衡 控制算法得到加速度之后，由相應(yīng)的電機(jī)控制器來進(jìn)行執(zhí)行。所述的過速度保護(hù)算法，即當(dāng)速度高于一定速度閾值時，該算法會輸出一個與速 度有關(guān)的角度偏移量。該偏移量將附加在所述傳感器融合算法所計算出來的角度上，用于 整個車體的減速，當(dāng)速度減小到一定閾值時，將逐漸撤去該偏移量，使得系統(tǒng)恢復(fù)正常。所述的應(yīng)急處理算法是在緊急情況下，如車體出現(xiàn)故障時，系統(tǒng)將自動啟動應(yīng)急 處理算法，該算法的實(shí)質(zhì)與過速度保護(hù)算法的實(shí)質(zhì)相同，不同之處在于，其速度閾值為0附 近，控制目標(biāo)是將車體的速度迅速降下來。以保證車載設(shè)備的安全。本發(fā)明能提高倒立擺的工作的穩(wěn)定性和可靠性，在保持倒立擺平衡的情況下，做 了較多的保護(hù)措施，使得整個倒立擺系統(tǒng)在復(fù)雜的外部環(huán)境中運(yùn)行的更加穩(wěn)定可靠。


圖1是本發(fā)明輪式倒立擺的結(jié)構(gòu)示意圖；圖2是本發(fā)明平衡控制算法的示意圖。
具體實(shí)施例方式參看圖1-2，本具體事實(shí)方式采用以下技術(shù)方案它是由擺桿1、帶控制器的驅(qū)動 電機(jī)2、輪子3、控制器和傳感器4、測速裝置5組成；擺桿1為倒立擺桿，也可以認(rèn)為是整個 倒立擺小車的負(fù)載；控制器和傳感器4通過讀取傳感器的信號進(jìn)行融合計算之后生成速度 控制量發(fā)送到帶控制器的驅(qū)動電機(jī)2中，帶控制器的驅(qū)動電機(jī)2根據(jù)收到的控制量通過讀 取測速裝置5所輸出的速度信息進(jìn)行反饋控制驅(qū)動電機(jī)，電機(jī)通過傳動裝置驅(qū)動輪子3從 而最終控制倒立擺的運(yùn)動?？刂破魇褂肧T公司的C0MTEX-M3內(nèi)核的處理器，傳感器使用ADI公司的陀螺儀和 加速度傳感器。電機(jī)的速度使用編碼器測量。傳感器的輸出通過控制器內(nèi)部ADC讀取后轉(zhuǎn)換為數(shù)字量。陀螺儀的輸出為 anglerate，加速度傳感器的輸出為acc，anglerate通過高通濾波器之后值為anglerate_f，acc通過低通濾波器之后值為acc_f，則最終結(jié)合兩個傳感器的輸出融合的角度值為 控制器將融合的角度值angle，以及陀螺儀輸出的角速度值anglerate，使用如下計算出輪子的輸出加速度SpeecLacc，然后將加速度輸入到電機(jī)控制器，控制器執(zhí) 行加速度命令。當(dāng)?shù)沽[運(yùn)行速度較大時，將導(dǎo)致電機(jī)輸出達(dá)到功率上限，從而影響最終的平衡 控制，因此，需要對倒立擺的運(yùn)動速度進(jìn)行限制。本實(shí)例使用電機(jī)的輸入電壓為門限，當(dāng)輸 入電壓值快要達(dá)到最大時，開啟過速度保護(hù)過程。使用如下公式計算出角度偏移量angle_offset = k氺(CurVoltage-ThresholdVoltage)式中，anglejffset是角度偏移量，用來控制當(dāng)前車體的角度偏移。CurVoltage 為當(dāng)前電壓，ThresholdVoltage為門限電壓，k為比例系數(shù)。通過上述公式，即可以得到當(dāng)前倒立擺所需要的偏移角度，從而控制倒立擺減速。當(dāng)?shù)沽[檢測出故障時，需要盡快停下來，此時，需要使用ThresholdVoltage為0 過速度保護(hù)過程，可以讓車體盡快停車，以達(dá)到保護(hù)效果。
angle = anglerate_f*dt+arcsin(acc_f)。
公式 Speed — acc 二糴1 * angle + k2* anglerate + k3
權(quán)利要求
一種輪式倒立擺的傳感器處理與平衡控制算法，其特征在于它包括傳感器處理算法，平衡控制算法，過速度保護(hù)算法，應(yīng)急處理算法。
2.根據(jù)權(quán)利要求1所述的一種輪式倒立擺的傳感器處理與平衡控制算法，其特征在于 所述的所述傳感器處理算法，包括加速度傳感器與陀螺儀數(shù)據(jù)融合算法以及濾波算法；力口 速度傳感器與陀螺儀的數(shù)據(jù)融合算法，旨在利用加速度傳感器和陀螺儀的輸出通過一定的 融合手段，用以測量出當(dāng)前倒立擺的偏轉(zhuǎn)角；本算法的特征在于利用加速度傳感器的低 頻特性，和陀螺儀輸出的高頻特性，用以得到一個在動態(tài)情況下較為穩(wěn)定的倒立擺偏轉(zhuǎn)角。
3.根據(jù)權(quán)利要求1所述的一種輪式倒立擺的傳感器處理與平衡控制算法，其特征在于 所述平衡控制算法，通過使用當(dāng)前倒立擺的偏轉(zhuǎn)角與倒立擺的偏轉(zhuǎn)角速度，來控制當(dāng)前車 體的加速度和角度；控制過程的輸入?yún)?shù)是當(dāng)其車體運(yùn)動的速度，當(dāng)其車體的偏轉(zhuǎn)角，以及 偏轉(zhuǎn)角速度；輸出為車體的加速度。
4.根據(jù)權(quán)利要求1所述的一種輪式倒立擺的傳感器處理與平衡控制算法，其特征在于 所述過速度保護(hù)算法，即當(dāng)速度高于一定速度閾值時，該算法會輸出一個與速度有關(guān)的角 度偏移量；該偏移量將附加在所述傳感器融合算法所計算出來的角度上，用于整個車體的 減速，當(dāng)速度減小到一定閾值時，將逐漸撤去該偏移量，使得系統(tǒng)恢復(fù)正常。
5.根據(jù)權(quán)利要求1所述的一種輪式倒立擺的傳感器處理與平衡控制算法，其特征在于 所述的應(yīng)急處理算法是在緊急情況下，如車體出現(xiàn)故障時，系統(tǒng)將自動啟動應(yīng)急處理算法， 該算法的實(shí)質(zhì)與過速度保護(hù)算法的實(shí)質(zhì)相同，不同之處在于，其速度閾值為O附近，控制目 標(biāo)是將車體的速度迅速降下來，以保證車載設(shè)備的安全。
全文摘要
一種輪式倒立擺的傳感器處理與平衡控制算法，它涉及自動控制技術(shù)領(lǐng)域。包括傳感器處理算法，平衡控制算法，過速度保護(hù)算法，應(yīng)急處理算法；輪式倒立擺由擺桿(1)、帶控制器的驅(qū)動電機(jī)(2)、輪子(3)、控制器和傳感器(4)、測速裝置(5)組成；能提高倒立擺的工作的穩(wěn)定性和可靠性，在保持倒立擺平衡的情況下，做了較多的保護(hù)措施，使得整個倒立擺系統(tǒng)在復(fù)雜的外部環(huán)境中運(yùn)行的更加穩(wěn)定可靠。
文檔編號B60W30/04GK101823485SQ20101013214
公開日2010年9月8日 申請日期2010年3月25日 優(yōu)先權(quán)日2010年3月25日
發(fā)明者周偉, 李一鵬, 林敬橋, 郭蓋華, 閆學(xué)凱, 陳志發(fā) 申請人:武漢若比特機(jī)器人有限公司