專(zhuān)利名稱(chēng)：基于紅外姿態(tài)檢測(cè)的兩輪直立式自平衡機(jī)器人及控制方法
技術(shù)領(lǐng)域：
本發(fā)明是一種基于紅外傳感器姿態(tài)檢測(cè)的兩輪直立式自平衡機(jī)器人及控 制方法，屬于機(jī)器人控制領(lǐng)域。
背景技術(shù)：
兩輪自平衡機(jī)器人是在伴隨著當(dāng)今對(duì)移動(dòng)式機(jī)器人不斷的深入研究的趨 勢(shì)下出現(xiàn)的新型機(jī)器人，輪式機(jī)器人具有成本低、機(jī)構(gòu)和控制算法簡(jiǎn)單、能 高速穩(wěn)定的移動(dòng)、動(dòng)力利用率高，同時(shí)能夠自學(xué)習(xí)，同過(guò)自強(qiáng)化學(xué)習(xí)來(lái)學(xué)會(huì) 之前不能完成的任務(wù)。這種機(jī)器人最大的特點(diǎn)就是能夠自平衡，擴(kuò)展了他適 應(yīng)的環(huán)境變化的能力，同時(shí)，可以進(jìn)行加速或者是減速以及實(shí)現(xiàn)原地回轉(zhuǎn)和 任意半徑轉(zhuǎn)向，移動(dòng)軌跡更為靈活易變，大大彌補(bǔ)了傳統(tǒng)三輪或四輪轉(zhuǎn)向不 足的缺點(diǎn)。
目前在美國(guó)的商品化兩輪代步機(jī)器人Segway以?xún)?nèi)置的精密固態(tài)陀螺儀 (Solid-State Gyroscopes)來(lái)判斷車(chē)身所處的姿勢(shì)狀態(tài)，透過(guò)精密且高 速的中央微處理器計(jì)算出適當(dāng)?shù)闹噶詈?，?qū)動(dòng)馬達(dá)來(lái)做到平衡的效果。但 是這種方法由于采用先進(jìn)的固態(tài)陀螺儀，所以成本比較高，不能得到普及。 傳統(tǒng)的兩輪自平衡機(jī)器人的姿態(tài)檢測(cè)系統(tǒng)多采用傾角儀，加速度計(jì)和陀 螺儀三者相結(jié)合的方式達(dá)到測(cè)量目的，這種方法成本較高，而且像陀螺儀， 傾角儀等器件容易受到溫度的影響，產(chǎn)生零點(diǎn)漂移。

