利用信号接收器计算姿态角的方法及设备与流程

本发明涉及导航技术领域，尤其涉及一种利用信号接收器计算姿态角的方法及设备。

背景技术：

随着科技进步，越来越多的设备具备导航定位功能，其中，在提供位置服务时，确定设备的空间姿态在例如无人驾驶车辆、无人机等设备中是至关重要的安全保障。因此获取精确的姿态角数据非常关键。

目前在基于位置服务的应用导航系统中，计算姿态角的方法一般都是通过惯性测量组合系统测量后计算获得，该惯性测量组合系统主要由加速度传感器，陀螺仪，磁传感器组合而成，其中加速度传感、陀螺仪可以有效计算设备的翻转角和俯仰角，而磁传感器可用来计算设备的偏航角，通过获得翻转角、俯仰角以及偏航角这三个数据就可以确定设备的空间姿态。但采用惯性测量组合系统测量与计算姿态角时，会因器件受到各种各样的影响导致计算误差较大。这些影响包括：加速度传感器的制作工艺水平，设备振动，周围环境对磁传感器的电磁干扰，以及陀螺仪的误差随着时间逐渐积累等。因此，采用惯性测量组合系统测量后计算的方法在干扰严重的环境和特定的条件下会导致设备无法正常工作或产生无法容忍的误差。还有就是一旦惯性测量组合系统发生系统失灵、仪器故障或误差不可容忍等情况时就会导致基于位置服务的应用出现严重的偏差。

有鉴于此，有必要提供一种能准确、稳定地计算姿态角的方法及设备，以避免出现现有技术中采用惯性测量组合系统发生系统失灵、仪器故障或误差不可容忍等情况时所导致基于位置服务的应用出现严偏差的问题。

技术实现要素：

本发明的目的在于提供一种利用信号接收器计算姿态角的方法及设备，用以解决现有技术中采用惯性测量组合系统发生系统失灵、仪器故障或误差不可容忍等情况时所导致基于位置服务的应用出现严重偏差的技术问题。

本发明提供一种利用信号接收器计算姿态角的方法，包括以下步骤：

s10利用至少4个所述信号接收器建立待定位目标的姿态角模型；

s20建立各所述信号接收器与预定数量信号发射器之间的第一双差分模型，计算任意时刻所述待定位目标的姿态角；

s30建立各所述信号接收器在连续时间点下的第二双差分模型，计算所述待定位目标位移变化时所述姿态角的相对变化量；

s40获取所述待定位目标满足预设条件的姿态角。

优选地，所述方法在步骤s10之前还包括：

s50控制一惯性测量组合系统测量所述待定位目标的常规姿态角；

s60若相邻两个时刻的常规姿态角之间的变化量超过预设阈值时，则进入所述步骤s10。

优选地，所述信号接收器为gps接收器、wi－fi信号接收器、蓝牙信号接收器、rfid信号接收器、声学传感器或者光学传感器。

优选地，当所述信号接收器为gps接收器时，所述信号发射器为卫星，步骤s20进一步包括以下步骤：

s21计算所述gps接收器与所述卫星之间的真实伪距以及载波相位的观测量；

s22依据所述载波相位的观测量建立各gps接收器之间的差分模型；

s23依据所述差分模型，建立各gps接收器分别与预定数量的所述卫星之间的双差分模型；

s24建立任意时刻所述待定位目标的姿态角模型；

s25计算所述待定位目标在任意时刻的姿态角。

优选地，步骤s30进一步包括：

s31依据各所述gps接收器之间的差分模型，建立各gps接收器之间在t1时刻的第一差分模型和在t2时刻的第二差分模型；

s32建立各gps接收器在所述t2时刻与所述t1时刻之间的双差分模型；

s33建立各gps接收器从所述t1时刻到所述t2时刻的姿态角相对变化量模型；

s34依据所述姿态角相对量变化模型，计算所述姿态角从所述t1时刻到所述t2时刻的相对变化值。

优选地，步骤s40利用可扩展性卡尔曼滤波获取所述待定位目标满足预设条件的姿态角，具体包括以下步骤：

