[發明專利]一種基于無跡卡爾曼濾波的偏振光SLAM方法

有效

• 申請號: 201810128645.1
• 申請日: 2018-02-08
• 公開（公告）號: CN108362288B
• 公開（公告）日: 2021-05-07
• 發明（設計）人: 杜濤;白鵬飛;郭雷;王華鋒;劉萬泉;王月海
• 申請（專利權）人: 北方工業大學;北京航空航天大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G06F17/16;G06F30/20
• 代理公司: 北京科迪生專利代理有限責任公司 11251
• 代理人: 楊學明;顧煒

1.一種基于無跡卡爾曼濾波的偏振光SLAM方法，其特征在于：包括以下步驟：

步驟(1)、選取無人機的姿態、速度、位置和路標點的位置為系統狀態，建立無人機的動力學模型；

步驟(2)、建立激光雷達的量測模型；

步驟(3)、建立偏振光傳感器的量測模型；

步驟(4)、系統初始化、地圖初始化；

步驟(5)、路標點匹配；

步驟(6)、利用路標點的激光雷達數據和偏振傳感器數據，設計UKF濾波器，估計無人機位置、速度、姿態和路標點的位置；

步驟(7)、地圖更新；

其中，所述步驟(1)選取無人機的姿態、速度、位置和路標點為系統狀態，建立無人機的動力學模型；以無人機起始位置為原點的世界坐標系，即w系，以正北方向為世界坐標系的x軸的正方向，以正西方向為世界坐標系的y軸的正方向，根據右手準則確定世界坐標系的z軸正方向；建立以無人機機體中心為原點的機體坐標系，即b系，以平行于機身縱軸指向機頭方向為機體坐標系x軸的正方向，以平行于機身橫軸指向左方為機體坐標系y軸的正方向，根據右手準則確定機體坐標系z軸正方向；選取無人機的位置、速度、姿態和路標點的位置作為系統狀態；則系統狀態轉移方程如下：

其中

表示k-1時刻的系統狀態，表示k-1時刻無人機在世界坐標系下的坐標，分別表示k-1時刻無人機在世界坐標系下x,y,z三個軸方向的坐標，表示k-1時刻無人機在世界坐標系下的速度，分別表示k-1時刻無人機在世界坐標系下指向x,y,z三個軸方向的速度，表示無人機的姿態，以歐拉角的形式表示，分別為滾轉角、俯仰角和偏航角，表示k-1時刻世界坐標系下的m個路標點的坐標，表示第i,i＝1,2,...,m個路標點在世界坐標系下的坐標，為k-1時刻機體坐標系下的比力加速度，為k-1時刻機體坐標系下的角速度，為k-1時刻世界坐標下的重加速度，Δt為采樣時間間隔，

為k-1時刻機體坐標系到世界坐標系的變換矩陣，n_k-1為系統噪聲，噪聲設定為高斯白噪聲，Q_k-1為系統噪聲協方差矩陣；

所述步驟(2)建立激光雷達的量測模型；選取激光雷達測量的距離、角度作為測量值，針對激光雷達量測的數據給出路標點的量測模型：

其中

表示第k時刻對第i個路標點的量測方程中的非線性函數，y_k,i,lidar＝[d_k,i θ_k,i λ_k,i]^T表示k時刻系統的量測值，d_k,i表示第k時刻激光雷達測量的無人機質心與第i個路標點的距離，θ_k,i表示第k時刻激光雷達測量的無人機質心與第i個路標點的距離俯仰角，λ_k,i表示第k時刻激光雷達測量的無人機質心與第i個路標點的方位角，u_k,lidar表示激光雷達量測噪聲，噪聲設定為高斯白噪聲，R_k,lidar為量測噪聲協方差矩陣；

所述步驟(3)建立偏振光傳感器的量測模型：選取偏振光傳感器量測的機體縱軸與太陽子午線的夾角量作為測值，針對偏振光給出的量測數據的觀測模型如下：

其中為偏振光傳感器獲取的機體長軸與太陽子午線的夾角，為太陽方位角，為太陽高度角，為太陽赤緯，為觀測點的緯度，為太陽時角，u_k,polar表示偏振光傳感器量測噪聲，噪聲設定為高斯白噪聲，R_k,polar為量測噪聲協方差矩陣；

所述步驟(4)系統初始化；將無人機坐標系轉換到世界坐標系并將無人機，給定初始時刻的系統狀態為初始協方差矩陣P₀，并建立以無人機起始位置為原點的全局地圖；

所述步驟(5)路標點匹配；對k時刻激光雷達量測到的數據，和在全局地圖中存儲的路標點利用迭代最近點(ICP)算法進行匹配，對于匹配成功的路標點，則將該路標點送入相應的濾波器進行后續的運算；

所述步驟(6)利用路標點的激光雷達數據和偏振光傳感器數據，設計UKF濾波器，估計無人機的位置、速度、姿態和路標點的位置；首先將k-1時刻的估計狀態和協方差矩陣，結合k-1時刻的IMU的輸出數據，進行一步遞推得到由IMU和狀態轉移矩陣推算出的機器人位置，然后根據預測值，加入激光傳感器信息和偏振光信息進行量測更新的計算，輸出無人機的位置、速度、姿態和路標點的位置，并同時輸出相應的協方差矩陣，具體步驟如下：

時間更新：

其中為狀態向量維度，P_k-1|k-1為k-1時刻的誤協方差矩陣，為尺度參數，計算方法為該式中ε為一小量取值范圍為[10^-4,1]，常量可取0或者3-n；

量測更新：

P_k|k＝P_k|k-1-K_kP_yy,k|k-1K_k^T

其中P_yy,k|k-1為自協方差矩陣，P_xy,k|k-1為互協方差矩陣；

所述步驟(7)地圖、狀態更新；對于全局地圖中已存在的路標點，將對應的濾波后的路標點作為最新路標點，對步驟(5)中未匹配成功的路標點，直接將其加入到全局地圖中。