一種基于多傳感器的助行機器人人機接口的避障控制方法
【專利摘要】本發(fā)明公開了一種基于多傳感器的助行機器人人機接口的避障控制方法；人機接口包括壓力采集模塊、障礙物探測模塊和控制器；壓力采集模塊用于檢測操作者手部的作用力；障礙物探測模塊用于探測周邊障礙物；控制器與壓力采集模塊和障礙物探測模塊連接，用于根據(jù)壓力采集模塊采集的數(shù)據(jù)以及障礙物探測模塊探測的數(shù)據(jù)計算助行機器人的速度，并根據(jù)助行機器人的速度控制助行機器人實現(xiàn)助行和避障功能；避障控制方法包括步驟：當在緊急避障區(qū)內(nèi)存在障礙物時，判斷操作者的意圖力FI是否有X軸負方向的分量，若是則助行機器人遵從操作者的意圖后退避障，若否則助行機器人將采取側(cè)向平移的方式來規(guī)避緊急避障區(qū)的障礙物。本發(fā)明控制有效、方便可靠且成本低廉。
【專利說明】
一種基于多傳感器的助行機器人人機接口的避障控制方法
[0001] 母案的申請日：2015年4月27日，申請?zhí)枺?015102098460，
[0002] 發(fā)明名稱:一種基于多傳感器的助行機器人人機接口及其避障控制方法
技術(shù)領(lǐng)域
[0003] 本發(fā)明屬于人機交互、康復(fù)輔具領(lǐng)域，更具體地，涉及一種基于多傳感器的助行機 器人人機接口的避障控制方法。
【背景技術(shù)】
[0004] 我國作為一個人口大國，約有1.5億老年人，殘疾人8296萬，涉及2.6億家庭人口。 根據(jù)世界衛(wèi)生組織提出的60歲以上的人群占社會總?cè)藬?shù)10%以上，或65歲以上的人群占社 會總?cè)藬?shù)的7 %以上時為老年化國家，據(jù)2000年統(tǒng)計，我國60歲以上的老年人口系數(shù) 10.18%，我國己進入老年化社會，到2020年我國老年人將高達15%以上，老年人的生活健 康問題，將成為社會問題。隨著我國進入老齡化社會，行走功能障礙的老年人及各種老年病 患者逐年增加，嚴重地影響了他們的生活質(zhì)量，給家庭和社會帶來沉重的負擔。
[0005] 行走不便幾乎是困擾老年人和下肢功能障礙者正常生活的最嚴重問題，為支援和 改善各種下肢功能障礙人群的步行能力，助行用下肢康復(fù)機器人已成為國內(nèi)外機器人與康 復(fù)醫(yī)學領(lǐng)域的研究熱點。下肢康復(fù)機器人有助行機器人和智能拐杖等裝置，基本原理是利 用傳感裝置提取患者上肢的力信號，根據(jù)經(jīng)過處理的力信號判斷患者的意圖，從而幫助患 者完成相應(yīng)方向的移動。就目前的傳感裝置來說，一種方案是利用價格高昂的六維力傳感 器，無疑提高了助行機器人的成本;另一種是通過多個一維力傳感器組成傳感器陣列，通過 相應(yīng)的數(shù)據(jù)和設(shè)計的公式進行計算從而判斷人的運動意圖。這種方案能極大的降低人機接 口的成本，但是它的研究尚處于初級階段，已有的設(shè)計存在各種不同程度的缺陷，不能有效 的采集相關(guān)方向的力的數(shù)據(jù)。
[0006] 與此同時，助行機器人系統(tǒng)的使用者多為老年人，他們普遍視力較差，并且比常人 更有可能錯誤地操作助行機器人從而帶來安全隱患。但是目前的助行機器人系統(tǒng)大多缺少 安全機制。
[0007] 因此目前亟需開發(fā)出一套能有效檢測出患者作用力，造價低廉，同時能夠矯正操 作者可能產(chǎn)生的錯誤意圖的人機接口以實現(xiàn)助行用下肢康復(fù)機器人與患者運動意圖之間 的協(xié)同控制，同時強化的環(huán)境適應(yīng)能力，以取得更好的助行效果，以期待逐步實現(xiàn)助行機器 人產(chǎn)品化和產(chǎn)業(yè)化。

【發(fā)明內(nèi)容】