s41依据所述相对变化值，基于所述姿态角相对变化量模型和各gps接收器分别与预定数量的所述卫星之间的双差分模型建立线性方程；

s42简化各gps接收器分别与预定数量的所述卫星之间的双差分模型，生成简化方程；

s43基于所述姿态角相对变化量模型及当前时刻的所述简化方程预测下一时刻的新简化方程，通过可扩展性卡尔曼滤波的增益权重，获取下一时刻的相对变化值及相应的四元数；

s44将所述四元数进行欧拉角转换，获取所述待定位目标的姿态角。

本发明还提供一种利用信号接收器计算姿态角的设备，所述设备包括：

姿态角模型建立模块，用于利用至少4个所述信号接收器建立待定位目标的姿态角模型；

姿态角计算模块，用于建立各所述信号接收器与预定数量信号发射器之间的第一双差分模型，计算任意时刻所述待定位目标的姿态角；

姿态角相对变化量计算模块，用于建立各所述信号接收器在连续时间点下的第二双差分模型，计算所述待定位目标位移变化时所述姿态角的相对变化量；

姿态角获取模块，用于获取所述待定位目标满足预设条件的姿态角。

优选地，所述姿态角计算模块具体包括：

第一计算单元，用于计算所述gps接收器与所述卫星之间的真实伪距以及载波相位的观测量；

差分模型建立单元，用于依据所述载波相位的观测量建立各gps接收器之间的差分模型；

双差分模型建立单元，用于依据所述差分模型，建立各gps接收器分别与预定数量的所述卫星之间的双差分模型；

姿态角模型建立单元，用于建立任意时刻所述待定位目标的姿态角模型；

第二计算单元，用于计算所述待定位目标在任意时刻的姿态角。

优选地，所述姿态角相对变化量计算模块具体包括：

各时刻差分模型建立单元，用于依据各所述gps接收器之间的差分模型，建立各gps接收器之间在t1时刻的第一差分模型和在t2时刻的第二差分模型；

时刻间双差分模型建立单元，用于建立各gps接收器在所述t2时刻与所述t1时刻之间的双差分模型；

姿态角相对变化量模型建立单元，用于建立各gps接收器从所述t1时刻到所述t2时刻的姿态角相对变化量模型；

相对变化值计算单元，用于依据所述姿态角相对量变化模型，计算所述姿态角从所述t1时刻到所述t2时刻的相对变化值。

本发明还提供一种利用信号接收器计算姿态角的设备，所述设备包括：处理器、存储器及至少4个信号接收器；所述信号接收器用于接收对一待定位目标进行定位的信号；所述存储器存储可被执行的计算机程序；所述处理器调用所述存储器中的计算机程序执行以下步骤：

s10利用至少4个所述信号接收器建立待定位目标的姿态角模型；

s20建立各所述信号接收器与预定数量信号发射器之间的第一双差分模型，计算任意时刻所述待定位目标的姿态角；

s30建立各所述信号接收器在连续时间点下的第二双差分模型，计算所述待定位目标位移变化时所述姿态角的相对变化量；

s40获取所述待定位目标满足预设条件的姿态角。

本发明的利用信号接收器计算姿态角的方法及设备，无需采用惯性测量组合系统即可准确获得待定位目标的姿态角，不会发生系统失灵、仪器故障或误差不可容忍等情况所导致的位置服务出现严重偏差的情况。同时，本发明具有完全不依赖于硬件传感器就能获得待定位目标的准确姿态角以进行导航定位服务的优点。

附图说明

图1示出了本发明实施方式一的利用信号接收器计算姿态角的方法的流程示意图。

图2示出了利用数个gps接收器建立姿态角模型时的示意图。

图3示出了图1中步骤s20的详细流程示意图。

图4示出了gps接收器之间建立差分计算模型的示意图。

图5示出了图1中步骤s30的详细流程示意图。

图6示出了计算姿态角的示意图。

图7是本发明实施方式二的利用信号接收器计算姿态角的设备的结构示意图。

图8是图7中姿态角计算模块的具体结构示意图。

图9是图7中姿态角相对变化量计算模块的具体结构示意图。

