CN106525050B

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

Info

Publication number: CN106525050B
Application number: CN201611040794.XA
Authority: CN
Inventors: 马旭; 赵雪娇; 王志强; 李艳秋
Original assignee: Beijing Institute of Technology BIT
Current assignee: Beijing Institute of Technology BIT
Priority date: 2016-11-11
Filing date: 2016-11-11
Publication date: 2019-04-09
Anticipated expiration: 2036-11-11
Also published as: CN106525050A

Abstract

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

Description

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

技术领域

本发明属于定位和导航技术领域，尤其涉及一种基于信号站的位置和姿态估计方法。

背景技术

位置和姿态估计技术在航空航天、移动式遥控装置、无人机导航或无人驾驶、室内导航等诸多领域均有应用。位置估计的主要目标是确定物体的空间坐标，而姿态估计的主要目标是确定连体坐标系与参考坐标系之间的相对转角。特别是在某些无人机和室内导航等应用场景中，需要定位导航系统具有成本低、功耗低、稳定性高、体积小等特点。另外，对于精确的实时导航系统，还需要位置和姿态估计方法具有解算精度高、计算复杂度低的特点。目前，满足上述要求的导航系统主要采用全球定位系统(global positioning system，简称GPS)、惯性导航系统(inertial navigation system，简称INS)、磁力计、地面信号站，以及视觉或光学系统进行位置和姿态估计。但是，GPS系统要求具备较高的时钟同步精度和信号采样频率，有时还会因为多径反射、无线电干扰等原因而无法使用；低成本INS系统的测量数据会因为误差积累而发生漂移；采用磁力计估计姿态必须借助于地球磁场分布数据，而且单纯使用磁力计无法获取物体的位置信息；基于信号站的位置和姿态估计精度也会受到信号能量衰减、无线电干扰等因素的影响；而视觉或光学系统通常需要配合参考地图，才能计算出物体的位置和姿态数据，并且容易受到障碍物遮挡等环境因素的影响。

相关文献(Technical Report ARL-CR-650,US Army Research Laboratory,Jun.2010)提出了一种基于地面信号站的位置和姿态估计方法，通过地面信号到待测物体的到达角，以及上一测量时刻的物体姿态信息，依次计算出物体在地固坐标系内的坐标和距离地面的高度，之后再根据物体的位置坐标，借助磁力计，计算出物体的姿态角。如此循环交替更新物体的位置和姿态参数，从而实现位置和姿态的实时估计。但是以上方法具有以下不足：第一、上述方法没有在位置和姿态参数的解算过程中采用有效的滤波手段，因此估计误差较大；第二、上述方法借助磁力计的测量数据估计姿态参数，因此需要地球磁场分布的先验数据；第三、该方法采用高采样率的原始地面信号计算其相对于待测物体的到达角，参与运算的数据量较大、计算复杂度较高，影响位置和姿态估计的实时性。针对上述位置和姿态估计的技术需求，以及现有方法的局限性，有必要将多种位置和姿态估计方法与滤波技术和信号处理技术相结合，提高现有方法的参数估计精度和运算效率。

发明内容

本发明的目的是提供一种基于信号站的位置和姿态估计方法。该方法采用压缩感知(compressive sensing，简称CS)技术降低接收信号站信号(下文简称“信号”)时的采样率，压缩信号维度，降低接收器成本，提高位置和姿态估计方法的运算效率。另外，该方法还采用扩展卡尔曼滤波(extended Kalman filter，简称EKF)技术提高了位置和姿态的估计精度。

实现本发明的技术方案如下：

一种基于信号站的位置和姿态估计方法，具体步骤为：

步骤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技术估计待测物体当前时刻的位置参数和姿态参数。

步骤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。

步骤4所述的对所接收的原始信号占据的子空间进行预估计具体步骤为：

步骤41、各个天线单元在从t₀时刻开始的L个等间隔时刻点接收到的原始信号为采用CS技术，使原始信号与一个随机信号进行卷积，并对卷积的结果进行等间距采样，对原始信号进行压缩得到低维度的压缩测量信号其中Φ_r为压缩投影矩阵，其中为离散傅里叶变换DFT基矩阵，令为第l个基函数，l∈[1,L]，为对应的DFT系数向量，为噪声向量，令为压缩基矩阵，则为第l个压缩基函数；

