本發(fā)明設(shè)計(jì)機(jī)器人技術(shù)領(lǐng)域，具體而言，涉及一種單片機(jī)機(jī)器人直線導(dǎo)航方法。

技術(shù)背景

目前，隨著信息技術(shù)的快速發(fā)展，機(jī)器人被應(yīng)用在各個(gè)領(lǐng)域，在工業(yè)上，機(jī)器人可以代替人力搬運(yùn)貨物；在生活中，機(jī)器人也可以代替人類執(zhí)勤巡檢等。

當(dāng)前，工業(yè)機(jī)器人搬運(yùn)貨物和機(jī)器人巡檢等，機(jī)器人的直線運(yùn)行是必不可少的，一般機(jī)器人都是用的磁條或激光來導(dǎo)航，但是磁條導(dǎo)航運(yùn)行速度有限，而且需要在地面貼上磁條；而激光導(dǎo)航又成本太高，且易受空中物流或障礙物的影響。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明要克服現(xiàn)有技術(shù)的上述缺點(diǎn)，提供一種單片機(jī)機(jī)器人直線導(dǎo)航方法，實(shí)現(xiàn)通過慣導(dǎo)傳感器和初始導(dǎo)軌確定機(jī)器所處姿態(tài)，并最終通過慣導(dǎo)傳感器與速度采集模塊共同作用來保證機(jī)器的直行。

一種單片機(jī)機(jī)器人直線導(dǎo)航方法，包括如下步驟：

步驟1，將機(jī)器在初始位置校零，

步驟2，獲取慣導(dǎo)傳感器數(shù)值,

步驟3，濾波，

步驟4，得到較為準(zhǔn)確的數(shù)值，并代入方向環(huán)；

步驟5，將方向環(huán)與速度環(huán)結(jié)合起來調(diào)節(jié)機(jī)器直線前進(jìn)

步驟6，待機(jī)器運(yùn)行一周，檢測(cè)是否發(fā)生零飄，若沒有，則轉(zhuǎn)入步驟2；若有，則轉(zhuǎn)入步驟1，以此循環(huán)，通過慣導(dǎo)傳感器和初始導(dǎo)軌確定機(jī)器所處姿態(tài)，并最終通過慣導(dǎo)傳感器與速度采集模塊共同作用來保證機(jī)器的直行。

步驟1所述的機(jī)器在初始位置校零是機(jī)械校零或按鍵校零；具體包括：

11,在初始位置有一段機(jī)械導(dǎo)軌，方便機(jī)器步入導(dǎo)軌機(jī)械校零，避免機(jī)器在初始位置前進(jìn)方向就存在偏差；

12，待機(jī)器姿態(tài)調(diào)正后按鍵復(fù)位校零。

步驟2所述的獲取慣導(dǎo)傳感器值，具體包括：

步驟s1021：采取定時(shí)中斷處理，為數(shù)據(jù)采集和處理提供時(shí)間。

步驟s1022：通過集成電路總線(iic)獲取繞z軸的角度和角速度,iic總線是雙向、兩線(scl、sda)、串行、多主控接口標(biāo)準(zhǔn)，具有總線仲裁機(jī)制；

步驟s1023：等待數(shù)據(jù)進(jìn)一步處理，為濾波做好準(zhǔn)備。

步驟3所述的濾波是所述軟件濾波，具體包括：

31.先連續(xù)取n個(gè)采樣值看成一個(gè)隊(duì)列；

32.每次采樣到一個(gè)新的數(shù)據(jù)放入隊(duì)尾，并扔掉原來隊(duì)首的一次數(shù)據(jù)；

33.最后把隊(duì)列中的n個(gè)數(shù)據(jù)進(jìn)行算術(shù)平均運(yùn)算。

步驟4所述的方向環(huán)包括：

41.先設(shè)置機(jī)器的角度目標(biāo)值，采集濾波結(jié)束后的角度和角速度；

42.再計(jì)算角度的偏差；通過整定參數(shù)kp1和參數(shù)kp2,從而進(jìn)一步調(diào)節(jié)機(jī)器的姿態(tài)。

步驟5所述的速度環(huán)包括：

51.先設(shè)置機(jī)器的速度目標(biāo)值，采集機(jī)器此刻的運(yùn)行速度；

52.再計(jì)算此次的速度偏差；同時(shí)將此次的偏差與上次的偏差做差；

53.通過調(diào)節(jié)參數(shù)kp2和參數(shù)ki2，從而得出速度的增量；

