某AGV搬運(yùn)車控制系統(tǒng)的車身姿態(tài)檢測模塊設(shè)計案例目錄TOC\o"1-3"\h\u16380某AGV搬運(yùn)車控制系統(tǒng)的車身姿態(tài)檢測模塊設(shè)計案例 162451傳感器選型 1194342車身姿態(tài)檢測電路 3268293車身姿態(tài)檢測模塊程序 3252824姿態(tài)與轉(zhuǎn)角換算 4241235車身姿態(tài)調(diào)整 6241186CAN接口設(shè)計 8152997人機(jī)接口設(shè)計 9前面已經(jīng)對電機(jī)的運(yùn)動控制作了說明，對于本系統(tǒng)來說，在AGV搬運(yùn)車工作時，懸架系統(tǒng)要能及時調(diào)整車身姿態(tài)保證車輛安全平穩(wěn)運(yùn)行，因此對于懸架系統(tǒng)來說，需要根據(jù)不同的工作場合進(jìn)行調(diào)整，對此下面將對AGV車身姿態(tài)檢測做說明。1傳感器選型MPU9250是一個的復(fù)合芯片，它集成了很多功能，具有陀螺儀特性，加速度特性，電子陀螺儀特性，能讀取各個軸的數(shù)據(jù)，是一款9軸的運(yùn)動跟蹤檢測裝置，在運(yùn)動設(shè)備中很常用，他在的封裝中融合了3軸加速度，3軸陀螺儀以及數(shù)字運(yùn)動處理器（DMP），數(shù)字運(yùn)動處理器可將采集或者反饋到信息做處理，MPU9250兼容了MPU6515的功能，MPU9250是由三個獨立檢測“X、Y、Z”軸的MEMS構(gòu)成，功能齊全。其較突出的I2C方案，在12C主模式中，該芯片可以自己讀取到以下“X、Y、Z”的數(shù)據(jù)，可直接輸出9軸的全部數(shù)據(jù)。它有三種特性：分別是陀螺儀特性，加速度特性，電子羅盤特性。三軸陀螺儀特性工作起來比較耗電，而且編程量程可變。而加速度特性和電子羅盤特性相對來說耗電不多都在幾百uA以內(nèi)，而加速度特性有兩個獨特的功能一個是用戶可編程中斷，一個是運(yùn)動中斷喚醒功能。這個對于低功耗設(shè)備是很重要的，在我們需要使用到這個功能時，我們可以將其喚醒，在不需要時可一直處于休眠狀態(tài)。圖1.9加速度和陀螺儀的方向和極性圖1.10電子羅盤的方向它的引腳圖如下：圖1.11MPU9250引腳圖2車身姿態(tài)檢測電路本系統(tǒng)中采用九軸陀螺儀加上電子指南針來檢測車身姿態(tài)，MPU9250內(nèi)部集成有3軸陀螺儀、3軸加速度計、3軸電子羅盤，輸出信號都是16位的數(shù)字量，該模塊中用到了穩(wěn)壓芯片。圖1.12姿態(tài)檢測原理圖3車身姿態(tài)檢測模塊程序MPU9250讀取部分代碼如下：獲取車身三軸角速度三軸加速度ucharRead(ucharREG_Address)//將指定地址處的數(shù)據(jù)讀出來，然后返回。{ucharREG_data;//定義一個臨時uchar類型的變量--REG_dataI2C_Start();I2C_Send(SlaveAddress);//CPU向MPU9250發(fā)送該器件的地址I2C_Send(REG_Address);//然后CPU向MPU9250發(fā)送要讀取地址處的地址I2C_Start();I2C_Send(SlaveAddress+1);//CPU向MPU9250發(fā)送該器件的地址值加1后的地址，REG_data=I2C_Recv();//CPU接收來自MPU9250發(fā)來的數(shù)據(jù)I2C_SendACK(1);I2C_Stop();returnREG_data;//返回讀到的數(shù)REG_data}int16Getdata(ucharREG_Address){ucharH,L;//定義2個臨時uchar類型的變量--H和LH=Read(REG_Address);//將第一個地址處的數(shù)據(jù)讀出來，然后賦值給HL=Read(REG_Address+1);return(H<<8)+L;}4姿態(tài)與轉(zhuǎn)角換算本系統(tǒng)采用51單片機(jī)控制4個伺服電機(jī)的轉(zhuǎn)角，以達(dá)到控制支腿的升降從而保證四驅(qū)AGV搬運(yùn)車的安全運(yùn)行，由于采用的是伺服電機(jī)，理論上電機(jī)的速度越快對本系統(tǒng)越有利，因為電機(jī)速度越快就越能縮短調(diào)平的時間，但在實際應(yīng)用中速度不能太快，首先受到調(diào)平精度的限制，如果速度過快一是可能會出現(xiàn)慣性力比較大的情況，對調(diào)平精度有影響，二是當(dāng)伺服電機(jī)上升到一定的速度時，轉(zhuǎn)矩會急速下降。這樣的話不僅在調(diào)平后電機(jī)不能夠自鎖，甚至在不能驅(qū)動支腿上升等，從而影響工作穩(wěn)定。經(jīng)過陀螺儀對步行梯傾角數(shù)據(jù)收集，可將數(shù)據(jù)傳送給單片機(jī)，因此根據(jù)采集回來的傾角，單片機(jī)控制伺服電機(jī)轉(zhuǎn)動相對應(yīng)的角度，由于在步行梯中有很大的尺寸約束，單片機(jī)通過控制伺服電機(jī)的正反轉(zhuǎn)來控制支撐腿的伸縮達(dá)到所需要調(diào)整的角度。經(jīng)過之前的計算得出在搬運(yùn)車工作時，懸架系統(tǒng)要對貨箱進(jìn)行角度調(diào)整，要調(diào)整的角度為18°，根據(jù)計算的結(jié)果單片機(jī)要控制四個伺服電動缸的伸縮從而調(diào)整平臺，由于所運(yùn)載的貨物體積大，故在調(diào)整角度是，伺服電動缸的運(yùn)行速度不能過快，若過快則可能導(dǎo)致出現(xiàn)貨物中心不穩(wěn)，前后搖晃的情況，故對于伺服電機(jī)的控制要更精確，本系統(tǒng)采用的伺服電機(jī)控制器，在對于控制電機(jī)運(yùn)行速度方面由有更突出的特點。伺服電機(jī)額定轉(zhuǎn)速為3000r/min，3000r/60s=50r/s;該伺服電機(jī)編碼器分辨率為4080；電機(jī)額定轉(zhuǎn)速時編碼器反饋脈沖頻率是4080×50r/s;；4）上位機(jī)發(fā)出的脈沖頻率=200×1000/s；5）當(dāng)上位機(jī)滿額發(fā)出脈沖時，伺服恰好額定速度運(yùn)行，這時的電子齒輪比：電子齒輪比=反饋脈沖頻率/上位機(jī)滿額發(fā)出脈沖頻率=（4080×50r/s）/200×1000/s=204000/200000=1.02已知電機(jī)編碼器是2500線，電子齒輪比大約為1：則發(fā)一個脈沖電機(jī)轉(zhuǎn)動1/2500圈，也就是0.14°圖1.13支撐腿示意圖俯仰①：L1、L4伸長相應(yīng)長度，L2、L3不動：當(dāng)檢測到車身傾角當(dāng)前為時，根據(jù)計算則需要調(diào)整到期望值為，則調(diào)整的角度為，才可使得AGV搬運(yùn)車安全運(yùn)行，已知同一側(cè)電動缸之間距離為1200mm，絲杠螺距為10mm，根據(jù)三角關(guān)系可得需要的電動缸伸長。絲杠螺距為10mm，則絲杠需要轉(zhuǎn)動25圈，減速比為N=60，則電機(jī)需要轉(zhuǎn)動25N圈，即為1500圈。則電機(jī)轉(zhuǎn)角為：因此當(dāng)AGV搬運(yùn)車工作時檢測但樓梯傾角，則對應(yīng)控制調(diào)整的伺服電機(jī)轉(zhuǎn)角度為：即需要的脈沖數(shù)為：②L1、L4收縮，L2、L3伸長：車身姿態(tài)當(dāng)前值，期望值收縮80，則對應(yīng)絲杠需要轉(zhuǎn)動=8圈，減速比，電機(jī)轉(zhuǎn)角為：伸長，則對應(yīng)絲杠需要轉(zhuǎn)動 圈，電機(jī)轉(zhuǎn)角為：翻滾當(dāng)車身姿態(tài)與水平面夾角為8°時，要使得安全運(yùn)行的期望值為與水平夾角為0°故調(diào)整角度為8°。根據(jù)三角關(guān)系可得需要伸長83mm，對應(yīng)絲杠需要轉(zhuǎn)動8.3圈，則電機(jī)需要轉(zhuǎn)動8.3N圈，N=60，因此需要的電機(jī)轉(zhuǎn)角為推廣可得：已知車身姿態(tài)當(dāng)前值故需要調(diào)整的角度為則電缸需要的伸縮長度為：則電機(jī)轉(zhuǎn)角為：5車身姿態(tài)調(diào)整由陀螺儀模塊采集到的傾角信息后，上面計算出了一些特定場合車身姿態(tài)下，懸架系統(tǒng)需要調(diào)整的角度，根據(jù)電動缸伸縮長度，對應(yīng)到伺服電機(jī)轉(zhuǎn)動的角度，因此，對于車身姿態(tài)的調(diào)整就是對伺服電動缸伸縮量的控制，也就是對伺服電機(jī)轉(zhuǎn)角的控制，因為此懸架系統(tǒng)采用四個支撐腿，故對于車身姿態(tài)的調(diào)整就是同時對四個伺服電機(jī)的控制。首先第一種情況就是：調(diào)整車身俯仰姿態(tài)，這兒有兩種情況，固定兩個電機(jī)不動，另外一側(cè)的兩個電機(jī)同時調(diào)整對應(yīng)的角度。兩側(cè)的電機(jī)同時差動調(diào)整，一側(cè)的電機(jī)控制的支腿升高，另外一側(cè)電機(jī)控制的支腿收縮，同時調(diào)整。在AGV搬運(yùn)車工作時在某一特定場合所遇到的樓梯傾角也不盡相同，因此此懸架系統(tǒng)需要根據(jù)不同的工作場合調(diào)整不同的角度，使得AGV搬運(yùn)車平穩(wěn)正常運(yùn)行。其次第二種情況就是:調(diào)整車身翻滾角；在AGV搬運(yùn)車工作時也可能遇到的情況是車身左右兩側(cè)高度不一，此時則需要調(diào)整左右兩側(cè)電機(jī)的轉(zhuǎn)角。車聲姿態(tài)調(diào)整硬件原理圖如下：伺服使能輸入用于伺服電機(jī)的使能或禁止。0V時驅(qū)動器將切斷電機(jī)電源，使電機(jī)處于自由狀態(tài)不響應(yīng)脈沖。脈沖指令PULSE+脈沖信號：脈沖上升沿有效，高電平時4-5V，低電平時0-0.5V，脈沖寬度應(yīng)大于1.6us，脈沖指令PULSE-方向指令DIR+方向信號：DIR+與DIR-之間輸入高電平時反轉(zhuǎn)，反之正轉(zhuǎn)。方向信號應(yīng)先于脈沖信號至少5us建立，高電平時4-5V，低電平時0-0.5V。方向指令DIR-電機(jī)接口信息車聲姿態(tài)調(diào)整控制程序:voidmain(){Timer0_Init();//初始化定時器Uart_Init();EA=1;//打開全局中斷Delay1ms(1000);//延時1swhile(1){Read_Key();//按動一次按鍵執(zhí)行一次if(puss_key==1){MOTOR_Run_Angle(1,1,1);//電機(jī)1正轉(zhuǎn)開啟MOTOR_Run_Angle(2,1,1);//電機(jī)2正轉(zhuǎn)開啟MOTOR_Run_Angle(3,0,1);MOTOR_Run_Angle(4,0,1);Delay1ms(RUN_Angle*bili);//電機(jī)轉(zhuǎn)動角度MOTOR_Run_Angle(1,0,1);//電機(jī)關(guān)閉MOTOR_Run_Angle(2,0,1);//電機(jī)關(guān)閉MOTOR_Run_Angle(3,0,1);//電機(jī)關(guān)閉MOTOR_Run_Angle(4,0,1);//電機(jī)關(guān)閉puss_key=0;}}}6CAN接口設(shè)計基于CAN總線的系統(tǒng)主要由主控制器和數(shù)據(jù)采集與控制單元組成。CAN總線接口由三部分構(gòu)成，是微處理器，CAN控制器，收發(fā)器，上位機(jī)是整個系統(tǒng)的操作平臺，負(fù)責(zé)實現(xiàn)對各單元，各功能模塊進(jìn)行監(jiān)測和控制功能；子模塊中，傳感器負(fù)責(zé)對車身姿態(tài)數(shù)據(jù)進(jìn)行采集，控制單元控制各模塊完成功能。CAN總線接口的作用是用于收集底下各個子模塊的信息，各個節(jié)點的數(shù)據(jù)，完成數(shù)據(jù)采集后通過收發(fā)器發(fā)送到上位機(jī)控制器中去，同時將上位機(jī)的指令傳達(dá)給各個模塊中的CAN收發(fā)器中。CAN的每一個節(jié)點必須包含有CAN收發(fā)器和CAN控制器，各節(jié)點模塊以微處理器52單片機(jī)為控制核心，通過各自的CAN總線接口，將采集的信息發(fā)送至CAN總線，并從CAN總線上監(jiān)聽控制單元所需的控制信息。CAN總線有很多優(yōu)點，首先是它是總線可以外接很多設(shè)備，第二是傳輸速度高，其次就是傳輸距離遠(yuǎn)，而且還有多主機(jī)的優(yōu)勢。CAN總線在時間上面有一定的容錯性，它允許每個設(shè)備上面的始終有一定的偏差；CAN總線有兩種方式，首先第一個是CAN硬同步，當(dāng)每一個設(shè)備在發(fā)送一個CAN幀之前先發(fā)送一個起始位。第二是CAN位重同步。通過CAN總線，對工作中的模塊數(shù)據(jù)數(shù)據(jù)進(jìn)行采集，并發(fā)送命令，通過CAN控制器，根據(jù)CAN總線接口傳來的指令完成相應(yīng)的動作，將采集數(shù)據(jù)發(fā)送到上位機(jī)中，完成信息交互。CAN通訊的節(jié)點結(jié)構(gòu)圖如下圖所示：圖1.14CAN通訊結(jié)構(gòu)圖7人機(jī)接口設(shè)計矩陣鍵盤是人機(jī)交互的重要設(shè)備。實際應(yīng)用場景中，用戶通過鍵盤按鍵產(chǎn)生激勵信號，然后系統(tǒng)按照鍵入的命令完成指定工作。此AGV搬運(yùn)車根據(jù)工作場合的情況可調(diào)整懸架的高度，在適用場合中，用戶可通過鍵盤產(chǎn)生信號，AGV搬運(yùn)車懸架產(chǎn)生相應(yīng)動作，此矩陣鍵盤中鍵入了多種AGV車聲姿態(tài)要調(diào)整相應(yīng)的角度?？筛鶕?jù)不同場合，用戶鍵入不同的按鍵指令。圖1.15矩陣鍵盤原理圖相應(yīng)功能程序如下*名稱:write_command(u