本發(fā)明涉及雙輪機(jī)器人控制策略領(lǐng)域，具體涉及一種控制雙輪機(jī)器人直線行走的方法和系統(tǒng)。

背景技術(shù)：

移動(dòng)機(jī)器人因?yàn)樵诟餍懈鳂I(yè)有著廣闊的應(yīng)用前景，已經(jīng)成為機(jī)器人研究領(lǐng)域的一個(gè)重要分支。按其移動(dòng)方式分類，有論事、履帶式、腿式、推進(jìn)式等，其中輪式移動(dòng)機(jī)器人具有成本低、結(jié)構(gòu)和控制簡(jiǎn)單、能量利用率高等特點(diǎn)而得到廣泛應(yīng)用。雙輪機(jī)器人是一種特殊的輪式移動(dòng)機(jī)器人，可以自我平衡，拓展了它適應(yīng)地形變化的能力，運(yùn)動(dòng)靈活、行駛速度和傾斜角度可以調(diào)節(jié)。雙輪機(jī)器人的左右輪分別由一個(gè)電機(jī)驅(qū)動(dòng),依靠差速實(shí)現(xiàn)轉(zhuǎn)向,轉(zhuǎn)向靈活。

隨著輪式機(jī)器人技術(shù)的不斷發(fā)展，兩輪呈左右對(duì)稱布置的兩輪移動(dòng)機(jī)器人被廣泛的應(yīng)用，其優(yōu)點(diǎn)是：左右輪分別由一個(gè)電機(jī)驅(qū)動(dòng),依靠差速實(shí)現(xiàn)轉(zhuǎn)向,轉(zhuǎn)向靈活。

雙輪自平衡機(jī)器人在運(yùn)動(dòng)過程中除了要解決平衡問題，還要解決雙輪機(jī)器人的機(jī)械系統(tǒng)存在的誤差，因?yàn)樵陔p輪機(jī)器人一直運(yùn)行的情況下，雙輪機(jī)器人的積累誤差會(huì)越來越明顯，從而使機(jī)器人無法直線行進(jìn)。尤其在負(fù)重不同的情況下，需要根據(jù)不同負(fù)重自動(dòng)采取不同的控制策略，否則容易造成不同負(fù)重情況下過調(diào)或者欠調(diào)導(dǎo)致的無法直線行走問題。

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

本發(fā)明提供了一種控制雙輪機(jī)器人直線行走的方法和系統(tǒng)，以解決現(xiàn)有的雙輪機(jī)器人機(jī)械系統(tǒng)誤差以及不同負(fù)重導(dǎo)致的雙輪機(jī)器人無法直線行走等問題。

根據(jù)本發(fā)明的一個(gè)方面，提供了一種控制雙輪機(jī)器人直線行走的方法，所述雙輪機(jī)器人內(nèi)設(shè)置有壓力傳感器、陀螺儀和左右電機(jī)，所述方法包括：利用所述陀螺儀獲取所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值，并根據(jù)所述角速度值計(jì)算所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值；

通過所述壓力傳感器獲取所述雙輪機(jī)器人的負(fù)重值，并根據(jù)所述負(fù)重值計(jì)算所述雙輪機(jī)器人進(jìn)行pd直線控制的比例控制參數(shù)p值和微分控制參數(shù)d值；

將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值與預(yù)存的零角度目標(biāo)值之差進(jìn)行所述p值的比例控制，將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值進(jìn)行所述d值的微分控制，計(jì)算所述雙輪機(jī)器人進(jìn)行pd直線控制的輸出量；

將所述輸出量輸出至所述左右電機(jī)，根據(jù)所述輸出量控制所述左右電機(jī)的步進(jìn)數(shù)，以實(shí)現(xiàn)對(duì)所述雙輪機(jī)器人直線行走的控制。

根據(jù)本發(fā)明的另一個(gè)方面，提供了一種控制雙輪機(jī)器人直線行走的系統(tǒng)，所述系統(tǒng)包括陀螺儀、壓力傳感器、左右電機(jī)和處理器；

