本發(fā)明涉及機器人系統(tǒng)領域，具體涉及了一種三輪全向移動的交互式自主導引系統(tǒng)及方法。

背景技術：

交互式自主導引系統(tǒng)在今天的商場導購、醫(yī)院導診、景區(qū)導游、慶典迎賓等場合大有用途。尤其是在勞動力成本不斷攀升、服務行業(yè)勞動力不足的今天，自主導引系統(tǒng)不僅能幫助企業(yè)減少用工量，而且能幫助服務員分擔客戶接待量。傳統(tǒng)的自主導引系統(tǒng)(比如，迎賓機器人)一般采用有軌移動底盤或兩輪差分自主驅動底盤，前者的移動范圍受限，后者的運動靈活性不足?？偟膩砜?，現(xiàn)有的導引系統(tǒng)智能水平比較低，自主導航能力不足、與人交互方式有限。

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

本發(fā)明所要解決的技術問題是提供一種三輪全向移動的交互式自主導引系統(tǒng)，有效地解決了自主導航和人機交互的問題；同時，在運動機構上采用了三輪全向移動底盤，擺脫了導軌對移動區(qū)域的限制、而且移動穩(wěn)定靈活。

本發(fā)明解決上述技術問題的技術方案如下：一種基于三輪全向移動的交互式自主導引系統(tǒng)，包括依次連接的導引交互單元、自主導航單元、三輪全向移動控制單元和三輪全向底盤單元；

所述導引交互單元，用于設定目標位置，并將所述目標位置發(fā)送給自主導航單元，同時，接收自主導航單元反饋的最新當前位置和最佳路線以供查看；

所述自主導航單元，用于根據(jù)設定的目標位置計算導引系統(tǒng)從當前位置到目標位置的最佳路線，更新當前位置，并將最佳路線和最新當前位置反饋給導引交互單元，同時，計算前往目標位置的運動參數(shù)，并發(fā)送給三輪全向移動控制單元；

所述三輪全向移動控制單元，用于根據(jù)運動參數(shù)生成控制信號，發(fā)送給三輪全向底盤單元，同時，將里程數(shù)據(jù)反饋給自主導航單元用于定位；

所述三輪全向底盤單元，用于根據(jù)控制信號控制移動速度和方向，同時，將電機實時轉速反饋給三輪全向移動控制單元用于里程計算。

本發(fā)明的有益效果是：

本發(fā)明提出的導引系統(tǒng)在三輪全向移動底盤上實現(xiàn)了可人機交互的自主導引系統(tǒng)架構，將三輪全向移動機構、人機交互和自主導航融為一個整體，完成了一套移動靈活、自主運動且操作界面友好的導引系統(tǒng)。

在上述技術方案的基礎上，本發(fā)明還可以做如下改進。

進一步，所述導引交互單元包括依次連接的人機接口模塊、場景地圖管理模塊和導航控制接口模塊；

所述人機接口模塊，用于提供圖形化的人機操作界面，以場景地圖的形式描述導引系統(tǒng)所處環(huán)境，將操作人員輸入的場景地圖目標位置點發(fā)送給場景地圖管理模塊；同時，以圖形方式在場景地圖中顯示導引系統(tǒng)當前位置點和運動軌跡點；

所述場景地圖管理模塊，用于將目標位置點轉換成概念地圖數(shù)據(jù)后發(fā)送給導航控制接口模塊；同時，將接收到的當前位置點和運動軌跡點轉換成場景地圖數(shù)據(jù)，并提交給人機接口模塊；

所述導航控制接口模塊，用于將概念地圖表示的目標位置點發(fā)送給自主導航單元，同時，用于接收自主導航單元反饋的當前位置點和運動軌跡點，并將所述當前位置點和運動軌跡點提交給場景地圖管理模塊；

所述人機接口模塊包括場景地圖顯示模塊、當前位置顯示模塊、目標位置指定模塊和路徑標識模塊；

所述場景地圖顯示模塊，用于以圖形方式把場景地圖直觀地顯示在終端屏幕上；

所述當前位置顯示模塊，用于以圖形方式把導引系統(tǒng)當前位置點顯示在場景地圖中；

所述目標位置指定模塊，用于以交互式圖形直觀顯示導引系統(tǒng)前往的目標位置點；