發(fā)明內(nèi)容
本發(fā)明提出了一種基于紅外傳感器作為姿態(tài)檢測(cè)的兩輪自平衡機(jī)器人及 其控制方法，此機(jī)器人通過(guò)紅外傳感器檢測(cè)與地面之間的距離來(lái)達(dá)到姿態(tài)檢 測(cè)的目的，此種方法大大節(jié)約了傳感器的成本，同時(shí)克服了零點(diǎn)漂移現(xiàn)象， 能夠有效準(zhǔn)確的對(duì)姿態(tài)進(jìn)行檢測(cè)。同時(shí)體積較小，甚至可以在普通的辦公桌
桌面上工作。
本發(fā)明的主體思路是當(dāng)機(jī)器人發(fā)生傾斜時(shí)，紅外傳感器檢測(cè)的其與地 面的距離將發(fā)生變化，紅外傳感器將變化的信號(hào)傳給主控制器，在主控制器
3中，將進(jìn)行一系列的信號(hào)處理，之后，將處理好的信號(hào)轉(zhuǎn)換成驅(qū)動(dòng)電機(jī)的P麗 波信號(hào)，該信號(hào)傳送到電機(jī)的控制電路板上，從而驅(qū)動(dòng)電機(jī)，使電機(jī)朝著機(jī) 器人傾斜方向轉(zhuǎn)動(dòng)，從而達(dá)到控制機(jī)器人自平衡的目的。
本發(fā)明采取的具體技術(shù)方案如下包括主體支架、固定在主體支架下端 的電機(jī)支架、固定在電機(jī)支架上的兩個(gè)電機(jī)、連接在兩個(gè)電機(jī)上的輪子、主 控制電路、固定在主體支架上端的紅外傳感器支架和兩個(gè)紅外傳感器。兩個(gè) 紅外傳感器分別固定在紅外傳感器支架的左右兩端，采集與地面之間的距離， 兩個(gè)紅外傳感器的輸出端分別與主控制電路的輸入端相連，主控制電路的輸 出端與電機(jī)相連。
上述機(jī)器人的自平衡控制方法包括以下步驟
1) 左右兩個(gè)紅外傳感器分別檢測(cè)與地面之間的距離，并將該距離以電壓 信號(hào)的形式傳送給主控制電路；設(shè)L1為左側(cè)紅外傳感器檢測(cè)到的與地面之間
的距離，設(shè)L2為右側(cè)紅外傳感器檢測(cè)到的與地面之間的距離，L0為機(jī)器人 處于平衡位置時(shí)紅外傳感器檢測(cè)到的與地面之間的距離；
2) 主控制電路通過(guò)接收到的距離L1、 L2判斷機(jī)器人的姿態(tài)信息，具體 為當(dāng)L1〉L0且L2〈L0時(shí)，判斷機(jī)器人為向右發(fā)生傾斜；
當(dāng)LKLO且L2>L0時(shí)，判斷機(jī)器人為向左發(fā)生傾斜；
3) 根據(jù)步驟2)得到的機(jī)器人姿態(tài)信息，主控制電路通過(guò)增量PID控制 算法，得出一組PWM波控制信號(hào)來(lái)控制電機(jī)，驅(qū)動(dòng)電機(jī)進(jìn)行轉(zhuǎn)動(dòng)，使機(jī)器人 重新達(dá)到平衡；具體為
當(dāng)機(jī)器人向右傾斜時(shí)，電機(jī)向右轉(zhuǎn)動(dòng)，通過(guò)車(chē)輪帶動(dòng)機(jī)器人向右移動(dòng)， 產(chǎn)生一段位移后，使機(jī)器人重新達(dá)到平衡；
當(dāng)機(jī)器人向左傾斜時(shí)，電機(jī)向左轉(zhuǎn)動(dòng)，通過(guò)車(chē)輪帶動(dòng)機(jī)器人向左移動(dòng)， 產(chǎn)生一段位移后，使機(jī)器人重新達(dá)到平衡。
本發(fā)明采用紅外傳感器作為姿態(tài)檢測(cè)的信號(hào)輸入，此種方法大大節(jié)約了傳 感器的成本，同時(shí)克服了零點(diǎn)漂移現(xiàn)象，能夠有效準(zhǔn)確的對(duì)姿態(tài)進(jìn)行檢測(cè)。