所述陀螺儀，用于獲取所述雙輪機(jī)器人的繞z軸旋轉(zhuǎn)的角速度值，并將獲取到的所述角速度值發(fā)送給所述處理器；

所述壓力傳感器，用于獲取所述雙輪機(jī)器人的負(fù)重值，并將所述負(fù)重值發(fā)送至所述處理器；

所述處理器，用于接收來自所述陀螺儀的角速度值，根據(jù)所述角速度值計(jì)算雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值；以及用于根據(jù)來自所述壓力傳感器的負(fù)重值計(jì)算所述雙輪機(jī)器人進(jìn)行pd直線控制的比例控制參數(shù)p值和微分控制參數(shù)d值；將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值與預(yù)存的零角度目標(biāo)值之差進(jìn)行所述p值的比例控制，將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值進(jìn)行所述d值的微分控制，計(jì)算所述雙輪機(jī)器人進(jìn)行pd直線控制的輸出量；并將所述輸出量輸出至所述左右電機(jī)；

所述左右電機(jī)，用于根據(jù)來自所述處理器的輸出量控制自身的步進(jìn)數(shù)，實(shí)現(xiàn)雙輪機(jī)器人的直線行走。

本發(fā)明的有益效果是：本發(fā)明的技術(shù)方案實(shí)現(xiàn)了一種雙輪機(jī)器人直線行走的控制策略，利用陀螺儀獲取雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度數(shù)據(jù)，并根據(jù)角速度值計(jì)算雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值；通過壓力傳感器測(cè)得雙輪機(jī)器人的外加負(fù)重大小，并根據(jù)外加負(fù)重大小計(jì)算雙輪機(jī)器人進(jìn)行pd直線控制的比例控制參數(shù)p值和微分控制參數(shù)d值以優(yōu)化pd直線控制；將雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值與預(yù)存的零角度目標(biāo)值之差進(jìn)行p值的比例控制，將雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值進(jìn)行d值的微分控制，計(jì)算雙輪機(jī)器人進(jìn)行pd直線控制的輸出量；將輸出量輸出至左右電機(jī)，根據(jù)輸出量控制左右電機(jī)的步進(jìn)數(shù)，消除了雙輪機(jī)器人機(jī)械系統(tǒng)中存在的誤差，并使得雙輪機(jī)器人可以在不同負(fù)重情況下直線行走。

附圖說明

圖1是本發(fā)明一個(gè)實(shí)施例的一種控制雙輪機(jī)器人直線行走的方法的流程圖；

圖2是本發(fā)明一個(gè)實(shí)施例的一種控制雙輪機(jī)器人直線行走的平滑處理過程的示意圖；

圖3是本發(fā)明一個(gè)實(shí)施例的一種控制控制雙輪機(jī)器人直線行走的系統(tǒng)的示意圖。

具體實(shí)施方式

本發(fā)明的設(shè)計(jì)構(gòu)思是：為了消除了雙輪機(jī)器人機(jī)械系統(tǒng)中存在的誤差，并使得雙輪機(jī)器人可以在不同負(fù)重情況下直線行走，利用陀螺儀獲取雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度數(shù)據(jù)，并計(jì)算雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值；通過壓力傳感器測(cè)得雙輪機(jī)器人的外加負(fù)重大小，自動(dòng)調(diào)節(jié)雙輪機(jī)器人進(jìn)行pd直線控制的比例控制參數(shù)p值和微分控制參數(shù)d值；將雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值與預(yù)存的零角度目標(biāo)值之差進(jìn)行p值的比例控制，將雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值進(jìn)行d值的微分控制，計(jì)算雙輪機(jī)器人進(jìn)行pd直線控制的輸出量；將輸出量輸出至左右電機(jī)，控制左右電機(jī)的步進(jìn)數(shù)。

實(shí)施例一

圖1是本發(fā)明一個(gè)實(shí)施例的一種控制雙輪機(jī)器人直線行走的方法的流程圖，如圖1所示，一種控制雙輪機(jī)器人直線行走的方法，所述雙輪機(jī)器人內(nèi)設(shè)置有壓力傳感器、陀螺儀和左右電機(jī)，所述方法包括：