所述路徑標識模塊，用于以圖形方式在場景地圖中顯示導引系統(tǒng)從當前位置點前往目標位置點的路徑軌跡點；

所述場景地圖管理模塊包括場景地圖模塊和場景-概念地圖映射模塊；

所述場景地圖模塊，用于查看周圍環(huán)境地形圖，通過圖形化的像素點來表示；

所述場景-概念地圖映射模塊，用于把場景地圖中的“點”轉換成自主導航單元的概念地圖中的“點”，或者把自主導航單元的概念地圖中的“點”轉換成場景地圖中的“點”。

采用上述進一步方案的有益效果是實現(xiàn)了導引交互的框架，操作人員可通過圖形化人機接口訪問導航系統(tǒng)，直觀地顯示當前位置、目標位置和移動軌跡，便于使用者了解所處方位和導引路線。

進一步，所述自主導航單元包括依次連接的交互接口模塊、路徑規(guī)劃模塊、基礎功能模塊和底盤接口模塊；

所述交互接口模塊，用于接收導引交互單元發(fā)送的目標位置點，并發(fā)送給路徑規(guī)劃模塊，同時把最新當前位置和路徑軌跡點序列反饋給導引交互單元；

所述路徑規(guī)劃模塊，用于計算概念地圖中當前位置點前往目標位置點最佳路徑、更新當前位置點，并將路徑軌跡點序列和最新當前位置點提交給交互接口模塊，同時，計算導引系統(tǒng)運動參數(shù)，并發(fā)送給底盤接口模塊；

所述基礎功能模塊，用于根據(jù)里程計和激光雷達數(shù)據(jù)計算導引系統(tǒng)當前位置點，并表示成概念地圖中的當前位置點；

所述底盤接口模塊，用于從底盤讀取里程計數(shù)據(jù)和激光雷達數(shù)據(jù)并反饋給基礎功能模塊，并將接收到的導引系統(tǒng)運動參數(shù)發(fā)送給三輪全向移動控制單元；

所述路徑規(guī)劃模塊包括全局路徑規(guī)劃模塊和局部路徑規(guī)劃模塊；

所述全局路徑規(guī)劃模塊，用于根據(jù)導引交互單元發(fā)來的目標位置點，在概念地圖中按最短路徑策略尋找一條從當前位置點前往目標位置點的最佳路徑，并將概念地圖中路徑軌跡點序列提交給交互接口模塊；

所述局部路徑規(guī)劃模塊，用于通過自主定位模塊更新概念地圖中的當前位置點，并將當前位置點通過交互接口發(fā)送給導引交互單元；同時，根據(jù)概念地圖中軌跡點序列中下一位置點，計算從當前位置點移動到所述下一位置點的線速度和角速度，并通過底盤接口模塊把線速度和角速度參數(shù)發(fā)送給三輪全向移動控制單元；

所述基礎功能模塊包括概念地圖管理模塊和自主定位模塊；

所述概念地圖管理模塊，用于創(chuàng)建概念地圖并存儲，包括概念地圖模塊；

所述概念地圖模塊，用于自主導航單元定位的方位圖，通過坐標化的柵格點來表示；

所述自主定位模塊，用于計算導引系統(tǒng)在概念地圖中的當前位置點，并在概念地圖中表示出來；

所述底盤接口模塊包括傳感器數(shù)據(jù)接口模塊和運動控制接口模塊；

所述傳感器數(shù)據(jù)接口模塊，用于讀取里程計數(shù)據(jù)和激光雷達數(shù)據(jù)；

所述運動控制接口模塊，用于把局部路徑規(guī)劃模塊計算的速度參數(shù)發(fā)送給三輪全向移動控制單元。

采用上述進一步方案的有益效果是實現(xiàn)了自主導航單元框架，尋找從當前位置前往目標位置的最佳路徑并避開障礙物，并能及時把導引系統(tǒng)當前位置和最新路徑通過導引交互單元反映出來。

進一步，所述三輪全向移動控制單元包括自主導航訪問接口模塊、里程計算模塊、電機調速模塊和電機控制接口模塊；

