[發明專利]點云稀疏場景下主動特征輔助的自適應融合定位方法

在審

• 申請號: 202210177446.6
• 申請日: 2022-02-24
• 公開（公告）號: CN114527487A
• 公開（公告）日: 2022-05-24
• 發明（設計）人: 徐啟敏;孔鑫;李旭;孔棟
• 申請（專利權）人: 東南大學
• 主分類號: G01S17/931
• 分類號: G01S17/931;G01S17/86;G01S17/08;G01S5/02;G06T7/73;G06K9/62;G06T7/80
• 代理公司: 南京眾聯專利代理有限公司 32206
• 代理人: 張天哲

1.一種點云稀疏場景下主動特征輔助的自適應融合定位方法，其特征在于：在激光SLAM因點云稀疏而導致定位精度的降低甚至無法定位的情況下，通過在路側布設主動特征來增加激光雷達的觀測量，并引入UWB距離觀測信息來識別激光雷達掃描到的主動特征編號；同時通過模糊邏輯分類算法來對激光雷達與UWB的觀測精度進行分類，從而自適應調節激光雷達與UWB的噪聲方差陣；最后通過擴展卡爾曼濾波算法實現車輛的融合定位；具體步驟如下：

步驟1：路側主動特征與UWB基站的布設

在激光雷達點云稀疏的區域每隔10-20m等間距的布置主動特征及UWB基站；其中主動特征為低成本的白色圓柱體泡沫，將主動特征安裝在相應支架上，在每個主動特征的頂部都安裝相應的UWB基站；以正東方向為OX軸，正北方向為OY軸，地球表面某一固定點為原點建立坐標系；各個主動特征及UWB基站在該坐標系下的位置可通過提前標定獲取；UWB信號接收標簽及激光雷達安裝在行駛車輛的頂端；

步驟2：激光雷達觀測量識別

根據步驟1中的布局，在同一時刻UWB標簽所能接收到的觀測信息為r₁，r₂，…，r_N，其中r_N表示第N個主動特征與車輛之間的距離值；激光雷達檢測到主動特征后所能解算的觀測信息為d，θ，其中d為路側某個未知的主動特征與激光雷達之間的距離值，θ為該主動特征與激光雷達之間相對于正北方向的夾角的絕對值；由于激光雷達對主動特征進行檢測時無法判斷該主動特征的編號，而主動特征頂部的UWB基站恰好彌補了這一缺點；通過將激光雷達解算的距離信息d與UWB標簽接收到的距離信息作對比，若|d-r_i|＜ε，其中，i＝1，2，…，N，ε為事先設定的閾值，則可判定當前激光雷達檢測的距離d與角度θ隸屬的主動特征編號i；

步驟3：基于模糊邏輯算法的UWB與激光雷達觀測精度分類

對于激光雷達與UWB來說，二者檢測物體與測距的精度都會隨著傳感器與被測物之間距離的增加而降低；基于此特征，采用模糊邏輯算法分別對UWB基站和激光雷達的觀測精度進行分類，一共分為A，B，C三個等級，對應UWB與激光雷達觀測值的精度由低到高，模糊邏輯分類算法的具體步驟如下：

1)確定距離特征的三個隸屬度函數，并對距離分類，分別為N，M，R，代表距離的“近”、“中”、“遠”；關于距離的三種隸屬度函數如下所示，其中，d，的單位為米：

2)根據模糊分類規則，確定模糊邏輯分類輸出的3個精度等級A，B，C，

模糊分類規則如下：

1.當距離為N時，精度等級為A；

2.當距離為M時，精度等級為B；

3.當距離為R時，精度等級為C；

步驟4：基于自適應擴展卡爾曼濾波的車輛定位

由步驟2可知，在離散時刻k可接收到的觀測值包括UWB基站提供的距離信息r₁，r₂，...，r_N以及激光雷達掃描到的主動特征的距離和角度信息d和θ，則可通過建立擴展卡爾曼濾波器獲取實時的車輛位置；建立離散的狀態方程和觀測方程為：

其中k表示離散的時間，X(k)＝[p_e(k)p_n(k)]′為狀態向量，上角標′表示對矩陣轉置，p_e(k)和p_n(k)分別表示車輛的東西和北向位置坐標，狀態轉移向量為系統的觀測向量，其中為N個UWB基站的距離觀測值，和分別為激光雷達檢測到的相應主動特征的距離和角度觀測值；h是相應的觀測函數：

(X_i，Y_i)是事先標定好的第i個主動特征的坐標，表示相應的觀測噪聲向量；w和v分別為系統的狀態噪聲和觀測噪聲向量，它們的協方差矩陣分別為Q和R，其中R＝diag[R^UWBR^Lidar]，R^UWB與R^Lidar分別為UWB與激光雷達的觀測噪聲方差陣，根據步驟3中所述的模糊分類算法對UWB基站與激光雷達的觀測精度進行分類，進而根據分類結果自適應調節UWB與激光雷達的噪聲方差陣R^UWB、R^Lidar，得到最終的觀測噪聲方差陣R(k)，具體方法如下：

1.計算第i個UWB基站與激光雷達檢測的第j個主動特征的觀測噪聲其中和分別為UWB距離觀測值和激光雷達主動特征觀測值的噪聲方差基礎值；μ和ρ為調節系數，由模糊分類的結果確定，三種不同的精度等級A，B，C對應的調節系數分別為μ_a，μ_b，μ_c和ρ_a，ρ_b，ρ_c；模型調節系數與精度等級如下表所示：

表1距離精度等級與模型調節系數

距離精度等級	模型調節系數
A	1
B	2.5
C	6

2.計算UWB與激光雷達的觀測噪聲方差陣：

3.更新噪聲方差陣R(k)：R(k)＝diag[R^UWBR^Lidar]；

根據上述步驟所述的系統狀態模型和觀測模型以及噪聲方差陣的自適應調節后，即可進行擴展卡爾曼濾波的迭代更新，標準擴展卡爾曼濾波過程包括時間更新和測量更新：

時間更新：

P(k，k-1)＝F(k，k-1)P(k-1)F′(k，k-1)+Q(k-1) (7)

測量更新：

K(k)＝P(k，k-1)H′(k)[H(k)P(k，k-1)H′(k)+R(k)]^-1 (8)

P(k)＝[I-K(k)H(k)]P(k，k-1) (10)

其中I表示單位矩陣，H是h(·)對X求導的雅可比矩陣；

通過濾波計算，則可計算出每一時刻車輛的位置信息，實現點云稀疏場景下車輛的可靠定位。