申純太, 上海電氣集團股份有限公司中央研究院

圖-1  主控制器Compact RIO的安裝位置

挑戰(zhàn):
        需要CompactRIO同時控制多個設備運動竿屹。2根5自由度機械臂運動，頭部1自由度旋轉(zhuǎn)運動灸姊，雙主動輪底盤運動控制拱燃。CompactRIO內(nèi)要部署機械臂運動控制算法，LIDAR數(shù)據(jù)采集和導航避障算法力惯，基于編碼器和光雷達數(shù)據(jù)以及FPGA的移動位置控制算法等扼雏。

解決方案:
        將機器人的功能進行劃分為兩部分。將雙5自由度機械臂運動夯膀，頭部1自由度的旋轉(zhuǎn)運動诗充，雙主動輪底盤的運動，光雷達數(shù)據(jù)采集處理和編碼器數(shù)據(jù)采集诱建，自主導航蝴蜓，輪椅模式切換判斷，機器人體表LED狀態(tài)情感或信息顯示武骆，電池電源管理等基本行為和設備層控制歸為一部分榛舍，由“小腦”控制；將家庭智能家居網(wǎng)絡交互硝锨，其他智能機器人監(jiān)控耽慌，語音識別對話和人臉識別的人機交互功能等歸為一部分，由“大腦”控制储佣。

        選用NI CompactRIO作為機器人的“小腦”辅任。NI CompactRIO是美國NI公司的工業(yè)級嵌入式控制器，集成以太網(wǎng)接口和RS232串行接口位蓉，具有體積小立漏，高可靠性，高性能冶媚，低功耗等優(yōu)點聪戳。

        將各種設備的控制算法例如機械臂運動控制，頭部運動控制彭旬，導航算法缰俊，基于以太網(wǎng)的光雷達數(shù)據(jù)采集，與大腦通訊等程序部署在RT中娩梨；將數(shù)字信號采集沿腰，例如限位開關和緊急停止邏輯還有編碼器數(shù)據(jù)采集和處理放案，地盤運動伺服控制等算法部署在FPGA中。

        選用工業(yè)嵌入式觸摸平板電腦IPC作為機器人的“大腦”矫俺。部署WinCE吱殉，具有人機界面功能以及觸摸屏交互功能，部署控制人臉識別模塊以及語音識別算法厘托，智能家居監(jiān)控管理算法等友雳。

用FSM來封裝每個設備對象：

        Finite-state machine (FSM)稱為有限狀態(tài)機廣泛用于數(shù)字電路和計算機程序。我們使用了兩種狀態(tài)機來封裝設備铅匹。String Based Queued State Machine基于字符串隊列狀態(tài)機和普通基于枚舉的狀態(tài)機押赊。使用狀態(tài)機作為一種機制來處理單個設備的狀態(tài)轉(zhuǎn)換，或者說包斑，用狀態(tài)機這種行為模型來用于單個設備的事件處理流礁。

圖 2 LIDAR 狀態(tài)機

        例如光雷達的狀態(tài)機框圖，它具有4個狀態(tài)罗丰，Idle神帅，Start，Running腊囤，Stop几菲。在Running狀態(tài)中，程序不斷向光雷達發(fā)送指令讀取數(shù)據(jù)韭展，然后由一個Shared Variable來傳出數(shù)據(jù)杉唇。使用一個LIRAR.vi來封裝這個狀態(tài)機。

圖 3 Remote Control Reaction 狀態(tài)機

        圖3是一個遠程控制指令解析狀態(tài)機酬裆，它使用String Based Queued State Machine來實現(xiàn)辟侠。這樣的好處是，可以使用隊列來組織動作葱有，較為靈活地實現(xiàn)各種不同組合以及復雜度眨柑。

        每一個設備的狀態(tài)機都用一個獨立的vi來封裝，并且單獨進行測試柑耙，這樣也有利于工程化述茂。

[DividePage:NextPage]

多狀態(tài)機的協(xié)作：

        單個狀態(tài)機只能完成部分功能搂瓣，我們需要把所有設備狀態(tài)機集成起來雷鸟，按照一定的通訊機制拼裝成一個完整的機器人。

        由于所有的設備狀態(tài)機都由一個VI來封裝氢拥，只需要把他們拖入一個主vi中蚌铜，就能調(diào)用他們。如圖所示：

圖 4  狀態(tài)機集成

        狀態(tài)機之間是通過Shared Variable來實現(xiàn)接受外部指令輸入以及自身狀態(tài)輸出的嫩海。同樣冬殃，這些Shared Variable都用一個VI進行封裝以便進行管理囚痴。

        這樣，一個狀態(tài)機便可視為一個子系統(tǒng)审葬。既然是系統(tǒng)深滚，自然有系統(tǒng)的輸入和輸出，通過get涣觉，set等方法的vi痴荐，外部系統(tǒng)便可對這些子系統(tǒng)進行操作或者交互。如圖所示官册，底盤狀態(tài)機使用getCommand.vi方法獲取外部命令生兆。

圖 5 getCommand.vi

        使用setCommand.vi方法讓外部系統(tǒng)傳輸命令給地盤狀態(tài)機。