图10是本发明实施方式二的利用信号接收器计算姿态角的设备的结构示意图。

具体实施方式

为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施方式及实施方式中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施方式的限制。

实施方式一

请参见图1至图6，本发明实施方式一提供一种利用信号接收器计算姿态角的方法，该方法用来定位待定位目标如移动设备的位置，并为待定位目标如移动设备移动时提供导航服务，这里的移动设备可以是飞行器、陆上交通工具、水上与水下交通工具。具体来说，飞行器可以是无人飞行器，陆上交通工具可以是汽车尤其是无人驾驶汽车；水上与水下交通工具可以是船舶或者潜艇等工具。这里的信号接收器可以gps接收器、wi－fi信号接收器、蓝牙信号接收器、rfid信号接收器、声学传感器或者光学传感器。以下实施例中优先选择gps接收器，当然其它信号接收器与此类似，例如wi－fi信号接收器可以针对空间范围不大时待定位目标移动进行定位，设置预定数量的wi－fi信号发射器，通过对待定位目标按照类似计算即可获得姿态角，具体的应用场景是室内无人飞行器定位与导航。还有，上述姿态角也叫航姿角，主要包括：翻转角、偏航角以及俯仰角等几个定位待定位目标的角度。

该利用信号接收器计算姿态角的方法主要包括以下步骤：

s10利用至少4个所述信号接收器建立待定位目标的姿态角模型；如图2所示，假设待定位目标中4个gps接收器对应关系如图中所示，取东北方向为参考坐标轴，4个gps接收器的位置分别为ρ1，ρ2，ρ3，ρ4，gps接收器4到gps接收器1、gps接收器2的距离分别为d1和d2，因此可以计算移动设备的基线矩阵b0＝[ρ41ρ42ρ43]，在上述基线矩阵b0中：

在东北方向为参考坐标轴的坐标系内，设置任意时刻时移动设备姿态角的仰俯角、翻转角、偏航角分别为θ,φ，则其旋转矩阵为a

则任意时刻移动设备位置矩阵b＝a′bo

由于四元数的轴－角表示法可以表示出三维空间的任意旋转，因此为了方便计算使用四元数表示旋转矩阵为a(q)，则

其中，w＝cos(α/2),x＝sin(α/2)cos(βx),y＝sin(α/2)cos(βy),z＝sin(α/2)cos(βz)

上面的α为绕旋转轴旋转的角度，cos(βx),cos(βy),cos(βz)为旋转轴在x，y，z方向的分量，表示旋转轴的方向朝向，由此可确定旋转轴。

则用四元数来表示设备任意时刻时位置坐标为b(q)＝a(q)′bo

而任意时刻基线向量为ρij(q)＝a(q)′ρij(q0)。

s20建立各所述信号接收器与预定数量信号发射器之间的第一双差分模型，计算任意时刻所述待定位目标的姿态角；

s30建立各所述信号接收器在连续时间点下的第二双差分模型，计算所述待定位目标位移变化时所述姿态角的相对变化量；

s40获取所述待定位目标满足预设条件的姿态角。这里的预设条件可以定位精度实际需要来设定，当然，优选是定位精准的、经相关计算得出的最优姿态角(详见后文描述)。

在一个变形实施例中，本发明实施方式一的利用信号接收器计算姿态角的方法还可以与惯性测量组合系统结合使用，该惯性测量组合系统主要由加速度传感器，陀螺仪，磁传感器组合而成，其中加速度传感、陀螺仪可以有效计算设备的翻转角和俯仰角，而磁传感器可用来计算设备的偏航角，通过获得翻转角、俯仰角以及偏航角这三个数据就可以确定设备的空间姿态。所述方法在步骤s10之前还包括：

s50控制一惯性测量组合系统测量所述待定位目标的常规姿态角；

s60若相邻两个时刻的常规姿态角之间的变化量超过预设阈值时，则进入所述步骤s10。具体来说，在测量到常规姿态角后，判断两次相邻时刻的常规姿态角角度的差值满足一下公式：

δ|pt12,rt12,yt12|>δ

