一种基于信号站的位置和姿态估计方法

摘要

本发明提供一种基于信号站的位置和姿态估计方法。本方法采用待测物体上的均匀直线阵和压缩感知技术，实时接收信号站信号并计算所有信号对于待测物体的达到角度。根据各个信号站的位置和所有信号的到达角度，采用扩展卡尔曼滤波技术对物体的位置和姿态参数进行实时估计。本发明的方法可以通过逐一求解不同信号的到达角，去除不同信号之间的耦合及相互干扰，降低不同信号之间的相互影响，获得较高的到达角估计精度；此外本方法采用压缩感知技术能够有效降低接收信号时的采样率和信号接收器的成本，降低位置和姿态估计算法的计算量，提高运算效率；同时，本发明采用扩展卡尔曼滤波技术，可进一步有效提高位置和姿态参数的估计精度。

主权项

一种基于信号站的位置和姿态估计方法，其特征在于，具体步骤为：步骤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技术估计待测物体当前时刻的位置参数和姿态参数。