步骤42、将残差向量初始化为将已选压缩基函数序号向量初始化为其中下标0表示初始化状态，表示空集，将原始信号对应的DFT系数向量初始化为将迭代次数初始化为n＝1；

步骤43、假设本次为第n次迭代，将各压缩基函数与残差向量作内积，并得到使内积绝对值最大的压缩基函数的对应序号l＝ind_n；

步骤44、根据序号ind_n，选出其对应的压缩基函数并将压缩基函数与第n-1次迭代中已选压缩基函数的正交分量进行正交化，得到压缩基函数的正交分量

步骤45、令为系数向量中的第ind_n个元素，则将更新为：

其中为第n-1次迭代中的残差向量，并将第n次迭代中的残差向量更新为：

步骤46、如果残差向量的模的平方大于预设的误差阈值ε或迭代次数小于预设的最大迭代次数loop_m，则返回步骤43，否则从当前更新后的系数向量中选出前K个最大的系数，及其对应的K个基函数并进入步骤48；

步骤48、对所有3N+1个天线单元均完成步骤41到步骤47的操作后，对所有天线单元对应的全部被选基函数进行统计，确定被选中次数最多的K个基函数得到原始信号对应的子空间矩阵其中为的每一列均代表一个子空间。

步骤5所述的计算每一个信号相对于待测物体的到达角(α_k,β_k)具体步骤为：

步骤51、根据各个天线单元所接收到的压缩测量信号分别估计各个信号子空间所对应的DFT系数向量

步骤52、根据系数向量将各个天线单元的压缩测量信号所对应的原始信号分解为K个单频信号的线性组合：

其中称为的第k项分解信号值，为的第k列，对应于第k个信号站发出的信号，为的第k个元素；

步骤53、设当前所在时刻为t_n，根据步骤52的信号分解结果，将第一条ULA中所有N个天线单元接收的原始信号的第k项分解信号值组合为一个信号向量将第二条ULA中的第1个到第N个天线单元接收的原始信号的第k项分解信号值组合为一个信号向量将第二条ULA中的第2个到第N+1个天线单元接收的原始信号的第k项分解信号值组合为一个信号向量将第三条ULA中所有N个天线单元接收的原始信号的第k项分解信号值组合为一个信号向量其中第k项分解信号值对应来自特定信号站的信号；

步骤54、将四个向量和组合成一个向量

步骤55、L个等间隔时刻点后，得到L个向量将这L个向量组成一个矩阵其中Δt为时刻间隔；将矩阵F分解为其中矩阵F₁和矩阵F₂的尺寸分别为K×L和(4N-K)×L；

步骤56、计算传播估计矩阵其中上角标H表示厄米算子，将传播估计矩阵分解为其中矩阵和的尺寸均为(N-K)×K，矩阵和的尺寸均为K×K；

步骤57、根据所有天线单元所接收到的原始信号的第k项分解信号值，依次计算对应于该频率的信号站信号的到达方位角β_k和到达仰角α_k：

其中上角标#表示伪逆运算符，eigen_m(·)表示求最大的特征值，arg[·]表示求复角，且到达角(α_k,β_k)与子空间矩阵中的各列一一对应。

步骤6所述的确定ULA所接收到的第k个信号是由第i个信号站所发出的具体步骤为：

步骤61、计算向量其中上标0表示初始化状态：

其中(x_o[0],y_o[0],z_o[0])为待测物体的初始位置参数，(θ[0],ψ[0],φ[0])为初始姿态参数，θ[0]为X_b轴与X-Y平面之间的夹角，ψ[0]为X轴与X_b-Z平面之间的夹角，φ[0]为待测物体绕X_b轴自旋的角度，且有

A₁₁＝cosψ[0]cosθ[0]，A₁₂＝sinψ[0]cosθ[0]，A₁₃＝-sinθ[0]，

A₂₁＝cosψ[0]sinθ[0]sinφ[0]-sinψ[0]cosφ[0]，A₂₂＝sinψ[0]sinθ[0]sinφ[0]+cosψ[0]cosφ[0]，

A₂₃＝cosθ[0]sinφ[0]，A₃₁＝cosψ[0]sinθ[0]cosφ[0]+sinψ[0]sinφ[0],