步驟s110，利用所述陀螺儀獲取所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值，并根據(jù)所述角速度值計(jì)算所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值；

在本發(fā)明的一個(gè)實(shí)施例中，陀螺儀為6軸角速度感測(cè)器(mpu6050)。mpu6050內(nèi)部嵌入了一個(gè)運(yùn)動(dòng)控制引擎(dmp)，通過使用dmp提供的驅(qū)動(dòng)庫，可以直接得到四元數(shù)，利用輸出的四元數(shù)來計(jì)算雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值。利用mpu6050內(nèi)部的運(yùn)動(dòng)引擎直接輸出四元數(shù)，減輕了外圍微處理器的工作負(fù)擔(dān)且避免了繁瑣的濾波和數(shù)據(jù)融合。

步驟s120，通過所述壓力傳感器獲取所述雙輪機(jī)器人的負(fù)重值，并根據(jù)所述負(fù)重值計(jì)算所述雙輪機(jī)器人進(jìn)行pd直線控制的比例控制參數(shù)p值和微分控制參數(shù)d值；

由于單一的控制參數(shù)無法滿足不同的負(fù)重情況下的調(diào)節(jié)需求，即較小的負(fù)重情況下的調(diào)節(jié)相對(duì)于較大負(fù)重情況下的調(diào)節(jié)過小，無法很好的調(diào)節(jié)到直線的效果；而較大負(fù)重情況下的調(diào)節(jié)參數(shù)相對(duì)于較小負(fù)重情況來說調(diào)節(jié)力度過大，容易造成過調(diào)。因此需要根據(jù)負(fù)重情況自動(dòng)調(diào)整控制策略，即調(diào)整pd直線控制參數(shù)。在本發(fā)明的一個(gè)實(shí)施例中，當(dāng)負(fù)重不為零時(shí)，為了簡(jiǎn)化計(jì)算減少系統(tǒng)運(yùn)算資源。根據(jù)公式pout＝kp*weight+p0計(jì)算p值，其中，pout為雙輪機(jī)器人適應(yīng)當(dāng)前負(fù)重的p值，kp為根據(jù)雙輪機(jī)器人的結(jié)構(gòu)和本身重量通過試驗(yàn)標(biāo)定的系數(shù)，weight為所述壓力傳感器獲得的當(dāng)前所述雙輪機(jī)器人的負(fù)重值，p0為雙輪機(jī)器人的負(fù)重為零時(shí)的p值。

根據(jù)公式dout＝kd*weight+d0計(jì)算d值，其中，dout為雙輪機(jī)器人適應(yīng)當(dāng)前負(fù)重的d值，kd為根據(jù)雙輪機(jī)器人的結(jié)構(gòu)和本身重量通過試驗(yàn)標(biāo)定的系數(shù)，weight為所述壓力傳感器獲得的當(dāng)前所述雙輪機(jī)器人的負(fù)重值，d0為雙輪機(jī)器人的負(fù)重為零時(shí)的d值。

需要說明的是，p值和d值這兩個(gè)參數(shù)有正負(fù)、大小之分，可以通過調(diào)試得到負(fù)重為零時(shí)的最佳參數(shù)值p0、d0。通過上述這種方法，實(shí)現(xiàn)了根據(jù)不同負(fù)重情況自動(dòng)調(diào)整pd直線控制參數(shù)，進(jìn)而保證了雙輪機(jī)器人在任何負(fù)重情況下都可以直線行走。

步驟s130，將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值與預(yù)存的零角度目標(biāo)值之差進(jìn)行所述p值的比例控制，將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值進(jìn)行所述d值的微分控制，計(jì)算所述雙輪機(jī)器人進(jìn)行pd直線控制的輸出量；

在本發(fā)明的一個(gè)實(shí)施例中，由于當(dāng)對(duì)雙輪機(jī)器人進(jìn)行pd直線控制時(shí)，只要使雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度為0，則雙輪機(jī)器人一直直線行走，因此將繞z軸旋轉(zhuǎn)的角度與設(shè)定的角度目標(biāo)值之差進(jìn)行p比例控制。為了保持雙輪機(jī)器人行進(jìn)時(shí)走直線，所以將角度目標(biāo)值設(shè)置為0，另外，將繞z軸旋轉(zhuǎn)的角速度進(jìn)行d微分控制，微分控制可以實(shí)現(xiàn)對(duì)干擾做出迅速的響應(yīng)。