圖6 MobileBase.lvlib:setCommand.vi

        底盤使用sendState.vi發(fā)送底盤狀態(tài)機狀態(tài)信息膝宁。如圖所示：

圖 7 MobileBase.lvlib.sendState.vi

        使用getState.vi方法獲取地盤狀態(tài)機信息鸦难。

圖 8 getState.vi

導航算法：

圖 9家居監(jiān)控機器人定位導航軟件控制框圖

總體設計

        針對移動機器人家居環(huán)境下的定位問題，提出了一種結合平直線段匹配籍喧、角匹配和里程計的組合定位方法水仑。該系統(tǒng)采用了Labview開發(fā)平臺和CompactRIO控制器，得到了很好的實時性效果铃踪。機器人首先通過二維激光測距儀通過TCP/IP得到環(huán)境點信息掌社，然后通過迭代適應點（IEPF）算法得到環(huán)境線段及最小二乘法得到線段參數(shù)。在基于線段基礎上窄切，得到局部的平直線段和角特征簇茉，再與已知平直線段和角特征做匹配，通過平直線段和角匹配算法實時更新機器人位置和姿態(tài)炫胡。分析里程計定位哈寂、平直線段匹配定位和角匹配定位的誤差，分配不同的權重得到優(yōu)化的組合定位算法狗城。

[DividePage:NextPage]

軟件控制框圖

        基于以上的設計佑茴，定位導航的軟件控制模塊設計如圖7所示。中心模塊為定位導航模塊蚊楞，該模塊讀取起點键闺、目標點、運動模式數(shù)據(jù)及編碼器數(shù)據(jù)澈驼，同時讀取激光測距儀數(shù)據(jù)提取角辛燥、平直線段特征和已知特征做匹配，其匹配算法輸出機器人位姿 缝其。對于給定的目標位姿挎塌，該模塊將計算出驅(qū)動指令（Forward Speed、Delta angle内边、Turnspd Right榴都、Turnspd Left對應前進速度待锈、方向轉(zhuǎn)角、左轉(zhuǎn)嘴高、右轉(zhuǎn)）和狀態(tài)信息如（bPos, bAngle）來判斷機器人是否到達目標位置和姿態(tài)竿音。

圖 10家居監(jiān)控機器人定位導航的輸入輸出信息部分

        圖10為定位導航模塊部分的輸入輸出部分，上半部分的輸入信息中的已知角特征和全局線段為為已知地圖信息拴驮，下半部分的部分輸出信息包括直線段提取示意圖谍失，路徑任務數(shù)組信息圖，實時顯示的機器人當前位置信息琢播、速度信息等铝比。

線段特征提取

        為得到環(huán)境的幾何線段信息，需要對激光測距儀的點集進行分割茄颈，其分割算法可分為下列步驟：坐標變換穷筒、區(qū)域分割、IEPF線段提取和最小二乘法計算線段參數(shù)去柿。其中區(qū)域分割：從初始點i=0開始檢測兩相鄰點 和 的距離弦撤，如距離值小于閾值D，則認為屬于同一區(qū)域點集幅裳，否則開始一個新的區(qū)域鼠基。該過程遍歷所有點集。如果某區(qū)域點數(shù)目小于4個铆寓，則認為噪聲區(qū)域假怔，舍棄這些噪聲點。

圖 11  IEPF線段提取

        IEPF提取線段：對于上述得到區(qū)域可能含有多條線段陵蜻，IEPF線段提取算法是一種有效的線段提取方法怕收。如圖11所示， Pm點到線段PsPe的距離大于閾值T舆瘪，該方法把點集

分為和

        IEPF為迭代算法片效，對于 和  重復上述的算法知道 小于閾值 。該算法也有分割過細的時候英古，如圖9右半部中間兩條線段可通過檢查線段參數(shù)的辦法淀衣，對于斜率傾角誤差小于一定閾值的線段加以合并處理。線段參數(shù)可通過最小二乘法計算求得召调。

        如圖12所示為通過Labview實現(xiàn)的IEPF算法的區(qū)域分割算法膨桥，其中子VI 為IEPF算法分割出的子區(qū)域。

圖 12 包含IEPF算法的區(qū)域分割算法

        具體組合定位算法可參考作者的另一篇發(fā)表在IEEE ICIA2010題為《A Corner and Straight line Matching Localization Method for Family Indoor Monitor Mobile Robot》的論文某残。

原型機照片：

圖 13家居監(jiān)控機器人在世博滬上生態(tài)家

總結：

        使用CompactRIO作為機器人主控制器配有集成的FPGA的支持国撵，具有性能優(yōu)越，體積小玻墅，安全可靠介牙，低功耗等特點，并且澳厢，數(shù)據(jù)采集能力和通訊能力強大环础，非常適合移動機器人的原型開發(fā)。

        可以基于LabVIEW來開發(fā)機器人程序姊康，相比字符代碼程序岗翔，圖形G語言程序可以用圖形來閱讀，有利于理解和調(diào)試碎瓢，再者LabVIEW仍舊可以運用很多軟件工程方法蹲鱼，使得它具有自己獨特的優(yōu)勢。