A₃₂＝sinψ[0]sinθ[0]cosφ[0]-cosψ[0]sinφ[0],A₃₃＝cosθ[0]cosφ[0]，

步骤62、根据待测物体的初始位置参数和初始姿态参数依次计算K个信号站所发出信号的到达角(α′_i,β′_i)，其中(α′_i,β′_i)对应第i个信号站：

步骤63、根据步骤5计算得到的第1个到第K个信号站所发出信号的到达角(α_k,β_k),k＝1,2,…,K，在(α_k,β_k)和(α′_i,β′_i)之间进行匹配，当α_k≈α′_i且β_k≈β′_i时，则子空间矩阵中的第k列和ULA所接收到的第k个信号对应于第i个信号站，完成ULA接收信号与信号站之间的配对。

有益效果

1、本发明中不同信号站所发出的信号使用不同的工作频率，之后再基于信号子空间估计和匹配方法，将来自不同方向的信号进行分离，从而将到达角估计问题分解为若干子问题，而每一个子问题的解就是某一个特定信号的达到角。因此，本发明的方法可以通过逐一求解不同信号的到达角，去除不同信号之间的耦合及相互干扰，降低不同信号之间的相互影响，获得较高的到达角估计精度，从而进一步提高位置和姿态参数的估计精度；

2、本发明所涉及的方法使用ULA作为接收信号的天线，ULA具有结构简单、紧凑，造价低廉的优点；

3、本发明采用CS技术接收信号，并计算每一个信号相对于待测物体的到达角，可以有效降低接收信号时的采样率，降低接收器成本，并压缩参与后续运算的数据维度，降低算法的数据处理量和计算复杂度，提高位置和姿态估计方法的运算效率；

4、本发明采用EKF技术，可有效提高位置和姿态参数的估计精度。

附图说明

图1为本发明基于信号站的位置和姿态估计方法流程图；

图2为本发明地固坐标系、连体坐标系和各姿态角的示意图；

图3为本发明信号站和待测物体的示意图；

图4为本发明ULA和信号到达角的示意图；

图5为基于本发明方法所得到的位置和姿态参数估计离差曲线；

图6为基于传统方法结合降采样方法得到的位置和姿态参数估计离差曲线；

图7为基于传统方法，使用未经压缩的原始信号得到的位置和姿态参数估计离差曲线。

具体实施方式

下面结合附图进一步对本发明进行详细说明。

本发明的原理：位置和姿态估计的主要目的就是要确定物体的位置和姿态参数。物体的位置可由该物体重心在地固坐标系中的坐标(x_o,y_o,z_o)表示，而物体的姿态可由连体坐标系与地固坐标系之间的相对转角(θ,ψ,φ)表示。本发明涉及的方法首先在空间中安放若干信号站，并在待测物体上安放ULA作为接收信号的天线，采用CS技术实时接收信号并计算各个信号相对于待测物体的达到角度。在物体开始运动之前，首先对信号子空间进行预估计，并对信号子空间和所有信号进行配对，确定每一个信号所占据的子空间。在物体开始运动之后，利用实时计算得到的信号到达角信息，采用EKF技术实时估计物体的位置和姿态参数。

本发明采用CS技术接收信号，并计算信号到达角，可以有效降低接收信号时的采样率和接收器成本，并降低算法的数据处理量和计算复杂度，提高位置和姿态估计方法的运算效率。同时，本发明采用EKF技术，可有效提高位置和姿态参数的估计精度。最后，本发明所涉及的方法使用ULA作为接收信号的天线，ULA具有结构简单、紧凑，造价低廉的优点。

如图1所示，本发明基于信号站的位置和姿态估计方法，具体步骤为：

步骤1、如图2所示，确定地固坐标系的三个相互正交的坐标轴分别为X轴、Y轴和Z轴，连体坐标系的三个相互正交的坐标轴分别为X_b轴、Y_b轴和Z_b轴；

步骤2、如图3所示，在空间中排布K个信号站，K个信号站在地固坐标系中的坐标分别为：(x₁,y₁,z₁)，(x₂,y₂,z₂)，…，(x_k,y_k,z_k)，…(x_K,y_K,z_K)，k＝1,2,...,K，K个信号站分别向外发射不同频率的信号，所述信号为单频信号，且其中最高频率为f_m；