根據(jù)公式straightcontrolout＝p*(yaw-car_zero)+d*gyro[2]計(jì)算進(jìn)行pd直線控制的輸出量，其中，straightcontrolout為pd直線控制的輸出量；yaw為所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值；car_zero_angle為預(yù)存的零角度目標(biāo)值；gyro[2]為陀螺儀測(cè)得的雙輪機(jī)器人在行走過程中繞z軸旋轉(zhuǎn)的角速度值；p為根據(jù)所述負(fù)重值計(jì)算得到的比例控制參數(shù)值，d為根據(jù)所述負(fù)重值計(jì)算得到的微分控制參數(shù)值。

步驟s140，將所述輸出量輸出至所述左右電機(jī)，根據(jù)所述輸出量控制所述左右電機(jī)的步進(jìn)數(shù)，以實(shí)現(xiàn)對(duì)所述雙輪機(jī)器人直線行走的控制。

通過圖1所示的方法可知，本發(fā)明的技術(shù)方案利用陀螺儀獲取雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度數(shù)據(jù)，并根據(jù)角速度值計(jì)算雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值；通過壓力傳感器測(cè)得雙輪機(jī)器人的外加負(fù)重大小，并根據(jù)外加負(fù)重大小計(jì)算雙輪機(jī)器人進(jìn)行pd直線控制的比例控制參數(shù)p值和微分控制參數(shù)d值以優(yōu)化pd直線控制；將雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值與預(yù)存的零角度目標(biāo)值之差進(jìn)行p值的比例控制，將雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值進(jìn)行d值的微分控制，計(jì)算雙輪機(jī)器人進(jìn)行pd直線控制的輸出量；將輸出量輸出至左右電機(jī)，根據(jù)輸出量控制左右電機(jī)的步進(jìn)數(shù)，消除了雙輪機(jī)器人機(jī)械系統(tǒng)中存在的誤差，并使得雙輪機(jī)器人可以在不同負(fù)重情況下直線行走。

當(dāng)雙輪機(jī)器人行走在不平坦路面時(shí)，導(dǎo)致左右輪的負(fù)重不同，為了使得雙輪機(jī)器人在不平坦里面依舊能夠直線行走。在本發(fā)明的一個(gè)實(shí)施例中，當(dāng)雙輪機(jī)器人內(nèi)設(shè)置有兩個(gè)壓力傳感器時(shí)，將兩個(gè)壓力傳感器分別對(duì)應(yīng)設(shè)置在雙輪機(jī)器人的左右輪。

利用步驟s110中的方法獲取雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值和角度值；

然后通過兩個(gè)壓力傳感器分別獲取雙輪機(jī)器人的左右輪的負(fù)重值weight1和weight2，并根據(jù)左右輪的負(fù)重值分別計(jì)算左右輪進(jìn)行pd直線控制的比例控制參數(shù)p1＝kp*weight1+p0、p2＝kp*weight2+p0和微分控制參數(shù)d1＝kd*weight1+d0、d2＝kd*weight2+d0；

對(duì)于雙輪機(jī)器人的左輪，分別將雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值與預(yù)存的零角度目標(biāo)值之差進(jìn)行對(duì)應(yīng)p1值的比例控制，將雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值進(jìn)行對(duì)應(yīng)d1值的微分控制，則雙輪機(jī)器人左輪的pd直線控制的輸出量為：

straightcontrolout1＝p1*(yaw-car_zero)+d1*gyro[2]

同理則雙輪機(jī)器人右輪的pd直線控制的輸出量為：

straightcontrolout2＝p2*(yaw-car_zero)+d2*gyro[2]

將straightcontrolout1輸出至雙輪機(jī)器人的左輪，控制左電機(jī)的步進(jìn)數(shù)，將straightcontrolout2輸出至雙輪機(jī)器人的右輪，控制右電機(jī)的步進(jìn)數(shù)，實(shí)現(xiàn)對(duì)雙輪機(jī)器人直線行走的控制。