所述自主導航訪問接口模塊，用于從里程計算模塊得到導引系統(tǒng)位姿向量，并以里程計數(shù)據(jù)的形式反饋給自主導航單元用于自主定位；從激光雷達得到當前距離點信息，并以測距數(shù)據(jù)的形式反饋給自主導航單元用于自主定位；將自主導航單元發(fā)送的運動參數(shù)傳遞給電機調速模塊；

所述里程計算模塊，用于以固定周期計算導引系統(tǒng)的位姿向量，并通過自主導航訪問接口模塊將所述位姿向量反饋給自主導航單元；

所述電機調速模塊，用于根據(jù)自主導航單元下發(fā)的運動參數(shù)計算電機轉速，并發(fā)送給電機控制接口模塊；

所述電機控制接口模塊，用于從三輪全向底盤單元的電機驅動器返回電機當前轉速值到里程計算模塊，以及發(fā)送電機調速模塊計算得到的轉速值到電機驅動器。

采用上述進一步方案的有益效果是實現(xiàn)三輪全向移動底盤控制框架，獲取底盤里程數(shù)據(jù)，并對其運動進行控制，配合自主導航單元完成可自主移動的三輪全向式導引功能。

本發(fā)明還提供了一種基于三輪全向移動的交互式自主導引方法，包括步驟：

(1)操作人員通過導引交互單元設定目標位置后，該位置點被發(fā)送給自主導航單元；

(2)自主導航單元根據(jù)設定的目標位置計算導引系統(tǒng)從當前位置到目標位置的最佳路線，并在更新當前位置后將最新當前位置點和最佳路線一并反饋給導引交互單元，同時，計算前往目標位置的運動參數(shù)，并發(fā)送給三輪全向移動控制單元；另外，收到反饋的當前位置點和最佳路線后，導引交互單元以圖形方式把它們顯示在終端屏幕上；

(3)三輪全向移動控制單元根據(jù)運動參數(shù)生成控制信號，發(fā)送給三輪全向底盤單元；

(4)三輪全向底盤單元根據(jù)控制信號控制移動速度和方向。

采用上述進一步方案的有益效果是實現(xiàn)了三輪全向移動的交互式自主導航流程，擺脫了導軌對移動區(qū)域的限制、而且移動穩(wěn)定靈活。

進一步，所述步驟(1)中的導引交互單元工作過程為，

(11)人機接口模塊的目標位置指定模塊接收操作人員在場景地圖中輸入位置點pe；

(12)場景-概念地圖映射模塊把場景地圖的當前位置點pe轉換成概念地圖的me；

(13)將概念地圖的me通過導航控制接口模塊發(fā)送給自主導航單元；

(14)等待接收自主導航單元反饋的路徑軌跡點，如果導航控制接口收到了概念地圖的路徑軌跡點{m1，m2…mn}，n為軌跡點數(shù)目，則繼續(xù)下一步；否則，繼續(xù)等待；

(15)等待接收自主導航單元反饋的最新當前位置點，如果導航控制接口收到了概念地圖的最新當前位置點ms，則繼續(xù)下一步；否則，繼續(xù)等待。

(16)場景-概念地圖映射模塊把ms和{m1，m2…mn}轉換成場景地圖的當前位置點ps和路徑軌跡點序列{p1，p2…pn}，n為軌跡點數(shù)目；

(17)人機接口模塊通過當前位置顯示模塊、路徑標識模塊分別顯示當前位置點ps、最佳路徑軌跡點{p1，p2…pn}，同時，通過場景地圖顯示模塊描述完整的環(huán)境場景圖；

(18)自主導航單元是否提示到達目標位置點；如果是，則退出；否則，返回到步驟(14)。

采用上述進一步方案的有益效果是實現(xiàn)了導引交互的功能流程，完成了使用者與底層系統(tǒng)的信息交互，把目標位置及時下達給最底層的控制模塊，也保證把運動結果及時呈現(xiàn)給使用者。

進一步，所述步驟(2)中的自主導航單元工作過程為，

(21)交互接口模塊接收導引交互單元發(fā)送的概念地圖目標位置點me，并轉發(fā)給全局路徑規(guī)劃模塊；

(22)在概念地圖中，全局路徑規(guī)劃模塊按最短路徑策略尋找一條從當前位置點ms前往目標位置點me的最佳路徑；

(23)全局路徑規(guī)劃模塊根據(jù)最佳路徑生成的概念地圖路徑軌跡點序列{m1，m2…mn}，再通過交互接口模塊把該序列反饋給導引交互單元；