54.最后將此次的速度偏差值賦給上次的偏差，最終達(dá)到控制機(jī)器速度的目的。

步驟5所述的方向環(huán)和速度環(huán)一起調(diào)節(jié)機(jī)器直線運(yùn)行包括：

s51.用第一獲取模塊采集機(jī)器直線運(yùn)行的角度值和角速度值；

s52.用第二獲取模塊獲取機(jī)器的速度值；

步驟6所述的待機(jī)器運(yùn)行一周檢測(cè)是否發(fā)生零飄包括：用確定是否零飄模塊判斷運(yùn)行一周是否發(fā)生零飄和校零。

第一方面，本發(fā)明提供了一種單片機(jī)機(jī)器人直線導(dǎo)航方法，所述方法包括：先將機(jī)器在初始位置校零，獲取慣導(dǎo)傳感器數(shù)值后，經(jīng)濾波得到較為準(zhǔn)確的數(shù)值，并代入方向環(huán)；緊接著將方向環(huán)與速度環(huán)結(jié)合起來調(diào)節(jié)機(jī)器直線前進(jìn)。待機(jī)器運(yùn)行一周，檢測(cè)是否發(fā)生零飄，若沒有，則繼續(xù)運(yùn)行并繼續(xù)讀取慣導(dǎo)傳感器的數(shù)值；若有，則重新校完零再繼續(xù)往下，以此循環(huán)。通過慣導(dǎo)傳感器和初始導(dǎo)軌確定機(jī)器所處姿態(tài)，并最終通過慣導(dǎo)傳感器與速度采集模塊共同作用來保證機(jī)器更快的直行運(yùn)動(dòng)。

結(jié)合第一方面，其中，所述在初始位置校零包括：采取機(jī)械校零和按鍵校零相結(jié)合。在初始位置布有一段機(jī)械導(dǎo)軌，方便機(jī)器步入導(dǎo)軌機(jī)械校零，避免機(jī)器在初始位置前進(jìn)方向就存在偏差；待機(jī)器姿態(tài)調(diào)正后可以按鍵復(fù)位校零。

結(jié)合第一方面，其特征在于，所述獲取慣導(dǎo)傳感器值，包括：進(jìn)入中斷后，通過iic(集成電路總線)獲取繞z軸的角度和角速度；待獲取完數(shù)值后，對(duì)數(shù)值進(jìn)行濾波處理。

結(jié)合第一方面，所述濾波包括：硬件濾波和軟件濾波。本發(fā)明實(shí)例所采取的傳感器內(nèi)部自帶卡爾曼濾波，因此只需考慮軟件濾波。所述軟件濾波方法包括：先連續(xù)取n個(gè)采樣值看成一個(gè)隊(duì)列；每次采樣到一個(gè)新的數(shù)據(jù)放入隊(duì)尾，并扔掉原來隊(duì)首的一次數(shù)據(jù)；最后把隊(duì)列中的n個(gè)數(shù)據(jù)進(jìn)行算術(shù)平均運(yùn)算。

結(jié)合第一方面，所述方向環(huán)包括：先設(shè)置機(jī)器的角度目標(biāo)值，采集濾波結(jié)束后的角度和角速度；再計(jì)算角度的偏差；通過整定參數(shù)kp1和參數(shù)kp2,從而進(jìn)一步調(diào)節(jié)機(jī)器的姿態(tài)。

結(jié)合第一方面，所述速度環(huán)包括：先設(shè)置機(jī)器的速度目標(biāo)值，采集機(jī)器此刻的運(yùn)行速度；再計(jì)算此次的速度偏差；同時(shí)將此次的偏差與上次的做差；通過調(diào)節(jié)參數(shù)kp2和參數(shù)ki2，從而得出速度的增量；最后將此次的速度偏差值賦給上次的偏差。同時(shí)利用定時(shí)中斷重復(fù)以上動(dòng)作。

第二方面，本發(fā)明提供了一種單片機(jī)機(jī)器人直線導(dǎo)航方法，所述方向環(huán)和速度環(huán)一起調(diào)節(jié)機(jī)器直線運(yùn)行包括：第一獲取模塊，用于采集機(jī)器直線運(yùn)行的角度值和角速度值；第二獲取模塊，用于獲取機(jī)器的速度值；以及確定是否零飄模塊，用于判斷運(yùn)行一周是否發(fā)生零飄和校零。

結(jié)合第二方面，所述第一獲取模塊包括：慣導(dǎo)傳感器，用于檢測(cè)機(jī)器所處角度與運(yùn)行的角速度；