圖2是本發(fā)明一個(gè)實(shí)施例的一種控制雙輪機(jī)器人直線行走的平滑處理過程的示意圖，如圖2所示，

步驟s210，根據(jù)負(fù)重值計(jì)算直線控制的p值和d值。采用步驟s120中的方法計(jì)算雙輪機(jī)器人負(fù)重時(shí)p值和d值的實(shí)際有效值pou和dout。

步驟s220，計(jì)算負(fù)重前后p值和d值的差值。根據(jù)公式δp＝pout-p0計(jì)算所述雙輪機(jī)器人負(fù)重前后進(jìn)行pd直線控制的p值的差值；其中，δp為所述雙輪機(jī)器人負(fù)重前后進(jìn)行pd直線控制的p值的差值，pout為雙輪機(jī)器人適應(yīng)當(dāng)前負(fù)重的p值；p0為雙輪機(jī)器人的負(fù)重為零時(shí)的p值；同理，根據(jù)公式δd＝dout-d0計(jì)算所述雙輪機(jī)器人負(fù)重前后進(jìn)行pd直線控制的d值的差值。

步驟s230，將負(fù)重前后p和d值的差值與最佳步進(jìn)值的比值累加到p0值。

根據(jù)公式p＝p0+δp/stepp，將所述雙輪機(jī)器人負(fù)重前后進(jìn)行pd直線控制的p值的差值與預(yù)存的相應(yīng)負(fù)重時(shí)左右電機(jī)的最佳步進(jìn)數(shù)的比值累加到雙輪機(jī)器人的負(fù)重為零時(shí)的p值，得到平滑處理后的p值；其中，p為平滑處理后的p值，p0為雙輪機(jī)器人的負(fù)重為零時(shí)的p值,δp為所述雙輪機(jī)器人負(fù)重前后進(jìn)行pd直線控制的p值的差值，stepp為預(yù)存的相應(yīng)負(fù)重時(shí)左右電機(jī)的最佳步進(jìn)數(shù)；同理，根據(jù)公式d＝d0+δd/stepd計(jì)算平滑處理后的d值。

步驟s240，平滑處理后pd直線控制輸出量計(jì)算。

根據(jù)公式bst-fstraightcontrolout＝p*(yaw-car_zero)+d*gyro[2]計(jì)算平滑處理后的直線控制輸出量。

步驟s250，輸出平滑后的直線控制量。將平滑處理后的直線控制量輸出至雙輪機(jī)器人的左右電機(jī)，控制雙輪機(jī)器人左右電機(jī)的步進(jìn)數(shù)。

步驟s250，判斷平滑后的直線控制量對(duì)應(yīng)的步進(jìn)數(shù)是否達(dá)到最佳步進(jìn)數(shù)的上限？也就是說，判斷根據(jù)平滑后的輸出量控制得到的左右電機(jī)的步進(jìn)數(shù)是否達(dá)到預(yù)存的相應(yīng)負(fù)重時(shí)左右電機(jī)的最佳步進(jìn)數(shù)。需要說明的是，最佳步進(jìn)數(shù)是根據(jù)實(shí)際試驗(yàn)測(cè)得的在相應(yīng)負(fù)重情況下雙輪機(jī)器人直線行走時(shí)步進(jìn)電機(jī)的步進(jìn)數(shù)，最佳步進(jìn)數(shù)是預(yù)存在控制系統(tǒng)中的。

步驟s260，若判斷為是，停止p值和d值的累加。若判斷為否，則重復(fù)上述步驟s220到步驟s260的操作。

