[發明專利]一種小型無人機視覺定位方法

有效

• 申請號: 202010116259.8
• 申請日: 2020-02-25
• 公開（公告）號: CN111288989B
• 公開（公告）日: 2021-11-05
• 發明（設計）人: 趙文杰;尹文財
• 申請（專利權）人: 浙江大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G06T7/55
• 代理公司: 杭州求是專利事務所有限公司 33200
• 代理人: 鄭海峰

1.一種小型無人機視覺定位方法，其特征在于包括如下步驟：

步驟1：從多路攝像頭中提取特征點

從兩路單目攝像頭、兩路雙目攝像頭中接收圖像數據，輸出視覺幀的特征點信息；

步驟2：使用深度濾波器對特征點進行深度融合，得到特征點的深度和方差；

所述的步驟2)具體為：

步驟2.1：利用特征點首次觀測幀與當前幀的位姿，使用三角測量方法，估算特征點深度，特征點深度滿足下述方程組：

s₁x₁^Tx₁-s₂x₁^TRx₂＝x₁^Tt

s₁(Rx₂)^Tx₁-s₂(Rx₂)^TRx₂＝(Rx₂)^Tt

其中x₁，x₂分別表示特征點在首次觀測幀與當前幀中的歸一化坐標，R表示幀間姿態的變換，t表示幀間位置的變換，s₁，s₂表示特征點在首次觀測幀與當前幀中的深度，對該方程組中的深度值進行求解；

步驟2.2：求解特征點單次估計的深度方差

使用幾何不確定度來對特征點深度方差進行估計，δp＝|||p||-||p′|||；

||p||表示特征點深度估計值，||p′||表示對特征點像素值進行一個像素的擾動后的新特征點深度估計值；δp即為深度的一個幾何不確定度，也就是深度融合的一個測量值所對應的均方誤差；

步驟2.3：將上一狀態最優深度與當前測量深度融合

對于系統內所有特征點，用一個近似均勻高斯分布來描述特征點的深度分布，

其中，為以真值為中心的高斯分布，為測量方差，ρ表示為該測量值屬于有效測量的概率，為均勻分布，d_min、d_max為其上下限；將真實的后驗分布近似表達為由四個參數控制，分別為Z，σ，a，b；當新的測量值d_k到來時，深度濾波器的更新做如下處理：

步驟2.3.1：計算深度融合初值：

Z_old，分別表示上一次深度融合后深度均值與方差，Z_est，分別表示當前觀測所計算的深度均值與方差；

步驟2.3.2：計算權重系數c₁：

a_old，b_old分 別表示上一次深度融合后均勻分布的左右邊界，權重系數c₁決定著新的觀測深度值和方差對本次融合深度值和方差的加權比重；

步驟2.3.3：計算權重系數c₂：

a_est，b_est分 別表示當前觀測所計算的均勻分布的左右邊界；Z_range為場景的平均深度，權重系數c₂由上一次融合后的Beta分布參數決定，控制由上一次融合后的深度得到的深度估計值對本次深度融合的加權比重；

步驟2.3.4：歸一化系數c₁，c₂：

步驟2.3.5：計算系數f、e用于更新Beta分布的參數a、b：

步驟2.3.6：更新融合后的深度值：

Z_new＝c₁·m+c₂·Z_est

步驟2.3.7：更新融合后的深度值方差：

步驟2.3.8：更新融合后的a、b：

步驟3：使用預積分技術對IMU數據進行預處理；

步驟4：使用IMU數據、視覺特征點對系統進行初始化；

步驟5：使用滑窗優化方式估計系統關鍵幀位姿。