運動定位裝置遠程控制器的 FPGA 實現

高振斌，吳金蘭，陳衛科，劉權震

（1. 河北工業大學 信息工程學院，天津 300401；2. 華南理工大學 電子與信息學院，廣東 廣州 510641）

運動定位裝置遠程控制器的 FPGA 實現

高振斌1，吳金蘭1，陳衛科1，劉權震2

（1. 河北工業大學 信息工程學院，天津 300401；2. 華南理工大學 電子與信息學院，廣東 廣州 510641）

設計并 實現了一種 基于 FPGA 和 SIM 908 模塊的遠程 定位運動系 統．在 SOPC Builder 中構建以 CPU、存儲器、定時器、Avalon 總線、UART 為核心的 NIOS 處理器，編程實現對 SIM 908 模塊 GPS 數據的接收以及控制指令的解析與處理，并根據指令計算電機驅動模塊控制參數，驅動電機使該裝置按照給定路徑行駛．系統使用軟硬件協同設計技術實現，控制命令解析由軟件編程實現，而電機驅動與調整由 FPGA 上邏輯電路實現，詳細闡述了軟硬件的設計過程．實驗結果表明該控制器能通過解析控制命令完成給定路徑行駛、實現定位．

GPS 定位；GPRS；控制指令；NIOS 處理器；中斷函數

智能小車是一種智能機器人，具有體積小、功耗低、可靠性好、操作簡單等特點，它集中運用了通信、人工智能和導航等技術，可以通過對其進行特殊的設計實現特定功能．目前智能小車可實現自動壁障、紅外線循跡、尋找光源進入車庫、檢測貼片、躲避懸崖等諸多功能，但在實現車身定位和借助網絡驅動小車動作的領域研究較少．

為了研究特殊環境下智能車的行走及定位能力，本設計搭建了三輪小車作為實驗平臺，前兩輪平行排列為主動輪，最后一個輪為從動輪．實驗平臺配有供電系統、電機驅動系統、GPS/GPRS 定位裝置[1]，光電檢測傳感器等模塊．在主控 FPGA 中構建一個 NIOS 軟核[2-3]作為整個系統的核心，通過 NIOS 外接電路實現對實驗平臺的運動控制．借助GPRS網絡發送控制指令使該實驗平臺完成給定路徑行駛，實現自身定位．該平臺可作為野外作業的基礎平臺，在后期實驗過程中加上圖像采集裝置，可以更精準的控制平臺動作．

1 系統功能及組成

該系統由FPGA開發板、SIM 908 模塊、電機驅動模塊和光電檢測模塊組成，如圖1 所示．

SIM 908 模塊包含GPS和GSM兩部分，分別完成系統定位與通信功能．NIOS 軟核是整個系統的核心，它解析GPS模塊接收到的定位數據和GSM模塊接收到的控制指令，經過處理后控制裝置運動．電機驅動模塊支持PWM調速．光電檢測設備負責檢測裝置轉速，反饋給驅動電路以實現電機的閉環控制．

圖1 整體設計框架圖Fig.1 The structureof overalldesign

2 運動控制原理

對實驗平臺的控制分為2種情況：通過指令獲取裝置當前位置信息；通過指令控制裝置按指定路徑行駛，其中行駛方式分為直行、左轉和右轉．電機驅動模塊接收PWM脈沖，從而決定平臺轉速，計算PWM脈沖所需參數從指令中獲取．運動類命令包括2種：直行和轉彎．

直行時通過指令設定行駛狀態、行駛路程和速度，以“SLxxxVxxx”格式發送指令．其中字母代表“直行”狀態；Lxxx代表行駛路程值，用三位整數表示；Vxxxx代表車輪速度，用四位數表示（包括小數點）．直行時設兩輪行駛路程為 L、速度為 V、轉速為 Q、行駛時間為 t、車輪半徑分別為 R ．

直行時路程與車輪半徑、速度和轉速間的關系為

