立體慣性導盲方法及裝置和系統與流程

本發明涉及視力殘疾人助行導航方法，更具體地說是指立體慣性導盲方法及裝置和系統。

背景技術：

目前，市場上已出現多種導盲設備，比如導盲仗以及其他導盲設備，導盲仗又包括普通導盲仗以及超聲導盲仗兩種，其他導盲設備包括加裝攝像頭的導盲設備，由于我國視力殘疾人數高達千萬人，因此，導盲設備的需求量很大。

但是，上述的普通導盲仗是最傳統的視力殘疾人用導盲產品，只能幫助視力殘疾人探測前方有限距離的障礙物，也沒法做障礙物體識別，從而提醒視力殘疾人避開障礙物；超聲導盲仗是在普通導盲仗的基礎上加上超聲波傳感器，但是超聲波傳感器探測距離有限，而且也無法做到全方位障礙物檢測，檢測方向只能在超聲傳感器的正前方，也無法做到物體識別，為視力殘疾人的導盲能力也是有限的；加裝攝像頭的導盲設備雖然可以做簡單的物體識別，識別紅綠燈等，但是只通過攝像頭無法檢測物體遠近，不能做基于距離的避障，也不能做導航。

中國專利201610057616.1公開了一種基于計算機雙目視覺與慣性測量的視力殘疾人行走定位方法，一方面利用佩戴在視力殘疾人頭部的雙目相機2采集場景圖像，通過圖像特征匹配方法查找場景中已知經緯度的標志物，采用計算機雙目立體視覺方法計算標志物在相機坐標系下的三維坐標，另一方面利用固定在相機上的組合慣性器件測量相機的姿態角，計算出相機坐標系關于視力殘疾人所在位置的地理坐標系的轉換矩陣；利用轉換矩陣將標志物的相機系坐標變換為視力殘疾人所在位置的地理系坐標，進而由標志物經緯度推算出視力殘疾人所在位置的經緯度。綜合計算機雙目視覺算法與慣性測量技術，實現視力殘疾人行走位置的定位，不僅簡單易行，而且準確性較高，特別適用于視力殘疾人出行導盲的定位。

上述的專利中并沒有同時具備對行走軌跡的正確度判斷以及在行走軌跡上遇到的障礙物的判斷，對于導盲而言，不夠全面。因此，有必要設計一種立體慣性導盲方法，實現既能做到全方位、遠距離的障礙物檢測，還提高三維空間定位的更新頻率，獲得更小的定位誤差，以便于提示視力殘疾人正確的避開障礙物。

技術實現要素：

本發明的目的在于克服現有技術的缺陷，提供立體慣性導盲方法及裝置和系統。

為實現上述目的，本發明采用以下技術方案：立體慣性導盲方法，所述方法包括：

軌跡獲取步驟，采用視覺慣性定位方式，確定首次行走的軌跡，同時記錄兩個相機所拍攝到的所有圖像中的特征點；

軌跡判斷步驟，判斷實際行走的軌跡是否與所述首次行走的軌跡一致；若一致，則進行點云獲取步驟，若不一致，則發送提醒信息，并進入點云獲取步驟；

點云獲取步驟，獲取實際行走的軌跡中的點云信息；

點云判斷步驟，判斷所述點云信息中是否有超過設定數量的點集合在某一規定的區域內；若有，則發送提醒信息，若無，則返回所述點云獲取步驟。

其進一步技術方案為：所述點云獲取步驟包括以下具體步驟：

獲取兩個相機的相對位置和角度；

獲取兩個所述相機同一時刻曝光的圖像，作為第一圖像以及第二圖像；

利用所述第一圖像以及所述第二圖像、兩個所述相機的相對位置和角度，獲取點云信息。

其進一步技術方案為：利用所述第一圖像以及所述第二圖像、兩個所述相機的相對位置和角度，獲取點云信息的步驟，包括以下具體步驟：

提取第一圖像中的所有像素，并提取每個像素周圍的圖像特征，在第二圖像中尋找與第一圖像中提取的像素對應的特征最相似的像素；

獲取所述第二圖像的像素相對于所述第一圖像對應的像素的位置，作為點的圖像位置；

利用三角定位的方式，確定所述點的真實三維位置；