[0008] 針對現(xiàn)有技術(shù)的缺陷，本發(fā)明的目的在于提供一種控制有效、方便可靠且成本低 廉的助行機器人人機接口的避障控制方法。
[0009] 本發(fā)明提供了一種基于多傳感器的助行機器人人機接口的避障控制方法，所述助 行機器人人機接口包括壓力采集模塊、障礙物探測模塊和控制器;所述壓力采集模塊用于 檢測操作者手部的作用力;所述障礙物探測模塊用于探測周邊障礙物;所述控制器與所述 壓力采集模塊和所述障礙物探測模塊連接，用于根據(jù)所述壓力采集模塊采集的數(shù)據(jù)以及所 述障礙物探測模塊探測的數(shù)據(jù)計算助行機器人的速度，并根據(jù)所述助行機器人的速度控制 所述助行機器人實現(xiàn)助行和避障功能;其特征在于，所述避障控制方法包括下述步驟:當在 緊急避障區(qū)內(nèi)存在障礙物時，判斷操作者的意圖力？:是否有X軸負方向的分量，若是，則助 行機器人遵從操作者的意圖后退避障，若否，則助行機器人將采取側(cè)向平移的方式來規(guī)避 緊急避障區(qū)的障礙物;其中，以助行機器人中心點為坐標原點，以助行機器人的相對正前方 為X軸正方向；以助行機器人的相對左方為Y軸正方向。
[0010]更進一步地，所述緊急避障區(qū)為矩形，矩形的長度為Length，寬度為Width，當激光 傳感器與激光傳感器的一個探測點之間的距離L滿足下式時，
；則認為 障礙物在緊急避障區(qū)內(nèi)，其中α為障礙物的某點與權(quán)利要求7中定義的X軸的夾角。
[0011]更進一步地，當障礙物在緊急避障區(qū)內(nèi)時，通過統(tǒng)計X軸左側(cè)和右側(cè)滿足矩形條件 的點的個數(shù)，選擇點數(shù)少的一側(cè)進行平移;在平移過程中實時檢測平移方向上是否有足夠 的避障空間，根據(jù)檢測情況可改變平移方向或停止。
[0012] 更進一步地，所述壓力采集模塊包括力傳感裝置、信號調(diào)理模塊和數(shù)據(jù)采集模塊； 所述力傳感裝置為二層套筒結(jié)構(gòu)，且外層是內(nèi)部被鏤空成長方體的圓柱體，內(nèi)層是方形柱 體，圓柱體與方形柱體之間有縫隙，在內(nèi)層方形柱體的四個側(cè)面分別設(shè)置有1個FSR傳感器 填充所述縫隙；用于采集操作者左右手作用力縱向和橫向的分量;所述信號調(diào)理模塊用于 對所述力傳感裝置采集的作用力進行調(diào)理后輸出；所述數(shù)據(jù)采集模塊用于對經(jīng)過調(diào)理后的 數(shù)據(jù)進行AD轉(zhuǎn)換后獲得電壓值。
[0013] 更進一步地，所述障礙物探測模塊包括激光傳感器以及用于連接所述激光傳感器 與所述控制器的USB高速數(shù)據(jù)線;所述激光傳感器用于采集周邊環(huán)境的障礙物信息，并通過 所述USB數(shù)據(jù)線將采集的障礙物信息發(fā)送到控制器。
[0014] 更進一步地，所述控制器用于根據(jù)人工勢場法將障礙物探測模塊采集的障礙物信 息轉(zhuǎn)換為斥力；計算斥力與操作者作用力的合力，并將合力轉(zhuǎn)換為用于控制助行機器人實 現(xiàn)助行的速度。
[0015] 本發(fā)明的優(yōu)點在于，首先在操作者作用力檢測方面利用廉價的FSR傳感器和設(shè)計 的力傳感裝置代替了傳統(tǒng)的六軸力傳感器，降低成本的同時又能有效檢測操作者的作用 力；利用激光傳感器對周邊障礙物進行掃描，使用掃描得到的障礙物信息對操作者進行引 導(dǎo)和矯正，保證了操作者和助行機器人的安全;利用多線程實現(xiàn)的控制方法同時滿足了助 行機器人避障和助行的需求，并且保證了實時性;相對簡單的結(jié)構(gòu)使得該助行機器人人機 接口可以適用于各種機械結(jié)構(gòu)的助行機器人，易于推廣應(yīng)用，滿足用戶不同的需求。
【附圖說明】
[0016] 圖1是本發(fā)明提供的多傳感器助行機器人人機接口系統(tǒng)框圖。
[0017] 圖2是本發(fā)明實施例的一個應(yīng)用實例示意圖。
[0018] 圖3是本發(fā)明所使用的FSR(Force Sensing Registers，力傳感記錄單元)傳感器 的機械裝置原理圖。
[0019] 圖4是本發(fā)明所使用的FSR傳感器的信號調(diào)理電路的原理圖，其中（a)是放大電路 的具體電路圖，（b)是參考電壓產(chǎn)生電路的具體電路圖。
[0020] 圖5是本發(fā)明實施例應(yīng)用于一個助行機器人實例的受力分析圖。
[0021 ]圖6是本發(fā)明實施例所使用的激光傳感器的工作原理圖。
[0022] 圖7是本發(fā)明所描述的障礙物的結(jié)構(gòu)示意圖。
[0023] 圖8是本發(fā)明所應(yīng)用的障礙物合并算法的示意圖；其中(a)是兩個鄰近障礙物合并 前的表示方法，（b)是兩個鄰近障礙物合并后的表示方法。
[0024] 圖9是本發(fā)明提出的緊急避障方案設(shè)置的緊急避障區(qū)域示意圖。
[0025] 圖10是本發(fā)明提出的緊急避障方案的狀態(tài)轉(zhuǎn)換圖。
[0026]圖11是本發(fā)明實施例的多線程程序流程圖。
【具體實施方式】
[0027]為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點更加清楚明白，以下結(jié)合附圖及實施例，對 本發(fā)明進行進一步詳細說明。應(yīng)當理解，此處所描述的具體實施例僅僅用以解釋本發(fā)明，并 不用于限定本發(fā)明。
[0028]本發(fā)明提供了一種控制有效、方便可靠且成本低廉的助行機器人人機接口的避障 控制方法。
[0029]如圖1所示，人機接口包括壓力采集模塊1，障礙物探測模塊2以及控制器3;壓力采 集模塊1用于檢測操作者手部的作用力；障礙物探測模塊2用于探測周邊障礙物;控制器3與 壓力采集模塊1和障礙物探測模塊2連接，用于根據(jù)壓力采集模塊1采集的數(shù)據(jù)以及障礙物 探測模塊2探測的數(shù)據(jù)計算助行機器人的速度，并根據(jù)助行機器人的速度控制所述助行機 器人實現(xiàn)助行和避障功能。
[0030] 壓力采集模塊1包括依次連接的力傳感裝置11、信號調(diào)理模塊12和數(shù)據(jù)采集模塊 13;力傳感裝置11為二層套筒結(jié)構(gòu)，外層為內(nèi)部被鏤空成長方體的圓柱體，內(nèi)層為方形柱 體；圓柱體與方形柱體之間有縫隙，在每一層的方形柱體的四個側(cè)面分別設(shè)置有1個FSR傳 感器填充所述縫隙；力傳感裝置11用于采集操作者左右手作用力縱向和橫向的分量;信號 調(diào)理模塊12用于對所述力傳感裝置采集的作用力進行調(diào)理后輸出；數(shù)據(jù)采集模塊13用于對 經(jīng)過調(diào)理后的數(shù)據(jù)進行AD轉(zhuǎn)換后獲得電壓值。
[0031] 障礙物探測模塊2包括激光傳感器21以及用于連接所述激光傳感器21與控制器3 的高速USB數(shù)據(jù)線;激光傳感器21用于采集周邊環(huán)境的障礙物信息，并通過所述高速USB數(shù) 據(jù)線將采集的障礙物信息發(fā)送到控制器。
[0032] 控制器3用于根據(jù)人工勢場法將障礙物探測模塊2采集的障礙物信息轉(zhuǎn)換為斥力； 計算斥力與操作者作用力的合力，并將合力轉(zhuǎn)換為用于控制助行機器人實現(xiàn)助行的速度。
[0033] 圖2示出了本發(fā)明實施例提供的助行機器人人機接口的一個應(yīng)用實例;對于壓力 采集模塊1，力傳感裝置11包括8個FSR((Force Sensing Registers，力傳感記錄單元））傳 感器，其機械結(jié)構(gòu)如圖3所示，是經(jīng)過特定布局和機械加工的力傳感裝置;結(jié)合信號調(diào)理模 塊12和數(shù)據(jù)采集模塊13,壓力采集模塊1能夠檢測操作者手部的作用力。障礙物探測模塊2 由激光傳感器21和用于與控制器3連接的高速USB數(shù)據(jù)線組成，可以探測周邊障礙物;控制 器3選用工控機，它與壓力采集模塊1和障礙物探測2模塊進行通信，對它們采集的數(shù)據(jù)進行 綜合分析和計算，得到一個合適的速度發(fā)送到外部通訊接口，并能對操作者的操作進行引 導(dǎo)，保證控制安全。與控制器相連的外部通訊接口在本實施例中用于連接全向電機底盤，電 機底盤可以帶動助行機器人的運動。
[0034]圖3是本發(fā)明壓力采集模塊1中的力傳感裝置11的機械原理圖，表現(xiàn)了圖2中操作 者手握部分，即力傳感裝置11的細節(jié)設(shè)計。力傳感裝置11為左右兩個二層套筒結(jié)構(gòu)，外層為 內(nèi)部被鏤空成長方體的圓柱體，內(nèi)層為方形柱體;4個經(jīng)過統(tǒng)一處理的FSR傳感器分別被粘 貼在了一個內(nèi)層長方形柱體的前后左右四個側(cè)面上。內(nèi)層和外層之間有一定縫隙，這個縫 隙可以由4個貼在方形柱體側(cè)面的處理過的傳感器恰好填充。在本發(fā)明實施例中，F(xiàn)SR傳感 器的正反兩面粘貼了柔軟平滑的膠皮，膠皮的外部為受力部分，如此處理的FSR傳感器可以 充分將外部壓力反應(yīng)到電阻的變化上，從而產(chǎn)生變化明顯的電壓信號。當手對外面的圓柱 體施加任意方向的推力時，這種力傳感裝置11都可以將作用力分解為縱向（前后）和橫向 (左右)的分力。力傳感裝置11由金屬制成，因此它不會因為握力產(chǎn)生形變，很好的過濾了力 信號中無用的握力，并且得到需要的推力或拉力。
[0035]圖4是本發(fā)明中的信號調(diào)理模塊的原理圖。信號調(diào)理模塊包括簡易放大電路和基 準電壓產(chǎn)生電路。基準電壓產(chǎn)生電路由分壓電路和電壓跟隨電路組成，它的輸出端被接入 到簡易放大電路的正向輸入端作為參考電壓Vref。簡易放大電路為由運算放大器組成的同 相放大電路，F(xiàn)SR傳感器作為電阻被接入到同相放大電路中，設(shè)其阻值為R fsr。查閱FSR傳感 器的產(chǎn)品資料可知壓力與電阻的大小近似成反比例函數(shù)關(guān)系，隨著壓力的增大，F(xiàn)SR傳感器 的電阻將迅速減小。設(shè)F = N X Rfs，1……（1 );同相放大器輸出電壓V。可表示為:Vo = Vref X (1 +RiXRfsr_1)……（2);將式（1)代入式（2)可得:￥。= ￥:(^\(1+1?1\?\『1)……（3);其中？為 作用在FSR傳感器上的壓力大小;N為比例系數(shù);心為圖4中簡易放大電路的可變電阻的阻 值。
[0036] 由式（1)(2)可知，當壓力很小時，同相放大電路中的FSR傳感器的電阻Rfsr遠大于 可變電阻心的值，電壓跟隨器的輸出即為基準電壓V ref。由式(3)可以看出，本發(fā)明采用的電 路設(shè)計可以在一定范圍內(nèi)讓壓力與輸出電壓的關(guān)系變成線性關(guān)系，輸出電壓隨著壓力的增 大而線性增大。電路的輸入接口分為兩組，每組均可接入4個FSR傳感器(共8個），恰好可接 入力傳感裝置11所必須的8個FSR傳感器。輸出接口有10個引腳，包括8路電壓跟隨器的輸出 電壓和兩個地線。輸出接口與數(shù)據(jù)采集模塊13相連（以研華的USB4716為例），因此輸出引腳 的排列順序與數(shù)據(jù)采集模塊13的模擬電壓輸入引腳的排列相一致以便于接線。
[0037] 本發(fā)明所使用的數(shù)據(jù)采集模塊13可以是研華公司生產(chǎn)的數(shù)據(jù)采集卡USB4716,可 以根據(jù)需要換成任意一種能進行大于12位的AD轉(zhuǎn)換的數(shù)據(jù)采集卡。由于數(shù)據(jù)采集卡 USB4716是技術(shù)上很成熟的產(chǎn)品，因此硬件上無需任何改動，需要做的就是根據(jù)產(chǎn)品說明書 編寫控制程序讓它完成相應(yīng)的AD轉(zhuǎn)換工作。
[0038]圖5所示的是本發(fā)明實施例壓力采集模塊11的受力分析圖，如權(quán)利要求7中所述， 以向前為X軸正方向，向左為Y軸正方向，建立助行機器人的局部坐標系，壓力采集模塊與障 礙物探測模塊與本發(fā)明實施例中所使用的底盤的坐標系相一致，因為統(tǒng)一坐標系有利于簡 化計算。結(jié)合圖2易于理解，圖5中FHL和FHR分別表示操作者左、右手的作用力，其中F 1X，F(xiàn)1Y， Frx，F(xiàn)rY分別是FHL和FHR沿X軸和Y軸方向的分解。Fr是根據(jù)障礙物探測模塊檢測到的障礙物 信息計算出來的前方障礙物對助行機器人產(chǎn)生的斥力。根據(jù)本實施例建立的統(tǒng)一坐標系， 結(jié)合圖5,設(shè)左邊套筒位于X軸正方向上的FSR傳感器所測量的壓力值為h，按順時針方向各 個FSR傳感器的測量值依次為F:\F4，同理，右邊套筒FSR傳感器的測量值為F 5~F8。由此我 們可以將操作者左右手的作用力沿X軸和Y軸的分量用Fi~F8的數(shù)值加減表示，如式(4)所
[0039] 助行機器人的移動主要可分為前后平移，左右平移和旋轉(zhuǎn)運動。力傳感裝置采集 的數(shù)據(jù)是兩個手的力分別在X軸和Y軸作用力的分量?？刂破?將障礙物產(chǎn)生的斥力進行分 解并計算合力，那么根據(jù)以下基本方法可以控制助行機器人的運動：
[0040] (1)如果X軸方向合力不為0且左右手力方向一致，那么助行機器人沿X軸的合力方 向運動；
[0041] (2)如果Y軸方向合力不為0,那么助行機器人沿Y軸的合力方向運動；
[0042] (3)如果X軸的左右手力方向相反，那么根據(jù)力臂計算力矩，助行機器人沿力矩方 向作旋轉(zhuǎn)運動；
[0043]以上三條基本方法可以任意組合，從而使助行機器人在二維平面上完成所有的基 本運動。
[0044]本發(fā)明實施例中的激光傳感器21是日本北陽公司生產(chǎn)的型號為URG-04LX-UG01的 激光傳感器，其工作原理圖如圖6所示，按之前建立的統(tǒng)一坐標系，定義X軸正方向為0°，那 么該激光傳感器可以實現(xiàn)按逆時針方向進行從-120°到120°的全方位掃描。激光傳感器21 的作用是掃描助行機器人的周邊的障礙物，并根據(jù)人工勢場法計算障礙物的斥力，具體如 下：
[0045]人工勢場法斥力計算的基本公式如下:Fri = KX(r-R〇rn……（5);式中K、Ro、n為常 數(shù)，一般取正整數(shù)。r為障礙物與機器人的距離，F(xiàn)ri*斥力的大小。
[0046] 對于環(huán)境中的每個障礙物，設(shè)id個六元組結(jié)構(gòu){rEdge，rAngle，lEdge，lAngle， Force，Angle}來作為障礙物鏈表中的單元內(nèi)容。如圖7所示，激光傳感器21在0點，rEdge表 示障礙物右邊沿與激光傳感器21的距離|0B|，rAngle表示障礙物右邊沿與激光傳感器21連 線與X軸正方向的夾角ZBOX。同理，lEdge與lAng 1 e分別表示障礙物左邊沿的距離值| 0A |與 角度ZAOXAorce表示斥力的大小，Angle表示斥力的方向與X軸正半軸的夾角。本文中對于 每個障礙物，選取障礙物邊沿與激光傳感器21組成的ΖΑ0Β角平分線0D方向作為斥力方向， r= |0D|代入式(5)，可得到單個障礙物產(chǎn)生的斥力Force的大小:Fri = KX( |〇D|-R〇rn…… (6);應(yīng)該注意到，當兩個障礙物距離很近時，它們之間的間隙可能不足以讓助行機器人從 中安全通過。由于障礙物產(chǎn)生的斥力只與它和助行機器人的相對距離有關(guān)，同一地點的障 礙物群產(chǎn)生的斥力經(jīng)過向量累加之后可能會遠遠大于操作者的作用力，從而阻礙助行機器 人正常行進。為避免相近方向的斥力的重復(fù)計算，應(yīng)當將距離較近的障礙物進行合并處理。 圖8給出了一個障礙物合并的示例。
[0047]對于兩個相鄰的障礙物如圖8(a)，根據(jù)之前設(shè)計的六元組結(jié)構(gòu)容易得到ZB0C的 值α，根據(jù)余弦定理，求出障礙物1與障礙物2的間隙|BC|:
[0049] 如果| BC |小于應(yīng)用本實施例的助行機器人的尺寸，則對障礙物進行合并，如圖8 (b)所示。將圖8(a)中障礙物1的右邊沿0D與障礙物2的左邊沿0A作為合并后障礙物的邊沿， 0E為AA0D的角平分線，根據(jù)三角形的角平分線公式可以得到0E長度|0E|，帶入(5)可以得 到合并后障礙物產(chǎn)生的斥力F ri = KX (| 0E | -R〇rn……(8);
[0050] 注意，計算|〇E|而不是直接取0E方向激光傳感器21的掃描值是因為線段0E延長線 可能剛好穿過兩個障礙物的間隙，此時激光傳感器21檢測不到障礙物，可能會影響斥力的 計算。
[0051 ]障礙物合并后最終被存儲在障礙物信息鏈表中，最終的斥力Fr為每個障礙物所產(chǎn) 生的斥力Fobi的向量和
；i為障礙物信息鏈表中的第i個障礙物，η為 障礙物的總數(shù)；
[0052]在實際中，為了便于計算以及與操作者手部作用力進行融合，我們分別計算斥力 在X軸和Υ軸方向上的分力并求和。力的正負與坐標軸的方向定義一致。
最終得 到的斥力向量為Fr=[F0bx FobY 0]τ。
[0053]在之前對圖5的描述中，我們已經(jīng)得到了表示操作者雙手作用力分量的變量Flx， ?1￥$)(^^。我們可以用一個四維向量?5來表示操作者的作用力向量作為人機接口的輸入 量，用一個三維向量Fi表示操作者的意圖，稱向量Fi為操作者的意圖力向量。則有:F S = [Fix FlY Frx FrY]T……(12)；Fi=[Fx Fy Mz]t……(13)；
[0054]對于Fi，F(xiàn)x表示操作者在X軸方向的意圖力，F(xiàn)y表示操作者在Y軸方向的意圖力，Mz 表示操作者對助行機器人施加的旋轉(zhuǎn)扭矩，以逆時針方向為正。結(jié)合之前根據(jù)人工勢場法 計算的斥力向量Fr，有如下公式成立:Fi = EisFs+Fr......(14);
[0055]公式（14)將操作者的作用力向量和人工勢場法的計算出的斥力向量相結(jié)合，作為 操作者的意圖力向量。其中EIS是3X4的轉(zhuǎn)換矩陣：：
其中L 表示圖5所示助行機器人寬度的一半。
[0056]得到操作者意圖力向量?:后，運用開環(huán)導(dǎo)納控制算法將操作者的意圖力向量轉(zhuǎn)換 為速度向量。設(shè)操作者的意圖力向量為FI(S)，助行機器人輸出的速度向量為V(s)，則根據(jù)導(dǎo) 納控制算法的傳遞函數(shù)可得：
其中K為比例系數(shù)，τ為時間常數(shù)。
[0057] 寫成連續(xù)時域模型有

[0060] 人工勢場法與導(dǎo)納控制算法相結(jié)合應(yīng)用于助行機器人距離障礙物較遠的情況，可 以有效引導(dǎo)操作者與周邊障礙物保持距離，同時為了保證助行機器人人機接口的安全性， 本實施例設(shè)計了一種緊急避障方案，詳述如下：
[0061] 當障礙物與助行機器人距離過近以至于隨時可能發(fā)生碰撞時，必須采取緊急避障 措施以保證操作者和助行機器人的安全。本實施例首先劃分緊急避障區(qū)，當在緊急避障區(qū) 發(fā)現(xiàn)障礙物時，如果操作者的意圖力向量內(nèi)有乂軸負方向的分量，助行機器人將遵從操作者 的意圖后退避障，否則助行機器人將采取側(cè)向平移的方式來嘗試規(guī)避緊急避障區(qū)的障礙 物。
[0062] 緊急避障區(qū)的選取方法使用矩形條件，如圖9所示，緊急避障區(qū)域為圖9中的矩形， 激光傳感器21在0點處。當激光傳感器21存在任意一個掃描點滿足矩形條件時則判定矩形 區(qū)域內(nèi)存在障礙物，矩形條件描述如下：
[0063]設(shè)A(x，y)為激光傳感器的一個探測點，|〇A| =L。設(shè)矩形長度為Length，寬度為 Width，當下面條件成立時矩形條件成立；
α為障礙物的某點 與X軸的夾角。
[0064]此時啟動緊急避障方案。緊急避障方案首先選擇Υ軸的正方向或負方向進行平移 避障，方向選擇利用了一種投票算法，即以X軸為界，統(tǒng)計X軸左側(cè)和右側(cè)滿足矩形條件的點 的個數(shù)，選擇點數(shù)少的一側(cè)進行平移。事實上這種投票算法選擇了助行機器人的一個初始 狀態(tài)。在平移避障的過程中助行機器人實時檢測助行機器人到邊緣墻體的距離如圖9,一旦 發(fā)現(xiàn)這個距離小于安全距離，則立即改變運動狀態(tài)。助行機器人在緊急避障區(qū)域存在障礙 物時會在各種狀態(tài)之間進行轉(zhuǎn)換，具體的緊急避障方案可用如圖10所示的狀態(tài)轉(zhuǎn)換圖表 示，下面表格詳細說明了各種狀態(tài)以及狀態(tài)轉(zhuǎn)換的各種條件。

[0067]本發(fā)明實施例采用多線程技術(shù)實現(xiàn)以上方案，以上敘述的方法可以簡要的概括為 圖11所示的多線程程序流程圖。在控制器初始化后主線程和支線程先后啟動，它們獨立，并 行工作，并且均進行周期性工作。兩個線程使用斥力向量F r和狀態(tài)變量State來共同完成對 助行機器人的控制。狀態(tài)變量State初始值為0，表示行駛狀態(tài)正常。
[0068]支線程的工作步驟如下：
[0069] 首先提取激光傳感器的掃描結(jié)果(結(jié)果為一個數(shù)組集合，集合中的每個元素表示 掃描范圍內(nèi)某個角度的障礙物距離），根據(jù)掃描結(jié)果，首先判斷緊急避障區(qū)內(nèi)是否存在障礙 物，如果存在，那么根據(jù)圖10中描述的緊急避障算法改變狀態(tài)變量State的值。
[0070] 如果不存在，那么根據(jù)圖7中建立的障礙物模型以及障礙物的六元組結(jié)構(gòu){rEdge， rAngle，lEdge，lAngle，F(xiàn)orce，Angle}，首先可以從激光傳感器得到rEdge = | 0B |，rAngle = ZBOX, lEdge= I 0AI，lAngle = ZAOX，根據(jù)rAngle和lAngle可以求出Angle = ZD0X= 1/2 X (ΖΑ0Χ+ΖΒ0Χ)，進而可以根據(jù)Angle，由激光傳感器讀取Angle對應(yīng)的距離值I 0D I，再利用 (6)式計算斥力Force的大小。按以上方法可以計算一次掃描所有的周邊障礙的信息，形成 障礙物鏈表，鏈表的單元內(nèi)容為上述六元組結(jié)構(gòu)。
[0071] 建立障礙物鏈表后，利用余弦定理(式(7))計算相鄰障礙物之間的間隙（圖8(a)中 的BC)是否能夠使助行機器人通過，如果不能則將兩個障礙物合并為一個進行處理（圖8 (b))，并根據(jù)式(8)重新計算斥力。對障礙物鏈表進行整理后，對鏈表中所有障礙物產(chǎn)生的 斥力進行矢量求和并沿設(shè)定的坐標軸分解(式(9)(10)(11))，得到障礙物鏈表中所有障礙 物產(chǎn)生的斥力的合力向量F r=[FQbx FQbY 0]τ，最后將狀態(tài)變量State的值改為0。以上為支線 程一個周期的工作內(nèi)容。
[0072] 主線程工作步驟如下：
[0073]主線程啟動AD轉(zhuǎn)換后定時讀取AD轉(zhuǎn)換結(jié)果，即數(shù)據(jù)采集模塊13采集的8路FSR傳感 器檢測到的作用力數(shù)值F:\F8,利用式(4)計算;FS=[F1X F1Y FrX FrY]T，根據(jù)支線程的斥力 向量計算結(jié)果Fr^Fobx FobY 0]τ和式（14)計算操作者的意圖力向量F^tFx FY MZ]T，再根 據(jù)式（19)計算最終的輸出速度VN=[V X VY VZ]T。最后讀取狀態(tài)變量State的值，如果為0則將 計算結(jié)果VN發(fā)送到底盤電機，助行機器人以速度V N運動;如果為1，助行機器人以固定速度向 右平移避障；如果為2，助行機器人以固定速度向左平移避障；如果為3，助行機器人緊急停 止。
[0074]綜上所述，本【具體實施方式】具有結(jié)構(gòu)簡單、易用、通用性高、實時性和安全性好，且 成本較為低廉的特點?？奢p易應(yīng)用于一般的助行機器人系統(tǒng)上從而增強行走能力損傷以及 弱視患者的行走能力，提高他們的生活質(zhì)量。
[0075]本領(lǐng)域的技術(shù)人員容易理解，以上所述僅為本發(fā)明的較佳實施例而已，并不用以 限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進等，均應(yīng)包含 在本發(fā)明的保護范圍之內(nèi)。
【主權(quán)項】
1. 一種基于多傳感器的助行機器人人機接口的避障控制方法，所述助行機器人人機接 口包括壓力采集模塊（1)、障礙物探測模塊(2)和控制器(3);所述壓力采集模塊（1)用于檢 測操作者手部的作用力;所述障礙物探測模塊(2)用于探測周邊障礙物;所述控制器(3)與 所述壓力采集模塊（1)和所述障礙物探測模塊(2)連接，用于根據(jù)所述壓力采集模塊（1)采 集的數(shù)據(jù)W及所述障礙物探測模塊(2)探測的數(shù)據(jù)計算助行機器人的速度，并根據(jù)所述助 行機器人的速度控制所述助行機器人實現(xiàn)助行和避障功能；其特征在于，所述避障控制方 法包括下述步驟： 當在緊急避障區(qū)內(nèi)存在障礙物時，判斷操作者的意圖力Fi是否有X軸負方向的分量，若 是，則助行機器人遵從操作者的意圖后退避障，若否，則助行機器人將采取側(cè)向平移的方式 來規(guī)避緊急避障區(qū)的障礙物； 其中，W助行機器人中屯、點為坐標原點，W助行機器人的相對正前方為X軸正方向；W 助行機器人的相對左方為Y軸正方向。2. 如權(quán)利要求1所述的避障控制方法，其特征在于，所述緊急避障區(qū)為矩形，矩形的長 度為Length,寬度為Width,當激光傳感器與激光傳感器的一個探測點之間的距離L滿足下 式時，則認為障礙物在緊急避障區(qū)內(nèi)，其中α為障礙物的某點與權(quán)利要 求7中定義的X軸的夾角。3. 如權(quán)利要求1或2所述的避障控制方法，其特征在于，當障礙物在緊急避障區(qū)內(nèi)時，通 過統(tǒng)計X軸左側(cè)和右側(cè)滿足矩形條件的點的個數(shù)，選擇點數(shù)少的一側(cè)進行平移;在平移過程 中實時檢測平移方向上是否有足夠的避障空間，根據(jù)檢測情況可改變平移方向或停止。4. 如權(quán)利要求1所述的助行機器人人機接口，其特征在于，所述壓力采集模塊（1)包括 力傳感裝置(11)、信號調(diào)理模塊(12)和數(shù)據(jù)采集模塊(13); 所述力傳感裝置（11)為二層套筒結(jié)構(gòu)，且外層是內(nèi)部被縷空成長方體的圓柱體，內(nèi)層 是方形柱體，圓柱體與方形柱體之間有縫隙，在內(nèi)層方形柱體的四個側(cè)面分別設(shè)置有1個 FSR傳感器填充所述縫隙；用于采集操作者左右手作用力縱向和橫向的分量； 所述信號調(diào)理模塊（12)用于對所述力傳感裝置（11)采集的作用力進行調(diào)理后輸出； 所述數(shù)據(jù)采集模塊(13)用于對經(jīng)過調(diào)理后的數(shù)據(jù)進行AD轉(zhuǎn)換后獲得電壓值。5. 如權(quán)利要求1所述的助行機器人人機接口，其特征在于，所述障礙物探測模塊(2)包 括激光傳感器(21) W及用于連接所述激光傳感器(21)與所述控制器(3)的USB高速數(shù)據(jù)線； 所述激光傳感器(21)用于采集周邊環(huán)境的障礙物信息，并通過所述USB數(shù)據(jù)線將采集的障 礙物信息發(fā)送到控制器(3)。6. 如權(quán)利要求1所述的助行機器人人機接口，其特征在于，所述控制器用于根據(jù)人工勢 場法將障礙物探測模塊(2)采集的障礙物信息轉(zhuǎn)換為斥力;計算斥力與操作者作用力的合 力，并將合力轉(zhuǎn)換為用于控制助行機器人實現(xiàn)助行的速度。
【文檔編號】G05D1/02GK106094811SQ201610033777
【公開日】2016年11月9日
【申請日】2015年4月27日 公開號201610033777.7, CN 106094811 A, CN 106094811A, CN 201610033777, CN-A-106094811, CN106094811 A, CN106094811A, CN201610033777, CN201610033777.7
【發(fā)明人】黃劍, 李奕彤, 晏箐陽
【申請人】華中科技大學