三軸數(shù)字陀螺儀篇

1、三軸數(shù)字陀螺儀手冊(cè)-目錄機(jī)器人衡態(tài)調(diào)整三軸數(shù)字陀螺儀篇北京森漢科技有限公司目錄1 .硬件介紹 11 .陀螺儀芯片 12 .陀螺儀的載板 23 . SHR-8s機(jī)器人三軸分布 34 . STC12C5A60卷片接口 45 .載板實(shí)物 42 .陀螺儀操作 51 .訪問格式 52 .軟件接口 53 .姿勢狀態(tài)分析 51 .自糾正歸位系統(tǒng)介紹 52 .自糾正跳位系統(tǒng)介紹 64 .姿態(tài)調(diào)整程序解析 81 .行走姿態(tài)調(diào)整程序 82 .靜止站立姿態(tài)調(diào)整程序 193 .左右平移姿態(tài)調(diào)整程序 25第1頁三軸數(shù)字陀螺儀手冊(cè)一.硬件介紹1.陀螺儀芯片如下圖1-1所示，三軸數(shù)字陀螺儀 L3G4200D芯片X、Y、Z三

2、軸互相正交分布。第1頁ddRE9PLLFFTGND(TOP VIEW) DIRECTIONS OF THE DETECTABLE ANGULAR RATES亙-BOTTOM VIEWOISVdd IOSCL/SPCSDZVSDI/SDOSDO/SAO圖1-1坐標(biāo)方向及背面圖圖1-1中明確標(biāo)出了 L3G4200D芯片X、Y、Z軸的坐標(biāo)方向及背面圖。本陀螺儀芯片可 供用戶選擇其尺度量程，量程范圍從 250 dps2000 dps,低量程數(shù)值用于高精度慢 速運(yùn)動(dòng)測量，而高量程則用于測量超快速的手勢和運(yùn)動(dòng)。這款器件提供一個(gè) 16位數(shù)據(jù)輸出,以及可配置的低通和高通濾波器等嵌入式數(shù)字功能。就算時(shí)間推移或溫

3、度變化，這款器件仍然保持連續(xù)穩(wěn)定的輸出。L3G4200D 特性（ST）:三種可選全尺度（250/500/2000存保計(jì)劃）I2C/SPI數(shù)字輸出接口16比特率值的數(shù)據(jù)輸出8比特溫度數(shù)據(jù)輸出兩個(gè)數(shù)字輸出線（中斷和 data ready ）集成低和高通濾波器的用戶可選帶寬在時(shí)間和溫度上平穩(wěn)嵌入式self-test寬電源電壓，2.4 V至ij 3.6 V低電壓兼容的IOS, 1.8 V嵌入式power-down和睡眠模式嵌入式溫度傳感器嵌入式FIFO緩存三軸數(shù)字陀螺儀手冊(cè)高抗撞擊能力擴(kuò)展的工作溫度范圍（-40 C到+85 C）ECOPACK? RoHS 和 “Green” 認(rèn)證2 .陀螺儀的載板（1

4、）本次采用陀螺儀的iic接口方式，訪問陀螺儀內(nèi)部的三軸數(shù)字值。其每一軸的數(shù)字值均為有符號(hào)的16進(jìn)制數(shù)，以補(bǔ)碼的形式存放。采用單片機(jī)兩個(gè)I/O 口（P4.1與P3.3）模IIC與陀螺儀IIC接口（ SCL、SDA相接，使用100kHz采樣速率對(duì)陀螺儀內(nèi)部寄存器 配置與三軸數(shù)字值的讀取。 其中三軸X、Y、Z數(shù)字值的存放寄存器分別為 OUT_X_L OUT_X_H OUT Y Uf OUT Y H OUT Z Uf OUT Z H只要對(duì)它們進(jìn)行讀取即可。其應(yīng)用載板正面及接線提示圖如下圖1-2所示。圖1-2載板正面圖（2）陀螺儀芯片的載板背面圖如圖1-3所示。圖1-3載板背面圖3 . SHR-8s機(jī)器

5、人三軸分布在此特別的說明一下，這里講到的三軸與L3G4200D芯片載板的三軸完全重合。根據(jù)三軸數(shù)字陀螺儀載板在機(jī)器人身長的插接方式，如下圖1-4所示的坐標(biāo)。Y、Z平成Y、Z平面圖1-4載板在機(jī)器人上的坐標(biāo)圖4 . STC12C5A60S 芯片接口STC12C5A60卷片是SHR-8s人形機(jī)器人的主控板上面的 MCU它負(fù)責(zé)機(jī)器人行走步伐函 數(shù)的發(fā)生，另外還負(fù)責(zé)紅外線接收與識(shí)別，與上位機(jī)三維圖形化軟件進(jìn)行通訊的功能。本次再給它加一個(gè)任務(wù)就是“三軸數(shù)字陀螺儀的數(shù)據(jù)采樣和分析”。所以使用該芯片的IIC采樣端口，對(duì)L3G4200DS行三軸數(shù)字值采樣。具體白端口連接參考下面圖1-5。su 胃一 SRXDI