濾波單元，用于獲得較為精準(zhǔn)的數(shù)值代入后續(xù)的方向環(huán)。

結(jié)合第二方面，所述第二獲取模塊包括：脈沖采集器(編碼器)，用于采集機(jī)器運(yùn)行的脈沖值，從而得出速度值。電機(jī)單元，用于控制速度輸出。

結(jié)合第二方面，所述的確定是否零飄模塊包括：慣導(dǎo)傳感器，用于采集運(yùn)行一周后，機(jī)器重新回到初始地點(diǎn)時(shí)的角度值。

初始導(dǎo)軌，用于保證機(jī)器運(yùn)行一周后重新回到初始地點(diǎn)時(shí)與出發(fā)時(shí)的位置保持一致，從而方便判斷傳感器是否零飄。

本發(fā)明的優(yōu)點(diǎn)是：提高了機(jī)器直線行走的準(zhǔn)確性，減小了偏離的誤差，從而可以控制機(jī)器更快的到達(dá)目的地。

附圖說明

圖1示出了本發(fā)明方法的流程圖。

圖2示出了獲取慣導(dǎo)傳感器值的流程圖。

圖3示出了軟件濾波的流程圖。

圖4示出了本發(fā)明提供的機(jī)器人直線導(dǎo)航方法中方向環(huán)的流程圖。

圖5示出了本發(fā)明提供的機(jī)器人直線導(dǎo)航方法中速度環(huán)的流程圖。

圖6示出了本發(fā)明提供的機(jī)器人直線導(dǎo)航方法中，方向環(huán)和速度環(huán)一起調(diào)節(jié)機(jī)器直線運(yùn)行的示意圖。

圖7示出了本發(fā)明提供的機(jī)器人直線導(dǎo)航方法中，確定是否零飄模塊的示意圖。

具體實(shí)施方法

下面將結(jié)合本發(fā)明中的附圖，對(duì)本發(fā)明進(jìn)行清楚完整地描述。

考慮到現(xiàn)有技術(shù)中，磁條導(dǎo)航運(yùn)行速度有限，而且需要在地面貼上磁條；而激光導(dǎo)航又成本太高，且易受空中物流或障礙物的影響。基于此，本發(fā)明提供了一種單片機(jī)機(jī)器人直線導(dǎo)航方法，實(shí)現(xiàn)通過慣導(dǎo)傳感器和初始導(dǎo)軌確定機(jī)器所處姿態(tài)，并最終通過慣導(dǎo)傳感器與速度采集模塊共同作用來保證機(jī)器的直行。提高了機(jī)器直線行走的準(zhǔn)確性，減小了偏離的誤差，從而可以控制機(jī)器更快地到達(dá)目的地。

參見圖1，步驟s101：初始校零。采取機(jī)械校零和按鍵校零相結(jié)合。在初始位置布有一段機(jī)械導(dǎo)軌，方便機(jī)器步入導(dǎo)軌機(jī)械校零，避免機(jī)器在初始位置前進(jìn)方向就存在偏差；待機(jī)器姿態(tài)調(diào)正后可以按鍵復(fù)位校零。

步驟s102：獲取慣導(dǎo)傳感器值。由于獲取的方法有很多，且數(shù)據(jù)的獲取和處理也需要時(shí)間，本發(fā)明中根據(jù)以下s1021-s1023實(shí)現(xiàn)獲取慣導(dǎo)傳感器值。

步驟s1021：采取定時(shí)中斷處理，為數(shù)據(jù)采集和處理提供時(shí)間。

步驟s1022：通過iic(集成電路總線)獲取繞z軸的角度和角速度,iic總線是雙向、兩線(scl、sda)、串行、多主控接口標(biāo)準(zhǔn)，具有總線仲裁機(jī)制，非常適合在器件之間進(jìn)行近距離的數(shù)據(jù)通信。在它的協(xié)議體系中，傳輸數(shù)據(jù)時(shí)都會(huì)帶上目的設(shè)備的設(shè)備地址，因此可以方便后期的設(shè)備組網(wǎng)。

步驟s1023：等待數(shù)據(jù)進(jìn)一步處理，為濾波做好準(zhǔn)備。