車輪轉彎示意圖如圖2所示．轉彎時由指令設定車輪轉彎方向、轉角、內側車輪轉彎半徑和轉速，以 “Z YxxrxxVxxxx” 格式發送指令．其中 Z 代表“左轉”，Y 代表“右轉”；xx代表車輪轉彎角度，用兩位整數表示；rxx代表轉彎半徑值，用兩位整數表示；Vxxxx代表前內側車輪速度，用四位數表示（包括小數點）．

裝置轉彎時設車輪半徑為 R，內輪轉彎半徑為 r，前兩輪之間的距離為 d，外輪轉彎半徑為 r+d ，內外輪轉彎角度均為，內外輪轉過的弧長分別為 L1、L2，內外輪轉速分別為 Q1、Q2．其中車輪半徑 R和前兩輪間的距離d可以通過實際測量獲得，下面以車輪左轉為例推導各個量之間的關系

圖2 車輪轉彎示意圖Fig.2 The schematic diagram ofwheel turn

其中 L1=Vt=2RQ1t ．

推出

同理可知右轉時各個量之間的關系為

計算出的時間用于定時器計時，定時時間到表明動作已完成，則運動停止．轉速 Q(Q1、Q2)經編程轉化為PWM控制字 C(C1、C2)，傳遞給驅動電路用于產生PWM控制字．

3 系統設計及仿真

硬件設計主要包括 3 部分：構建NIOS軟核，設計PWM脈沖產生模塊和光電檢測模塊．

3.1 整體運動控制電路

圖3 所示為NIOS軟核對裝置的運動控制過程．實驗平臺的運動方式包括直行 S、左轉 Z 和右轉 Y ，編程解析出指令中的運動方式控制量并傳遞給比較模塊．光電檢測傳感器用于檢測車輪實際轉速，將檢測到的脈沖個數in1、in2 經定時計數后參與比較．當運動方式為直行時，比較模塊輸出脈沖個數的比較差值；運動方式為左轉和右轉時，比較模塊輸出的差值為 0．NIOS軟核將指令中的轉速值轉化為相應的PWM控制字并通過I/O口輸出給調整模塊．調整模塊根據比較器輸出的差值對PWM控制字進行調整再輸出，同時調整模塊輸出車輪的使能控制信號．

圖3 整體電路控制電路圖Fig.3 The circuitdiagram of overallmotion control

3.2 構建 NIOS 軟核

在 QuartusⅡ中新建一個圖形文件作為頂層文件，利用SOPCBuilder軟件構建NIOS II軟核[4-5]并添加到頂層文件中．構建的處理器結構主要包括 NIOS IICPU、定時器、Flash 存儲器、SDRAM 控制器、IO 口、串口等．NIOS II處理器各部分之間的連接關系如圖4 所示，其中比較模塊根據I/O口輸出的運動方式控制量決定比較輸出，調整模塊根據 I/O 口和比較模塊的輸出值調整PWM控制字．

3.3 PWM 脈沖產生模塊

裝置的運轉由 PWM[6]脈沖驅動．圖5為產生PWM脈沖的電路，圖6為該電路仿真波形圖．

仿真過程中時鐘周期設置為 10 ns，計數器 cntA 對時鐘上升沿個數進行計數，PWM控制字 C 每隔 10.24s變化 1次，每次增幅為 30．計數器的輸出值A和PWM控制字C進行比較，當A小于C時，PWM輸出為 1，否則輸出為 0．從仿真圖可知，該模塊能根據控制字C產生PWM脈沖，C越大，PWM脈沖的占空比越大，車輪轉速越大；反之越小．

圖4 NIOSⅡ處理器結構Fig.4 The structureof NIOS Ⅱ processor

圖5 PWM脈沖產生電路圖Fig.5 The diagram of PWM pulsegenerating circuit

圖6 PWM脈沖仿真圖Fig.6 The simulation diagram of PWM pulse

3.4 光電檢測電路