集合所述第一圖像以及所述第二圖像所有像素點所對應的點，獲取點云；

將所述點云中所有點對應的真實三維位置與所述點云對應，獲取點云信息。

其進一步技術方案為：所述軌跡獲取步驟中，采用視覺慣性定位的方式包括以下具體步驟：

獲取兩個所述相機在某一時刻曝光的兩張圖像；

提取其中一張所述圖像的角點以及所述角點的周圍的圖像特征，作為特征點；

計算所述特征點在另一張圖像中的極線，并在所述極線上尋找與所述特征點最接近的點，作為輔助特征點；

結合特征點以及輔助特征點，通過三角法計算所述特征點所對應的真實位置，以特征點、輔助特征點以及所述真實位置作為標記點；

給定某一角點的周圍的圖像特征，將其和所有已有標記點所對應的圖像特征對比；如果匹配，則把所述角點列入所述標記點的特征中；

獲取所有標記點串聯而成的路線，將慣性傳感器中的陀螺儀信息加入角度測算，將慣性傳感器中的加速度信息加入行走速度測算，獲取行走的軌跡。

其進一步技術方案為：所述軌跡獲取步驟之前，還包括：

判斷是否屬于第一次使用；

若是，則進行所述軌跡獲取步驟；

若不是，則進行圖像匹配，確定初始位置；

結合所述初始位置以及相對于所述導航路徑所處的位置，進行導航，并進入所述軌跡判斷步驟。

本發明還提供了立體慣性導盲裝置，包括裝置本體、兩個相機、慣性傳感器、服務器以及播放結構；兩個所述相機、所述慣性傳感器以及所述播放結構連接在所述裝置本體上。

其進一步技術方案為：所述裝置本體為導盲眼鏡、導盲仗以及導盲機器人中至少一種。

本發明還提供了立體慣性導盲系統，包括軌跡獲取單元、軌跡判斷單元、點云獲取單元以及點云判斷單元；

所述軌跡獲取單元，用于采用視覺慣性定位方式，確定首次行走的軌跡，同時記錄兩個相機所拍攝到的所有圖像中的特征點；

所述軌跡判斷單元，用于判斷實際行走的軌跡是否與所述首次行走的軌跡一致；若一致，則進行點云獲取，若不一致，則發送提醒信息，并進入點云獲取；

所述點云獲取單元，用于獲取實際行走的軌跡中的點云信息；

所述點云判斷單元，用于判斷所述點云信息中是否有超過設定數量的點集合在某一規定的區域內；若有，則發送提醒信息，若無，則進行點云獲取。

其進一步技術方案為：所述點云獲取單元包括基礎信息獲取模塊、圖像獲取模塊以及點云信息獲取模塊；

所述基礎信息獲取模塊，用于獲取兩個相機的相對位置和角度；

所述圖像獲取模塊，用于獲取兩個所述相機同一時刻曝光的圖像，作為第一圖像以及第二圖像；

所述點云信息獲取模塊，用于利用所述第一圖像以及所述第二圖像、兩個所述相機的相對位置和角度，獲取點云信息。

其進一步技術方案為：所述點云信息獲取模塊包括像素提取子模塊、圖像位置獲取子模塊、真實位置確定子模塊、點云獲取子模塊以及對應子模塊；

所述像素提取子模塊，用于提取第一圖像中的所有像素，并提取每個像素周圍的圖像特征，在第二圖像中尋找與第一圖像中提取的像素對應的特征最相似的像素；

所述圖像位置獲取子模塊，用于獲取所述第二圖像的像素相對于所述第一圖像對應的像素的位置，作為點的圖像位置；

所述真實位置確定子模塊，用于利用三角定位的方式，確定所述點的真實三維位置；

所述點云獲取子模塊，用于集合所述第一圖像以及所述第二圖像所有像素點所對應的點，獲取點云；

所述對應子模塊，用于將所述點云中所有點對應的真實三維位置與所述點云對應，獲取點云信息。

