[發明專利]一種基于環視視覺和慣導融合的地下車庫庫位跟蹤方法

有效

• 申請號: 201811119444.1
• 申請日: 2018-09-25
• 公開（公告）號: CN109443348B
• 公開（公告）日: 2022-08-23
• 發明（設計）人: 余卓平;許明煜;熊璐;張培志;康戎;嚴森煒
• 申請（專利權）人: 同濟大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/18
• 代理公司: 上海科盛知識產權代理有限公司 31225
• 代理人: 葉敏華

1.一種基于環視視覺和慣導融合的地下車庫庫位跟蹤方法，其特征在于，該方法包括以下步驟：

1)啟動車輛的環視相機和慣性測量單元，利用環視相機采集車輛行駛過程中的全景環視圖像，利用慣性測量單元獲取當前預積分；

2)判斷環視相機和慣性測量單元是否已完成初始化，若是，則執行步驟3)，否則，對環視相機和慣性測量單元進行初始化；

3)根據全景環視圖像提取庫位角點，根據庫位角點在觀測時刻相對車輛的二維坐標，獲取實際車輛坐標與庫位角點坐標之間的相對位置；

4)根據慣性測量單元的當前預積分獲取慣性姿態約束，對多個相鄰數據執行相同處理并積分，獲取相鄰數據的相對位姿變換關系；

5)將步驟3)中提取的庫位角點進行三角化并插入到局部映射中，剔除兩幀圖像特征匹配的非庫位角點，將可見的所有庫位角點保存在本地地圖中；

6)對讀取的環視圖像進行分類，將不同類型的環視圖像加入不同地圖中進行優化，結合相鄰數據的相對位姿變換關系，優化目標庫位角點的環境地圖全局坐標，進而完成庫位跟蹤；

所述的環視相機包括四個設于車輛前后左右四個方向的環視魚眼相機；

對環視相機和慣性測量單元進行初始化具體包括以下步驟：

201)啟動環視相機和慣性測量單元，獲取全景環視圖像和當前預積分模型，選定一初始位置作為全局坐標系原點，用以建立庫位角點地圖；

202)采用棋盤格對環視相機進行標定，獲得四個環視魚眼相機的內參、畸變系數以及與車輛的相對位置；

203)通過五點法分解一個本質矩陣獲取初始化環視相機運動，將初始化的平移作為單位尺度；

204)通過三角測量的方法恢復圖像特征點深度，完成環視相機初始化；

205)依據初始化環視相機獲取兩圖像幀之間的旋轉，并將其與利用慣性測量單元的預積分模型獲取的旋轉進行對比，以偏差為節點變量，最小化二者的差值，獲取慣性測量單元初始偏置值；

206)根據初始偏置值對慣性測量單元進行矯正，對每一個圖像幀的速度、重力和尺度進行估計，完成環視相機和慣性測量單元初始化操作；

步驟3)具體包括以下步驟：

301)利用機器視覺方法提取全景環視圖像中的庫位角點特征，并獲取庫位角點在觀測時刻相對車輛的二維坐標；

302)根據庫位角點在觀測時刻相對車輛的二維坐標，使用單應矩陣描述處于共同平面上的一些點在兩張圖像之間的變換關系，對兩張圖像之間的特征點進行匹配；

303)根據匹配點獲取單應性矩陣，并將其分解以計算旋轉和平移；

304)根據步驟2)標定得到的環視魚眼相機的內參、畸變系數以及與車輛的相對位置，結合旋轉和平移獲取實際車輛坐標與庫位角點坐標之間的相對位置。

2.根據權利要求1所述的一種基于環視視覺和慣導融合的地下車庫庫位跟蹤方法，其特征在于，步驟4)中，根據慣性測量單元的當前預積分獲取慣性姿態約束具體包括以下步驟：

401)根據慣性測量單元的當前預積分獲取加速度和角速度；

402)利用加速度推算速度和位移，利用角速度推算旋轉，獲取如下的積分關系：

其中，R_WB為慣導元件在世界坐標系下的旋轉姿態，v_W為慣導元件在世界坐標系下的速度，p_W為慣導元件在世界坐標系下的位移，為陀螺儀測量的角速度值，a_w(τ)為加速度計測量的加速度值；

403)將積分形式轉為差分形式；

404)考慮實際觀測值，將偏差和噪聲加入到差分形式，獲取插入后的旋轉姿態方程，即：

其中，b^g(t)和η^gd(t)分別為陀螺儀零偏和噪聲，b^a(t)和η^ad(t)分別為加速度計的零偏和噪聲。