基于微型激光雷達的盲人室內避障系統

杜年豐， 陳 偉， 史澤地， 楊增汪

（江蘇師范大學物理與電子工程學院，江蘇徐州221116）

0 引 言

國外GPS和國內北斗導航已經進入大眾群體中，人們在日常生活中也越來越離不開它。經調查，當下全球有很多致力于解決盲人室外行動難題的研究團隊，他們的研究成果很多也做得很好，如盲人GPS導航儀、電子導盲杖等［1］。然而，對于室內導航卻鮮有成果出現，這是因為現在的導航信號（GPS、北斗等）會被較厚的墻壁阻擋，這就使得導航設備在大型建筑這樣的場景中會因為衛星信號斷聯而無法定位［1］，該技術難題極大地限制了室內導航技術的發展。而現實生活中，盲人活動大多在室內進行，即便外出，盲人對于樓梯口、拐彎處無法感知，容易受傷，特別是盲人在一個復雜陌生的室內環境，在沒有別人幫助的情況下，容易發生碰撞［1］。

為此，本文提出了一種盲人室內避障系統。該系統在分析視障人士室內行走的心理特征以及視覺缺失特點的基礎上，以樹莓派為控制核心，將激光雷達、陀螺儀、加速度計等不同的傳感器定位融合，最終依據空間單位進制劃分，對室內物體進行線性建模，建立起一個適用于視障人士室內導航的室內模型。在模型通過樹莓派控制器處理后，劃分出危險層次，盲人可以通過播報的語音信息在室內避開障礙物。當盲人走出室外，則結合GPS設備輸出的經緯度信號，樹莓派控制器可以根據場景需要將GPS經緯度信息轉換為平面坐標x、y、z，融合在激光雷達傳感器的矩陣運算、障礙物信息實時采集中，從而很好地將此避障系統與室外導航結合。

1 整體設計

本系統整體設計主要由感知層、控制層、輸出層組成。感知層包括陀螺儀、加速度計、微型激光雷達、GPS/北斗作為主要傳感器，樹莓派控制器對傳感器A/D轉換后，經過對數據的矩陣融合運算，生成一個狀態估計的過程模型，預估在對盲人所處環境測量中，過程模型以及卡爾曼濾波就可以完成對盲人周邊障礙物/行人的狀態進行估計。

此外，微型激光雷達會對盲人所處室內環境不斷掃描獲取數據，將入口與可行路徑以柵格單元的形式記錄，通過掃描整個室內環境，最終構建室內2D柵格地圖，樹莓派控制器根據柵格值的不同劃分障礙物危險系數。本設計還會基于危險度計算規則、室內層次信息和拓撲信息，根據室內導航中出現的障礙物危險系數分層次語音提醒。整體架構如圖1所示。

圖1 系統整體架構圖

2 盲人室內避障系統關鍵技術

2.1 微型激光雷達SLAM柵格構圖原理

采用微型激光雷達可以為盲人實時構建2D柵格地圖。通過微型激光雷達的精準測距，可以得到較為精確的空間信息，如角度和距離。占據柵格地圖一般用來表示微型激光雷達SLAM建立的地圖，因為柵格具有存儲緊湊和格柵是以概率的形式表示被占據的概率的特性，所以非常適用于檢測障礙物［5］。

SLAM：這是一種從未知環境、地點中出發，在運動過程中通過反復觀察空間特性從而對自身位置和姿態進行定位，接著會根據自身位置為中心來構筑周圍空間的增量式地圖，通過這種方式構建地圖和實時定位。它包括傳感器輸入、濾波處理、回環檢測、定位融合、非線性優化、信號濾波等［6-7］，如圖2所示。

圖2 SLAM系統模塊

SLAM構建的示意圖黑色線條表示激光雷達探測到的障礙物邊緣，表示此格不通；灰白色區域表示可能有窗戶或門，微型激光雷達部分點散射了出去。通過掃描整個室內環境，可以形成一幅2D的激光雷達柵格地圖，紅色區域表示當前位置，如圖3所示。

圖3 微型激光雷達SLAM示意圖

一般用宏觀環境和微觀環境來表示盲人在室內活動時的室內信息，室內空間可以看作是一個個空間單元，空間單元又可細分為通道和通用單元；這樣就可以將通用單元中除了障礙物和附屬物的部分賦予可以讓盲人通行的功能，并且用入口將其與道路連接起來；為了實現精準避障過程中為盲人提供更加詳實的信息，本設計利用一套語意拓展策略用于描述室內的空間單元；在功能單元入口和道路的關聯下，就可以將室內空間單元的拓撲信息進行保存，這可為生成避障指引提供基礎。本系統還會基于危險度計算規則、層次信息和拓撲信息，探討室內導航中出現的障礙物危險系數，為盲人在室內空間安全行走提供依據。