本發明與現有技術相比的有益效果是：本發明的立體慣性導盲方法，通過兩個相機拍攝的圖像以及慣性傳感器的數據，測算出當前視力殘疾人所在的位置，以及測算周圍物體相對于視力殘疾人的三維坐標，若有障礙物，則識別物體，并將盲人本身的三維坐標以及物體相對于視力殘疾人而言的位置，通過語音播報等形式實時反饋給視力殘疾人，將實際行走軌跡正確的判斷與行走軌跡上的障礙物判斷結合在一起，利用相機拍攝以及慣性傳感器的配合，既能做到全方位、遠距離的障礙物檢測，還提高三維空間定位的更新頻率，獲得更小的定位誤差，以便于提示視力殘疾人正確的避開障礙物。

下面結合附圖和具體實施例對本發明作進一步描述。

附圖說明

圖1為本發明具體實施例提供的立體慣性導盲方法的流程圖；

圖2為本發明具體實施例提供的點云獲取的具體流程圖；

圖3為本發明具體實施例提供的獲取點云信息的具體流程圖；

圖4為本發明具體實施例提供的視覺慣性定位的具體流程圖；

圖5為本發明具體實施例一提供的立體慣性導盲裝置的立體結構示意圖；

圖6為本發明具體實施例二提供的立體慣性導盲裝置的立體結構示意圖；

圖7為本發明具體實施例三提供的立體慣性導盲裝置的立體結構示意圖；

圖8為本發明具體實施例提供的立體慣性導盲系統的結構示意圖；

圖9為本發明具體實施例提供的點云獲取單元的結構示意圖；

圖10為本發明具體實施例提供的點云信息獲取模塊的結構示意圖；

圖11為本發明具體實施例提供的軌跡獲取單元的結構示意圖。

具體實施方式

為了更充分理解本發明的技術內容，下面結合具體實施例對本發明的技術方案進一步介紹和說明，但不局限于此。

如圖1～11所示的具體實施例，本實施例提供的立體慣性導盲方法，可以運用在導盲的過程中，幫助視力殘疾人進行行走引導、躲避障礙物以及物體識別，實現既能做到全方位、遠距離的障礙物檢測，還提高三維空間定位的更新頻率，獲得更小的定位誤差，并且可以對障礙物進行識別，以便于提示視力殘疾人正確的避開障礙物；當然，也可以運用在其他導航的過程中。

如圖1所示，立體慣性導盲方法包括：

s1、判斷是否屬于第一次使用；

s2、若是，則進入軌跡獲取步驟，采用視覺慣性定位方式，確定首次行走的軌跡，同時記錄兩個相機所拍攝到的所有圖像中的特征點，并進入s5步驟；

s3、若不是，則進行圖像匹配，確定初始位置；

s4、結合所述初始位置以及相對于所述導航路徑所處的位置，進行導航，并進入s5步驟。

s5、軌跡判斷步驟，判斷實際行走的軌跡是否與所述首次行走的軌跡一致；若一致，則進行s6、點云獲取步驟，若不一致，則s8、發送提醒信息，并進入s6、點云獲取步驟；

s6、點云獲取步驟，獲取實際行走的軌跡中的點云信息；

s7、點云判斷步驟，判斷所述點云信息中是否有超過設定數量的點集合在某一規定的區域內；若有，則s8、發送提醒信息，若無，則返回所述s6、點云獲取步驟。

上述的s1步驟，判斷是否屬于第一次使用，指的是是否是第一次使用該立體慣性導盲裝置，若是，則需要幫助視力殘疾人佩戴立體慣性導盲裝置沿某路徑行走，以作為路徑的記錄，若不是第一次使用，則圖像匹配，尋找與之行走的路線對應的記錄的路徑，直接調取，效率高。

如圖4所示，在上述的s2步驟中，軌跡獲取步驟中，采用視覺慣性定位方式包括以下具體步驟：

s21、獲取兩個所述相機2在某一時刻曝光的兩張圖像；

s22、提取其中一張所述圖像的角點以及所述角點的周圍的圖像特征，作為特征點；

s23、計算所述特征點在另一張圖像中的極線，并在所述極線上尋找與所述特征點最接近的點，作為輔助特征點；

s24、結合特征點以及輔助特征點，通過三角法計算所述特征點所對應的真實位置，以特征點、輔助特征點以及所述真實位置作為標記點；

