[發明專利]一種基于信號站的位置和姿態估計方法

有效

• 申請號: 201611040794.X
• 申請日: 2016-11-11
• 公開（公告）號: CN106525050B
• 公開（公告）日: 2019-04-09
• 發明（設計）人: 馬旭;趙雪嬌;王志強;李艷秋
• 申請（專利權）人: 北京理工大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01S5/02;G01S5/10
• 代理公司: 北京理工大學專利中心 11120
• 代理人: 郭德忠;李愛英

1.一種基于信號站的位置和姿態估計方法，其特征在于，具體步驟為：

步驟1、確定地固坐標系的三個相互正交的坐標軸分別為X軸、Y軸和Z軸，連體坐標系的三個相互正交的坐標軸分別為X_b軸、Y_b軸和Z_b軸；

步驟2、在空間中排布K個信號站，其中第k個信號站在地固坐標系中的坐標為(x_k,y_k,z_k),k＝1,2,...,K，且K至少為2；K個信號站分別向外發射不同頻率的信號，將第k個信號站所發出的信號稱為第k個信號；所述信號為單頻信號，分別對應一個子空間，其中子空間由對應的單頻信號構成，每一個單頻信號的子空間就是單頻信號序列形成的一個列向量；

步驟3、在待測物體上安放三條均勻直線陣ULA作為信號接收天線，其中第一條ULA包含N個天線單元，第二條ULA包含N+1個天線單元，第三條ULA包含N個天線單元；其中N至少為2；

步驟4、在待測物體開始運動之前，三條ULA實時接收所有信號站發射的信號，并對所接收的原始信號占據的子空間進行預估計，確定接收的原始信號所占據的子空間范圍；

步驟5、在待測物體開始運動之前，基于三條ULA實時接收所有信號站發射的信號，并計算每一個信號相對于待測物體的到達角(α_k,β_k)，其中α_k為ULA所接收到的第k個信號的到達仰角，β_k為ULA所接收到的第k個信號的到達方位角，(α_k,β_k)對應ULA所接收到的第k個信號；

步驟6、在待測物體開始運動之前，根據待測物體的初始位置參數、初始姿態參數依次計算K個信號站與待測物體連線相對于待測物體的到達角(α′_i,β′_i)，其中i＝1,2,...,K，(α′_i,β′_i)對應第i個信號站；在(α′_i,β′_i)和(α_k,β_k)之間進行匹配，當α_k≈α′_i且β_k≈β′_i時，在步驟4確定的子空間范圍內，確定第i個信號站與ULA所接收到的第k個信號對應于相同的子空間，從而確定ULA所接收到的第k個信號是由第i個信號站所發出的；

步驟7、在待測物體開始運動之后的時刻t_n，三條ULA實時接收所有信號站發射的信號，并按照步驟5的方法計算每一個信號相對于待測物體的到達角；并且根據步驟6中的信號與信號站之間的配對結果，將接收的各個信號相對于待測物體的到達角與其發射源信號站一一對應；

步驟8、根據待測物體開始運動前的初始位置參數和姿態參數、待測物體在上一個時刻的位置參數和姿態參數、沿X軸、Y軸和Z軸的平移速度以及繞X_b軸、Y_b軸和Z_b軸旋轉的角速度，建立EKF的狀態模型；同時采用根據步驟5的方法計算的各個信號相對于待測物體的到達角、以及與各個到達角對應的發射源信號站的空間坐標建立EKF的觀測模型；最后采用EKF技術估計待測物體當前時刻的位置參數和姿態參數。

2.如權利要求1所述的一種基于信號站的位置和姿態估計方法，其特征在于，步驟3所述的三條均勻直線陣ULA兩兩平行且平行于Y_b軸，其中X_b軸平行于第一條和第二條ULA所決定的平面，Z_b軸平行于第二條和第三條ULA所決定的平面；第一條ULA包含N個等間距的天線單元；第二條ULA包含N+1個等間距的天線單元；第三條ULA包含N個等間距的天線單元；每條ULA中，所有相鄰的兩個天線單元的間距為Δd；同時，第一條和第二條ULA相同位置天線單元之間的連線平行于X_b軸，且這兩個天線單元的間距為Δd，第二條和第三條ULA相同位置天線單元之間的連線平行于Z_b軸，且這兩個天線單元的間距為Δd。