步骤3、如图4所示，在待测物体上安放三条两两平行且平行于Y_b轴的均匀直线阵uniform linear array，即ULA作为信号接收天线，其中X_b轴分别垂直于第一条和第二条ULA，且平行于第一条和第二条ULA所决定的平面，Z_b轴分别垂直于第二条和第三条ULA，且平行于第二条和第三条ULA所决定的平面。第一条ULA包含N个等间距的天线单元，分别记为a₁，…，a_j，…a_N，其中j＝1,2,...N；第二条ULA包含N+1个等间距的天线单元，分别记为a_N+1,a_N+2,...,a_N+j,...,a_2N+1，第三条ULA包含N个等间距的天线单元，分别记为a_2N+2,a_2N+3,...,a_2N+j,...,a_3N+1，在每一条ULA中，所有相邻的两个天线单元的间距为Δd＝v_c/(2f_m)，其中v_c为光速。当j＝1,2,...N时，a_j和a_j+N之间的连线平行于X_b轴，且间距为Δd，a_j+2N+1和a_j+N之间的连线平行于Z_b轴，且间距为Δd；

步骤4、在待测物体开始运动之前，三条ULA实时接收所有信号站发射的信号，并对所接收的原始信号所占据的子空间进行预估计，确定接收的原始信号所占据的子空间范围；

所述子空间由对应的单频信号构成，每一个单频信号的子空间就是该单频信号序列形成的一个列向量。

本发明步骤4中所述的对所接收的原始信号所占据的子空间进行预估计的具体步骤为：

步骤41、如步骤3所述，三条ULA中共包含3N+1个天线单元，设当前所考虑的天线单元序号为w，其中w＝1,2,...,3N+1，并将其初始化为w＝1；令ε和loop_m分别为预先设定的残差向量的误差阈值和最大迭代次数；

步骤42、令第w个天线单元在从t₀时刻开始的L个等间隔时刻点(t_n,t_n+Δt,...,t_n+(L-1)Δt)上接收到的原始信号为其中Δt为时刻间隔，代表L×1的复数空间，可表示为

其中为离散傅里叶变换(discrete Fourier transform，简称DFT)基矩阵，令为第l个基函数，l∈[1,L]，为对应的DFT系数向量，为噪声向量。采用CS技术，使原始信号与一个随机信号进行卷积，并对卷积结果进行等间距采样，共采样M次，其中K＜＜M＜＜L，得到采样后的信号记为即：

其中称为压缩测量信号，Φ_r为压缩投影矩阵，令为压缩基矩阵，则为第l个压缩基函数；

步骤43、将残差向量初始化为将已选压缩基函数序号向量初始化为其中下标0表示初始化状态，表示空集，将原始信号对应的DFT系数向量初始化为将迭代次数初始化为n＝1，其中n∈[1,loop_m]；

步骤44、在第n次迭代中，选定与残差向量内积绝对值最大的压缩基函数，并得到该压缩基函数的对应序号：

其中为第n-1次迭代中的残差向量，之后将上述压缩基函数的序号加入到第n-1次迭代中的已选压缩基函数序号向量中，即：

其中和分别为第n-1次和第n次迭代中的已选压缩基函数序号向量；

步骤45、根据步骤44中的压缩基函数序号ind_n，选出其对应的压缩基函数并将压缩基函数进行正交化：

其中为上一迭代周期中已选压缩基函数的正交分量，为已选压缩基函数的正交分量；

步骤46、令为系数向量中的第ind_n个元素，则将更新为：

将第n次迭代中的残差向量更新为：

步骤47、令ε和loop_m分别为预先设定的误差阈值和最大迭代次数，若或迭代次数n＜loop_m，则令n＝n+1并返回步骤44，否则从当前更新后的系数向量中选出前K个最大的系数，及其对应的K个基函数并进入步骤48；

步骤48、若尚未遍历所有3N+1个天线单元，则设w＝w+1，并返回步骤42，否则对所有3N+1个天线单元对应的全部被选基函数进行统计，挑选出被选中次数最多的K个基函数组成原始信号对应的子空间矩阵其中的每一列均代表一个子空间。