s25、給定某一角點的周圍的圖像特征，將其和所有已有標記點所對應的圖像特征對比，如果匹配，則把所述角點列入所述標記點的特征中；

s26、獲取所有標記點串聯而成的路線，將慣性傳感器中的陀螺儀信息加入角度測算，將慣性傳感器中的加速度信息加入行走速度測算，獲取行走的軌跡。

例如：兩個相機2在t時刻曝光，獲取兩張圖像i(t,1)和圖像ii(t,2)，在圖像i(t,1)中提取一定的角點(例如70個)。對于每個角點，提取其周圍圖像特征，對于第i個在i(t,1)中的的特征點p(t,1,i)，計算其在圖像ii中的極線，在極線上尋找與特征點p1(t,1,i)的圖像特征最接近的點，記為p2(t,2,i)，給定p1(t,1,i)和p2(t,2,i)通過三角法計算對應三維點的位置p(i,t)，一組p1(t,1,i)、p2(t,2,i)、p(i,t)記為一個“標記點”l(i,t)；給定特征點p1(t,1,i)的特征，將其和所有已有標記點l(i,t)(t<t)的圖像特征對比，如果匹配，則把p1(t,1,i)加入l(i,t)的特征中；獲取所有標記點l(i,t)串聯而成的路線，作為行走的軌跡。

對于上述的s24中的標志點，對任何一個標記點，都有一個三維點p(i,t)和一組在不同圖像中對應的特征位置。

給定拍攝圖像時的兩個相機2以及慣性傳感器1imu的位置和角度(x,y,z,quaternion)，都可以計算三維點p(i,t)在所拍攝的圖像中的幾何投影位置，幾何投影位置和圖像特征位置的不同是參數誤差，所有參數誤差的l2求和記做e(i,t)。

如圖2所示，對于s6步驟，點云獲取步驟包括以下具體步驟：

s61、獲取兩個相機的相對位置和角度；

s62、獲取兩個所述相機同一時刻曝光的圖像，作為第一圖像以及第二圖像；

s63、利用所述第一圖像以及所述第二圖像、兩個所述相機的相對位置和角度，獲取點云信息。

對于s61步驟，在獲取兩個相機2的位置和角度時，給定所有標記點l(i,t)和imu數據，求一組慣性傳感器1的位置(x,y,z)和角度(quaternion)使得所有標記點l(i,t)的投影誤差e(i,t)之和、imu數據和位置、角度的參數的不同程度最小，只要給定慣性傳感器1在初始時刻的位置，就可以求出兩個相機2在任何時刻的位置和角度。

如圖3所示，s63的步驟，利用所述第一圖像以及所述第二圖像、兩個所述相機2的相對位置和角度，獲取點云信息的步驟，包括以下具體步驟：

s631、提取第一圖像中的所有像素，并提取每個像素周圍的圖像特征，在第二圖像中尋找與第一圖像中提取的像素對應的特征最相似的像素；

s632、獲取所述第二圖像的像素相對于所述第一圖像對應的像素的位置，作為點的圖像位置；

s633、利用三角定位的方式，確定所述點的真實三維位置；

s634、集合所述第一圖像以及所述第二圖像所有像素點所對應的點，獲取點云；

s635、將所述點云中所有點對應的真實三維位置與所述點云對應，獲取點云信息。

比如，兩個相機2在同一時刻曝光，獲取兩張圖像：圖像1和圖像2；對于圖像1的每一個像素(x,y)，提取其周圍圖像的特征，在圖像2中尋找與像素(x,y)特征最相似的像素(u,v)，(x,y)-(u,v)對應真實空間的一個點，利用三角定位方式，確定(x,y)-(u,v)對應的真實的點的三維坐標p，將所有的像素對應的點收集在一起，獲得點云，此過程中像素的坐標，是利用慣性傳感器1獲得重力方向，在點云中去除垂直于重力方向的平面(地面)。

上述的s7步驟，判斷時，主要是將兩個相機2所拍攝的前方區域切分成扇形區域，統計每個扇形區域的點的數量，若某一規定的區域(也就是某一扇形區域)超過設定數量，則認為即將遇到障礙物。