6、E3IH EG T d K班.科再 HEJF3.7 X1AL5 XIAL1 fD S?F4 0 /P20 戶3用工 AS(I/P1.2 和小， 育吧格用 ”口73 2FE 41A匚4仁口巴反等F) 3MDC 3 /CCro/TSMFl LE口 1.Pl 0J(ftDC0CLKDUT2P4 OE/RW mg。*后艙空C641骷QT如FIT+5SOFT44一VOr43XQVT4WTUI 35鈉TOTTUIrv j ew iH.L2 幽口二eFnatTi?237TK圖1-5 STC12C5A60S芯片接口圖根據(jù)上圖1-5,使用如下的端口協(xié)議：STC12C5A60A宓片：P4.1 載板 SCL;P3.

7、3 -載板 SDA5 .載板實(shí)物載板的實(shí)物圖，如下圖1-6所示。實(shí)物暫時(shí)未出圖1-6載板實(shí)物圖陀螺儀操作本陀螺儀操作接口含有 IIC與SPI兩種，由于考慮到SPI接口多占用I/O 口的問題，所 以此次采用IIC接口方式對(duì)陀螺儀芯片操作， 即一根時(shí)鐘線SCL和一根數(shù)據(jù)線SDA連接陀螺 儀，實(shí)現(xiàn)數(shù)據(jù)傳輸?；跇?biāo)準(zhǔn)IIC通信格式，對(duì)陀螺儀內(nèi)部寄存器進(jìn)行訪問。采樣數(shù)率使用100kHz,分別依次從六個(gè)寄存器(每一軸的數(shù)字值由低 8位與高8位組合)讀取出三軸X、Y、Z的數(shù)字值, 其中六個(gè)寄存器分別為 OUT X L_W OUT X H OUT Y 與 OUT Y H OUT Z Uf OUT Z H2.軟

8、件接口基于標(biāo)準(zhǔn)IIC 的通信協(xié)議，編寫訪問陀螺儀內(nèi)部數(shù)據(jù)的接口子程序(void Read_IIC_XYZAxis(void);其依次讀取出每一軸的高低8位，再分別組合存放于有符號(hào)16 位的整型變量當(dāng)中，共 3 個(gè)變量(L3G4200D_dis_data0 、L3G4200D_dis_data1、 L3G4200D_dis_data2),分別對(duì)應(yīng)于 X、Y、Z三軸的角速度值。由于用于控制機(jī)器人相對(duì) 較快行走，所以降低陀螺儀返回的角速度值，以縮小 100倍處理。三.姿勢狀態(tài)分析機(jī)器人在行走運(yùn)動(dòng)時(shí)欲維持其各種平衡狀態(tài)， 首先要研究立正姿勢下的平衡方程。 由于 立正姿勢本身就是一種靜止?fàn)顟B(tài)， 而且它相

9、對(duì)于所有的穩(wěn)定狀態(tài)都可以直接過渡， 所以要把 立正姿勢下的各種平衡關(guān)系分類研究透徹。首先要探討一下平衡的概念， 所謂的平衡，也就是說系統(tǒng)在一定范圍內(nèi)保持著規(guī)律性的 運(yùn)動(dòng)。比如說鐘擺本身就是一種特殊的平衡，還有像彈簧的往復(fù)震動(dòng)， 小孩子玩的陀螺不停的旋轉(zhuǎn)等等都是一種穩(wěn)定狀態(tài)也就是一種平衡。在人體的研究范疇里面，平衡狀態(tài)的種類是最多的，它的種類要比天上的戰(zhàn)斗機(jī)還多上百倍。經(jīng)?？吹降碾s技演員，在高空做的各種驚險(xiǎn)動(dòng)作，其實(shí)都是在從一個(gè)穩(wěn)定態(tài)變化到另外的一個(gè)穩(wěn)定態(tài)的過程。在這個(gè)過程中，我們發(fā)現(xiàn)過渡過程都是運(yùn)動(dòng)的，而穩(wěn)定態(tài)都是靜止的。不過，我們把各種狀態(tài)歸類總結(jié)成一下2種。1 .自糾正歸位系統(tǒng)介紹在日常生

