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

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

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

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

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