對于s7中，若所述點云信息中是否有超過設定數量的點集合在某一規定的區域內，則發送提醒信息之前還會進行s9、識別區域內的物體；識別區域內的物體時，主要是將兩個相機2拍攝到的圖像輸入到物體識別cnn網絡系統中，由物體識別cnn網絡系統識別后，輸出物體對應的大致模型和名字以及其在圖像中出現的位置。

上述的發送提醒信息，在本實施例中，采用的是語音播報方式，以一定的頻率(例如3秒鐘1次)向視力殘疾人播報兩個相機2所拍攝的每個扇形區域點的數量，播報的方式可以利用環繞立體聲，利用省掉高低表示每個方向的點的數量，讓視力殘疾人聽到潛在障礙物和整體環境的幾何信息。接收到的物體識別結果，通過語音播報當前的圖像中實現規定的物體的出現情況以及在當前的圖像中的位置。上述的立體慣性導盲方法，通過兩個相機2拍攝的圖像以及慣性傳感器1的數據，測算出當前視力殘疾人所在的位置，以及測算周圍物體相對于視力殘疾人的三維坐標，若有障礙物，則識別物體，并將盲人本身的三維坐標以及物體相對于視力殘疾人而言的位置，通過語音播報等形式實時反饋給視力殘疾人，將實際行走軌跡正確的判斷與行走軌跡上的障礙物判斷結合在一起，利用相機2拍攝以及慣性傳感器1的配合，既能做到全方位、遠距離的障礙物檢測，還提高三維空間定位的更新頻率，獲得更小的定位誤差，并且可以對障礙物進行識別，以便于提示視力殘疾人正確的避開障礙物。

如圖5所示為本實施例提供的立體慣性導盲裝置，包括裝置本體、兩個相機2、慣性傳感器1、服務器以及播放結構；兩個所述相機2、所述慣性傳感器1以及所述播放結構連接在所述裝置本體上。

在本實施例中，上述的裝置本體為導盲眼鏡，于其他實施例，上述的裝置本體為導盲仗或者導盲機器人，即上述的裝置本體為導盲眼鏡、導盲仗以及導盲機器人中至少一種。

當裝置本體為導盲眼鏡時，兩個相機2裝在眼鏡的鏡框5上，該眼鏡的鏡腳7上設有耳機3，播放結構為嵌入在耳機3內的喇叭，慣性傳感器1也安裝在鏡框5上。

另外，立體慣性導盲裝置還包括電池，電池與慣性傳感器1、播放結構以及相機2分別連接。

如圖6所示，當裝置本體為導盲仗時，兩個相機2以及慣性傳感器1安裝在仗桿上，服務器以及電池安裝在仗柄4上，另外，仗柄4上設有耳機3，播放結構為嵌入在耳機3內的喇叭。

如圖7所示，當裝置本體為導盲機器人時，兩個相機2以及慣性傳感器1安裝在機器人本體6的前端，服務器以及電池安裝在機器人本體6內，播放結構為嵌入在耳機3內的喇叭，該耳機3與服務器為無線連接。

上述的立體慣性導盲裝置，檢測水平角度120度內、最遠到10米的障礙物，可以識別簽發物體類別，例如紅綠燈顏色，紙幣面額，人臉的姓名等，三維空間定位可以將定位的更新頻率大幅提高(例如從30hz提高到300hz)，由于慣性傳感器1的imu數據可以一同被用作位置測算，定位誤差減小可以達到50％或者更多，可以獲得更小的定位誤差，并且可以進行靜止狀態的三維障礙物檢測。

如圖8所示為本實施例提供的立體慣性導盲系統，包括軌跡獲取單元8、軌跡判斷單元9、點云獲取單元10以及點云判斷單元11；

軌跡獲取單元8，用于采用視覺慣性定位方式，確定首次行走的軌跡，同時記錄兩個相機所拍攝到的所有圖像中的特征點；

軌跡判斷單元9，用于判斷實際行走的軌跡是否與所述首次行走的軌跡一致；若一致，則進行點云獲取，若不一致，則發送提醒信息，并進入點云獲取；

點云獲取單元10，用于獲取實際行走的軌跡中的點云信息；

點云判斷單元11，用于判斷所述點云信息中是否有超過設定數量的點集合在某一規定的區域內；若有，則發送提醒信息，若無，則進行點云獲取。