本盲人導航設計采用樹莓派3B＋芯片，作為整個系統的控制器，微型激光雷達對障礙物信息進行采集后傳送到樹莓派組成的控制電路，樹莓派控制器通過算法根據微型激光雷達SLAM建立的地圖進行數據分析和矩陣運算。

2.2 多傳感器定位融合原理和室內障礙物位置估計

（1）多傳感器的定位融合原理。室內定位是本系統非常重要的實驗內容，核心是要解決盲人“我在哪”和“要去哪”的問題。普通人在陌生的室內環境下可以根據室內建筑物的引導牌和一些熟悉的標記點知道自己在哪。進一步地講，根據人腦的神經元來認知，這些是在一瞬間完成的，但是對于控制器和程序則需要多傳感器的信息數據融合，才能夠精確地完成定位計算。

目前室外的定位技術比較成熟的有國外的GPS和國內的北斗，但是在室內復雜的建筑物和場景下，這種成熟的技術由于信號、場景復雜等問題無法適用。GPS和北斗的定位原理是基于三角定位法，3個衛星組成三角形，通過融合幾何數據，從而獲得精確的位置信息。那么是否可以將GPS/北斗的幾何數據與盲人室內穿戴的其他傳感器獲取的數據融合呢？如果可以的話，就可以同時實現盲人的室內導航和室外導航了。

通常GPS設備輸出的信號主要為經緯度信息、時間同步等信號，GPS/北斗結合盲人穿戴的多傳感器融合需要將經緯度信號轉換為平面坐標x、y、z。盲人室內穿戴設備有陀螺儀、加速度計、微型傳感器等讀取出來的加速度、以及對應的角度信息，速度信息我們用vx、vy、vz表示，位置和角度用Roll、pitch表示。現在就有了x、y、z、vx、vy、vz、Roll、pitch共8個狀態向量來表示盲人的姿態和位置信息。有了這些信息，可以結合慣性測量單元（Inertial Measurement Unit，IMU）的慣性參數來預防盲人摔倒［8-9］。

同理，還可以在盲人設備上加入其他傳感器設備，比如超聲波、里程計等，這些傳感器都有其優缺點，可以根據這些傳感器讀取的不同維度的測量數據轉換后融合，如圖4所示，得到更加精準的盲人姿態數據。

圖4 多傳感器融合示意圖

（2）室內障礙物/過道行人位置估算。各種傳感器都有其優缺點，如何讓盲人在陌生的室內環境中“眼觀八方”？那就要最大程度利用各個傳感器的優點，匯聚后安裝在盲人穿戴設備上，讓盲人根據多傳感器來知道周邊環境的情況。那么多傳感器如何穩定、可靠地給盲人估算位置和姿態信息呢？本實驗是基于卡爾曼濾波的位置估算解決。

首先根據多傳感器采集的信息，例如這樣的場景：盲人在室內行走時，前方突然出現障礙物或者行人，設備就需要對前方的障礙物和行人做一個預估和判斷，從而語音指導盲人避開，那么如何預估和判斷呢？我們根據盲人的x、y、z、vx、vy、vz、Roll、pitch共8個狀態向量可以記錄盲人的姿態，由1.1導盲設備的室內SLAM構圖闡述得知，通過配備微型激光雷達，可以完成測距信息，這樣可以得到前方突然出現障礙物/行人的位置和運動速度，這里用p表示障礙物/行人的位置，用vz表示運動速度，用一個向量來表示某個狀態：

上述方程表示了障礙物/行人x、y方向的位置分量px、py，以及速度分量vx，vy。在確定了想要估計的對象狀態后，需要一個用于生成狀態估計的過程模型，假設模型為

擴展為

將其展開得到：

在此過程模型預測的障礙物/行人的運動速度有可能為0，在這個問題中就會形成過程噪聲，上述公式中過程噪聲就是v，在預估盲人周圍的障礙物/行人運動狀態和位置時，障礙物/行人突然加減速度時，過程噪聲就轉變為了障礙物/行人的突然加減速度。那么原來的模型表達式可以優化為

