[發明專利]基于校準源的運動固態體定位方法

在審

• 申請號: 202310176232.1
• 申請日: 2023-02-28
• 公開（公告）號: CN116007633A
• 公開（公告）日: 2023-04-25
• 發明（設計）人: 郝本建;程帥;李贊;陳小軍;趙越;李思文;黃淑冰;趙曼
• 申請（專利權）人: 西安電子科技大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 陜西電子工業專利中心 61205
• 代理人: 陳宏社;王品華

1.一種基于校準源的運動固態體定位方法，其特征在于，包括如下步驟：

(1)構建運動固態體的定位場景并初始化參數：

構建包括安裝有N個傳感器的運動固態體、分布在運動固態體周圍處于靜止狀態的M個錨節點和一個校準源的運動固態體的三維定位場景；初始化所述N個傳感器在以運動固態體幾何中心為原點的局部參考系Oq₁q₂q₃下的位置坐標為c＝[c₁,c₂,…,c_n,…,c_N]^T；所述M個錨節點、校準源在全球參考系Oxyz下的位置坐標分別為a＝[a₁,a₂,…,a_m,…,a_M]^T、p＝[p_x,p_y,p_z]^T，a、p對應的位置誤差分別為Δa＝[Δa₁,Δa₂,…,Δa_m,…,Δa_M]^T、Δp＝[Δp_x,Δp_y,Δp_z]^T，Δa、Δp的標準差分別為σ_a、σ_p；所述運動固態體在全球參考系下含有誤差的方向信息、位置信息、角速度信息、速度信息分別為q＝[α,β,γ]^T、t＝[t_x,t_y,t_z]^T、w＝[w_x,w_y,w_z]^T、其中，N≥5，M≥6，c_n表示第n個傳感器在局部參考系下的位置坐標，a_m表示第m個錨節點在全球參考系下位置坐標，a_m＝[a_mx,a_my,a_mz],Δa_m表示a_m對應的位置誤差，Δa_m＝[Δa_mx,Δa_my,Δa_mz]，α、β、γ表示所述固態體在全球參考系中的偏航角、翻滾角、俯仰角，·^T表示轉置操作；

(2)構建測量向量：

通過每個傳感器在局部參考系下的位置坐標c_n、全球參考系下每個錨節點的位置坐標a_m和運動固態體含有誤差的方向信息q、位置信息t、角速度信息w和速度信息計算每個傳感器和每個錨節點之間的距離測量值r_mn及多普勒測量值得到距離測量向量r＝[r₁₁,r₂₁,…,r_mn,…,r_MN]^T和多普勒測量向量同時通過在全球坐標系下每個錨節點的位置信息a_m和校準源的位置信息p，計算每個錨節點與校準源之間的距離測量值d_m，得到距離測量向量d＝[d₁,d₂,...,d_m,...,d_M]^T，并通過r、d以及步驟(1)中校準源的位置信息p和M個錨節點的位置坐標a，構建測量向量

(3)構造估計向量并獲取估計向量的最大似然解：

(3a)通過全球參考系下固態體的方向估計量、位置估計量、角速度估計量、速度估計量q^o、t^o、w^o、以及M個錨節點位置坐標a的估計量a^o和校準源位置坐標p的估計量p^o，構造估計向量同時通過M個錨節點與N個傳感器之間距離測量向量r、多普勒測量向量的誤差協方差矩陣R_v、M個錨節點位置誤差Δa的協方差矩陣R_a、校準源位置誤差Δp的協方差矩陣R_p、M個錨節點與校準源之間的距離測量誤差協方差矩陣R_d，構建測量誤差矩陣其中，Bdiag[]表示分塊對角矩陣；

(3b)通過測量向量ζ、估計向量ψ^o及測量誤差矩陣R，構建最小二乘模型ψ：

ψ＝arg_ψmin(ζ-ζ(ψ^o))^TR^-1(ζ-ζ(ψ^o))

其中，ζ(ψ^o)表示包含估計向量ψ^o的測量向量，R^-1表示測量誤差矩陣R的逆矩陣，arg_ψmin(*)表示(*)取得最小值時ψ的值；

(3c)基于高斯-牛頓迭代法則，對最小二乘模型ψ中的測量向量ζ(ψ^o)進行k_max次更新，實現對ψ的迭代優化，得到估計向量ψ^o的最大似然解

其中，k_max≥100；

(4)獲取運動固態體的定位結果：

通過最大似然解計算全球參考系下運動固態體無誤差的方向信息位置信息角速度信息速度信息