其中，pt12，rt12，yt12分别表示惯性测量组合系统在t1，t2时刻计算的俯仰角差值、翻转角差值、偏航角差值；δ||表示差值绝对值之和的平均值；δ为常规姿态角变化量的预设阈值，即常规姿态角在采集的相邻时刻内变化的允许的最大常量值，可取值为2°。

当相邻两个时刻的常规姿态角之间的变化量超过预设阈值时，即认为惯性测量组合系统失灵或误差在不可容忍范围内，需切换到本发明提出的利用信号接收器计算姿态角的方法，启至少4个gps接收器进行差分测量，获得移动设备的姿态角。

本发明的利用信号接收器计算姿态角的方法，在采用惯性测量组合系统发生系统失灵、仪器故障或误差不可容忍等情况时，自动切换利用信号接收器计算姿态角的方式，避免移动设备如无人飞行器、无人驾驶汽车等在移动时出现交通安全事故。

请参见图3与图4，当所述信号接收器为gps接收器时，所述信号发射器为卫星，步骤s20进一步包括以下步骤：

s21计算所述gps接收器与所述卫星之间的真实伪距以及载波相位的观测量；

具体来说，由gps接收器的伪距测量方法可知，

其中，表示gps接收器i与信号发射器(卫星s)的真实伪距和测量的几何距离；ti，t^s分别表示gps接收器i、卫星s的时钟偏差；a表示大气层中折射所引起的距离误差，mi为多径效应引起的误差，为星站观测噪声，c为光速。

与伪距测量类似，载波相位的观测量可表示为

其中表示gps接收器i与卫星s间的测量伪距；为初始整周未知数，其具有整周特性；为相位偏差，表示不足一周的小数部分；λ载波波长；a表示大气层中折射所引起的距离误差，mi为多径效应引起的误差，为卫星站观测噪声。

s22依据所述载波相位的观测量建立各gps接收器之间的差分模型；具体来说，对于gps接收器j与卫星s间的载波相位观测量可表示为

因此，在多台gps接收器i，j同时观测的情况下，对于同一颗卫星s，可得多gps接收器间差分模型如下：

其中，表示gps接收器i与gps接收器j到卫星s伪距之差，δtij＝ti-tj表示gps接收器钟差之差，表示相位初始整周未知数之差，表示gps接收器相位偏差之差。

根据如图2中gps接收器i，j之间的空间位置关系，可得

其中ρij表示gps接收器i到gps接收器j的连线向量，表示gps接收器到卫星s方向上的单位向量。所以多gps接收器间差分模型可写为

s23依据所述差分模型，建立各gps接收器分别与预定数量的所述卫星之间的双差分模型；具体来说，根据第二步可知，对于gps接收器i，j与卫星k间的差分模型，可表示为：

因此，多台gps接收器i，j同时观测多颗卫星s，k的双差模型为：

即多gps接收器与预定数量的卫星两两之间建立双差分模型可以消除各gps接收器间的时钟偏差，然而包含有多接收器、预定数量的卫星间的相位初始整周未知数之差。

s24建立任意时刻所述待定位目标的姿态角模型；在具体实施例中，根据多gps接收器建立的姿态角模型，多gps接收器与预定数量的卫星两两之间建立的双差模型可表示为

其中qn为初始估计的四元数；δθ旋转向量，其对应的四元数为δq；表示向量叉乘。

s25计算所述待定位目标在任意时刻的姿态角。具体来说，通过多个卫星对sk(s＝12，13，23，…)和不同gps接收器对ij(ij＝41，42，43)建立多个多gps接收器多卫星间双差方程，再通过最小二乘法估算最优旋转向量δθ，则可以计算旋转向量δθ所对应的四元数δq，

则在地球参考坐标系中的四元数为其中为四元数间乘积计算。

再通过四元数到欧拉角的转换公式

上述转换公式可获得移动设备在任意时刻的姿态角。

请参见图5，步骤s30进一步包括：

s31依据各所述gps接收器之间的差分模型，建立各gps接收器之间在t1时刻的第一差分模型和在t2时刻的第二差分模型；具体来说，根据多gps接收器间差分模型，建立在t1时刻和t2时刻时多gps接收器间差分模型：