10、活中經(jīng)常會(huì)碰到類似的情況，正常行走的行人一不留神， 腳踩到一個(gè)西瓜皮后,這個(gè)人掙扎了幾下又直起身來，差一點(diǎn)就摔到了。這種現(xiàn)象歸類為自動(dòng)糾正歸位系統(tǒng)現(xiàn)象， 如下面的流程圖3-1所示。出段穩(wěn)定態(tài) 以是靜 也可以 動(dòng)態(tài)?！胺祷囟巍笔怯?反饋量進(jìn)入系 統(tǒng)而產(chǎn)生。k態(tài) 是“跳出段”是由 外界干擾系統(tǒng) 而產(chǎn)生。加急定態(tài)(A)過渡態(tài)過渡態(tài)都是動(dòng)態(tài)。圖3-1當(dāng)穩(wěn)定的系統(tǒng)受到外界干擾的時(shí)候，自糾正歸位系統(tǒng)圖系統(tǒng)就面臨著失穩(wěn)的風(fēng)險(xiǎn)。如果系統(tǒng)的自糾正能力上面的例子里，正常行走的行很強(qiáng)，那么他就會(huì)很快地進(jìn)行自我糾正并回到初始的穩(wěn)定態(tài)。人就是一個(gè)穩(wěn)定態(tài)，它本身就是動(dòng)態(tài)平衡的，行走本身就是一種周期性的運(yùn)動(dòng)。突然踩到一 個(gè)

11、西瓜皮代表一個(gè)很強(qiáng)的外界干擾因素隨機(jī)性地進(jìn)入系統(tǒng)，行人馬上會(huì)手舞足蹈地翻騰起 來，因?yàn)樗幌胨さ?。這個(gè)翻騰的過程就是過度態(tài)，它相對(duì)來說會(huì)比較短暫。隨后這個(gè)人依 靠軀干和四肢的運(yùn)動(dòng)，保持住了自己的重心，沒有摔倒。最后，他又繼續(xù)向前行走。我們把 踩到西瓜皮的瞬間歸為跳出段，把四肢翻騰的過程歸為返回段。2 .自糾正跳位系統(tǒng)介紹還是剛才的那個(gè)行人，可是這回的情況不同了，這次他最后摔倒了。這個(gè)行人，他起初 是在正常行走，一不留神，腳踩到一個(gè)西瓜皮，這個(gè)人掙扎了幾下卻無濟(jì)于事，結(jié)果就摔到了。這種現(xiàn)象歸類為自動(dòng)跳位系統(tǒng)現(xiàn)象，如下面的流程圖3-2所示。當(dāng)穩(wěn)定的系統(tǒng)受到外界干擾的時(shí)候,穩(wěn)定態(tài)( 可以是 態(tài)也可

12、是動(dòng)態(tài)。一B)靜以系統(tǒng)就面臨著失穩(wěn)的風(fēng)險(xiǎn)。如果系統(tǒng)的自糾正能力在某一方面不是很強(qiáng)，那么它就會(huì)被另外的穩(wěn)定態(tài)所代替。在這個(gè)例子里，正常行走的行人就是穩(wěn)定態(tài)（A）。突然踩到一個(gè)西瓜皮代表一個(gè)很強(qiáng)的外界干擾因素隨機(jī)性地進(jìn)入系統(tǒng)，行人馬上會(huì)四肢翻騰，這個(gè)翻騰的過程就是過度態(tài)。行人在拼命保持自己的重心，但是最后他 還是摔倒了。摔倒后，坐在了地上就是穩(wěn)定態(tài) （B）。我們把踩到西瓜皮的瞬間歸類為跳出段， 把四肢翻騰但是仍然倒下的過程歸類為跳位段。參考以上的兩個(gè)自糾正系統(tǒng)的特點(diǎn)，下面進(jìn)行研究“人體立正姿勢、雙腿劈叉姿勢、單腿站立姿勢、原地踏步姿勢、雙足行走姿勢等”各種姿勢的穩(wěn)定性和過渡性的特點(diǎn)。本次使用陀螺儀

13、，就是用于控制機(jī)器人處于這兩種自糾正系統(tǒng)下維持它的穩(wěn)態(tài)平衡。也就是，在每一種穩(wěn)態(tài)情況下，在一定較小時(shí)間內(nèi)啟動(dòng)陀螺儀維持該穩(wěn)態(tài)平衡，使其當(dāng)前穩(wěn)態(tài)的姿勢狀況不變。比如機(jī)器人在行走一步時(shí)，走完前半步處于穩(wěn)態(tài) （A）,再走完下半步處于穩(wěn)態(tài)（B）。在前半步的穩(wěn)態(tài)（A時(shí)，啟動(dòng)陀螺儀及時(shí)修正該穩(wěn)態(tài)的偏移，以維持該狀態(tài)不 變。下半步依然如此。如果機(jī)器人處于站立等靜止?fàn)顟B(tài)時(shí)，就等同處于一種穩(wěn)態(tài)的情況，依 然啟動(dòng)陀螺儀及時(shí)修正偏移，以維持穩(wěn)態(tài)的姿勢。 欲使機(jī)器人所有運(yùn)動(dòng)姿勢正?；?，首先調(diào)節(jié)好機(jī)器人所有運(yùn)動(dòng)姿勢中每一種穩(wěn)態(tài)正常化。如下面流程圖3-3所示。BAA是靜態(tài)也 可以是動(dòng)穩(wěn)定態(tài)（B 可以是靜態(tài) 也可以是動(dòng)加急