步骤5、在待测物体开始运动之前，三条ULA实时接收所有信号站发射的信号，并计算每一个信号相对于待测物体的到达角，将第k个信号站所发出的信号称为第k个信号，并将第k个信号的到达角记为(α_k,β_k)，其中α_k为到达仰角，β_k为到达方位角。

本发明步骤5中所述的计算每一个信号相对于待测物体的到达角的具体过程为：

步骤501、如步骤3所述，三条ULA中共包含3N+1个天线单元，设当前所考虑的天线单元序号为w，其中w＝1,2,...,3N+1，并将其初始化为w＝1；

步骤502、利用第w个天线单元所接收到的压缩测量信号估计信号子空间所对应的DFT系数向量

其中C_nr是压缩测量信号中所包含的噪声向量的协方差矩阵；

步骤503、根据步骤502计算得出的系数向量将压缩测量信号所对应的原始信号分解为K个单频信号的线性组合：

其中称为的第k项分解信号，为的第k列，对应于第k个信号站发出的信号，为的第k个元素；

步骤504、若尚未遍历所有3N+1个天线单元，则设w＝w+1，并返回步骤502，否则将进入步骤505；

步骤505、设当前所考虑的是第k个信号，并设k＝1；

步骤506、设当前所在时刻为t，并设t＝t_n；

步骤507、在时刻t，根据步骤503的信号分解结果，将第一条ULA中所有N个天线单元接收的原始信号的第k项分解信号值分别记为并将上述N个信号值组合为一个信号向量将第二条ULA中的第1个到第N个天线单元接收的原始信号的第k项分解信号值分别记为并将上述N个信号值组合为一个信号向量将第二条ULA中的第2个到第N+1个天线单元接收的原始信号的第k项分解信号值分别记为并将上述N个信号值组合为一个信号向量将第三条ULA中所有N个天线单元接收的原始信号的第k项分解信号值分别记为并将上述N个信号值组合为一个信号向量

步骤508、将步骤507计算得到的四个向量和组合成一个向量

步骤509、若当前所在的时刻t＜t_n+(L-1)Δt，则设t＝t+Δt，并返回步骤507，否则进入步骤510；

步骤510、将步骤506至步骤509计算得到的与L个等间隔时刻点对应的L个向量组成一个矩阵该矩阵的尺寸为4N×L，将矩阵F分解为其中矩阵F₁和矩阵F₂的尺寸分别为K×L和(4N-K)×L；

步骤511、计算传播估计矩阵该矩阵的尺寸为K×(4N-K)，其中上角标H表示厄米算子(Hermitian operator)，将传播估计矩阵分解为其中矩阵和的尺寸均为(N-K)×K，矩阵和的尺寸均为K×K；

步骤512、依次计算第k个信号的到达方位角β_k和到达仰角α_k：

其中上角标#表示伪逆运算符，eigen_m(·)表示求最大的特征值，arg[·]表示求复角；

在步骤513、若k<K,则设k＝k+1，并返回步骤506，否则终止算法，并输出所有信号的到达角(α_k,β_k)，且到达角(α_k,β_k)与子空间矩阵中的各列一一对应。

步骤6、在待测物体开始运动之前，根据待测物体的初始位置参数、初始姿态参数依次计算K个信号站与待测物体连线相对于待测物体的到达角(α′_i,β′_i)，其中(α′_i,β′_i)对应第i个信号站；在(α′_i,β′_i)和(α_k,β_k)之间进行匹配，当α_k≈α′_i且β_k≈β′_i时，在步骤4确定的子空间范围内，确定第i个信号站与ULA所接收到的第k个信号对应于相同的子空间，进而确定ULA所接收到的第k个信号是由第i个信号站所发出的；

本发明步骤6中所述的确定ULA所接收到的第k个信号是由第i个信号站所发出的具体过程为：

步骤61、在待测物体开始运动之前，令待测物体的初始位置参数为(x_o[0],y_o[0],z_o[0])、初始姿态参数为(θ[0],ψ[0],φ[0])，设当前所考虑的信号站序号为k，并令k＝1；

步骤62、计算向量其中上标0表示初始化状态：

其中

A₂₃＝cosθ[0]sinφ[0]，A₃₁＝cosψ[0]sinθ[0]cosφ[0]+sinψ[0]sinφ[0]，