光電檢測電路用于檢測前兩輪的實際轉速，并對其進行比較．en 為'1'時電路生效，in1、in2 為光電檢測傳感器的輸出端脈沖，脈沖周期的大小用于指示前兩個車輪的實際轉速大小，先對in1、in2 去抖動操作并在時鐘上升沿對其進行計數，同一個計時間隔內的計數結果分別為 a、b．信號cnt為對時鐘上升沿個數進行計數，當cnt計數溢出時電路對 a、b 進行比較．當時 a ＞ b，out3=01，比較差值 diff1=a b；當時 a=b，out3 =00，比較差值 diff1=diff2=0；當時 a ＜ b，out3=10，比較差值 diff2=b a．仿真如圖7 所示，由仿真圖知該電路能檢測車輪實際轉速并輸出比較差值．

圖7 光電檢測模塊波形仿真圖Fig.7 Thewaveform simulation diagram of photoelectric detectionmodule

3.5 調整電路

led1、led2 為軟核I/O口的輸出，out3、diff1、diff2 為光電檢測電路的輸出，兩個電路的輸出作為調整電路的輸入．L1、L2、L3、Z_F為調整模塊輸出的車輪使能端和正反轉控制端，DB1、DB2、DB3 為調整模塊輸出的 PWM 控制字．調整模塊根據 out3、diff1、diff2 對 led1、led2 進行調整，將實際計數值小的一路脈沖加上比較差值再輸出，使直行時車輪轉速盡量保持一致．調整模塊仿真如圖8所示，由仿真圖知調整模塊能根據比較結果對led1、led2 進行調整．

4 軟件設計

軟件設計主要指編程實現模塊的初始化功能，接收 SIM 908 模塊數據并解析 GPS 定位信息[7]、接收并解析指令、向控制臺發送定位數據等功能．

4.1 主程序流程圖

圖8 調整模塊波形仿真圖Fig.8 Thewaveform simulation diagram of adjustmentmodule

圖9 為主程序執行過程．初始化過程包括GSM和GPS兩個中斷初始化和SIM 908 模塊初始化．然后判斷實驗平臺當前動作時間是否到達，若定時到則判斷命令隊列是否有新命令未執行，若無未執行命令或定時未到，轉去判斷是否收到控制命令．若未收到則返回判斷定時時間是否到達；若收到則判斷命令是否有效．若命令為 dw，則獲取GPS 數據并發送；若命令以、或開頭，則存儲命令到命令隊列，同時置標志位．如果命令隊列有新命令未執行，則讀取新命令，編程計算出、和的值，將和的值輸出給外部調整電路，設定定時初值啟動定時器，最后刪除已執行命令．

4.2 GPS 中斷服務函數

GPS中斷接收服務程序的執行過程如圖10所示，設定$GPRMC格式的定位數據以字符“$”開頭，以字符“*”結尾，接收到的頭和尾以及中間所有字符均存到一個數組里，便于解析函數使用．

4.3 GSM 中斷服務函數介紹

一般AT指令的返回值以回車符開頭和結尾，接收寄存器接收到字頭和自尾回車符后分別置標志位，特殊情況下單獨判斷．獲取控制指令內容的返回值中包含3個回車符，當接收到字符串“+ CMGR”之后清除接收數組中的內容，只存儲包含控制命令內容的兩個回車符之間的數據．GSM中斷服務程序流程圖如圖11所示．

圖9 主函數流程圖Fig.9 The flow chartof themain function

圖10 GPS中斷接收服務函數流程圖Fig.10 The flow chartof GPS interrupt receiving service function

圖11 GSM中斷服務函數流程圖Fig.11 The flow chartof GSM interruptservice function

5 設計結果及分析

5.1 定位數據的接收

裝置在行駛過程中，當接收到 “dw”指令就會返回給目標控制端處理后的定位數據，數據包括當前日期、時間、經緯度、對地速度等．

5.2 裝置速度的標定

實驗中令裝置在平坦路面上直線行駛，分別給定前兩輪相同的 PWM 控制字，依次為 78、85、92、99、106、113、120、127，記錄下裝置行駛 2m的時間，分別計算出裝置運行速度為 0.14m/s、0.15m/s、0.16m/s、0.17m/s、0.18m/s、0.19m/s、020m/s、0.21m/s．速度 v 和PWM 控制字 C 的大致關系如圖12 所示．