14、定態(tài)（B“跳位段”也是由反饋量 進(jìn)入系統(tǒng)而產(chǎn)生。段圖3-3陀螺儀應(yīng)用系統(tǒng)圖四.姿態(tài)調(diào)整程序解析1 .行走姿態(tài)調(diào)整程序在機(jī)器人行走過程中，當(dāng)處在雙腿著地某一姿態(tài)時(shí)，循環(huán)啟動(dòng)陀螺儀周期性地采集三軸 數(shù)字陀螺儀的三軸數(shù)字值，緊接著存儲(chǔ)在數(shù)組L3G4200D_dis_data3變量當(dāng)中，并比較數(shù)組L3G4200D_dis_data3與對(duì)應(yīng)設(shè)定角速度幅限值，依據(jù)是否超幅限值改變相應(yīng)從0至U11號(hào)舵機(jī)的值，調(diào)整機(jī)器人姿勢使其保持當(dāng)前姿態(tài)穩(wěn)定。當(dāng)機(jī)器人保持在其當(dāng)前姿態(tài)時(shí)，就退出循環(huán)，繼續(xù)行走，同樣也依舊進(jìn)入下一個(gè)姿態(tài)當(dāng)中去。若在某一姿態(tài)的平衡狀態(tài)被迫改變，就會(huì)同樣進(jìn)入循環(huán)當(dāng)中，及時(shí)修正機(jī)器人的姿勢。本調(diào)整程

15、序工作邏輯方式是，快速反饋， 快速回程。主要使用的函數(shù)名稱列舉如下：void Read_IIC_XYZAxis（void） /采集三軸數(shù)字值函數(shù)void ExecuteL3G4200D_TwoSetDown（char FB_Amplitude, char LR_Amplitude, uchar delays_value）/調(diào)整姿勢函數(shù)參數(shù)：FB_Amplitude -前后擺動(dòng)旋轉(zhuǎn)角速度幅限值（陀螺儀Y軸旋轉(zhuǎn)角速度）LR_Amplitude -左右擺動(dòng)旋轉(zhuǎn)角速度幅限值（陀螺儀X軸旋轉(zhuǎn)角速度） delays_value 行走速度值（一個(gè)值代表 500us延時(shí)）此三個(gè)參數(shù) FB_Amplitude、

16、LR_Amplitude、delays_value 分別由無線遙控器調(diào)節(jié)設(shè)定。參數(shù)在遙控器屏幕上顯示位置與相應(yīng)調(diào)整按鈕調(diào)整功能，如下從圖4-1到圖4-4所示。第28頁參：FB_Amplitude參學(xué)LR_Amplitude圖4-1參數(shù)顯示位置圖圖4-2進(jìn)入與退出參數(shù)模式圖按一下25鍵, 就減小速度參 數(shù)一個(gè)值，但 速度就快一個(gè) 值。按一下 26 鍵，就增加速 度參數(shù)一個(gè) 值，但速度就 慢一個(gè)值。圖4-3速度參數(shù)設(shè)置圖按一下12 鍵，就減小 一個(gè)左右擺 動(dòng)旋轉(zhuǎn)角速 度幅限值。按一下11 鍵，就減小 一個(gè)前后擺 動(dòng)旋轉(zhuǎn)角速 度幅限值。按一下 16 鍵，就增加 一個(gè)前后擺 動(dòng)旋轉(zhuǎn)角速 度幅限值。陀螺

17、儀是 否插上？退出調(diào)整函數(shù)按一下 15 鍵，就增加 一個(gè)左右擺 動(dòng)旋轉(zhuǎn)角速 度幅限值。圖4-4陀螺儀參數(shù)設(shè)定圖注意：機(jī)器人在行走當(dāng)中每一種半步穩(wěn)態(tài)，只需調(diào)用該ExecuteL3G4200D_TwoSetDown函數(shù)，即可調(diào)整行走當(dāng)中因慣性導(dǎo)致機(jī)器人身體前后左右的偏移，前提是機(jī)器人的行走步伐要無誤。其中，該姿態(tài)調(diào)整函數(shù) ExecuteL3G4200D_TwoSetDown的實(shí)現(xiàn)流程圖4-5所示。開始保存臨時(shí)狀態(tài)讀取陀螺儀三軸數(shù)字值否前擺動(dòng)角速度超限否？J是調(diào)整舵機(jī)從 0到5 號(hào)的值，使機(jī)器人往 后傾移后擺動(dòng)角速 度超限否？標(biāo)志前后穩(wěn)定調(diào)整舵機(jī)從 0到5 號(hào)的值，使機(jī)器人往 前傾移否左擺動(dòng)角速度超