如圖9所示，上述的點云獲取單元10包括基礎信息獲取模塊30、圖像獲取模塊31以及點云信息獲取模塊32；

基礎信息獲取模塊30，用于獲取兩個相機2的相對位置和角度；

圖像獲取模塊31，用于獲取兩個所述相機2同一時刻曝光的圖像，作為第一圖像以及第二圖像；

點云信息獲取模塊32，用于利用所述第一圖像以及所述第二圖像、兩個所述相機2的相對位置和角度，獲取點云信息。

基礎信息獲取模塊30在獲取兩個相機2的位置和角度時，給定所有標記點l(i,t)和imu數據，求一組慣性傳感器1的位置(x,y,z)和角度(quaternion)使得所有標記點l(i,t)的投影誤差e(i,t)之和、imu數據和位置、角度的參數的不同程度最小，只要給定慣性傳感器1在初始時刻的位置，就可以求出兩個相機2在任何時刻的位置和角度。

如圖10所示，上述的點云信息獲取模塊32包括像素提取子模塊321、圖像位置獲取子模塊322、真實位置確定子模塊323、點云獲取子模塊324以及對應子模塊325；

像素提取子模塊321，用于提取第一圖像中的所有像素，并提取每個像素周圍的圖像特征，在第二圖像中尋找與第一圖像中提取的像素對應的特征最相似的像素；

圖像位置獲取子模塊322，用于獲取所述第二圖像的像素相對于所述第一圖像對應的像素的位置，作為點的圖像位置；

真實位置確定子模塊323，用于利用三角定位的方式，確定所述點的真實三維位置；

點云獲取子模塊324，用于集合所述第一圖像以及所述第二圖像所有像素點所對應的點，獲取點云；

對應子模塊325，用于將所述點云中所有點對應的真實三維位置與所述點云對應，獲取點云信息。

對于上述的點云信息獲取模塊32，兩個相機2在同一時刻曝光，獲取兩張圖像：圖像1和圖像2；對于圖像1的每一個像素(x,y)，提取其周圍圖像的特征，在圖像2中尋找與像素(x,y)特征最相似的像素(u,v)，(x,y)-(u,v)對應真實空間的一個點，利用三角定位方式，確定(x,y)-(u,v)對應的真實的點的三維坐標p，將所有的像素對應的點收集在一起，獲得點云，此過程中像素的坐標，是利用慣性傳感器1獲得重力方向，在點云中去除垂直于重力方向的平面(地面)。

上述的立體慣性導盲系統還包括首次使用判斷單元12、以及匹配單元20；

首次使用判斷單元12，用于判斷是否屬于第一次使用，若是，則進行軌跡獲取；

若不是，則進行圖像匹配，確定初始位置；

匹配單元20，用于結合所述初始位置以及相對于所述導航路徑所處的位置，進行導航，并進入軌跡判斷。

首次使用判斷單元12判斷是否屬于第一次使用，指的是是否是第一次使用該立體慣性導盲裝置，若是，則需要幫助視力殘疾人佩戴立體慣性導盲裝置沿某路徑行走，以作為路徑的記錄，若不是第一次使用，則圖像匹配，尋找與之行走的路線對應的記錄的路徑，直接調取，效率高。

如圖11所示，上述的軌跡獲取單元8還包括曝光圖像獲取模塊81、特征點提取模塊82、輔助特征點提取模塊83、標志點獲取模塊84、角點列入模塊85以及獲取軌跡模塊86；

曝光圖像獲取模塊81，用于獲取兩個所述相機2在某一時刻曝光的兩張圖像；

特征點提取模塊82，用于提取其中一張所述圖像的角點以及所述角點的周圍的圖像特征，作為特征點；

輔助特征點提取模塊83，用于計算所述特征點在另一張圖像中的極線，并在所述極線上尋找與所述特征點最接近的點，作為輔助特征點；

標志點獲取模塊84，用于結合特征點以及輔助特征點，通過三角法計算所述特征點所對應的真實位置，以特征點、輔助特征點以及所述真實位置作為標記點；

角點列入模塊85，用于給定某一角點的周圍的圖像特征，將其和所有已有標記點所對應的圖像特征對比，如果匹配，則把所述角點列入所述標記點的特征中；

