[发明专利]一种基于信号站的位置和姿态估计方法

权利要求书

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。