18、限否？調(diào)整舵機(jī)從8到11 號(hào)的值，使機(jī)器人往 右傾移標(biāo)志左右穩(wěn)定右擺動(dòng)角速度超限否？均穩(wěn)定?調(diào)整舵機(jī)從8到11 號(hào)的值，使機(jī)器人往 左傾移圖4-5行走姿態(tài)調(diào)整流程圖該姿態(tài)調(diào)整函數(shù)中各塊代表代碼如下：陀螺儀是 否插上？該段代碼用于判斷陀螺儀是否插上, 調(diào)整函數(shù)，其如下：if(L3G_flag_plug1 = 1)/*if(psx_mode != 0xFF)L3G_Delay10ms();if(flag_acc_enable = 1) adc_adjust(); */return;未插上則退出保存臨時(shí)狀態(tài)該段代碼用于保存機(jī)器人當(dāng)前所處某一姿勢時(shí)相關(guān)要調(diào)整的舵機(jī)各值，其中舵機(jī)有從0號(hào)到11號(hào), 除了

19、 6號(hào)和7號(hào)舵機(jī)之外。具體代碼如下：position_temp0 = position0;position_temp1 = position1;position_temp2 = position2;position_temp3 = position3;position_temp4 = position4;position_temp5 = position5;position_temp8 = position8;position_temp9 = position9;position_temp10 = position10;position_temp11 = position11;讀取陀螺儀三軸數(shù)

20、 字值該段代碼用于調(diào)用讀取出陀螺儀三軸數(shù)字值的處 理函數(shù)，其如下：Read_IIC_XYZAxis();前擺動(dòng)角速 度超限否？I是調(diào)整舵機(jī)從0到5 號(hào)的值，使機(jī)器人往 后傾移后擺動(dòng)角速度超限否？是調(diào)整舵機(jī)從 0到5 號(hào)的值，使機(jī)器人往 前傾移該段代碼用于判斷機(jī)器人當(dāng)前所處某一姿勢時(shí) 是否發(fā)生向前傾移，如傾移則調(diào)整舵機(jī)從0號(hào)到5號(hào)的值，使機(jī)器人向后傾移。具體代碼如 下：if(L3G4200D_dis_data1FB_Amp1) / 向后傾 position0 -= 1; position1 -= 1; position2 += 1; position3 += 1; position4 += 1;

21、 position5 -= 1; PWM_24();Lowlevel_500u(delays_value); else over_leftflag |= 0x01;左擺動(dòng)角速 度超限否？丫是調(diào)整舵機(jī)從8到11 號(hào)的值，使機(jī)器人往 右傾移右擺動(dòng)角速 度超限否？是調(diào)整舵機(jī)從8到11 號(hào)的值，使機(jī)器人往 左傾移該段代碼用于判斷機(jī)器人當(dāng)前所處某一姿勢時(shí) 是否發(fā)生向左傾移，如傾移則調(diào)整舵機(jī)從 8號(hào) 到11號(hào)的值，使機(jī)器人向右傾移。具體代碼如 下：if(L3G4200D_dis_data0 LR_Amp1)/右偏position8 -= 1;position10 -= 1;position11 += 1;

