[發明專利]一種適用于車載光電平臺的運動目標解算方法

有效

• 申請號: 202110075732.7
• 申請日: 2021-01-20
• 公開（公告）號: CN113238072B
• 公開（公告）日: 2022-11-08
• 發明（設計）人: 王曉天;彭富倫;郭城;許李偉;李宏濤;于帥;李瓊;王靜;郭玉玉;曹越;尚瓊珺;李輝;高強
• 申請（專利權）人: 西安應用光學研究所
• 主分類號: G01P3/68
• 分類號: G01P3/68;G01P13/02;G01S19/47;G01C21/16
• 代理公司: 中國兵器工業集團公司專利中心 11011
• 代理人: 劉二格

1.一種適用于車載光電平臺的運動目標解算方法，其特征在于，所述車載光電平臺包括車載偵察系統和信息處理與控制系統；車載偵察系統包括光電轉塔、北斗和慣導定位定向設備；信息處理與控制系統包括車載CAN總線和操控模塊；光電轉塔發現運動目標并進行激光測距后，手動或者自動跟蹤情況下，經光電轉塔給出目標的方位和俯仰角；北斗和慣導組合后輸出本車地理坐標，同時慣導設備測量出相對于北-東-天坐標系的車體姿態角；車載CAN總線采集和傳輸包括目標距離、目標在車體坐標系下的方位、俯仰角、車體姿態角、本車坐標的整車信息數據至操控模塊，操控模塊結合慣導和光電轉塔剛性連接的安裝誤差修正值，解算出運動目標在前后兩個不同時刻的空間坐標，根據測距時間差，解算出目標運動速度和運動方向；

所述光電轉塔通過電視或者紅外光電傳感器的偵察手段發現運動目標；

所述目標的方位和俯仰角位于平臺坐標系下，本車地理坐標位于地心坐標系下，車體姿態角為慣導坐標系下測量出的車體北向角、車體俯仰角和車體橫滾角；

所述目標解算方法由操控模塊實現，運行操控模塊后，執行以下步驟：

第一步：由光電傳感器發現運動目標，對目標進行偵察，以確定其運動速度和運動方向，在操控模塊中啟用光電運動目標偵察功能；

第二步：光電轉塔自動進入跟蹤模式，此時，觸發第一次測距，由激光測距機對目標進行測量，獲得目標距離Tg_Distance，并持續對該運動目標進行跟蹤；

第三步：由第二步激光測距操作觸發目標坐標解算；

目標坐標解算包括以下步驟：

3.1：坐標系均采用右手系，依次定義平臺坐標系、慣導坐標系、本地坐標系、地心坐標系，除地心坐標系原點位于地心外，其余坐標系原點均位于平臺中心；

3.2：經緯度及投影坐標定義

為表示地球表面附近任意一點的空間坐標，采用經緯度、高程坐標表示，其中經度以地面上的點所在的經線平面對于本初子午線平面的夾角來表示，緯度為參考橢球子午線上的該點的法線與赤道面的夾角；并采用高斯投影將橢球表面投影到平面，投影坐標以(x，y，h)格式表示；高斯投影是正形投影，根據正形投影條件，通過求解黎曼方程，利用復變函數的泰勒級數展開，得到經緯度到高斯投影坐標的級數逼近公式，展開到6次項：

式(1)中，中央子午線弧長X的計算采用二項式展開成級數形式，通過逐項積分求解，展開到8次項為：

各系數為：

3.3:姿態角定義及旋轉變換關系

姿態角采用如下的三歐拉角定義：旋轉后的坐標系I′[O′：X′，Y′，Z′]相對于坐標系I[O：X，Y，Z]的歐拉角為即，坐標系I′由坐標系I分別繞OZ軸，OY軸，OX軸分別旋轉θ、γ角得到，角度方向按照右手準則定義；

坐標系旋轉變換的步驟如下：

1)(OXYZ)繞OZ軸旋轉角度得到(Oxyz)；

2)(Oxyz)繞Oy軸旋轉θ角度得到(Ox′y′z′)；

3)(Ox′y′z′)繞Ox旋轉γ角度得到(OX′Y′Z′)；

根據矢量轉動原理，得旋轉變換關系為：

