[發(fā)明專利]一種單片機(jī)機(jī)器人直線導(dǎo)航方法

在審

• 申請?zhí)枺?/td>: 201710156788.9
• 申請日: 2017-03-16
• 公開（公告）號: CN107132839A
• 公開（公告）日: 2017-09-05
• 發(fā)明（設(shè)計）人: 沙煉;陳江;李昀達(dá);徐燁
• 申請（專利權(quán)）人: 浙江工業(yè)大學(xué)
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 杭州天正專利事務(wù)所有限公司33201
• 代理人: 王兵,黃美娟

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

2.根據(jù)權(quán)利要求1所述的單片機(jī)機(jī)器人直線導(dǎo)航方法，其特征在于：步驟1所述的機(jī)器在初始位置校零是機(jī)械校零或按鍵校零；具體包括：

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

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

3.根據(jù)權(quán)利要求1所述的單片機(jī)機(jī)器人直線導(dǎo)航方法，其特征在于：步驟2所述的獲取慣導(dǎo)傳感器值，具體包括：

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

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

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

4.根據(jù)權(quán)利要求1所述的單片機(jī)機(jī)器人直線導(dǎo)航方法，其特征在于：步驟3所述的濾波是所述軟件濾波，具體包括：

31.先連續(xù)取N個采樣值看成一個隊列；

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

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

5.根據(jù)權(quán)利要求1所述的單片機(jī)機(jī)器人直線導(dǎo)航方法，其特征在于，步驟4所述的方向環(huán)包括：

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

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

6.根據(jù)權(quán)利要求1所述的單片機(jī)機(jī)器人直線導(dǎo)航方法，其特征在于：步驟5所述的速度環(huán)包括：

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

52.再計算此次的速度偏差；同時將此次的偏差與上次的偏差做差；

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

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

7.根據(jù)權(quán)利要求1所述的單片機(jī)機(jī)器人直線導(dǎo)航方法，其特征在于：步驟5所述的方向環(huán)和速度環(huán)一起調(diào)節(jié)機(jī)器直線運(yùn)行包括：

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

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

8.根據(jù)權(quán)利要求1所述的單片機(jī)機(jī)器人直線導(dǎo)航方法，其特征在于：步驟6所述的待機(jī)器運(yùn)行一周檢測是否發(fā)生零飄包括：用確定是否零飄模塊判斷運(yùn)行一周是否發(fā)生零飄和校零。