22、 /*position9 += 1; /*PWM_24();Lowlevel_500u(delays_value);else over_leftflag |= 0x02;恢復(fù)臨時(shí)狀態(tài)該段代碼用于恢復(fù)機(jī)器人調(diào)整前所處某一姿勢，其中需要恢復(fù)的舵機(jī)有從0號(hào)到11號(hào)，除了 6號(hào)和7號(hào)舵機(jī)之外。 具體代碼如下：if(over_leftflag = 0x03) position0 = position_temp0; position1 = position_temp1;/ position2 = position_temp2; position3 = position_temp3; position4 =

23、 position_temp4;/ position5 = position_temp5; position8 = position_temp8; position10 = position_temp10;position9 = position_temp9;positional = position_temp11;PWM_24();Lowlevel_500u(delays_value); break;以下表為在木地板上不同速度下行走時(shí)參考參數(shù):環(huán) 境速度 值S前后擺動(dòng)旋轉(zhuǎn)角速度幅限值X左右擺動(dòng)旋轉(zhuǎn)角速度幅限值Y備注在 木 地 板 上( 稍 光滑)臨界 值較佳 值臨界 值較佳 值(1)當(dāng)要左右

24、平移或者轉(zhuǎn)動(dòng)時(shí)， 建議將速度 值增大到5以上，避免機(jī)器人因自身產(chǎn)生較 大的慣性力而使其平移或轉(zhuǎn)動(dòng)角度過大導(dǎo)致 失常態(tài)。(2)調(diào)節(jié)速度值越小時(shí)，調(diào)整參數(shù)的臨界值 也相應(yīng)變大(各30以上)，較佳值亦同。(3)調(diào)節(jié)速度值越小時(shí)，調(diào)整參數(shù)的臨界值 也相應(yīng)變小(最小到25為止)，較佳值亦同。030453045130453045230453045330453045430453045525402540.3025302530以下表為在水泥地板上不同速度下行走時(shí)參考參數(shù):環(huán) 境速度 值S前后擺動(dòng)旋 轉(zhuǎn)角速度幅限值X左右擺動(dòng)旋轉(zhuǎn)角速度幅限值Y備注在 水 泥 地 板 上臨界 值較佳 值臨界 值較佳 值(4)當(dāng)要左右

25、平移或者轉(zhuǎn)動(dòng)時(shí)，建議將速度值增大到5以上，避免機(jī)器人因自身產(chǎn)生較 大的慣性力而使其平移或轉(zhuǎn)動(dòng)角度過大導(dǎo)致 失常態(tài)。(5)調(diào)節(jié)速度值越小時(shí)，調(diào)整參數(shù)的臨界值 也相應(yīng)變大(35以上)，較佳值亦同。(6)調(diào)節(jié)速度值越小時(shí)，調(diào)整參數(shù)的臨界值 也相應(yīng)變小(最小 30),較佳值亦同。035453545135453545235453545335453545435453545530403040.3030403040以下表為在木板桌上不同速度下行走時(shí)參考參數(shù):環(huán) 境速度 值S前后擺動(dòng)旋轉(zhuǎn)角速度幅限值X左右擺動(dòng)旋轉(zhuǎn)角速度幅限值Y備注在 木 板 桌 上臨界 值較佳 值臨界 值較佳 值(7)當(dāng)要左右平移或者轉(zhuǎn)動(dòng)時(shí)，

26、建議將速度 值增大到5以上，避免機(jī)器人因自身產(chǎn)生較 大的慣性力而使其平移或轉(zhuǎn)動(dòng)角度過大導(dǎo)致 失常態(tài)。(8)調(diào)節(jié)速度值越小時(shí)，調(diào)整參數(shù)的臨界值 也相應(yīng)變大(30以上)，較佳值亦同。(9)調(diào)節(jié)速度值越小時(shí)，調(diào)整參數(shù)的臨界值 也相應(yīng)變小(最小 25),較佳值亦同。030453045130453045230453045330453045430453045525402540.3025402540以下表為在地毯、卵石地板等上不同速度下行走時(shí)參考參數(shù):環(huán) 境速度 值S前后擺動(dòng)旋轉(zhuǎn)角速度幅限值X左右擺動(dòng)旋轉(zhuǎn)角速度幅限值Y備注在 地毯、 卵 石 地 板 等 上臨界 值較佳 值臨界 值較佳 值(10)當(dāng)要左右平移

27、或者轉(zhuǎn)動(dòng)時(shí)，建議將速 度值增大到5以上，避免機(jī)器人因自身產(chǎn)生 較大的慣性力而使其平移或轉(zhuǎn)動(dòng)角度過大導(dǎo) 致失常態(tài)。(11)調(diào)節(jié)速度值越小時(shí)，調(diào)整參數(shù)的臨界 值也相應(yīng)變大(40以上)，較佳值亦同。(12)調(diào)節(jié)速度值越小時(shí)，調(diào)整參數(shù)的臨界 值也相應(yīng)變小(最小 35),較佳值亦同。(13)在這些較為復(fù)雜一點(diǎn)的地面上行走時(shí)，調(diào)節(jié)的參數(shù)均偏大些，至少 40以上，否 則會(huì)出現(xiàn)失常行走。340454045440454045540454045635453545735453545835403540.3035403540參數(shù)調(diào)節(jié)注意事項(xiàng)：(1)當(dāng)機(jī)器人在不如木板那么整平的地面上或者更復(fù)雜的地面上行走的時(shí)候，速 度值

28、參數(shù)盡可能加大，陀螺儀 X、Y值也盡可能加大。也就是，地面越復(fù)雜，越要慢速 行走，否則機(jī)器人就會(huì)失常。(2)機(jī)器人在行走時(shí)，其中某些舵機(jī)有時(shí)會(huì)出現(xiàn)突然抖動(dòng)，會(huì)造成行走常態(tài)突發(fā) 改變，這種情況下最好先讓機(jī)器人停一下再接著行走。出現(xiàn)這種現(xiàn)象，在于機(jī)器人行 走時(shí)某一時(shí)間某些舵機(jī)因供電不足或者線性度不良所導(dǎo)致的。此下，只能調(diào)大參數(shù)， 使機(jī)器人慢速行走。(3)當(dāng)機(jī)器人在正常行走時(shí)，調(diào)節(jié)陀螺儀參數(shù)頭Y基本上在35到50之間變化。當(dāng)出現(xiàn)地面復(fù)雜時(shí)，建議把行走速度S調(diào)到3以上。2 .靜止站立姿態(tài)調(diào)整程序機(jī)器人靜止站立姿態(tài)的調(diào)整原理，等同機(jī)器人在行走過程中當(dāng)雙腿著地時(shí)某一姿態(tài)的調(diào) 整原理，但有其特殊之處。區(qū)別是

29、，當(dāng)靜止?fàn)顟B(tài)被迫改變時(shí)，依舊循環(huán)采集陀螺儀數(shù)值，及 時(shí)判斷是否超限并且修改相應(yīng)舵機(jī)的值，以至使機(jī)器人回歸到靜止站立姿態(tài)，不過使機(jī)器人回歸站立姿態(tài)過程，采用了軟回程的方式，而行走時(shí)采用快速回程的方式。另外，機(jī)器人前后、左右的回程以分開的方式控制。該調(diào)整程序的工作邏輯方式是，快速反饋，緩速回程。主要使用的函數(shù)名稱列舉如下：void Read_IIC_XYZAxis(void) /采集三軸數(shù)字值函數(shù)void ExecuteL3G4200D_TwoStandUp(char FB_Amplitude, char LR_Amplitude, uchar delays_value)/調(diào)整姿勢函數(shù)在 Exec

30、uteL3G4200D_TwoStandUp 調(diào)整姿勢函數(shù)中，三個(gè)參數(shù) FB_Amplitude、 LR_Amplitude、delays_value分別由無線遙控器調(diào)節(jié)設(shè)定。具體調(diào)節(jié)方式，等同上文行走姿態(tài)調(diào)整程序的調(diào)節(jié)方式。該姿態(tài)調(diào)整函數(shù)白實(shí)現(xiàn)流程圖4-6所示。否 一一M退出調(diào)整函數(shù)開始陀螺儀是否插上？是111al1111HB11Mli11nli保存臨時(shí)狀態(tài)讀取陀螺儀數(shù)值是前后回程結(jié)束標(biāo)志置位否？否v否前擺動(dòng)角速I度超限否？是調(diào)整舵機(jī)從 0到5 號(hào)的值，使機(jī)器人往 后傾移否 啟動(dòng)前后的軟回程置前后回程結(jié)束標(biāo)志后擺動(dòng)角速 度超限否？是調(diào)整舵機(jī)從 0到5 號(hào)的值，使機(jī)器人往 前傾移是 jk 左右

31、回程結(jié)束標(biāo)志置位否?否左擺動(dòng)角速 度超限否？調(diào)整舵機(jī)從8、9號(hào) 的值，使機(jī)器人往左 撇腿啟動(dòng)左右的軟回程右擺動(dòng)角速度超限否？置左右回程結(jié)束標(biāo)志調(diào)整舵機(jī)從 10、11 號(hào)的值，使機(jī)器人往 右撇腿前后左右回程標(biāo)志置位否？否 一、返回讀取陀螺儀數(shù)值是退出調(diào)整函數(shù)圖4-6靜止站立姿態(tài)調(diào)整流程圖該姿態(tài)調(diào)整函數(shù)中各塊代表代碼具體如下：陀螺儀是 否插上？該段代碼用于判斷陀螺儀是否插上, 調(diào)整函數(shù)，其如下：if(L3G_flag_plug1 = 1)/*if(psx_mode != 0xFF)L3G_Delay10ms();if(flag_acc_enable = 1) adc_adjust(); */ret