為了使本方案清楚，以p值的平滑處理為例，若p0＝5，stepp＝10，pou＝15，則計(jì)算出來的δp＝pou-p0＝15-5＝10，則p＝p0+δp/stepp＝5+10/10＝6。以d值的平滑處理為例，若d0＝5，stepp＝10，dou＝25，則計(jì)算出來的δd＝dout-d0＝25-5＝20，則d＝d0+δd/stepd＝5+20/10＝7；由此可見，平滑處理后的p值是按照一定的調(diào)用周期根據(jù)步進(jìn)數(shù)為1的計(jì)數(shù)機(jī)制進(jìn)行平滑；平滑處理后的d值是按照一定的調(diào)用周期根據(jù)步進(jìn)數(shù)為2的計(jì)數(shù)機(jī)制進(jìn)行平滑；由于p值和d值是漸增的，則bst_fstraightcontrolout也是漸增的，則每一次疊加到電機(jī)上的輸入量是緩慢增加的，直到達(dá)到最佳步進(jìn)數(shù)的上限停止p值和d值的平滑處理。利用漸增的bst_fstraightcontrolout來取代突變的直線控制量straightcontrolout疊加到電機(jī)上，優(yōu)化了雙輪機(jī)器人的直線行走控制，并有效緩解了負(fù)重突然增大時(shí)導(dǎo)致的瞬間卡頓問題。

實(shí)施例二

圖3是本發(fā)明一個(gè)實(shí)施例的一種控制控制雙輪機(jī)器人直線行走的系統(tǒng)的示意圖，如圖3所示，一種控制雙輪機(jī)器人直線行走的系統(tǒng)300，所述系統(tǒng)300包括陀螺儀320、壓力傳感器310、左右電機(jī)340和處理器330；

所述陀螺儀320，用于獲取所述雙輪機(jī)器人的繞z軸旋轉(zhuǎn)的角速度值，并將獲取到的所述角速度值發(fā)送給所述處理器；

所述壓力傳感器310，用于獲取所述雙輪機(jī)器人的負(fù)重值，并將所述負(fù)重值發(fā)送至所述處理器330；

所述處理器330，用于接收來自所述陀螺儀的角速度值，根據(jù)所述角速度值計(jì)算雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值；以及用于根據(jù)來自所述壓力傳感器310的負(fù)重值計(jì)算所述雙輪機(jī)器人進(jìn)行pd直線控制的比例控制參數(shù)p值和微分控制參數(shù)d值；將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值與預(yù)存的零角度目標(biāo)值之差進(jìn)行所述p值的比例控制，將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值進(jìn)行所述d值的微分控制，計(jì)算所述雙輪機(jī)器人進(jìn)行pd直線控制的輸出量；并將所述輸出量輸出至所述左右電機(jī)340；

所述左右電機(jī)340，用于根據(jù)來自所述處理器330的輸出量控制自身的步進(jìn)數(shù)，實(shí)現(xiàn)雙輪機(jī)器人的直線行走。

通過圖3所示的方法可知，本發(fā)明的技術(shù)方案利用陀螺儀320獲取雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度數(shù)據(jù)，并根據(jù)角速度值計(jì)算雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值；通過壓力傳感器310測(cè)得雙輪機(jī)器人的外加負(fù)重大小，并根據(jù)外加負(fù)重大小計(jì)算雙輪機(jī)器人進(jìn)行pd直線控制的比例控制參數(shù)p值和微分控制參數(shù)d值以優(yōu)化pd直線控制；將雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值與預(yù)存的零角度目標(biāo)值之差進(jìn)行p值的比例控制，將雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值進(jìn)行d值的微分控制，計(jì)算雙輪機(jī)器人進(jìn)行pd直線控制的輸出量；將輸出量輸出至左右電機(jī)340，根據(jù)輸出量控制左右電機(jī)340的步進(jìn)數(shù)，消除了雙輪機(jī)器人機(jī)械系統(tǒng)中存在的誤差，并使得雙輪機(jī)器人可以在不同負(fù)重情況下直線行走。

當(dāng)雙輪機(jī)器人行走在不平坦路面時(shí)，導(dǎo)致左右輪的負(fù)重不同，為了使得雙輪機(jī)器人在不平坦里面依舊能夠直線行走。在本發(fā)明的一個(gè)實(shí)施例中，所述壓力傳感器310的數(shù)量為兩個(gè)，分別對(duì)應(yīng)所述雙輪機(jī)器人的左右輪設(shè)置；

