專利名稱：頂棚投影器輔助導(dǎo)航系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種機(jī)器人導(dǎo)航定位系統(tǒng)，屬于機(jī)器人導(dǎo)航定位技術(shù)領(lǐng)域。

背景技術(shù)：
目前機(jī)器人的導(dǎo)航、定位多依賴機(jī)器人本身攜帶的傳感器(如超聲傳感器，激光傳感器等)，對(duì)環(huán)境信息進(jìn)行感知，而后對(duì)所獲得信息進(jìn)行處理，以決定機(jī)器人的下一步運(yùn)動(dòng)策略。普遍存在的現(xiàn)象為，機(jī)器人自身需進(jìn)行復(fù)雜的邏輯運(yùn)算和數(shù)據(jù)處理，對(duì)機(jī)器人的性能要求較高。同時(shí)，機(jī)器人需攜帶多種傳感器，有的價(jià)格昂貴(如激光傳感器)。
出于降低機(jī)器人導(dǎo)航定位成本的考慮，研究如何利用其他導(dǎo)航、定位手段以減少對(duì)機(jī)器人性能的要求，同時(shí)減少所需傳感器的種類就顯得尤為重要。


發(fā)明內(nèi)容
本發(fā)明針對(duì)機(jī)器人本體在導(dǎo)航過程中存在的需進(jìn)行復(fù)雜運(yùn)算和處理、并需攜帶多種傳感器的問題，提出了一種導(dǎo)航成本低的頂棚投影器輔助導(dǎo)航系統(tǒng)。
本發(fā)明的頂棚投影器輔助導(dǎo)航系統(tǒng)采用以下技術(shù)方案 該頂棚投影器輔助導(dǎo)航系統(tǒng)包括轉(zhuǎn)動(dòng)平臺(tái)、控制器、無線通信模塊、激光器、上位機(jī)和頂棚投影器，頂棚投影器安裝在轉(zhuǎn)動(dòng)平臺(tái)上，頂棚投影器與控制器連接，兩個(gè)步進(jìn)電機(jī)垂直放置在轉(zhuǎn)動(dòng)平臺(tái)內(nèi)，兩個(gè)步進(jìn)電機(jī)的轉(zhuǎn)軸分別與安裝于轉(zhuǎn)動(dòng)平臺(tái)上的兩個(gè)連桿連接，兩個(gè)步進(jìn)電機(jī)與控制器連接，激光器固定于連桿上，上位機(jī)與控制器通過無線通信模塊連接。
控制器采用單片機(jī)STC12C2052，其P3.2～P3.5引腳接限位開關(guān)，由于頂棚投影器包含兩個(gè)步進(jìn)電機(jī)，因此需要四個(gè)限位開關(guān)；P1.0～P1.7引腳接驅(qū)動(dòng)芯片ULN2003A。驅(qū)動(dòng)芯片的輸出接步進(jìn)電機(jī)，單片機(jī)P1.0～P1.7引腳與驅(qū)動(dòng)芯片之間加施密特觸發(fā)器。
頂棚投影器的步進(jìn)電機(jī)帶動(dòng)連桿實(shí)現(xiàn)二自由度旋轉(zhuǎn)，將轉(zhuǎn)動(dòng)平臺(tái)懸掛于墻上時(shí)，裝于旋轉(zhuǎn)平臺(tái)上的連桿可實(shí)現(xiàn)繞底部連接點(diǎn)做墻面所在平面上的旋轉(zhuǎn)和垂直于墻面所在平面的旋轉(zhuǎn)。激光器隨著連桿的運(yùn)動(dòng)，實(shí)現(xiàn)在地面上投射出運(yùn)動(dòng)的光斑，指引機(jī)器人的運(yùn)動(dòng)。
本發(fā)明的工作過程如下 首先為路徑規(guī)劃，上位機(jī)按程序?qū)⒁?guī)劃好的路徑信息經(jīng)過坐標(biāo)轉(zhuǎn)化等換算為相應(yīng)電機(jī)的控制命令，而后將控制命令按無線通信的協(xié)議打包，通過無線通信模塊發(fā)送到頂棚投影器，頂棚投影器將控制命令進(jìn)行解析，提取出各個(gè)步進(jìn)電機(jī)的運(yùn)動(dòng)步數(shù)和方向信息，控制電路依據(jù)控制信息向驅(qū)動(dòng)器發(fā)送脈沖信號(hào)，驅(qū)動(dòng)電機(jī)的轉(zhuǎn)動(dòng)，電機(jī)帶動(dòng)連桿運(yùn)動(dòng)，在頂棚投影器處于開啟狀態(tài)時(shí)即可實(shí)現(xiàn)規(guī)劃的路徑的投射。
本發(fā)明將路徑規(guī)劃功能交由上位機(jī)完成，同時(shí)將規(guī)劃好的路徑信息通過一定方式發(fā)送給投影器，投影器據(jù)此完成路徑信息的投射，指引機(jī)器人的前進(jìn)，使得機(jī)器人無需進(jìn)行路徑規(guī)劃，降低了機(jī)器人本體的數(shù)據(jù)運(yùn)算和處理量，同時(shí)減輕了機(jī)器人所需攜帶的傳感器的種類和數(shù)量，降低了成本，為機(jī)器人提供一種簡(jiǎn)單有效的導(dǎo)航方式。