32、urn;未插上則退出保存臨時(shí)狀態(tài)該段代碼用于保存機(jī)器人當(dāng)前所處某一姿勢時(shí)相關(guān)要調(diào)整的舵機(jī)各值，其中舵機(jī)有從0號(hào)到11號(hào), 除了 6號(hào)和7號(hào)舵機(jī)之外。具體代碼如下：position_temp0 = position0;position_temp1 = position1;position_temp2 = position2;position_temp3 = position3;position_temp4 = position4;position_temp5 = position5;position_temp8 = position8;position_temp9 = position9;pos

33、ition_temp10 = position10;position_temp11 = position11;該段代碼用于調(diào)用讀取出陀螺儀三軸數(shù)字值的處理函數(shù)，其如下：Read_IIC_XYZAxis();該段代碼用于檢測與調(diào)整前后的姿態(tài)，需操作的舵機(jī)有從0號(hào)到5號(hào)。其如下：if(L3G4200D_dis_data1 FB_Amp1)/后傾position0 -= 2;position1 -= 1;position2 += 1;position3 += 2;position4 += 1;position5 -= 1;PWM_24();Lowlevel_500u(delays_value);po

34、sition_temp6-; else此處調(diào)用前后軟回程的代碼段三軸數(shù)字陀螺儀手冊(cè)該段代碼用于機(jī)器人調(diào)整姿勢之后，需要恢復(fù)前后方向的舵機(jī)到靜止站立時(shí)的舵機(jī)狀態(tài)，恢復(fù)的舵機(jī)有從0號(hào)啟動(dòng)前后的軟回程到5號(hào)，即恢復(fù)事先保存的臨時(shí)狀態(tài)值，并置前后回程結(jié)束標(biāo)志。具體代碼如下：over_leftflag |= 0x01;if(position_temp6 != 125)position_change0=position_temp0-position0;position_change1=position_temp1-position1;position_change2=position_temp2-posi

35、tion2;position_change3=position_temp3-position3;position_change4=position_temp4-position4;position_change5=position_temp5-position5;if(position_temp6 125) Point_ExecuteFB(delays_value, position_change3+6);elseif(position_temp6125)Point_ExecuteFB(delays_value, position_change0+6);該段代碼用于機(jī)器人調(diào)整姿勢之后，需要恢復(fù)