圖l自平衡機(jī)器人的機(jī)械結(jié)構(gòu)2自平衡機(jī)器人的電氣結(jié)構(gòu)示意圖 圖3自平衡機(jī)器人電氣結(jié)構(gòu)框圖 圖4自平衡機(jī)器人動(dòng)作示意圖
圖5自平衡機(jī)器人控制方法示意圖 圖6自平衡機(jī)器人控制方法原理圖 圖7整個(gè)控制過(guò)程的軟件算法流程圖 圖8 (a)電機(jī)支架結(jié)構(gòu)前視圖 圖8 (b)電機(jī)支架結(jié)構(gòu)上視圖 圖8 (c)電機(jī)支架結(jié)構(gòu)右視圖 圖9 (a)傳感器支架前視圖 圖9 (b)傳感器支架上視圖 圖9 (c)傳感器支架右視圖
具體實(shí)施例方式
下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步說(shuō)明
首先結(jié)合圖l說(shuō)明本發(fā)明的機(jī)械部分。本發(fā)明的機(jī)械結(jié)構(gòu)比較簡(jiǎn)單，成 本低，容易實(shí)現(xiàn)。本機(jī)器人包括兩個(gè)獨(dú)立的輪子，機(jī)器人的輪子l采用的是 硬質(zhì)的塑料，塑料外圈套用橡膠制成的輪胎，比起普通標(biāo)準(zhǔn)的輪子，這樣不 僅能大大減少機(jī)器人的重量，同時(shí)大大減輕了電機(jī)5的負(fù)擔(dān)，從而減低了電 機(jī)5的功率。進(jìn)而可以減少了電機(jī)的成本。
電機(jī)支架3 (參見(jiàn)圖8)采用的是鋁質(zhì)材料，電機(jī)5通過(guò)上下各兩個(gè)固定 螺絲固定到電機(jī)支架3上。電機(jī)支架3通過(guò)左右個(gè)4個(gè)螺絲固定到機(jī)器人主 體支架2的下端，這樣保證了即使機(jī)器人在劇烈運(yùn)動(dòng)時(shí)，機(jī)器人的驅(qū)動(dòng)部分 仍然非常牢固。同時(shí)，電機(jī)支架3又與機(jī)器人電機(jī)5相連，構(gòu)成機(jī)器人的底 盤(pán)及驅(qū)動(dòng)執(zhí)行機(jī)構(gòu)。
機(jī)器人主體支架2的主要材料為優(yōu)質(zhì)的3毫米厚亞格力塑料板(塑料玻 璃板)。這樣的材料大大減輕了機(jī)器人的重量，而且在機(jī)器人發(fā)生傾斜時(shí)，由 于采用較輕的塑料板替代傳統(tǒng)的鋁板，可以使機(jī)器人迅速的恢復(fù)的平衡位置， 給系統(tǒng)變得更加容易控制。同時(shí)，亞格力塑料板方便打孔，可以便于以后的擴(kuò)展功能。
如圖9所示，紅外傳感器支架4連接到機(jī)器人主體支架2的上端，同時(shí),
紅外傳感器支架4的兩端分別連接有一個(gè)紅外傳感器6，構(gòu)成機(jī)器人的姿態(tài)檢 測(cè)部分，為機(jī)器人提供姿態(tài)檢測(cè)的信息。紅外傳感器支架4采用的是鋁質(zhì)的 支架，因?yàn)槿绻捎盟芰喜ＡО?，由于支架較細(xì)，非常容易折斷，因此采用 鋁質(zhì)支架。紅外傳感器支架4由一個(gè)固定螺絲固定到機(jī)器人主體支架2上。
下面結(jié)合圖4說(shuō)明本發(fā)明的電氣部分。本發(fā)明的電氣結(jié)構(gòu)包括兩個(gè)直流 無(wú)刷電機(jī)5、 DSP主控制電路板7、電源管理模塊8和電池9。其中兩個(gè)紅外 反射式傳感器6分別與DSP控制板7的A/D采樣電路相連，A/D采樣電路將 紅外傳感器的模擬信號(hào)轉(zhuǎn)換成數(shù)字信號(hào)，進(jìn)而便于在DSP控制器內(nèi)進(jìn)行處理。 在DSP控制器中將采樣信號(hào)進(jìn)行相應(yīng)的處理后，由DSP輸出電機(jī)控制信號(hào) PWM波，電機(jī)5的驅(qū)動(dòng)電路直接與DSP控制板7相連，接收由DSP發(fā)出的 PWM波控制信號(hào)。驅(qū)動(dòng)電路將信號(hào)進(jìn)行處理后進(jìn)而直接控制電機(jī)5轉(zhuǎn)動(dòng)。
DSP控制板7的核心芯片采用的是TI公司生產(chǎn)TMS320F2812，該處理 器采用的是改進(jìn)的哈佛總線(xiàn)結(jié)構(gòu)，內(nèi)部有數(shù)據(jù)總線(xiàn)和程序總線(xiàn)。時(shí)鐘頻率最 高可達(dá)到150MHZ,完全滿(mǎn)足的自平衡機(jī)器人實(shí)時(shí)性的要求。其中 TMS320C28X系列DSP是目前控制領(lǐng)域最高性能的處理器，具有精度高，速 度快，集成度高等特點(diǎn)。
電機(jī)5是采用著名的集成舵機(jī)生產(chǎn)商FUTABA公司生產(chǎn)的S148型電機(jī)。 該電機(jī)為直流電機(jī)，驅(qū)動(dòng)電路與電機(jī)集成，機(jī)身小巧(40.5X20.0X38.0)并且 性能高，采用5V直流供電，并且配合齒輪減速裝置，最大轉(zhuǎn)速為60rpm，扭 矩為3.40kg-cm，完全滿(mǎn)足機(jī)器人所需要的動(dòng)力性能要求。同時(shí)支持PWM波 控制，可以完成系統(tǒng)的基本要求。
紅外傳感器6采用的是SHARP公司生產(chǎn)的GP2Y0A02YK。該傳感器采用 5V供電，為模擬量輸出，探測(cè)范圍為0cm-150cm，提供寬角度的測(cè)量范圍， 響應(yīng)時(shí)間為39ms，該傳感器可以在極短的時(shí)間內(nèi)采集到信號(hào)，為機(jī)器人提供 了極佳的動(dòng)態(tài)性能。
電源管理模塊8采用的是集成的DC-DC變壓電路，他的主要作用是將電池(12v)轉(zhuǎn)換成DSP控制板7、電機(jī)5、紅外傳感器6所需要的電壓(5v)。 電源管理模塊8所能承受的最大功率是2w，而機(jī)器人實(shí)際工作的最大功率不 足1.5w，所以不會(huì)存在燒毀器件的問(wèn)題。
結(jié)合圖4 6說(shuō)明本發(fā)明的控制過(guò)程當(dāng)機(jī)器人由于外界擾動(dòng)發(fā)生傾斜時(shí)， 紅外傳感器6檢測(cè)到的離地面10的距離信號(hào)將發(fā)生變化，紅外傳感器5將信 號(hào)傳給DSP控制板7， DSP控制板7將采集到的信號(hào)進(jìn)行相應(yīng)的處理后，從 而得出具體的機(jī)器人傾斜的程度以及機(jī)器人傾斜的方向，進(jìn)而通過(guò)處理，將 傾斜的程度以及傾斜的方向轉(zhuǎn)化成用于電機(jī)轉(zhuǎn)動(dòng)控制的PWM波。左右電機(jī)5 的驅(qū)動(dòng)電路接收來(lái)自DSP控制板7的PWM波控制信號(hào)，在經(jīng)過(guò)信號(hào)調(diào)理后， 驅(qū)動(dòng)電機(jī)進(jìn)行轉(zhuǎn)動(dòng)。通過(guò)電機(jī)5的轉(zhuǎn)動(dòng)，帶動(dòng)與電機(jī)5直接相連的車(chē)輪1，使 機(jī)器人達(dá)到平衡。例如當(dāng)機(jī)器人向右邊傾斜時(shí)，機(jī)器人的重心隨之偏離機(jī) 器人的軸線(xiàn)，電機(jī)5亦向右轉(zhuǎn)動(dòng)，通過(guò)車(chē)輪帶動(dòng)機(jī)器人向右移動(dòng)，產(chǎn)生了一 段位移，使機(jī)器人的重心重新回到所在的軸線(xiàn)上，進(jìn)而機(jī)器人重新達(dá)到平衡。
本發(fā)明的DSP控制算法采用的是增量式PID控制。具體方法如圖5所示， 以小車(chē)車(chē)輪1與地面10的切點(diǎn)為原點(diǎn)，分別以水平和豎直方向?yàn)閄,Y軸，建 立平面直角坐標(biāo)系。當(dāng)機(jī)器人發(fā)生傾斜時(shí)，機(jī)器人的重心如圖位置所示，由 于機(jī)器人的傾斜，導(dǎo)致了紅外傳感器6測(cè)得與地面的距離L1、L2發(fā)生了變化， 通過(guò)對(duì)L1、 L2長(zhǎng)度的變化，可以得知機(jī)器人姿態(tài)的信息。設(shè)L0為平衡位置 時(shí)紅外傳感器測(cè)得的與地面的距離，當(dāng)L1〉L0且L2《0時(shí)，機(jī)器人向右發(fā)生 傾斜；當(dāng)LKL0且L2〉L0時(shí)，機(jī)器人向左發(fā)生傾斜。由此，可以判斷出機(jī)器 人傾斜的方向，已決定電機(jī)5的轉(zhuǎn)動(dòng)方向。以機(jī)器人向右發(fā)生傾斜為例，說(shuō) 明通過(guò)測(cè)量L1、 L2的長(zhǎng)度來(lái)決定機(jī)器人傾斜的程度。當(dāng)機(jī)器人發(fā)生傾斜時(shí)， 傾斜程度越大，Ll的長(zhǎng)度越長(zhǎng)，而L2的長(zhǎng)度越短，進(jìn)而通過(guò)增量PID控制 算法得出一組PWM波控制信號(hào)來(lái)控制電機(jī)5，驅(qū)動(dòng)電機(jī)5進(jìn)行轉(zhuǎn)動(dòng)，使機(jī)器 人重新達(dá)到平衡，具體為
其中增量式PID控制算法可簡(jiǎn)單的表述如下
(" = Aw, (A) — Aw, (A: —— 1) A",、(l丄,(HI丄,(A:-l)-4l)+Al丄,("-丄ol + M""—化
72(|丄,(^-1)-丄0| + ^-2)-Z0|) (1=1,2) A"^)表示第k時(shí)刻的控制量，、表示比例系數(shù)，^表示積分系數(shù)，^表 示微分系數(shù)，lA("-Aj表示第k時(shí)刻與平衡位置的誤差。
由此算法可以得出一組PWM波控制信號(hào)來(lái)控制電機(jī)，當(dāng)傾斜程度越大
時(shí)，電機(jī)所產(chǎn)生的扭矩越大，從而克服機(jī)器人傾斜的力矩越大，因而可以通 過(guò)電機(jī)的轉(zhuǎn)動(dòng)使機(jī)器人的重心回到軸線(xiàn)上。
權(quán)利要求
1、一種基于紅外姿態(tài)檢測(cè)的兩輪直立式自平衡機(jī)器人及控制方法，包括主體支架(2)、固定在主體支架(2)下端的電機(jī)支架(3)、固定在電機(jī)支架上的兩個(gè)電機(jī)(5)、連接在兩個(gè)電機(jī)(5)上的輪子(1)，其特征在于還包括主控制電路、固定在主體支架(2)上端的紅外傳感器支架(4)和兩個(gè)紅外傳感器(6)，兩個(gè)紅外傳感器(6)分別固定在紅外傳感器支架(4)的左右兩端，采集與地面之間的距離，兩個(gè)紅外傳感器的輸出端分別與主控制電路的輸入端相連，主控制電路的輸出端與電機(jī)(5)相連。
2、 一種權(quán)利要求1中所述的一種基于紅外姿態(tài)檢測(cè)的兩輪直立式自平衡機(jī)器 人及控制方法，其特征在于，該方法包括以下步驟1) 左右兩個(gè)紅外傳感器分別檢測(cè)與地面之間的距離，并將該距離以電壓 信號(hào)的形式傳送給主控制電路；設(shè)L1為左側(cè)紅外傳感器檢測(cè)到的與地面之間 的距離，設(shè)L2為右側(cè)紅外傳感器檢測(cè)到的與地面之間的距離，L0為機(jī)器人 處于平衡位置時(shí)紅外傳感器檢測(cè)到的與地面之間的距離；2) 主控制電路通過(guò)接收到的距離L1、 L2判斷機(jī)器人的姿態(tài)信息，具體 為當(dāng)LPL0且L2《0時(shí)，判斷機(jī)器人為向右發(fā)生傾斜；當(dāng)LKLO且L2>L0時(shí)，判斷機(jī)器人為向左發(fā)生傾斜；3) 根據(jù)步驟2)得到的機(jī)器人姿態(tài)信息，主控制電路通過(guò)增量PID控制 算法，得出一組PWM波控制信號(hào)來(lái)控制電機(jī)(5)，驅(qū)動(dòng)電機(jī)(5)進(jìn)行轉(zhuǎn)動(dòng)， 使機(jī)器人重新達(dá)到平衡；具體為當(dāng)機(jī)器人向右傾斜時(shí)，電機(jī)(5)向右轉(zhuǎn)動(dòng)，通過(guò)車(chē)輪帶動(dòng)機(jī)器人向右移 動(dòng)，產(chǎn)生一段位移后，使機(jī)器人重新達(dá)到平衡；當(dāng)機(jī)器人向左傾斜時(shí)，電機(jī)(5)向左轉(zhuǎn)動(dòng)，通過(guò)車(chē)輪帶動(dòng)機(jī)器人向左移 動(dòng)，產(chǎn)生一段位移后，使機(jī)器人重新達(dá)到平衡。
全文摘要
本發(fā)明是一種基于紅外姿態(tài)檢測(cè)的兩輪直立式自平衡機(jī)器人及控制方法，屬于機(jī)器人控制領(lǐng)域。本機(jī)器人主要包括兩個(gè)彼此相互獨(dú)立的輪子、兩個(gè)直流電機(jī)、兩個(gè)紅外反射式傳感器和機(jī)器人的主體骨架。同時(shí)，本機(jī)器人還包括信息處理系統(tǒng)，以及電源供電系統(tǒng)。本發(fā)明的方法是通過(guò)設(shè)置與機(jī)器人上部的紅外傳感器檢測(cè)其與地面的距離，從而得出姿態(tài)信號(hào)，并根據(jù)得出的姿態(tài)信號(hào)進(jìn)行信息處理，從而產(chǎn)生電機(jī)控制信號(hào)驅(qū)動(dòng)電機(jī)，使機(jī)器人保持平衡。該方法簡(jiǎn)單易行，能夠準(zhǔn)確的掌握機(jī)器人的姿態(tài)信息，同時(shí)受環(huán)境影響較小，不容易產(chǎn)生零點(diǎn)漂移，并且成本低廉。
文檔編號(hào)B25J19/00GK101537615SQ200910082949
公開(kāi)日2009年9月23日 申請(qǐng)日期2009年4月24日 優(yōu)先權(quán)日2009年4月24日
發(fā)明者航 劉, 武衛(wèi)霞, 胡靜敏, 阮曉鋼 申請(qǐng)人:北京工業(yè)大學(xué)