[發明專利]確定無人機位置和飛行方向的方法、裝置及無人機

有效

• 申請號: 201811356896.1
• 申請日: 2018-11-15
• 公開（公告）號: CN111192318B
• 公開（公告）日: 2023-09-01
• 發明（設計）人: 沈亢偉
• 申請（專利權）人: 杭州?？低晹底旨夹g股份有限公司
• 主分類號: G06T7/70
• 分類號: G06T7/70;G06T7/80;G05D1/12
• 代理公司: 北京德琦知識產權代理有限公司 11018
• 代理人: 衣淑鳳;宋志強

1.一種確定無人機位置和飛行方向的方法，其特征在于，該方法包括：

根據無人機的云臺搭載的相機拍攝的包含m個固定點的圖像，計算m個固定點在相機坐標系下的坐標，m≥4；所述m個固定點同時位于一個標記物上；所述m個固定點為：標記物上具有明顯特征的特征點，或者與特征點有固定關系的點；且，根據該標記物的特征點的特征找到特征點，從而找到固定點，或者，根據該標記物的特征點的特征找到特征點，然后根據固定點與特征點的關系找到固定點；且，當無人機云臺上的相機對準該標記物時，從標記物上的文字或二維碼或索引標記或索引號中獲得m個固定點的世界坐標；

根據m個固定點在世界坐標系下的坐標和在相機坐標系下的坐標，計算相機坐標系到世界坐標系的轉換關系；

根據相機拍攝到所述包含m個固定點的圖像時云臺相對無人機的旋轉角度，計算無人機坐標系到相機坐標系的轉換關系；

根據相機坐標系到世界坐標系的轉換關系，以及無人機坐標系到相機坐標系的轉換關系，計算無人機在世界坐標系中的位置坐標以及無人機在世界坐標系中的飛行方向向量。

2.根據權利要求1所述的方法，其特征在于，所述計算相機坐標系到世界坐標系的轉換關系包括：

計算相機坐標系到世界坐標系的旋轉矩陣R和平移向量T；

所述計算無人機坐標系到相機坐標系的轉換關系包括：

計算無人機坐標系到相機坐標系的旋轉矩陣Q；

所述計算無人機在世界坐標系中的位置坐標以及無人機在世界坐標系中的飛行方向向量包括：

確定無人機在世界坐標系中的位置坐標為平移向量T，以及計算出無人機在世界坐標系中的飛行方向向量為：無人機在無人機坐標系中的飛行方向向量與旋轉矩陣Q和旋轉矩陣R的乘積。

3.根據權利要求2所述的方法，其特征在于，當采用無人機的正前方作為無人機在無人機坐標系中的飛行方向向量時，無人機在無人機坐標系中的飛行方向向量為[0,0,1]。

4.根據權利要求1或2所述的方法，其特征在于，所述云臺相對無人機的旋轉角度包括：翻滾角θ、俯仰角ф和偏航角

所述計算無人機坐標系到相機坐標系的轉換關系包括：

根據如下原則：將無人機先繞偏航角對應軸旋轉角度、再繞俯仰角對應軸旋轉ф角度、最后繞翻滾角對應軸旋轉θ角度，構造無人機坐標系到相機坐標系的旋轉矩陣Q。

5.根據權利要求4所述的方法，其特征在于，所述旋轉矩陣Q符合以下公式要求：

6.一種確定無人機位置和飛行方向的裝置，該裝置應用于無人機，所述無人機具有搭載相機和角度傳感器的云臺，其特征在于，該裝置包括：

旋轉角度獲取模塊，用于從角度傳感器獲取相機拍攝到包含m個固定點的圖像時云臺相對無人機的旋轉角度，m≥4；所述m個固定點同時位于一個標記物上；所述m個固定點為：標記物上具有明顯特征的特征點，或者與特征點有固定關系的點；且，根據該標記物的特征點的特征找到特征點，從而找到固定點，或者，根據該標記物的特征點的特征找到特征點，然后根據固定點與特征點的關系找到固定點；且，當無人機云臺上的相機對準該標記物時，從標記物上的文字或二維碼或索引標記或索引號中獲得m個固定點的世界坐標；

固定點坐標計算模塊，用于根據相機拍攝到的所述包含m個固定點的圖像，計算m個固定點在相機坐標系下的坐標；

第一坐標系轉換關系計算模塊，用于根據m個固定點在世界坐標系下的坐標和在相機坐標系下的坐標，計算相機坐標系到世界坐標系的轉換關系；

第二坐標系轉換關系計算模塊，用于根據相機相對無人機的旋轉角度，計算無人機坐標系到相機坐標系的轉換關系；

無人機位置和飛行方向計算模塊，用于根據相機坐標系到世界坐標系的轉換關系，以及無人機坐標系到相機坐標系的轉換關系，計算無人機在世界坐標系中的位置坐標以及無人機在世界坐標系中的飛行方向向量。