步驟s103:濾波。濾波包括：硬件濾波和軟件濾波。本發(fā)明所采取的傳感器內(nèi)部自帶卡爾曼濾波，因此只需考慮軟件濾波。所述軟件濾波方法包括s1031-s1033：先連續(xù)取n個(gè)采樣值看成一個(gè)隊(duì)列；每次采樣到一個(gè)新的數(shù)據(jù)放入隊(duì)尾，并扔掉原來隊(duì)首的一次數(shù)據(jù)；最后把隊(duì)列中的n個(gè)數(shù)據(jù)進(jìn)行算術(shù)平均運(yùn)算。此濾波方法對(duì)周期性干擾有良好的抑制作用，平滑度高。

步驟s104：將獲得的較為準(zhǔn)確值代入方向環(huán)。經(jīng)濾波處理后所得到的角度值和角速度值準(zhǔn)確度都將進(jìn)一步提高，而所述的方向環(huán)又如圖4所示。先設(shè)置機(jī)器的角度目標(biāo)值target_angle，采集濾波結(jié)束后的角度angle和角速度gyro；再計(jì)算角度的偏差ε＝target_angle-angle；將kp1*ε+kd1*gyro的計(jì)算結(jié)果賦予a，通過整定參數(shù)kp1和參數(shù)kp2,輸出a，為后續(xù)和速度環(huán)一起調(diào)節(jié)直行做準(zhǔn)備，從而進(jìn)一步調(diào)節(jié)機(jī)器的姿態(tài)。

所述的速度環(huán)如圖5所示：先設(shè)置機(jī)器的速度目標(biāo)值target_speed和初值ε(k-1)＝ε(k)＝0，采集機(jī)器此刻的運(yùn)行速度speed；再計(jì)算此次的速度偏差ε(k)＝target_speed-speed；同時(shí)將此次的偏差與上次的做差ε(k)-ε(k-1)；緊接著計(jì)算增量△u＝kp2*(ε(k)-ε(k-1))+ki2*ε(k)，通過調(diào)節(jié)參數(shù)kp2和參數(shù)ki2，從而得出速度的增量△u；然后將△u+b重新賦給b,最后將此次的速度偏差值賦給上次的偏差。b是最終輸出的速度。

步驟s105：方向環(huán)和速度環(huán)一起調(diào)節(jié)機(jī)器直線運(yùn)行。整個(gè)方案涉及到了：第一獲取模塊s1，用于采集機(jī)器直線運(yùn)行的角度值和角速度值；第二獲取模塊s2，用于獲取機(jī)器的速度值；以及確定是否零飄模塊s3，用于判斷運(yùn)行一周是否發(fā)生零飄和校零。

所述第一獲取模塊s1包括：慣導(dǎo)傳感器，用于檢測(cè)機(jī)器所處角度與運(yùn)行的角速度；濾波單元，用于獲得較為精準(zhǔn)的數(shù)值代入后續(xù)的方向環(huán)。

所述第二獲取模塊s2包括：脈沖采集器(編碼器)，用于采集機(jī)器運(yùn)行的脈沖值，從而得出速度值。電機(jī)單元，用于控制速度的輸出。

步驟s106，待機(jī)器運(yùn)行一周，是否發(fā)生零飄。所述的確定是否零飄模塊s3包括：第一獲取模塊s1，用于采集運(yùn)行一周后，機(jī)器重新回到初始地點(diǎn)時(shí)的角度值。

確定單元s31：初始導(dǎo)軌。用于保證機(jī)器運(yùn)行一周后重新回到初始地點(diǎn)時(shí)與出發(fā)時(shí)的位置保持一致，從而方便判斷傳感器是否零飄。初始導(dǎo)軌上可以布置卡槽等機(jī)構(gòu)，機(jī)器運(yùn)行一周后重新回到初始位置，確保和初始校零時(shí)所處角度和位置一致，此時(shí)只需觀察第一獲取模塊s1的數(shù)值就可以判斷是否發(fā)生零飄。

本發(fā)明實(shí)現(xiàn)通過第一獲取模塊s1,第二獲取模塊s2,確定是否零飄模塊s3共同作用來保證機(jī)器的直行。提高了機(jī)器直線行走的準(zhǔn)確性，減小了偏離的誤差，并且可以控制機(jī)器更快地到達(dá)目的地。

本說明書實(shí)施例所述的內(nèi)容僅僅是對(duì)發(fā)明構(gòu)思的實(shí)現(xiàn)形式的列舉，本發(fā)明的保護(hù)范圍不應(yīng)當(dāng)被視為僅限于實(shí)施例所陳述的具體形式，本發(fā)明的保護(hù)范圍也及于本領(lǐng)域技術(shù)人員根據(jù)本發(fā)明構(gòu)思所能夠想到的等同技術(shù)手段。