36、左右方向啟動(dòng)左右的軟回程的舵機(jī)到靜止站立時(shí)的舵機(jī)狀態(tài)，恢復(fù)的舵機(jī)有從8號(hào)到11號(hào)，即恢復(fù)事先保存的臨時(shí)狀態(tài)值，并置左右回程結(jié)束標(biāo)志。具體代碼如下：over_leftflag |= 0x02;position_change8=position_temp8-position8;position_change9=position_temp9-position9;if(position_change8 0)Point_ExecuteLR(delays_value,position_change10);第29頁三軸數(shù)字陀螺儀手冊(cè)左擺動(dòng)角速度超限否？調(diào)整舵機(jī)從8、9號(hào) 的值，使機(jī)器人往左 撇腿右擺動(dòng)角速度

37、超限否？調(diào)整舵機(jī)從 10、11 號(hào)的值，使機(jī)器人往 右撇腿該段代碼用于檢測與調(diào)整左右的姿態(tài)，需操作的舵機(jī)有從8號(hào)到11號(hào)。其如下：if(L3G4200D_dis_data0卜LR_Amp0) 左傾position8 += 3;position9 -= 1;PWM_24();Lowlevel_500u(delays_value); else if(L3G4200D_dis_data0LR_Amp1)右傾position10 -= 3;position11 += 1;PWM_24();Lowlevel_500u(delays_value); else 此處調(diào)用左右軟回程的代碼段 3 .左右平移姿態(tài)

38、調(diào)整程序機(jī)器人左右平移姿態(tài)的調(diào)整原理，等同機(jī)器人在行走過程中當(dāng)雙腿著地時(shí)某一姿態(tài)的調(diào) 整原理，但有其特殊之處。區(qū)別是，當(dāng)每平移完一步處于站立狀態(tài)時(shí)，再做姿態(tài)調(diào)整。在平 移完一步之后的狀態(tài)被迫改變時(shí)，依舊循環(huán)采集陀螺儀數(shù)值，及時(shí)判斷是否超限并且修改相應(yīng)舵機(jī)的值，以至使機(jī)器人回歸到站立姿態(tài)，不過使機(jī)器人回歸站立姿態(tài)過程，前后采用快回程的方式，左右采用了軟回程的方式，與靜止站立狀態(tài)的回程方式一樣，而行走時(shí)則采用了快速回程的方式。另外，機(jī)器人前后、左右的回程以分開的方式控制。該調(diào)整程序的工作邏輯方式是，快速反饋，緩速與快速回程。主要使用的函數(shù)名稱列舉如下：void Read_IIC_XYZAxis(v

39、oid) /采集三軸數(shù)字值函數(shù)void ExecuteL3G4200D_TwoTranslation(charFB_Amplitude, char LR_Amplitude,uchar delays_value)/調(diào)整姿勢函數(shù)在 ExecuteL3G4200D_TwoTranslation調(diào)整姿勢函數(shù)中，三個(gè)參數(shù)FB_Amplitude、LR_Amplitude、delays_value 分別由無線遙控器調(diào)節(jié)設(shè)定。具體調(diào)節(jié)方式，等同上文行走 姿態(tài)調(diào)整程序的調(diào)節(jié)方式。該姿態(tài)調(diào)整函數(shù)白實(shí)現(xiàn)流程圖4-7所示。第38頁是左右回程結(jié)束 標(biāo)志置位否？否否/g/左擺動(dòng)角速一度超限否？是*調(diào)整舵機(jī)從8、9號(hào)

40、的值，使機(jī)器人往左 撇腿右擺動(dòng)角速、否 度超限否？啟動(dòng)左右的軟回程是 調(diào)整舵機(jī)從 10、11 號(hào)的值，使機(jī)器人往 右撇腿置左右回程結(jié)束標(biāo)志前后左右回程標(biāo)志置位否？返回讀取陀螺儀數(shù)值退出調(diào)整函數(shù)圖4-7平移姿態(tài)調(diào)整流程圖該姿態(tài)調(diào)整函數(shù)的各塊代表代碼如下:陀螺儀是 否插上？該段代碼用于判斷陀螺儀是否插上，未插上則退出 調(diào)整函數(shù)，其如下：if(L3G_flag_plug1 = 1)if(psx_mode != 0xFF)L3G_Delay10ms();if(flag_acc_enable = 1)adc_adjust();return;保存臨時(shí)狀態(tài)該段代碼用于保存機(jī)器人當(dāng)前所處某一姿勢時(shí)相關(guān) 要調(diào)整的舵機(jī)各值，其中舵機(jī)有從0號(hào)到11號(hào)，除 了 6號(hào)和7號(hào)舵機(jī)之外。具體代碼如下：position_temp0 = position0;position_temp1 = position1;position_temp2 = position2;position_temp3 = position3;position_temp4 = position4;position_temp5 = position5;position_temp8 = position8;position_temp9 = position9;position_temp