再根據線性代數的方法（P′K＝AP k－1AT）來表示預測，預測后的過程應為

式中，Q是過程噪聲的協方差矩陣。這是狀態A的更新過程，它的本質是估計狀態概率分布的協方差矩陣。因為障礙物/行人的突然加減速度造成的過程噪聲是隨機帶入的，所以過程噪聲v本質是一個高斯分布：v～N（0，Q），Q是過程噪聲的協方差矩陣：是傳感器的固有性質，往往由傳感器廠商提供。綜上，可以求得

式中，C表示引入值，根據盲人所穿戴設備的傳感器性質決定，因為有的傳感器只能測量v，位置y獲取不到；I符號代表單位矩陣；

在對盲人所處環境測量步驟中，通過過程模型以及卡爾曼濾波就可以完成對盲人周邊障礙物/行人的狀態進行估計。基于此，通過以上所述原理，以多傳感器數據信息為節點，將SLAM構圖中可以通行的通道轉化為邊，提取室內空間單元內部對象為柵格，將不可行走的區域在構建的2D激光雷達柵格地圖中，用黑色界限表示，控制器通過慣性測量單元IMU等傳感器數據融合獲取的參數預估盲人及周邊障礙物和來人的運動姿態，緊急轉化為語音進行提醒。

3 測試與實驗

本設計測試與實驗主要由兩部分組成：①搭建仿真環境，本測試采取實驗室為盲人仿真室內環境，通過微型激光雷達的精準測距，獲得較為精確的空間信息，進行SLAM構圖，并形成實際構圖效果與實驗室真實過道、障礙物實物對比。②多傳感器融合測試，基于卡爾曼濾波的位置估算預估盲人周邊障礙物/來往行人的狀態，將預估、測量數據與真實結果進行對比。

3.1 針對盲人所處室內環境的SLAM構圖

在導盲設備樹莓派上安裝微型激光雷達，如圖5所示，可以獲取室內環境深度信息，然后根據盲人所處室內環境設定可檢測的起始角度（angle_min）和可檢測范圍的終止角度（angle_max），組成微型激光雷達的可檢測范圍，相鄰數據幀之間的角度步長以及采集一幀數據所需要的時間，最近可檢測深度的闕值（range_min），最遠可檢測深度的闕值（range_max）等［10］。

圖5 微型激光雷達實物圖

完成上述根據盲人環境設定的深度信息后，再根據盲人當前位置設置坐標，包括盲人的x、y、z三軸位置及方向參數，以及用于校正誤差的協方差矩陣。

確定仿真環境。本設計測試采取實驗室為盲人室內仿真陌生環境，如圖6所示為實驗室平面圖，仿真軟件為Vmvare，通過設置可以即時顯示微型激光雷達SLAM構建的2D柵格地圖。

圖6 實驗室平面圖

具體實驗步驟如下：

（1）首先在Vmware虛擬機中添加基于SLAM的功能應用包gmapping，gmapping功能包包括二維柵格地圖和Rao-Blackwellized粒子濾波算法［11］，總體框架圖如圖7所示。

圖7 gmapping功能包的總體框架

（2）初始化工作空間，向樹莓派中輸入測試指令如下：

（3）根據系統導航框架中的蒙特卡羅定位方法，可以在盲人導盲設備構建的SLAM室內地圖中加入二維環境定位，使用粒子濾波跟蹤盲人的姿態。導盲設備中的導航框架，結合多傳感器融合定位：可以估算機器人在地圖坐標系/map下的位姿信息，提供/base、/odo/map之間的TF變換，根據盲人室內環境特征配置參數move_base節點。

（4）設置密鑰，設置環境在～/catkin_ws/src中創建柵格構圖源文件。

（5）創建仿真。連接樹莓派在Vmware執行如下指令：

（6）啟動hector_slam演示，通過Vmware查看微型激光雷達設備對盲人周邊環境柵格構圖，實際構圖效果如圖8所示，通過掃描整個室內環境，可以形成一幅2D的激光雷達柵格地圖，黑色線條表示激光雷達探測到的障礙物邊緣，表示此格不通，灰白色區域表示可能有窗戶或門。

圖8 盲人所處環境SLAM柵格構圖

根據盲人所處環境，控制器樹莓派通過算法根據微型激光雷達SLAM建立的地圖進行數據分析和矩陣運算，可以形成一幅2D的激光雷達柵格地圖（見圖9）。根據柵格地圖呈現的柵格值劃分區域，劃分依據盲人真實實驗數據均值。

