[發明專利]一種機器人視覺慣性點線特征定位方法及裝置

有效

• 申請號: 201811604130.0
• 申請日: 2018-12-26
• 公開（公告）號: CN109752003B
• 公開（公告）日: 2021-03-02
• 發明（設計）人: 王越;焦艷梅;熊蓉
• 申請（專利權）人: 浙江大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16
• 代理公司: 杭州中成專利事務所有限公司 33212
• 代理人: 李亦慈;唐銀益

1.一種機器人視覺慣性點線特征定位方法，其特征在于，包括以下步驟：

獲取當前場景的先驗三維地圖和慣性傳感器的測量數據，所述先驗三維地圖是事先構建的，且包含三維點線特征；

獲取機器人的當前圖像和慣性傳感器的測量數據；

根據當前機器人的慣性傳感器的測量數據與所述先驗三維地圖中的慣性傳感器的測量數據計算當前機器人位姿的俯仰角和翻滾角；對機器人當前圖像進行二維點線特征的檢測，并將當前圖像中測量到的二維點線特征與所述先驗三維地圖中測量到的三維點線特征進行匹配；

根據匹配到的兩對二維到三維點特征或者一對二維到三維匹配的點特征加一對二維到三維匹配的線特征計算當前機器人位姿的其余未知量；

所述的根據匹配到的兩對二維到三維點特征計算先驗三維地圖相對于當前圖像機器人坐標系的位姿的其余未知量，具體包括以下子步驟：

根據地圖坐標系{W₀}中匹配到的兩個三維點P₁⁰,P₂⁰，引入一個中間坐標系{W₁}，其中兩個三維點在所述中間坐標系下表示為P₁＝(0 0 0)^T,P₂＝(X₂ Y₂ Z₂)^T，而{W₀}到{W₁}的變換則表示為P₁⁰→P₁的簡單平移；根據所述機器人當前圖像中匹配到的二維點和相機標定的內參矩陣K可得二維點在機器人相機坐標系{C}下的歸一化平面上的三維坐標：

其中C為相機的光心；

根據投影幾何可得三點(C,D₁,RP₁+t)共線，且三點(C,D₂,RP₂+t)共線，其中R為{W₁}相對于{C}的旋轉變換矩陣，t為{W₁}相對于{C}的平移變換向量；根據{W₀}相對于{C}的俯仰角和翻滾角，可將R簡化為：

其中α為未知量，表示為地圖坐標系相對于機器人當前相機坐標系的偏航角，設t＝[t₁t₂ t₃]^T，則全部未知量一共有四個，分別為α,t₁,t₂,t₃；

根據三點(C,D₁,RP₁+t)共線，可得兩個方程如下：

a₁t₂-b₁t₁＝0

a₁t₃-t₁＝0

根據三點(C,D₂,RP₂+t)共線，可得兩個方程如下：

a₂(R₂₁X₂+R₂₂Y₂+R₂₃Z₂+t₂)-b₁(R₁₁X₂+R₁₂Y₂+R₁₃Z₂+t₁)＝0

a₂(R₃₁X₂+R₃₂Y₂+R₃₃Z₂+t₃)-(R₁₁X₂+R₁₂Y₂+R₁₃Z₂+t₁)＝0

聯立以上四個方程即可求得四個未知量，從而求得機器人在地圖中的當前位姿。