A₃₂＝sinψ[0]sinθ[0]cosφ[0]-cosψ[0]sinφ[0]，A₃₃＝cosθ[0]cosφ[0]，

步骤63、根据步骤402计算得出的计算第k个信号站所发出信号的到达角(α′_i,β′_i)：

步骤64、若当前所考虑的信号站序号为k<K，则设k＝k+1，并返回步骤62，否则进入步骤65；

步骤65、根据步骤501至步骤513的计算结果，将第1个到第K个信号站所发出信号的到达角分别记为：(α_k,β_k),k＝1,2,…,K，其中(α_k,β_k)对应子空间矩阵中的第k列根据步骤61至步骤64的计算结果，将第1个到第K个信号站所发出信号的到达角分别记为：(α′_i,β′_i),i＝1,2,…,K,其中(α′_i,β′_i)对应第i个信号站。在(α_k,β_k)和(α′_i,β′_i)之间进行匹配，当α_k≈α′_i且β_k≈β′_i时，确定子空间矩阵中的第k列和ULA所接收到的第k个信号对应于第i个信号站，即第i个信号站所发出信号占据的子空间为完成ULA接收信号与信号站之间的配对。

步骤7、在待测物体开始运动之后的时刻t_n，三条ULA实时接收原始信号，并按照步骤5的方法计算每一个信号相对于待测物体的到达角；并且根据步骤6的信号与信号站的配对结果，将接收的各个信号相对于待测物体的到达角与其发射源信号站一一对应；

步骤8、根据待测物体开始运动前的初始位置参数和姿态参数、待测物体在上一个时刻的位置参数和姿态参数、沿X轴、Y轴和Z轴的平移速度以及绕X_b轴、Y_b轴和Z_b轴旋转的角速度，建立EKF的状态模型；同时根据各个原始信号相对于待测物体的到达角、所有信号站的空间坐标建立EKF的观测模型；最后采用EKF技术估计待测物体当前时刻的位置参数和姿态参数。

待测物体当前的位置参数指待测物体质心在地固坐标系中的坐标(x_o[n],y_o[n],z_o[n])，待测物体当前的姿态参数指连体坐标系与地固坐标系之间的相对转角(θ[n],ψ[n],φ[n])，其中θ[n]为俯仰角，即X_b轴与X-Y平面之间的夹角，ψ[n]为偏航角，即X轴与X_b-Z平面之间的夹角，φ[n]为滚转角，即待测物体绕X_b轴自旋的角度。

本发明所述步骤8中采用EKF技术估计待测物体当前时刻的位置和姿态参数的具体过程为：

步骤81、建立EKF的状态模型：

其中(x_o[n],y_o[n],z_o[n])和(θ[n],ψ[n],φ[n])分别为当前估计时刻待测物体的位置参数和姿态参数，(x_o[n-1],y_o[n-1],z_o[n-1])和(θ[n-1],ψ[n-1],φ[n-1])分别为上一次估计时刻待测物体的位置参数和姿态参数，当n＝1时，(x_o[n-1],y_o[n-1],z_o[n-1])为待测物体开始运动前的初始位置参数，(θ[n-1],ψ[n-1],φ[n-1])为待测物体开始运动前的初始姿态参数，(u[n-1],v[n-1],w[n-1])为上一次估计时刻物体沿X轴、Y轴和Z轴的平移速度，(p[n-1],q[n-1],r[n-1])为上一次估计时刻物体绕X_b轴、Y_b轴和Z_b轴旋转的角速度，Δn是两个相邻估计时刻之间的时间差。矩阵T_eb和矩阵T_ang分别表示为：

其中

A₁₁＝cosψ[n-1]cosθ[n-1]，A₁₂＝sinψ[n-1]cosθ[n-1]，A₁₃＝-sinθ[n-1]，

A₂₁＝cosψ[n-1]sinθ[n-1]sinφ[n-1]-sinψ[n-1]cosφ[n-1]，

A₂₂＝sinψ[n-1]sinθ[n-1]sinφ[n-1]+cosψ[n-1]cosφ[n-1]，

A₂₃＝cosθ[n-1]sinφ[n-1]，A₃₁＝cosψ[n-1]sinθ[n-1]cosφ[n-1]+sinψ[n-1]sinφ[n-1]，