(24)局部路徑規(guī)劃模塊根據(jù)軌跡點序列中下一位置點m1，計算從ms移動到m1各速度參數(shù)：導引系統(tǒng)的局部坐標系xmrym下xm軸向線速度、ym軸向線速度和角速度，通過運動控制接口模塊以速度向量形式[umvmwm]向三輪全向移動控制單元發(fā)送調節(jié)電機速度命令；

(25)以t為周期，局部路徑規(guī)劃模塊通過自主定位模塊更新概念地圖的當前位置點ms，并通過交互接口模塊把ms反饋給導引交互單元；

(26)判斷當前位置點ms與目標位置點me距離是否為0；如果是，則退出并通過交互接口模塊提示到達目標位置點；否則，返回步驟(22)。

采用上述進一步方案的有益效果是實現(xiàn)了自主導航功能流程，完成自主定位、自主路徑規(guī)劃、自主運動控制并把運行信息反饋給上層人機交互單元。

進一步，所述步驟(25)中的以t為周期，局部路徑規(guī)劃模塊通過自主定位模塊更新概念地圖當前位置點ms，其具體過程為，

(251)將當前位置點ms設置為濾波器初始狀態(tài)x0；

(252)里程訪問模塊通過傳感器數(shù)據(jù)接口模塊發(fā)出訪問里程計數(shù)據(jù)命令并得到里程計當前數(shù)據(jù)ut，再根據(jù)前一時刻狀態(tài)xt-1和ut估算當前時刻狀態(tài)

(253)激光雷達數(shù)據(jù)訪問模塊通過傳感器數(shù)據(jù)接口模塊得到底盤上激光雷達測量的數(shù)據(jù)zt，再根據(jù)和zt修正最新當前時刻狀態(tài)xt；

(254)如果沒有達到濾波器定位要求，跳轉到步驟(252)；否則，返回最新當前時刻狀態(tài)xt，并將其具體值還原為最新當前位置點ms。

采用上述進一步方案的有益效果是實現(xiàn)了一個濾波器定位流程，利用概率方法計算出當前位置，完成導引系統(tǒng)的自主準確定位。

進一步，所述步驟(3)中的三輪全向移動控制單元工作過程為，

(31)三輪全向移動控制單元收到自主導航單元通過底盤控制接口發(fā)出的命令，并進行解析；

(32)判斷命令是否為訪問里程計數(shù)據(jù)，如果是，則由里程計算模塊計算導引系統(tǒng)位姿向量，并通過自主導航訪問接口把位姿向量反饋給自主導航單元，并等待下一條命令；否則，判斷命令是否為調節(jié)電機速度，如果是，則由電機調速模塊計算出三臺電機的轉速值，并通過電機控制接口把轉速值發(fā)送給三輪全向底盤單元的電機驅動器，并等待下一條命令；否則，執(zhí)行其它處理，并返回到步驟(31)。

進一步，所述步驟(32)中的里程計算模塊工作過程為，

(321)通過電機控制接口模塊讀取三個電機的轉速ω1、ω2和ω3，并根據(jù)轉速計算出三個電機的線速度分別為q1、q2和q3；

(322)根據(jù)三輪全向移動模型的正向運動學方程計算導引系統(tǒng)在世界坐標系xwoyw中的速度向量

其中，向量為底盤三個電機的線速度向量，θ為導引系統(tǒng)

在世界坐標系xwoyw中的轉角，p(θ)是電機速度與導引系統(tǒng)世界坐標系運

動速度的關系矩陣，計算公式如下：

其中，l為輪子中心與底盤中心的距離，δ為輪子正向軸線與坐標系xmrym的xm軸夾角；

(323)根據(jù)下述公式以固定周期⊿t推算導引系統(tǒng)的運動增量⊿x、⊿y和⊿θ；再迭代累計x和y方向上的坐標以及旋轉角度，即得里程數(shù)據(jù)[x,y,θ]；

所述步驟(33)中的電機調速模塊工作過程為，

(331)接收自主導航單元的局部路徑規(guī)劃模塊發(fā)送的速度um,vm和wm；

(332)根據(jù)三輪全向移動模型的逆運動學方程分別計算底盤三個電機的線速度向量