其中，表示在t1时刻gps接收器i到gps接收器j间的向量，δtij(t1)表示在t1时刻gps接收器i、j的钟差之差，表示在t1时刻gps接收器i，j相位初始整周未知数之差，表示在t1时刻gps接收器i、j相位偏差之差，λ,c分别表示载波波长和光速。

同理，在t2时刻的gps接收器i，j间的差分模型为：

s32建立各gps接收器在所述t2时刻与所述t1时刻之间的双差分模型；具体来说，由于在没有周调产生的情况下，因此在t1时刻和t2时刻时多gps接收器间差分模型相减可得多gps接收器不同时间点的双差分模型如下:

其中，表示在t1，t2时刻gps接收器i到gps接收器j间的向量，δtij(t12)表示在t1，t2时刻gps接收器i、j的钟差之差，表示在t1，t2时刻gps接收器i、j间相位偏差之差，λ,c分别表示载波波长和光速。

s33建立各gps接收器从所述t1时刻到所述t2时刻的姿态角相对变化量模型；具体来说，根据多gps接收器建立的姿态角模型，则多gps接收器不同时间点时双差分模型可表示为：

其中q1为在t1时位置转四元数；δθ为t1时刻到t2时刻时旋转向量，其对应的四元数为δq；表示向量叉乘。

s34依据所述姿态角相对量变化模型，计算所述姿态角从所述t1时刻到所述t2时刻的相对变化值。具体来说，通过多个卫星s(s＝1，2，3，…)和不同gps接收器对ij(ij＝41，42，43)建立多个多gps接收器不同时间点时双差方程，再通过最小二乘法估算最优旋转向量δθ，则可以计算旋转向量δθ所对应的四元数δq。

而在t2时刻的四元数为其中为四元数间乘积运算，再通过四元数到欧拉角的转换公式：

可计算出移动设备的初始姿态角。

在一个具体实施例中，步骤s40利用可扩展性卡尔曼滤波获取所述待定位目标满足预设条件的姿态角，具体包括以下步骤：

s41依据所述相对变化值，基于所述姿态角相对变化量模型和各gps接收器分别与预定数量的所述卫星之间的双差分模型建立线性方程；具体来说，利用估计的旋转向量，将状态移模型写成线性一般式方程

δθk+1＝f·δθk+wk，

其中k，k+1为连续时间点，wk为gps信号的信噪比，可看作为卡尔曼滤波系统的过程噪声；δθ为估计的旋转向量。

s42简化各gps接收器分别与预定数量的所述卫星之间的双差分模型，生成简化方程；具体来说，将测量模型写成一般式

y-y0＝h·δθk+1+vt

上式中：

k表示卫星和采样下标数，vt为gps信号信噪比，可看作为卡尔曼滤波系统的测量噪声。

s43基于所述姿态角相对变化量模型及当前时刻的所述简化方程预测下一时刻的新简化方程，通过可扩展性卡尔曼滤波的增益权重，获取下一时刻的相对变化值及相应的四元数；具体来说，利用状态转移线性模型和k时刻的测量模型计算k+1时刻的预测模型，将k+1时刻的测量模型和k+1时刻的预测模型通过扩展性卡尔曼滤波的增益权重计算，可获取下一时刻(k+1时刻)的旋转向量最优估计δθk+1和对应的四元数δqk+1，则k+1时刻的四元数最优估计值为

s44将所述四元数进行欧拉角转换，获取所述待定位目标的姿态角。具体描述如下：通过四元数到欧拉角的转换公式，即可获取移动设备最优的姿态角估计。其中，四元数到欧拉角的转换公式：

通过上述公式，可计算出移动设备的姿态角。

实施方式二

请参见图6、图7及图8，本发明实施方式二在上述实施方式一的基础上还提供一种利用信号接收器计算姿态角的设备，所述设备包括：

姿态角模型建立模块10，用于利用至少4个所述信号接收器建立待定位目标的姿态角模型；

姿态角计算模块20，用于建立各所述信号接收器与预定数量信号发射器之间的第一双差分模型，计算任意时刻所述待定位目标的姿态角；