A₃₂＝sinψ[n-1]sinθ[n-1]cosφ[n-1]-cosψ[n-1]sinφ[n-1]，A₃₃＝cosθ[n-1]cosφ[n-1]，

B₁₁＝1，B₁₂＝sinφ[n-1]tanθ[n-1]，B₁₃＝cosφ[n-1]tanθ[n-1]，B₂₁＝0，B₂₂＝cosφ[n-1]，

B₂₃＝-sinφ[n-1]，B₃₁＝0，B₃₂＝sinφ[n-1]/cosθ[n-1]，B₃₃＝cosφ[n-1]/cosθ[n-1]，

另外，

(n_u[n],n_v[n],n_w[n])为服从均值为0、协方差矩阵为C_xyz的联合高斯分布的噪声向量，(n_p[n],n_q[n],n_r[n])为服从均值为0、协方差矩阵为C_φθψ的联合高斯分布的噪声向量，

其中为噪声的方差值；

步骤82、建立EKF的观测模型：

其中对于任意的变量k，k＝1,2,...,K，有[n_r11,n_r12,n_r13,…n_rk1,n_rk2,n_rk3,…n_rK1,n_rK2,n_rK3]^T是服从均值为0，协方差矩阵为C_ob的联合高斯分布的噪声向量，同时该噪声向量表示向量[sinα₁cosβ₁,sinα₁sinβ₁,cosα₁,…,sinα_Kcosβ_K,sinα_Ksinβ_K,cosα_K]^T的计算结果与其真值相比的误差；其中所述C_ob为所述误差的协方差矩阵；

步骤83、采用步骤81中的状态模型，根据上一次的位置参数和姿态参数估计结果，即(x_o[n-1],y_o[n-1],z_o[n-1])和(θ[n-1],ψ[n-1],φ[n-1])，预测当前的位置参数和姿态参数(x_o[n|n-1],y_o[n|n-1],z_o[n|n-1])和(θ[n|n-1],ψ[n|n-1],φ[n|n-1])：

步骤84、计算EKF状态模型的协方差矩阵并更新预测均方差矩阵M[n|n-1]＝M[n-1]+C_e，其中M[n|n-1]和M[n-1]分别为当前估计时刻和上一次估计时刻的预测均方差矩阵，且当n＝1时，M[n-1]为全零矩阵；

步骤85、更新增益矩阵K[n]＝M[n|n-1]J^T(C_ob+JM[n|n-1]J^T)^-1,其中J为观测向量[sinα₁cosβ₁,sinα₁sinβ₁,cosα₁,…,sinα_Kcosβ_K,sinα_Ksinβ_K,cosα_K]^T对于状态参数向量(x_o[n],y_o[n],z_o[n],φ[n],θ[n],ψ[n])的雅克比矩阵；

步骤86、采用当前的预测的位置参数和姿态参数，得到当前观测向量的预测值

其中T_eb、norm₁、norm₂、…、norm_K均采用预测的状态参数(x_o[n|n-1],y_o[n|n-1],z_o[n|n-1],φ[n|n-1],θ[n|n-1],ψ[n|n-1])计算得出；

步骤87、更新当前的状态参数向量(x_o[n],y_o[n],z_o[n],θ[n],ψ[n],φ[n])^T：

其中[sinα₁cosβ₁,sinα₁sinβ₁,cosα₁,…,sinα_Kcosβ_K,sinα_Ksinβ_K,cosα_K]^T为当前时刻观测向量的实际值；

步骤88、更新当前的预测均方差矩阵M[n]＝(I-K[n]J)M[n|n-1]，用于下一个迭代周期中增益矩阵的更新，其中I为单位矩阵。

步骤9、在时刻t_n+1，如果还需要继续估计物体的位置和姿态参数，则令t_n＝t_n+1并返回步骤7，否则停止位置和姿态估计过程。

本发明的实施实例：

如图5所示为采用本发明方法得到的位置和姿态参数估计离差随时间变化的曲线，接收信号时的采样率为9.1×10⁶Hz。其中估计离差指参数估计值与参数真值之差。图5(a)中x_o估计值的均方根误差(mean square error，简称MSE)为3.1×10^-2(m²),图5(b)中y_o估计值的MSE为3.4×10^-1(m²),图5(c)中|z_o|估计值的MSE为1.8×10^-1(m²),图5(d)中φ估计值的MSE为7.6×10^-6(rad²),图5(e)中θ估计值的MSE为3.6×10^-6(rad²),图5(f)中ψ估计值的MSE为2.8×10^-6(rad²)。