其中，導引系統(tǒng)局部坐標系xmrym中p(δ)是導引系統(tǒng)局部坐標系移動速度與電機運動速度的關系矩陣，計算公式如下：

其中，l為輪子中心與底盤中心的距離，δ為輪子正向軸線與坐標系xmrym的xm軸夾角；

(333)根據(jù)電機線速度分別計算出三個電機的轉速ω1、ω2和ω3，并通過電機控制接口把轉速值發(fā)給電機驅動器來調節(jié)電機轉速。

采用上述進一步方案的有益效果是實現(xiàn)里程和電機調節(jié)參數(shù)計算流程，為系統(tǒng)的定位和運動控制提供基礎數(shù)據(jù)。

附圖說明

圖1是基于三輪全向移動的交互式自主導引系統(tǒng)原理框圖；

圖2是導引交互單元原理框圖；

圖3是導引交互單元工作流程圖；

圖4是自主導航單元原理框圖；

圖5是自主導航單元工作流程圖；

圖6是三輪全向移動控制單元原理框圖；

圖7是三輪全向移動模型圖；

圖8是三輪全向移動控制單元工作流程圖；

圖9是三輪全向移動底盤結構圖；

圖10是全向輪結構圖。

具體實施方式

以下結合附圖對本發(fā)明的原理和特征進行描述，所舉實例只用于解釋本發(fā)明，并非用于限定本發(fā)明的范圍。

如圖1所示，基于三輪全向移動的交互式自主導引系統(tǒng)包括依次連接的導引交互單元、自主導航單元、三輪全向移動控制單元和三輪全向底盤單元。導引交互單元是一臺移動終端，其上運行的交互軟件支持操作人員指定目標位置；自主導航單元根據(jù)目標位置計算導引系統(tǒng)從當前位置到目標位置的最佳路線，并反饋給導引交互單元顯示出來，同時，計算前往目標位置的運動參數(shù)，并發(fā)送給三輪全向移動控制單元；三輪全向移動控制單元根據(jù)運動參數(shù)生成控制信號，發(fā)送給三輪全向底盤單元，控制其移動速度和方向。

如圖2所示，導引交互單元運行在一臺有人機接口的移動終端上，通過交互軟件來接收操作人員指定的目標位置,并通過人機接口的圖形化界面顯示導引系統(tǒng)當前位置與目標位置之間的最佳路徑。

導引交互單元包括：人機接口模塊、場景地圖管理模塊和導航控制接口模塊。

其中，場景地圖管理模塊包括：場景地圖模塊和場景-概念地圖映射模塊。場景地圖模塊是指操作人員通過交互軟件看到的周圍環(huán)境地形圖，通過圖形化的像素點來表示(場景地圖中，當前位置點表示為ps，目標位置點表示為pe)；相對應的，概念地圖模塊是指自主導航單元用于定位的方位圖，通過坐標化的柵格點來表示(概念地圖中，當前位置點表示為ms，目標位置點表示為me)；場景-概念地圖映射模塊把場景地圖中的“點”轉換成概念地圖中的“點”，或者把概念地圖中的“點”轉換成場景地圖中的“點”，只有表示成場景地圖中的“點”，當前位置點、目標位置點和連接二者的路徑軌跡點才能通過人機接口模塊正確顯示出來。

人機接口模塊提供圖形化的人機操作界面，以場景地圖的形式描述導引系統(tǒng)所處環(huán)境，將操作人員輸入的場景地圖目標位置點發(fā)送給場景地圖管理模塊；同時，以圖形方式在場景地圖中顯示導引系統(tǒng)當前位置點和運動軌跡點，具體包括四個模塊：場景地圖顯示模塊、當前位置顯示模塊、目標位置指定模塊和路徑標識模塊。場景地圖顯示模塊以圖形方式把環(huán)境地圖直觀地顯示在移動終端屏幕上，當前位置顯示以圖形方式把導引系統(tǒng)當前位置點ps顯示在移動終端屏幕上，目標位置指定模塊以交互式圖形直觀顯示導引系統(tǒng)前往的目標位置點pe，路徑標識模塊以圖形方式在終端屏幕上顯示ps前往pe的路徑軌跡點{p1，p2…pn}，n為軌跡點數(shù)目。