[x',y',z']^T＝A[x,y,z]^T (3)

其中

則逆變換為：

[x,y,z]^T＝A^-1[x',y',z']^T (5)

3.4：通過激光測距操作，獲得目標距離Tg_Distance，慣導姿態角、本車經緯度、平臺的方位、俯仰角均為實時輸出數據，每一次激光測距操作均得到一組測量結果，即變量數據；其中，GD_Yaw、GD_Pitch指示了目標在平臺坐標系下的方位角，Tg_Distance表示目標與平臺坐標系原點的距離，據這三個參數計算出目標在平臺坐標系下的坐標：

3.5：進行一次旋轉變換消除平臺坐標系與慣導坐標系的安裝誤差；

建立過渡坐標系x_by_bz_b，x_cy_cz_c，其坐標軸定義為：

則：

[x_c,y_c,z_c]^T＝A₁^-1[x_b,y_b,z_b]^T (8)

A₁由式(4)計算出，其中ZeroEr_Yaw為方位偏差，ZeroEr_Pitch為俯仰偏差，ZeroEr_Roll為橫滾偏差；

慣導坐標系到本地坐標系的轉換：

建立過渡坐標系x_by_bz_b，x_cy_cz_c，其坐標軸定義為：

則，[x_c,y_c,z_c]^T＝A₂^-1[x_b,y_b,z_b]^T (11)

A₂由式(4)計算出，其中DH_Yaw為慣導方位角，DH_Pitch為慣導俯仰角，DH_Roll為慣導橫滾角；

3.6：根據經緯度的定義，本地坐標系與地心坐標系方位、俯仰偏離分別為L₀，B₀，平移量即為本地坐標系原點在地心坐標系下的xyz坐標{x₀,y₀,z₀}，因此，變換關系為：

[x',y',z']^T＝A₃^-1[x,y,z]^T+[x₀,y₀,z₀]^T (13)

A₃由式(4)計算出，其中

根據參考橢球參數及經緯度定義，推導出：

其中：

地心坐標轉換到經緯度：

根據式(16)，導出地心坐標到經緯度的反算公式，其中緯度B的計算需采用遞歸算法求解，其中地心坐標系下目標坐標為(x＂，y＂)；

高斯投影坐標(x，y，h)由式(1)算出；

第四步：將第一次測距時目標距離、方位角、俯仰角、車體北向角、車體俯仰角、車體橫滾角、車輛和經解算的經度、緯度、高程目標坐標、測距時間T₁記錄至數據結構中，同時判斷穩定跟蹤持續時間是否不小于3秒，該條件滿足，則進入下一步，否則若小于3秒，返回至第二步，重新選擇目標；

第五步：進行第二次激光測距操作，觸發目標坐標解算，解算完成后，將第二次測距時目標距離、方位角、俯仰角、車體北向角、車體俯仰角、車體橫滾角、車輛和經解算的經度、緯度、高程目標坐標、測距時間T₂記錄至數據結構中；

第六步：判斷是否處于行車狀態，若為駐車狀態，執行第七步；若為行車狀態，執行第八步；

第七步：若處于駐車狀態，以北-東-天坐標系為基準計算出第二步和第五步兩次測距后目標的坐標(x₁，y₁，z₁)、(x₂，y₂，z₂)，根據以下公式(17)，計算出兩次測距后兩點之間的距離差值D_d；

根據時間差T₂-T₁，由公式(18)計算出運動目標速度V_T；

V_T＝D_d/(T₂-T₁) (18)

由公式(19)計算出基于北向夾角的運動目標方向Ψ_T，其中dy、dx分別為北-東-天坐標系下兩次測距目標坐標差值；

Ψ_T＝atan(dy/dx) (19)

第八步：若處于行車狀態，由本車和目標的經緯度坐標，計算出兩次測距后目標在地心坐標系下的坐標，坐標差值為dx，dy，dz，再由第一次測距后目標的經度L₁、緯度B₁分別作為旋轉變換的θ、γ為0，計算出旋轉矩陣A，再由(dx，dy，dz)乘以矩陣A得到(dx₂，dy₂，dz₂)，由公式(19)計算出基于北向夾角的運動目標方向Ψ_T，其中，dy＝dy₂，dx＝dx₂。