所述兩個(gè)壓力傳感器310，用于分別獲取所述雙輪機(jī)器人的左右輪的負(fù)重值，并將獲取的負(fù)重值分別傳送至所述處理器330，

所述處理器330，具體用于根據(jù)來自所述兩個(gè)壓力傳感器310的左右輪的負(fù)重值分別計(jì)算左右輪進(jìn)行pd直線控制的比例控制參數(shù)p值和微分控制參數(shù)d值；對(duì)于所述雙輪機(jī)器人的左右輪340，分別將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值與預(yù)存的零角度目標(biāo)值之差進(jìn)行對(duì)應(yīng)所述p值的比例控制，將所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值進(jìn)行對(duì)應(yīng)所述d值的微分控制，分別計(jì)算所述雙輪機(jī)器人左右輪340的pd直線控制的輸出量；并將對(duì)應(yīng)左右輪的兩個(gè)所述輸出量分別輸出至所述左右電機(jī)340；

所述左右電機(jī)340，用于根據(jù)對(duì)應(yīng)的所述輸出量分別控制自身的步進(jìn)數(shù)，實(shí)現(xiàn)所述雙輪機(jī)器人的直線行走。

在本發(fā)明的一個(gè)實(shí)施例中，由于單一的控制參數(shù)無法滿足不同的負(fù)重情況下的調(diào)節(jié)需求，即較小的負(fù)重情況下的調(diào)節(jié)相對(duì)于較大負(fù)重情況下的調(diào)節(jié)過小，無法很好的調(diào)節(jié)到直線的效果；而較大負(fù)重情況下的調(diào)節(jié)參數(shù)相對(duì)于較小負(fù)重情況來說調(diào)節(jié)力度過大，容易造成過調(diào)。因此需要根據(jù)負(fù)重情況自動(dòng)調(diào)整控制策略，即調(diào)整pd直線控制參數(shù)。

所述處理器330，具體用于根據(jù)公式pout＝kp*weight+p0計(jì)算p值，其中，pout為雙輪機(jī)器人適應(yīng)當(dāng)前負(fù)重的p值，kp為根據(jù)雙輪機(jī)器人的結(jié)構(gòu)和本身重量通過試驗(yàn)標(biāo)定的系數(shù)，weight為所述壓力傳感器獲得的當(dāng)前所述雙輪機(jī)器人的負(fù)重值，p0為雙輪機(jī)器人的負(fù)重為零時(shí)的p值；

根據(jù)公式dout＝kd*weight+d0計(jì)算d值，其中，dout為雙輪機(jī)器人適應(yīng)當(dāng)前負(fù)重的d值，kd為根據(jù)雙輪機(jī)器人的結(jié)構(gòu)和本身重量通過試驗(yàn)標(biāo)定的系數(shù)，weight為所述壓力傳感器獲得的當(dāng)前所述雙輪機(jī)器人的負(fù)重值，d0為雙輪機(jī)器人的負(fù)重為零時(shí)的d值；

以及，

所述處理器330，具體用于根據(jù)公式

straightcontrolout＝p*(yaw-car_zero)+d*gyro[2]計(jì)算進(jìn)行pd直線控制的輸出量，其中，straightcontrolout為pd直線控制的輸出量；yaw為所述雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值；car_zero_angle為預(yù)存的零角度目標(biāo)值；gyro[2]為陀螺儀測(cè)得的雙輪機(jī)器人在行走過程中繞z軸旋轉(zhuǎn)的角速度值；p為根據(jù)所述負(fù)重值計(jì)算得到的比例控制參數(shù)值，d為根據(jù)所述負(fù)重值計(jì)算得到的微分控制參數(shù)值。由于當(dāng)對(duì)雙輪機(jī)器人進(jìn)行pd直線控制時(shí)，只要使雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度為0，則雙輪機(jī)器人一直直線行走，因此將繞z軸旋轉(zhuǎn)的角度與設(shè)定的角度目標(biāo)值之差進(jìn)行p比例控制。為了保持雙輪機(jī)器人行進(jìn)時(shí)走直線，所以將角度目標(biāo)值設(shè)置為0，另外，將繞z軸旋轉(zhuǎn)的角速度進(jìn)行d微分控制，微分控制可以實(shí)現(xiàn)對(duì)干擾做出迅速的響應(yīng)。