姿态角相对变化量计算模块30，用于建立各所述信号接收器在连续时间点下的第二双差分模型，计算所述待定位目标位移变化时所述姿态角的相对变化量；

姿态角获取模块40，用于获取所述待定位目标满足预设条件的姿态角。

本发明的利用信号接收器计算姿态角的设备具有完全不依赖于硬件传感器就能获得待定位目标的准确姿态角以进行导航定位服务的优点。

在一个具体实施例中，所述设备还包括：

常规姿态角测量模块，用于控制一惯性测量组合系统测量所述待定位目标的常规姿态角；

切换模块，用于当相邻两个时刻的常规姿态角之间的变化量超过预设阈值时，关闭惯性测量组合系统并启动信号接收器工作。

本发明的利用信号接收器计算姿态角的设备，在采用惯性测量组合系统发生系统失灵、仪器故障或误差不可容忍等情况时，自动切换到利用信号接收器计算姿态角的方式，避免移动设备如无人飞行器、无人驾驶汽车等在移动时出现交通安全事故。

如图8所示，在一个具体实施例中，所述姿态角计算模块20具体包括：

第一计算单元21，用于计算所述gps接收器与所述卫星之间的真实伪距以及载波相位的观测量；

差分模型建立单元22，用于依据所述载波相位的观测量建立各gps接收器之间的差分模型；

双差分模型建立单元23，用于依据所述差分模型，建立各gps接收器分别与预定数量的所述卫星之间的双差分模型；

姿态角模型建立单元24，用于建立任意时刻所述待定位目标的姿态角模型；

第二计算单元25，用于计算所述待定位目标在任意时刻的姿态角。

如图9所示，在一个具体实施例中，所述姿态角相对变化量计算模块30具体包括：

各时刻差分模型建立单元31，用于依据各所述gps接收器之间的差分模型，建立各gps接收器之间在t1时刻的第一差分模型和在t2时刻的第二差分模型；

时刻间双差分模型建立单元32，用于建立各gps接收器在所述t2时刻与所述t1时刻之间的双差分模型；

姿态角相对变化量模型建立单元33，用于建立各gps接收器从所述t1时刻到所述t2时刻的姿态角相对变化量模型；

相对变化值计算单元34，用于依据所述姿态角相对量变化模型，计算所述姿态角从所述t1时刻到所述t2时刻的相对变化值。

通过获得姿态角从所述t1时刻到所述t2时刻的相对变化值，为预测下一时刻的姿态角作好准备。

实施方式三

请参见图10，本发明还提供一种利用信号接收器计算姿态角的设备，所述设备包括：处理器100、存储器200及至少4个信号接收器310、320、330以及340；上述至少4个信号接收器310、320、330以及340用于接收对一待定位目标进行定位的信号；所述存储器200存储可被执行的计算机程序；所述处理器100调用所述存储器200中的计算机程序执行以下步骤：

s10利用至少4个所述信号接收器建立待定位目标的姿态角模型；

s20建立各所述信号接收器与预定数量信号发射器之间的第一双差分模型，计算任意时刻所述待定位目标的姿态角；

s30建立各所述信号接收器在连续时间点下的第二双差分模型，计算所述待定位目标位移变化时所述姿态角的相对变化量；

s40获取所述待定位目标满足预设条件的姿态角。

本发明的利用信号接收器计算姿态角的设备具有完全不依赖于硬件传感器就能获得待定位目标的准确姿态角以进行导航定位服务的优点。

此外，上述处理器还可以调用存储器存储的计算机程序在步骤s10之前执行以下步骤：

s50控制一惯性测量组合系统测量所述待定位目标的常规姿态角；

s60若相邻两个时刻的常规姿态角之间的变化量超过预设阈值时，则进入所述步骤s10。

本发明的利用信号接收器计算姿态角的设备，在采用惯性测量组合系统发生系统失灵、仪器故障或误差不可容忍等情况时，自动切换利用信号接收器计算姿态角的方式，避免移动设备如无人飞行器、无人驾驶汽车等在移动时出现交通安全事故。

以上仅为本发明的优选实施方式而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。