[發明專利]基于視覺慣性導航系統的激光測距儀及測距方法

有效

• 申請號: 202010158980.3
• 申請日: 2020-03-09
• 公開（公告）號: CN111220118B
• 公開（公告）日: 2021-03-02
• 發明（設計）人: 李艷文;劉建鑫;唐天鵬;梁汛;閆世林;王定煜
• 申請（專利權）人: 燕山大學
• 主分類號: G01C3/00
• 分類號: G01C3/00;G01C21/16
• 代理公司: 北京孚睿灣知識產權代理事務所(普通合伙) 11474
• 代理人: 劉翠芹

1.一種基于視覺慣性導航系統的激光測距儀，其特征在于：其包括用于進行空間定位和姿態檢測的視覺慣性導航系統、激光測距系統以及保護外殼，所述視覺慣性導航系統用于對測距儀進行空間定位，所述激光測距系統用于測量被測目標到測距儀的距離，之后結合激光測距系統進行測量時的空間定位信息和激光測距系統返回的測量值計算出被測目標點的世界坐標；

所述視覺慣性導航系統包括一個具有固定基線的雙目相機以及一個慣性測量單元，所述慣性測量單元位于雙目相機的中心處，所述視覺慣性導航系統上開設有四個安裝孔用于與保護外殼之間進行固定；

所述激光測距系統包括兩個一字激光和一個激光測距模塊，兩個一字激光平行放置于保護外殼的上部和其中一個側部，所述激光測距模塊固定在保護外殼上，且激光測距模塊的位置處在兩個一字激光的交線上，從而保證激光測距模塊的測定線與兩個一字激光發射的激光面的交線處于共線；

所述一字激光上設置有用于軸向固定的圓環形的槽，所述圓環形的槽與保護外殼上的環形凸起相嵌合，一字激光的兩側分別開有一個用于周向固定的長鍵；

在激光測距時，首先求得視覺慣性導航系統與激光測距模塊之間的標定，得到兩者之間相對的旋轉矩陣和偏置矩陣之后確定世界坐標系相對于地面的旋轉矩陣以及世界坐標系的位置，計算得到的兩個被測目標相對于世界坐標系的位置，從而確定兩個被測目標間的距離；

基于所述的基于視覺慣性導航系統的激光測距儀進行激光測距的方法，其包括以下步驟：

S1、對視覺慣性導航系統與激光測距模塊之間進行標定得到相對的旋轉矩陣和偏置矩陣

步驟S1中，在標定板上選取三個角點，三個角點相對于標定板左上角的位置為(c₁，d₁，0)、(c₂，d₂，0)、(c₃，d₃，0)，雙目相機經過角點檢測求得三個角點在圖像中的像素坐標，之后通過P3P算法，得到標定板相對于雙目相機的空間姿態與之后確定兩個一字激光在標定板上的投影位置，兩個投影位置的交點即為激光測距模塊的測量點，通過霍夫變換求得交點在雙目相機中的像素坐標點為[x_cv，y_cv]，基于交點與標定板共面，設交點相對于標定板左上角的位置為(c₄，d₄，0)，利用上面P3P算法得到的標定板相對于相機的空間姿態R_p與T_p，進一步得到交點相對于相機的空間坐標為結合相機模型的相機坐標系到像素平面坐標系的公式可以得到：

求解方程組可以得到：

從而得到交點相對于相機的空間位置[x_c，y_c，z_c]，之后使用測光測距儀所返回的測量值Z_vv，建立式需測量3組以上交點通過最小二乘法，求得視覺慣性導航系統與激光測距模塊之間的標定，從而得到相對的旋轉矩陣和偏置矩陣

S2、確定測距儀初始放置平面的世界坐標系相對于地面的旋轉矩陣

步驟S2中，將測距儀放于第一位置，并靜止放于工作面，以此處作為測量時的世界坐標系的位置，通過加速度計測得此時三軸的加速度為a_w＝[a_x，a_y，a_z]，以當地的重力加速度a_g＝[0，g，0]為基準，由求出世界坐標系相對于地面的旋轉矩陣

S3、確定第一個被測目標的相對于世界坐標系的位置：將測距儀由第一位置移動至第二位置，通過兩個一字激光調整測距儀姿態使兩個一字激光的交點處對準第一被測目標，測試讀出激光測距模塊返回的距離為Z_v1＝[0，0，Z₁]，通過雙目和慣性測量單元使用視覺慣性導航系統計算出測距儀在第一位置相對于第二位置時的位移和姿態從而計算出第一被測目標相對于世界坐標系的位置

S4、確定第二個被測目標的相對于世界坐標系的位置：將測距儀由第二位置移動至第三位置處，通過兩個一字激光筆調整測距儀姿態使兩個一字激光的交點處對準第二被測目標，測試讀出激光測距模塊返回的距離為Z_v2＝[0，0，Z₂]，通過雙目和慣性測量單元使用視覺慣性導航系統計算出測距儀在第三位置相對于第二位置時的位移和姿態從而計算出第二被測目標相對于世界坐標系的位置

S5、根據上述計算得到的兩個被測目標相對于世界坐標系的位置，求得在世界坐標系中第二被測目標相對于第一被測目標的空間位置B_A＝B-A，由S2得到的測距儀初始放置平面相對于地面的姿態求得在以地面為基準的情況下第二被測目標相對于第一被測目標的空間位置第二被測目標和第一被測目標之間的距離length_AB＝|B_A|＝|B_Aw|。