獲取軌跡模塊86，用于獲取所有標記點串聯而成的路線，將慣性傳感器中的陀螺儀信息加入角度測算，將慣性傳感器中的加速度信息加入行走速度測算，獲取行走的軌跡。

上述軌跡確認單元在使用過程中，若兩個相機2在t時刻曝光，獲取兩張圖像i(t,1)和圖像ii(t,2)，在圖像i(t,1)中提取一定的角點(例如70個)。對于每個角點，提取其周圍圖像特征，對于第i個在i(t,1)中的的特征點p(t,1,i)，計算其在圖像ii中的極線，在極線上尋找與特征點p1(t,1,i)的圖像特征最接近的點，記為p2(t,2,i)，給定p1(t,1,i)和p2(t,2,i)通過三角法計算對應三維點的位置p(i,t)，一組p1(t,1,i)、p2(t,2,i)、p(i,t)記為一個“標記點”l(i,t)；給定特征點p1(t,1,i)的特征，將其和所有已有標記點l(i,t)(t<t)的圖像特征對比，如果匹配，則把p1(t,1,i)加入l(i,t)的特征中；獲取所有標記點l(i,t)串聯而成的路線，作為行走的軌跡。

對于標志點獲取模塊84獲取的標志點，對任何一個標記點，都有一個三維點p(i,t)和一組在不同圖像中對應的特征位置。

給定拍攝圖像時的兩個相機2以及慣性傳感器1imu的位置和角度(x,y,z,quaternion)，都可以計算三維點p(i,t)在所拍攝的圖像中的幾何投影位置，幾何投影位置和圖像特征位置的不同是參數誤差，所有參數誤差的l2求和記做e(i,t)。基礎信息獲取單元40在獲取兩個相機2的位置和角度時，給定所有標記點l(i,t)和imu數據，求一組慣性傳感器1的位置(x,y,z)和角度(quaternion)使得所有標記點l(i,t)的投影誤差e(i,t)之和、imu數據和位置、角度的參數的不同程度最小，只要給定慣性傳感器1在初始時刻的位置，就可以求出兩個相機2在任何時刻的位置和角度。

上述的點云判斷單元11在判斷時，主要是將兩個相機2所拍攝的前方區域切分成扇形區域，統計每個扇形區域的點的數量，若某一規定的區域(也就是某一扇形區域)超過設定數量，則認為即將遇到障礙物。

另外，在識別區域內的物體時，主要是將兩個相機2拍攝到的圖像輸入到物體識別cnn網絡系統中，由物體識別cnn網絡系統識別后，輸出物體對應的大致模型和名字以及其在圖像中出現的位置。

播放單元播報所述物體的位置時，在本實施例中，采用的是語音播報方式，以一定的頻率(例如3秒鐘1次)向視力殘疾人播報兩個相機2所拍攝的每個扇形區域點的數量，播報的方式可以利用環繞立體聲，利用省掉高低表示每個方向的點的數量，讓視力殘疾人聽到潛在障礙物和整體環境的幾何信息。接收到的物體識別結果，通過語音播報當前的圖像中實現規定的物體的出現情況以及在當前的圖像中的位置。

上述的立體慣性導盲系統，通過兩個相機2拍攝的圖像以及慣性傳感器1的數據，測算出當前視力殘疾人所在的位置，以及測算周圍物體相對于視力殘疾人的三維坐標，若有障礙物，則識別物體，并將盲人本身的三維坐標以及物體相對于視力殘疾人而言的位置，通過語音播報等形式實時反饋給視力殘疾人，將實際行走軌跡正確的判斷與行走軌跡上的障礙物判斷結合在一起，利用相機2拍攝以及慣性傳感器1的配合，既能做到全方位、遠距離的障礙物檢測，還提高三維空間定位的更新頻率，獲得更小的定位誤差，并且可以對障礙物進行識別，以便于提示視力殘疾人正確的避開障礙物。

上述僅以實施例來進一步說明本發明的技術內容，以便于讀者更容易理解，但不代表本發明的實施方式僅限于此，任何依本發明所做的技術延伸或再創造，均受本發明的保護。本發明的保護范圍以權利要求書為準。