導航控制接口模塊連接了導引交互單元和自主導航單元。導引交互單元通過場景-概念地圖映射模塊把pe轉換成me，并通過導航控制接口模塊發(fā)送給自主導航單元。同時，導引交互單元通過該導航控制接口模塊接收自主導航單元反饋的最新當前位置點ms、最佳路徑軌跡點序列{m1，m2…mn}，n為軌跡點數(shù)目，并通過場景-概念地圖映射模塊把上述點轉換成場景地圖中的ps和軌跡點序列{p1，p2…pn}。

如圖3所示，導引交互單元工作流程如下：

第一步，接收目標位置點pe。人機接口的目標位置指定模塊接收操作人員輸入的位置點pe；

第二步，把場景地圖的pe轉換成概念地圖的me。場景-概念地圖映射模塊把pe轉換成me，便于后面的自主導航單元在概念地圖中定位操作人員輸入的目標位置點；

第三步，把概念地圖的me發(fā)送給自主導航單元。概念地圖中的目標位置me生成后，導引交互單元通過導航控制接口把me發(fā)送給自主導航單元。

第四步，等待接收自主導航單元反饋的路徑軌跡點{m1，m2…mn}。如果導航控制接口收到了路徑軌跡點{m1，m2…mn}，則繼續(xù)下一步；否則，繼續(xù)等待；

第五步，等待接收自主導航單元反饋的最新當前位置ms。如果導航控制接口收到了導引系統(tǒng)最新當前位置ms，則繼續(xù)下一步；否則，繼續(xù)等待；

第六步，把ms和{m1，m2…mn}分別轉換成ps和{p1，p2…pn}。場景-概念地圖映射模塊把概念地圖中的ms和{m1，m2…mn}轉換成場景地圖中的當前位置ps和路徑軌跡點序列{p1，p2…pn}；

第七步，在場景地圖中顯示當前位置點ps、最佳路徑軌跡點{p1，p2…pn}，人機接口通過當前位置顯示模塊和路徑標識模塊分別顯示ps和{p1，p2…pn}，同時，通過場景地圖顯示模塊描述完整的環(huán)境場景圖；

第八步，自主導航單元提示是否到達目標位置。如果是，則退出；否則，跳轉至第四步。

如圖4所示，自主導航單元運行在一臺嵌入式計算機上，根據(jù)設定的目標位置計算導引系統(tǒng)從當前位置到目標位置的最佳路線，更新當前位置，并將最佳路線和最新當前位置反饋給導引交互單元，同時，計算前往目標位置的運動參數(shù)，并發(fā)送給三輪全向移動控制單元；包括：交互接口模塊、路徑規(guī)劃模塊、基礎功能模塊和底盤接口模塊。

其中，交互接口模塊：自主導航單元通過交互接口模塊接收導引交互單元發(fā)送的目標位置me，同時把最新當前位置ms和路徑軌跡點序列{m1，m2…mn}反饋給導引交互單元。

路徑規(guī)劃模塊包括兩個模塊：全局路徑規(guī)劃模塊和局部路徑規(guī)劃模塊。其中，全局路徑規(guī)劃模塊根據(jù)導引交互單元發(fā)來的目標位置me，在概念地圖中按最短路徑策略尋找一條從當前位置ms前往me的最佳路徑，并將沿途的軌跡點序列{m1，m2…mn}通過交互接口模塊發(fā)送給導引交互單元。局部路徑規(guī)劃模塊通過自主定位模塊更新當前位置ms，并將ms通過交互接口發(fā)送給導引交互單元；同時，根據(jù)軌跡點序列中下一位置點m1，計算從ms移動到m1的線速度和角速度(在導引系統(tǒng)的局部坐標系xmrym下xm軸向線速度、ym軸向線速度和角速度分別為um、vm和wm)，并通過運動控制接口模塊把速度向量[umvmwm]發(fā)送給三輪全向移動控制單元。

基礎功能模塊包括概念地圖管理和自主定位兩個模塊。其中，概念地圖管理模塊用來創(chuàng)建概念地圖并存儲。這里的概念地圖是指自主導航單元用于定位的方位圖，采用坐標化的柵格點來表示，通過slam方法來創(chuàng)建。自主定位模塊用來計算導引系統(tǒng)在概念地圖中的當前位置ms，通過如下的貝葉斯濾波定位步驟得到：