圖1是本發(fā)明頂棚投影器導(dǎo)航系統(tǒng)的結(jié)構(gòu)原理框圖。
圖2是本發(fā)明頂棚投影器導(dǎo)航系統(tǒng)的控制電路的電氣原理圖。
圖3是本發(fā)明頂棚投影器導(dǎo)航系統(tǒng)的主控制流程圖。
圖4是脈沖產(chǎn)生控制流程圖。
圖5是限位控制流程圖。
圖6是步進(jìn)電機(jī)變速控制流程圖。
圖7是步進(jìn)電機(jī)變速控制的分段處理子流程圖。
表1是上位機(jī)與無線通訊模塊(Zigbee協(xié)調(diào)器)之間通信數(shù)據(jù)格式。
表2是Zigbee終端節(jié)點(diǎn)與投影器控制器間通信的數(shù)據(jù)格式。
圖中，1、上位機(jī)，2、無線通訊模塊，3、單片機(jī)，4、電壓轉(zhuǎn)化模塊，5、開關(guān)電源，6、限位開關(guān)，7、電壓轉(zhuǎn)化模塊，8、CMOS六施密特觸發(fā)器，9、驅(qū)動(dòng)芯片，10、步進(jìn)電機(jī)，11、激光器。

具體實(shí)施例方式 圖1給出了本發(fā)明頂棚投影器導(dǎo)航系統(tǒng)的結(jié)構(gòu)原理。包括轉(zhuǎn)動(dòng)平臺(tái)、控制器、無線通信模塊2、激光器11、上位機(jī)1和頂棚投影器。頂棚投影器安裝在轉(zhuǎn)動(dòng)平臺(tái)上，頂棚投影器與控制器連接，頂棚投影器包括兩個(gè)步進(jìn)電機(jī)10，步進(jìn)電機(jī)型號(hào)為35BY48HJ30，兩個(gè)步進(jìn)電機(jī)垂直放置在轉(zhuǎn)動(dòng)平臺(tái)內(nèi)，兩個(gè)步進(jìn)電機(jī)的轉(zhuǎn)軸分別與安裝于轉(zhuǎn)動(dòng)平臺(tái)上的兩個(gè)連桿連接，兩個(gè)步進(jìn)電機(jī)與控制器連接，控制器采用STC12C2052型單片機(jī)3，激光器11固定于連桿上，上位機(jī)與控制器(單片機(jī)3)通過無線通信模塊2連接。激光器11通過電壓轉(zhuǎn)化模塊7與開關(guān)電源5連接，單片機(jī)3和CMOS六施密特觸發(fā)器8均與開關(guān)電源5連接，無線通訊模塊2通過電壓轉(zhuǎn)化模塊4與開關(guān)電源5連接。
如圖2所示，控制器采用STC12C2052型單片機(jī)3，單片機(jī)3的P3.2～P3.5引腳接限位開關(guān)6，P1.0～P1.7引腳接驅(qū)動(dòng)芯片9，驅(qū)動(dòng)芯片9采用ULN2003A。由于頂棚投影器包含兩個(gè)步進(jìn)電機(jī)11，因此需要四個(gè)限位開關(guān)6。驅(qū)動(dòng)芯片9的輸出接步進(jìn)電機(jī)11。單片機(jī)3的P1.0～P1.7引腳與驅(qū)動(dòng)芯片9之間加CMOS六施密特觸發(fā)器8，即40106芯片。
圖3給出了控制器的主控制流程圖，單片機(jī)3初始化后，開中斷，接收數(shù)據(jù)，當(dāng)數(shù)據(jù)接收完后，判斷數(shù)據(jù)的有效性，若無效，則丟棄數(shù)據(jù)等待下一組數(shù)據(jù)；若有效，則判斷接收數(shù)據(jù)是否為復(fù)位或停止，是則根據(jù)相應(yīng)命令設(shè)置運(yùn)行，否則提取其中的步進(jìn)電機(jī)轉(zhuǎn)動(dòng)步數(shù)和方向信息，確定電機(jī)的引線通電順序，并通過定時(shí)器發(fā)送脈沖，驅(qū)動(dòng)步進(jìn)電機(jī)10轉(zhuǎn)動(dòng)。
圖4給出了脈沖產(chǎn)生控制流程圖。脈沖信號(hào)產(chǎn)生的頻率由定時(shí)器初始值來決定，定時(shí)器T0負(fù)責(zé)產(chǎn)生脈沖。當(dāng)已產(chǎn)生脈沖數(shù)目累計(jì)等于電機(jī)需要轉(zhuǎn)動(dòng)的步數(shù)時(shí)，關(guān)閉定時(shí)器，不再產(chǎn)生脈沖，從而控制電機(jī)停止轉(zhuǎn)動(dòng)。
同時(shí)，為防止導(dǎo)線的纏繞，限制頂棚投影器的轉(zhuǎn)動(dòng)角度，配置了四個(gè)限位開關(guān)6，以保證頂棚投影器在運(yùn)行過程中，即使程序跑飛，程序限位不起作用時(shí)，依然可以通過限位開關(guān)6強(qiáng)制停止頂棚投影器轉(zhuǎn)動(dòng)。
限位控制流程如圖5所示。四個(gè)限位開關(guān)6分別與單片機(jī)3的P3.2、P3.3、P3.4、P3.5口相連，未發(fā)生碰撞時(shí)，限位開關(guān)輸出均為1，若發(fā)生碰撞，則對(duì)應(yīng)端口置0.例如，當(dāng)P3.2＝0時(shí)，說明限位開關(guān)1發(fā)生了碰撞，因此不能繼續(xù)在同一方向上繼續(xù)運(yùn)行，此時(shí)，定時(shí)器不再產(chǎn)生脈沖，若轉(zhuǎn)動(dòng)方向相反，則根據(jù)上位機(jī)發(fā)送的控制命令，繼續(xù)給驅(qū)動(dòng)器發(fā)送脈沖，控制電機(jī)運(yùn)動(dòng)。
驅(qū)動(dòng)芯片9的選擇決定了步進(jìn)電機(jī)10的控制方式，選用ULN2003A驅(qū)動(dòng)芯片，驅(qū)動(dòng)電機(jī)轉(zhuǎn)動(dòng)時(shí)，必須以給電機(jī)各相輪流通電的來控制電機(jī)的運(yùn)行。所選電機(jī)為六線四相電機(jī)，投影器內(nèi)置兩個(gè)電機(jī)，而ULN2003A只有7個(gè)引腳，因此采用兩片ULN2003A芯片來控制頂棚投影器。
步進(jìn)電機(jī)10的變速控制流程如圖6所示。當(dāng)需投射的軌跡為一段斜線時(shí)，兩個(gè)步進(jìn)電機(jī)10需同時(shí)運(yùn)行，但步數(shù)不盡相同，此時(shí)，為達(dá)到同步運(yùn)行的目的，需改變電機(jī)運(yùn)行速度，即改變兩個(gè)驅(qū)動(dòng)器的脈沖頻率。
本發(fā)明通過單片機(jī)3的一個(gè)定時(shí)器來產(chǎn)生兩個(gè)步進(jìn)電機(jī)10的驅(qū)動(dòng)脈沖，定時(shí)器中斷一次，產(chǎn)生一個(gè)脈沖。首先根據(jù)步進(jìn)電機(jī)10運(yùn)行步數(shù)的比值計(jì)算出電機(jī)的速度比值，據(jù)此，分三種情況處理 1)當(dāng)x或y為0時(shí)，例如，當(dāng)x＝0，則x方向步進(jìn)電機(jī)10不轉(zhuǎn)，定時(shí)器每中斷一次，y方向步進(jìn)電機(jī)10轉(zhuǎn)一步； 2)當(dāng)x、y均不為0時(shí)，若x或y為1時(shí)，例如，當(dāng)x＝1，定時(shí)器每中斷1次，y方向步進(jìn)電機(jī)10轉(zhuǎn)一步，定時(shí)器每中斷y+1次，x方向步進(jìn)電機(jī)10轉(zhuǎn)一步； 3)當(dāng)x、y均非1、非0時(shí)，由于此時(shí)x、y沒有公約數(shù)，x、y比值較大時(shí)(如10∶9)，若直接控制定時(shí)器每中斷x次，y方向步進(jìn)電機(jī)10轉(zhuǎn)一步，定時(shí)器每中斷y次，x方向電機(jī)轉(zhuǎn)一步，則每一步中間間隔較長(zhǎng)，導(dǎo)致頂棚投影器外部無明顯運(yùn)行動(dòng)作，導(dǎo)致投影運(yùn)行時(shí)間過長(zhǎng)，不符合快速輔助導(dǎo)航的要求。
針對(duì)x、y均非1、非0的情況，采取分段處理，具體流程如圖7所示。以xstep＞ystep為例，求出xstep與ystep的商t，余數(shù)r，然后將運(yùn)行軌跡分兩段處理，第一段中x方向步進(jìn)電機(jī)10轉(zhuǎn)動(dòng)r*(t+1)步，y方向電機(jī)轉(zhuǎn)動(dòng)r步，步數(shù)之比為(t+1)∶1；第二段中x方向步進(jìn)電機(jī)10轉(zhuǎn)動(dòng)xstep-r*(t+1)步，y方向步進(jìn)電機(jī)10轉(zhuǎn)動(dòng)ystep-r步，步數(shù)之比為t∶1.其中，處理第一段和第二段為先后順序，不能同時(shí)運(yùn)行。
由于頂棚投影器安裝位置較高，通訊方面若采用導(dǎo)線連接的方式，需要配備較長(zhǎng)的導(dǎo)線。導(dǎo)線過長(zhǎng)，一方面導(dǎo)線容易接觸不良，其次需要考慮布線問題，并避免因?qū)Ь€可能引起的問題，本發(fā)明的無線通訊模塊2采用Zigbee無線網(wǎng)絡(luò)，實(shí)現(xiàn)投影器的遠(yuǎn)程通訊，即增加了安全性又保證了安裝的方便性。Zigbee模塊的單片機(jī)采用CC2430。
本發(fā)明中的通信包括上位機(jī)1與Zigbee協(xié)調(diào)器(無線通訊模塊2)間的串行通信、Zigbee協(xié)調(diào)器與終端節(jié)點(diǎn)間的無線通信、Zigbee終端節(jié)點(diǎn)與控制器(單片機(jī)3)之間的串行通信。
1、上位機(jī)與Zigbee協(xié)調(diào)器間通信 本發(fā)明對(duì)投影器通信的實(shí)時(shí)性要求較高，數(shù)據(jù)格式相對(duì)固定，而且數(shù)據(jù)傳輸量不大，考慮Zigbee模塊內(nèi)部芯片的接口，上位機(jī)與Zigbee協(xié)調(diào)器之間采用RS232串行通信。由于Zigbee網(wǎng)絡(luò)之間通信時(shí)需要尋址，故，定義如下表所示通信格式。一個(gè)數(shù)據(jù)包共14個(gè)字節(jié)，112位，其中Zigbee節(jié)點(diǎn)物理地址占八個(gè)字節(jié)，兩個(gè)步進(jìn)電機(jī)的轉(zhuǎn)動(dòng)方向各占一個(gè)字節(jié)，轉(zhuǎn)動(dòng)步數(shù)各占兩個(gè)字節(jié)。
2、Zigbee無線網(wǎng)絡(luò)通信 協(xié)調(diào)器節(jié)點(diǎn)創(chuàng)建Zigbee網(wǎng)絡(luò)，并向上位機(jī)發(fā)送組網(wǎng)成功指示，然后等待終端節(jié)點(diǎn)加入。當(dāng)終端節(jié)點(diǎn)上電后，會(huì)自動(dòng)查找空間中存在的Zigbee網(wǎng)絡(luò)，查找到后加入網(wǎng)絡(luò)，并將終端節(jié)點(diǎn)物理地址發(fā)送給協(xié)調(diào)器，協(xié)調(diào)器把終端節(jié)點(diǎn)的地址信息等通過串口發(fā)送給計(jì)算機(jī)進(jìn)行保存。
3、Zigbee終端節(jié)點(diǎn)與投影器控制器間的通信 將Zigbee終端節(jié)點(diǎn)CC2430的TXD\RXD引腳直接與投影器控制器STC12C2052的RXD\TXD引腳互聯(lián)，不經(jīng)過RS232格式轉(zhuǎn)換，據(jù)此，定義如下表所示通信數(shù)據(jù)格式，一個(gè)數(shù)據(jù)包共6個(gè)字節(jié)，48位，其中兩個(gè)電機(jī)的轉(zhuǎn)動(dòng)方向各占一個(gè)字節(jié)，轉(zhuǎn)動(dòng)步數(shù)各占兩個(gè)字節(jié)。
可通過上位機(jī)1手動(dòng)輸入設(shè)置路徑信息，在數(shù)據(jù)x和y的編輯框中輸入數(shù)據(jù)，由于在初始狀態(tài)下，投影器處于復(fù)位狀態(tài)，即所投光斑點(diǎn)在原點(diǎn)處，故輸入數(shù)據(jù)時(shí)，第一組數(shù)據(jù)需為0，0.點(diǎn)擊存儲(chǔ)按鈕，即可將數(shù)據(jù)存儲(chǔ)并顯示在下方網(wǎng)格中。此外，可以點(diǎn)擊“讀入”按鈕，實(shí)現(xiàn)將數(shù)據(jù)庫(kù)中規(guī)劃好的路徑信息讀入并顯示在網(wǎng)格中。
當(dāng)數(shù)據(jù)在網(wǎng)格中顯示出來后，點(diǎn)擊“運(yùn)行”按鈕，則系統(tǒng)自動(dòng)將網(wǎng)格中第一組數(shù)據(jù)作為起始點(diǎn)，第二組數(shù)據(jù)作為目標(biāo)點(diǎn)，經(jīng)過坐標(biāo)轉(zhuǎn)化，換算出起始點(diǎn)和目標(biāo)點(diǎn)之間的電機(jī)轉(zhuǎn)動(dòng)步數(shù)并顯示在界面上，當(dāng)轉(zhuǎn)動(dòng)步數(shù)為正時(shí)，電機(jī)按順時(shí)針轉(zhuǎn)動(dòng)，當(dāng)轉(zhuǎn)動(dòng)步數(shù)為負(fù)時(shí)，轉(zhuǎn)動(dòng)方向?yàn)槟鏁r(shí)針。同時(shí)，將電機(jī)轉(zhuǎn)動(dòng)步數(shù)和方向信息按表1格式生成數(shù)據(jù)包，發(fā)送給Zigbee協(xié)調(diào)器。并啟動(dòng)延時(shí)程序，延時(shí)時(shí)間按所發(fā)送的電機(jī)轉(zhuǎn)動(dòng)步數(shù)，電機(jī)需運(yùn)行的時(shí)間設(shè)定。
延時(shí)完畢后，自動(dòng)將當(dāng)前目標(biāo)點(diǎn)取為起始點(diǎn)，當(dāng)前目標(biāo)點(diǎn)下一組數(shù)據(jù)取為目標(biāo)點(diǎn)，再次計(jì)算電機(jī)轉(zhuǎn)動(dòng)步數(shù)、向Zigbee協(xié)調(diào)器發(fā)送數(shù)據(jù)包。
依次計(jì)算并發(fā)送數(shù)據(jù)，當(dāng)最后一組數(shù)據(jù)設(shè)置為起始點(diǎn)后，所有數(shù)據(jù)發(fā)送完畢，電機(jī)停止轉(zhuǎn)動(dòng)。
期間點(diǎn)擊“停止”按鈕，則向Zigbee協(xié)調(diào)器發(fā)送兩個(gè)電機(jī)轉(zhuǎn)動(dòng)步數(shù)為0的數(shù)據(jù)包，可讓投影器立即停止轉(zhuǎn)動(dòng)。
當(dāng)步進(jìn)電機(jī)10停止運(yùn)動(dòng)后，點(diǎn)擊“復(fù)位”按鈕，可使頂棚投影器復(fù)位。
權(quán)利要求
1.一種頂棚投影器輔助導(dǎo)航系統(tǒng)，包括轉(zhuǎn)動(dòng)平臺(tái)、控制器、無線通信模塊、激光器、上位機(jī)和頂棚投影器，其特征是頂棚投影器安裝在轉(zhuǎn)動(dòng)平臺(tái)上，頂棚投影器與控制器連接，兩個(gè)步進(jìn)電機(jī)垂直放置在轉(zhuǎn)動(dòng)平臺(tái)內(nèi)，兩個(gè)步進(jìn)電機(jī)的轉(zhuǎn)軸分別與安裝于轉(zhuǎn)動(dòng)平臺(tái)上的兩個(gè)連桿連接，兩個(gè)步進(jìn)電機(jī)與控制器連接，激光器固定于連桿上，上位機(jī)與控制器通過無線通信模塊連接。
2.根據(jù)權(quán)利要求1所述的頂棚投影器輔助導(dǎo)航系統(tǒng)，其特征是所述控制器采用單片機(jī)。
全文摘要
本發(fā)明提出了一種頂棚投影器輔助導(dǎo)航系統(tǒng)，包括轉(zhuǎn)動(dòng)平臺(tái)、控制器、無線通信模塊、激光器、上位機(jī)和頂棚投影器，頂棚投影器安裝在轉(zhuǎn)動(dòng)平臺(tái)上，頂棚投影器與控制器連接，兩個(gè)步進(jìn)電機(jī)垂直放置在轉(zhuǎn)動(dòng)平臺(tái)內(nèi)，兩個(gè)步進(jìn)電機(jī)的轉(zhuǎn)軸分別與安裝于轉(zhuǎn)動(dòng)平臺(tái)上的兩個(gè)連桿連接，兩個(gè)步進(jìn)電機(jī)與控制器連接，激光器固定于連桿上，上位機(jī)與控制器通過無線通信模塊連接。本發(fā)明將路徑規(guī)劃功能交由上位機(jī)完成，同時(shí)將規(guī)劃好的路徑信息通過一定方式發(fā)送給投影器，投影器據(jù)此完成路徑信息的投射，指引機(jī)器人的前進(jìn)，使得機(jī)器人無需進(jìn)行路徑規(guī)劃，降低了機(jī)器人本體的數(shù)據(jù)運(yùn)算和處理量，同時(shí)減輕了機(jī)器人所需攜帶的傳感器的種類和數(shù)量，降低了成本。
文檔編號(hào)G01C21/34GK101713662SQ200910246578
公開日2010年5月26日 申請(qǐng)日期2009年11月26日 優(yōu)先權(quán)日2009年11月26日
發(fā)明者田國(guó)會(huì), 張濤濤, 郝君勇, 吳皓, 張治慧, 臺(tái)宏達(dá) 申請(qǐng)人:山東大學(xué)