在本發(fā)明的一個(gè)實(shí)施例中，所述處理器330，具體用于根據(jù)預(yù)存的相應(yīng)負(fù)重時(shí)左右電機(jī)的最佳步進(jìn)數(shù)對(duì)計(jì)算出的p值和d值進(jìn)行平滑處理；根據(jù)平滑處理后的p值和d值計(jì)算進(jìn)行pd直線控制的平滑后的輸出量；將計(jì)算出的所述平滑后的輸出量輸出至所述左右電機(jī)；判斷根據(jù)所述平滑后的輸出量控制得到的所述左右電機(jī)的步進(jìn)數(shù)是否達(dá)到預(yù)存的相應(yīng)負(fù)重時(shí)所述左右電機(jī)的最佳步進(jìn)數(shù)；若是，則停止所述p值和d值的平滑處理；若否，則重復(fù)上述操作；

以及，

所述處理器330，具體用于根據(jù)公式δp＝pout-p0計(jì)算所述雙輪機(jī)器人負(fù)重前后進(jìn)行pd直線控制的p值的差值；其中，δp為所述雙輪機(jī)器人負(fù)重前后進(jìn)行pd直線控制的p值的差值，pout為雙輪機(jī)器人適應(yīng)當(dāng)前負(fù)重的p值；p0為雙輪機(jī)器人的負(fù)重為零時(shí)的p值；

根據(jù)公式p＝p0+δp/stepp，將所述雙輪機(jī)器人負(fù)重前后進(jìn)行pd直線控制的p值的差值與預(yù)存的相應(yīng)負(fù)重時(shí)左右電機(jī)的最佳步進(jìn)數(shù)的比值累加到雙輪機(jī)器人的負(fù)重為零時(shí)的p值，得到平滑處理后的p值；其中，p為平滑處理后的p值，p0為雙輪機(jī)器人的負(fù)重為零時(shí)的p值,δp為所述雙輪機(jī)器人負(fù)重前后進(jìn)行pd直線控制的p值的差值，stepp為預(yù)存的相應(yīng)負(fù)重時(shí)左右電機(jī)的最佳步進(jìn)數(shù)；

同理，根據(jù)公式δd＝dout-d0和d＝d0+δd/stepd計(jì)算平滑處理后的d值。由此可知，采用平滑處理機(jī)制，優(yōu)化了雙輪機(jī)器人的直線行走控制，并有效緩解了負(fù)重突然增大時(shí)導(dǎo)致的瞬間卡頓問題。

綜上所述，本發(fā)明的技術(shù)方案實(shí)現(xiàn)了一種雙輪機(jī)器人直線行走的控制策略，利用陀螺儀獲取雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度數(shù)據(jù)，并根據(jù)角速度值計(jì)算雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值；通過壓力傳感器測(cè)得雙輪機(jī)器人的外加負(fù)重大小，并根據(jù)外加負(fù)重大小計(jì)算雙輪機(jī)器人進(jìn)行pd直線控制的比例控制參數(shù)p值和微分控制參數(shù)d值以優(yōu)化pd直線控制；將雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角度值與預(yù)存的零角度目標(biāo)值之差進(jìn)行p值的比例控制，將雙輪機(jī)器人繞z軸旋轉(zhuǎn)的角速度值進(jìn)行d值的微分控制，計(jì)算雙輪機(jī)器人進(jìn)行pd直線控制的輸出量；將輸出量輸出至左右電機(jī)，根據(jù)輸出量控制左右電機(jī)的步進(jìn)數(shù)，消除了雙輪機(jī)器人機(jī)械系統(tǒng)中存在的誤差，并使得雙輪機(jī)器人可以在不同負(fù)重情況下直線行走。

以上僅為本發(fā)明的較佳實(shí)施例而已，并非用于限定本發(fā)明的保護(hù)范圍。凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換、改進(jìn)等，均包含在本發(fā)明的保護(hù)范圍內(nèi)。