如图6所示为采用传统方法结合降采样方法得到的位置和姿态参数估计离差随时间变化的曲线。为了与图5中的结果进行比较，图6仿真中对未经压缩的原始信号进行降采样，使得信号采样率与图5仿真一样，也为9.1×10⁶Hz。图6(a)中x_o估计值的MSE为4.0×10⁷(m²),图6(b)中y_o估计值的MSE为5.2×10⁶(m²),图6(c)中|z_o|估计值的MSE为3.9×10⁴(m²),图6(d)中φ估计值的MSE为2.1×10⁴(rad²),图6(e)中θ估计值的MSE为4.8×10(rad²),图6(f)中ψ估计值的MSE为7.7×10³(rad²)。对比图5和图6可知，在采样率相同的情况下，采用本发明的方法可以有效提高位置和姿态参数估计的精度。

图7为采用传统方法，使用未经压缩的原始信号得到的位置和姿态参数估计离差随时间变化的曲线。图7仿真中直接利用未经压缩的原始信号进行位置和姿态参数估计，此时信号采样率为3.0×10⁸Hz。图7(a)中x_o估计值的MSE为1.9×10^-3(m²),图7(b)中y_o估计值的MSE为3.4×10^-3(m²),图7(c)中|z_o|估计值的MSE为4.1×10^-3(m²),图7(d)中φ估计值的MSE为1.3×10^-7(rad²),图7(e)中θ估计值的MSE为1.3×10^-7(rad²),图7(f)中ψ估计值的MSE为1.8×10^-7(rad²)。对比图5和图7可知，在不对信号进行压缩的前提下，直接使用高采样率的原始信号进行计算，能够提高位置和姿态参数的估计精度，但是这种方法需要更高的信号采样率，会增加信号接收器的成本，同时也极大地增加了算法的计算复杂度，降低了位置和姿态估计算法的计算效率，从而影响位置和姿态估计的实时性。

虽然结合了附图描述了本发明的具体实施方式，但是对于本领域技术人员来说，在不脱离本发明原理的前提下，还可以做出若干变形、替换和改进，这些也应视为属于本发明的保护范围。

Claims

1.一种基于信号站的位置和姿态估计方法，其特征在于，具体步骤为：

步骤2、在空间中排布K个信号站，其中第k个信号站在地固坐标系中的坐标为(x_k,y_k,z_k),k＝1,2,...,K，且K至少为2；K个信号站分别向外发射不同频率的信号，将第k个信号站所发出的信号称为第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个信号站所发出的；

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。

3.如权利要求1所述的一种基于信号站的位置和姿态估计方法，其特征在于，步骤4所述的对所接收的原始信号占据的子空间进行预估计具体步骤为：

步骤45、令为系数向量中的第ind_n个元素，则将更新为：

步骤48、对所有3N+1个天线单元均完成步骤41到步骤47的操作后，对所有天线单元对应的全部被选基函数进行统计，确定被选中次数最多的K个基函数得到原始信号对应的子空间矩阵其中：的每一列均代表一个子空间。

4.如权利要求3所述的一种基于信号站的位置和姿态估计方法，其特征在于，步骤5所述的计算每一个信号相对于待测物体的到达角(α_k,β_k)具体步骤为：

步骤54、将四个向量和组合成一个向量

5.如权利要求4所述的一种基于信号站的位置和姿态估计方法，其特征在于，步骤6所述的确定ULA所接收到的第k个信号是由第i个信号站所发出的具体步骤为：

步骤61、计算向量其中上标0表示初始化状态：

其中(x_o[0],y_o[0],z_o[0])为待测物体的初始位置参数，(x_i,y_i,z_i)为第i个信号站在地固坐标系中的坐标，(θ[0],ψ[0],φ[0])为初始姿态参数，θ[0]为X_b轴与X-Y平面之间的夹角，ψ[0]为X轴与X_b-Z平面之间的夹角，φ[0]为待测物体绕X_b轴自旋的角度，且有