第一步，把當前位置ms設置為濾波器初始狀態(tài)x0；

第二步，里程訪問子模塊通過傳感器數(shù)據(jù)接口模塊得到底盤里程計當前數(shù)據(jù)ut，再根據(jù)上一時刻狀態(tài)xt-1和ut估算當前狀態(tài)xt；

第三步，激光雷達數(shù)據(jù)訪問子模塊通過傳感器數(shù)據(jù)接口模塊得到底盤上激光雷達測量的數(shù)據(jù)zt，再根據(jù)和zt修正最新當前狀態(tài)xt；

第四步，如果沒有達到濾波器定位要求，跳轉到第二步；否則，返回最新當前狀態(tài)xt，并將其具體值還原為最新當前位置ms。

底盤接口模塊包括兩個模塊：傳感器數(shù)據(jù)接口模塊和運動控制接口模塊。傳感器數(shù)據(jù)接口模塊支持從底盤讀取里程計數(shù)據(jù)和激光雷達數(shù)據(jù)；而運動控制接口模塊把局部路徑規(guī)劃模塊計算的速度向量[umvmwm]發(fā)送給三輪全向移動控制單元。

自主導航單元工作流程如圖5所示，詳細步驟如下：

第一步，接收導引交互單元發(fā)送的目標位置me，交互接口模塊接收導引交互單元發(fā)送的概念地圖中目標位置me，并轉發(fā)給全局路徑規(guī)劃模塊；

第二步，計算當前位置ms與目標位置me間的最佳路徑。在概念地圖中，全局路徑規(guī)劃模塊按最短路徑策略尋找一條從當前位置ms前往me的最佳路徑；

第三步，生成路徑軌跡點序列{m1，m2…mn}并反饋給導引交互單元。全局路徑規(guī)劃模塊根據(jù)最佳路徑生成的路徑軌跡點序列{m1，m2…mn}，再通過交互接口模塊把該序列反饋給導引交互單元；

第四步，計算導引系統(tǒng)的運動參數(shù)[umvmwm]并發(fā)送給三輪全向移動控制單元。局部路徑規(guī)劃模塊根據(jù)軌跡點序列中下一位置點m1，計算從ms移動到m1各速度參數(shù)：導引系統(tǒng)的局部坐標系xmrym下xm軸向線速度、ym軸向線速度和角速度，并通過運動控制接口模塊以速度向量形式[umvmwm]發(fā)送給三輪全向移動控制單元；

第五步，更新當前位置ms并反饋給導引交互單元。以t為周期，局部路徑規(guī)劃模塊通過自主定位模塊更新當前位置ms，并通過交互接口把ms反饋給導引交互單元；

第六步，判斷ms與me距離是否為0，如果是，則退出并通過交互接口提示導航交互單元導航結束；否則，跳轉至第二步。

如圖6所示，三輪全向移動控制單元運行在一臺嵌入式處理器上，負責導引系統(tǒng)的移動控制，包括了自主導航訪問接口模塊、里程計算模塊、電機調速模塊和電機控制接口模塊，其中，自主導航訪問接口模塊從里程計算模塊得到導引系統(tǒng)位姿向量[x,y,θ]，并以里程計數(shù)據(jù)ut的形式反饋給自主導航單元用于自主定位；從激光雷達得到當前距離點信息，并以測量數(shù)據(jù)zt的形式反饋給自主導航單元用于自主定位；以向量[um,vm,wm]的形式把自主導航單元發(fā)送的運動參數(shù)傳遞給電機調速模塊。電機控制接口模塊可返回電機轉速、也可設置電機轉速值。

1)里程計算模塊：里程計算模塊以固定周期⊿t計算導引系統(tǒng)的世界坐標系xwoyw位姿向量[x,y,θ]。當導引系統(tǒng)需要自主定位時，自主導航單元的里程訪問模塊通過傳感器數(shù)據(jù)接口模塊獲取上述位姿向量。

基于圖7中三輪全向移動模型，里程計算模塊依如下步驟計算導引系統(tǒng)的世界坐標系xwoyw位姿向量[x,y,θ]；

①通過電機控制接口模塊讀取三個電機的轉速ω1、ω2和ω3，并根據(jù)轉速計算出三個電機的線速度分別為q1、q2和q3。