圖9 柵格地圖區域值劃分

柵格值設置致命障礙：柵格值為254，障礙物與盲人的中心重合，此時障礙物必然與盲人發生碰撞。

內切障礙：柵格值為253，障礙物處于盲人周邊的內切圓內，此時盲人也必然與障礙物發生碰撞。

外切障礙：柵格值為252～127，障礙物處于盲人周邊的外切圓內，此時盲人與障礙物臨界接觸，不一定發生碰撞。

自由區域～內切障礙：柵格值為127～2，障礙物附近區域，一旦盲人進入該區域，將有較小概率發生碰撞，屬于安全區。

自由區域：柵格值為0～2，此處沒有障礙物，盲人可以自由通過。

未知區域：柵格值為255，此處還沒有探知是否有障礙物，盲人可以前往繼續建圖。

3.2 多傳感器融合測試

根據盲人穿戴的陀螺儀、加速度計、微型傳感器等讀取出來的加速度，以及對應的角度信息，速度信息用vx、vy、vz表示，位置和角度我們用Roll、pitch表示，前方的障礙物和行人位置分量用px、py表示、速度分量用vx，vy表示。

下面在pycharm中執行代碼，pycharm是一種Python IDE交互式編輯器，帶有一整套可以幫助用戶在使用Python語言開發時提高其效率的工具，方便Project管理。

主要步驟如下：

（1）在pycharm中載入必要的庫，如圖10所示。

圖10 pycharm載入庫界面

（2）通過傳感器讀入障礙物和來往行人的狀態，包括在SLAM構圖柵格中的位置以及速度等信息，程序中載入2.2.2中的優化模型，代碼中用F表示，測量矩陣H如圖11所示，計算測量噪聲的協方差矩陣和過程噪聲的協方差矩陣Q［12］。

圖11 在pycharm載入狀態轉移矩陣F及觀測矩陣H

（3）在pycharm定義一個單位矩陣，產生一些隨機測量數據陣和一些過程值。

（4）在pycharm中執行卡爾曼濾波。位姿估計算法的研究是解決SLAM問題的關鍵。目前，已有大量位姿估計算法，根據這些算法的基本原則大致可分為以下幾類：擴展卡爾曼濾波（EKF）、最大似然方法、稀疏的擴展信息濾波器（SEIFs）、平滑技術、Rao-Blackwellized粒子濾波器（RBPF）以及GraphSLAM［13-14］。其中本實驗使用的是卡爾曼濾波法，如圖12所示。卡爾曼濾波過程如圖13所示。

圖12 在pycharm中載入卡爾曼濾波公式

圖13 卡爾曼濾波過程

在pycharm中執行代碼，通過傳感器讀入障礙物和來往行人的狀態，包括在SLAM構圖柵格中的位置以及速度等信息，程序中載入處理矩陣F及測量矩陣H，計算測量噪聲的協方差矩陣和過程噪聲矩陣Q。最終在pycharm中展示預估、預測及真實結果對比［15］。在pycharm中可以讀取到傳感器檢測的vx與vy，如圖14所示。在pycharm中載入卡爾曼濾波公式后，多傳感器融合測試，基于卡爾曼濾波的位置估算預估盲人周邊障礙物/來往行人的狀態（見圖15）。

圖14 傳感器實際測量值

圖15 預估數據與過濾狀態圖

4 結 語

在傳統的GPS和北斗導航無法滿足室內復雜的環境下，本文建立起一個適用于視障人士室內導航的室內模型，在模型通過樹莓派控制器處理后，設計出最優路徑，盲人可以通過播報的語音信息在室內行走。當盲人走出室外，則結合GPS設備輸出的經緯度信號，樹莓派控制器可以根據場景需要將GPS經緯度信息轉換為平面坐標x、y、z，融合在激光雷達傳感器的矩陣運算、障礙物信息實時采集中，從而很好地將室內導航與室外導航結合。

主要有以下特點：①采用了微型激光雷達可以為盲人實時構建室內導航地圖（SLAM），微型激光雷達部分點散射出去，通過掃描整個室內環境，可以形成一幅2D的激光雷達柵格地圖，根據盲人所處環境的柵格值，提示盲人精準避障。②多傳感器定位融合原理和卡爾曼濾波，當盲人在室內行走時，前方突然出現障礙物或者行人，系統就需要對前方的障礙物和行人狀態做一個預估和判斷，通過語音提醒盲人。