整理得到直行速度 v和PWM控制字C 之間的經驗公式為

5.3 裝置運動結果及誤差分析

直線運動中，由控制指令給定速度值計算出PWM控制字 C，給定速度與實際速度對照如表1所示．觀察表可知，實際速度值與給定速度值接近．

表1 給定速度與實際速度對照表Tab.1 The comparison between the known speed and actualspeed

圖12 V_C對應關系圖Fig.12 The diagram of V_C corresponding

轉彎運動中，由控制指令給定的轉彎角度與實際運動角度對照如表2所示．觀察表可知，給定角度與實際運轉角度值接近．

表2 給定轉角與實際轉角度對照表Tab.2 The comparison between the known angle and actualangle

通過測試不同路面的行駛情況發現，裝置運行的實際速度與理論速度之間存在差值，實際測得的轉彎角度和給定轉彎角度之間略有偏差，這與車輪行駛的路面情況不完全一致有關．

6 結束語

本文針對遠程定位運動控制技術的實際應用需求，研究了一種基于FPGA的遠程定位運動控制實驗平臺，并對最終的設計成果進行了不同路況下的測試．測試結果表明該裝置具有較強的道路適應性和可靠性，裝置能通過解析控制命令實現自身定位，并完成按給定路徑行駛．后續的工作將在現有基礎上進一步改進調整算法，使得裝置運行速度和拐彎角度更準確．

[1] 王濤，董海瑞．車載衛星通信站天線自動控制系統設計 [J]．電視技術，2001（11）：34-35．

[2] 王永州，范多旺，金靜，等．基于NIOS Ⅱ處理器GPS/GSM車輛監控系統終端設計 [J]．鐵路計算機應用，2007，16（2）：26-29．

[3] 黃海波，蔣偉榮，劉勇，等．基于SOPC的智能車輛道路識別與跟蹤 [J]．儀器儀表學報，2012， 33（2）：321-325．

[4] 李振峰，肖建．基于NIOSⅡ的SOPC實驗系統設計與開發 [M]．成都：西南交通大學，2012：23-42．

[5] 劉小雍．基于SOPC的GPS無線定位終端設計與實現 [M]．成都：西南交通大學，2011：12-28．

[6] 王玉花，郭書軍．NIOSⅡ系統Avalon 總線PWM設計 [J]．現代電子技術，2010（1）：183-185．

[7] 劉斌，梁美美，李欣．基于NIOSⅡ的GPS信息接收系統設計與實現 [J]．現代電子技術，2013，36（6）：105-108．

[責任編輯 代俊秋]

The implementation of a remote controller for motion positioning device based on FPGA

GAO Zhen-bin1，WU Jin-lan1，CHEN Wei-ke1，LIU Quan-zhen2
( 1. School of Information Engineering, Hebei University of Technology, Tianjin 300401, China; 2. School of Electronic Information Engineering, South China University of Technology, Guangdong Guangzhou 510641, China )

The paper designs and implements a remote controller formotion positioning device based on FPGA and SIM 908module.Thedesign buildsaNIOSprocessorconsistofNIOSⅡ CPU,memory,the timer,Avalon bus,UARTs. The processor receivesGPSdata of SIM 908module,analysesand processesdata of control instructions,calculates the control parameters for themotor drivemodule and drives the device along a given path.The system uses hardware and software codesign technology.Control instructions are parsed by software,but the driver and adjustmentofmotor are implemented by the logic circuit.The paper introduces the design and implementation processof thehardware and software in detail.Experimental results show that the controller can complete a given path under the controlof control instructions,and realize positioningw ithin theacceptable error.

GPSpositioning;GPRS;control instruction;NIOSprocessor;the interrupt function

1007-2373(2014)05-0027-06

TP273.5

A

10.14081/j.cnki.hgdxb.2014.05.005

2014-06-25

河北省科技支撐計劃（13210305D）

高振斌（1973-），男（漢族），副教授，博士．