②根據(jù)三輪全向移動模型的正向運動學方程計算導引系統(tǒng)在世界坐標系xwoyw中的速度向量

其中，向量為底盤三個電機的線速度向量，θ為導引系統(tǒng)在世界坐標系xwoyw中的轉角，p(θ)是電機速度與導引系統(tǒng)全局坐標系運動速度的關系矩陣，計算公式如下：

其中，l為輪子中心與導引系統(tǒng)中心的距離，δ為輪子正向軸線與坐標系xmrym的xm軸夾角。

③根據(jù)下述公式以固定周期⊿t推算導引系統(tǒng)的運動增量；再迭代累計x和y方向上的坐標以及旋轉角度，即得里程數(shù)據(jù)[x,y,θ]；

2)電機調速模塊：當局部路徑規(guī)劃模塊需要驅動底盤移動時，運動控制參數(shù)通過運動控制接口發(fā)送給電機調速模塊，這些速度可用向量[um,vm,wm]表示。基于圖7中三輪全向移動模型，電機調速模塊依如下步驟調節(jié)電機；

①接收自主導航單元的局部路徑規(guī)劃模塊發(fā)送的速度um,vm和wm。

②根據(jù)三輪全向移動模型的逆運動學方程計算底盤三個電機的線速度向量

其中，導引系統(tǒng)局部坐標系xmrym中p(δ)是導引系統(tǒng)局部坐標系移動速度與電機運動速度的關系矩陣，可用如下公式計算，

其中，l為輪子中心與底盤中心的距離，δ為輪子正向軸線與坐標系xmrym的xm軸夾角。

③根據(jù)電機線速度計算出三個電機的轉速ω1，ω2和ω3，并通過電機控制接口把轉速值發(fā)給電機驅動器來調節(jié)電機轉速。

三輪全向移動控制單元按圖8流程工作，具體步驟如下：

第一步，接收自主導航單元下發(fā)的命令。三輪全向移動控制單元收到自主導航單元通過底盤控制接口發(fā)出的命令，并進行解析；

第二步，判斷命令是否為訪問里程計數(shù)據(jù)，如果是，則由里程計算模塊計算導引系統(tǒng)位姿向量，并通過自主導航訪問接口把位姿向量反饋給自主導航單元，并等待下一條命令；否則，判斷命令是否為調節(jié)電機速度，如果是，則由電機調速模塊計算出三臺電機的轉速值，并通過電機控制接口把轉速值發(fā)送給電機驅動器，并等待下一條命令；否則，執(zhí)行其它處理，并返回到第一步。

三輪全向移動控制單元所控制的導引系統(tǒng)底盤結構如圖9所示，三輪全向底盤單元安裝了三個全向輪、三個電機、三個電機驅動器、激光雷達、電源以及底盤支架；全向輪為多方向可自由滾動的運動機構；電機用于給全向輪滾動提供動力；電機驅動器用于根據(jù)三輪全向移動控制單元發(fā)送的轉速值控制電機轉動、并反饋當前電機實際轉速值；激光雷達用于提供導引系統(tǒng)到周圍障礙物的測距信息；電源用于提供導引系統(tǒng)各組成單元及其模塊的工作電源；底盤支架用于承載并固定導引系統(tǒng)各組成單元及其模塊的機構。底盤支架中心通過連接軸把三個全向輪連接起來，軸間夾角120°，電機安裝在連接軸上。這種結構非常適合機器人沿各個方向自由移動、并繞底盤中心360°旋轉。

全向輪結構如圖10所示，包括兩個相互錯開的輪體，輪體上徑向均勻排列著與輪圈外周切線方向垂直的從動輪，保證輪體在多個方向自由移動。在運動機構上設置三輪全向移動底盤，不僅擺脫了導軌對移動區(qū)域的限制、而且移動靈活；在自主導航上采用了自主路徑規(guī)劃和自定位，提高了導引系統(tǒng)的智能性；在人機交互上采用了圖形化交互接口，方便使用者指定導引目標、了解當前位置和導引路線。

以上所述僅為本發(fā)明的較佳實施例，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內，所作的任何修改、等同替換、改進等，均應包含在本發(fā)明的保護范圍之內。