利用信號接收器計算姿態角的方法及設備與流程
本發明涉及導航技術領域，尤其涉及一種利用信號接收器計算姿態角的方法及設備。
背景技術：
隨著科技進步，越來越多的設備具備導航定位功能，其中，在提供位置服務時，確定設備的空間姿態在例如無人駕駛車輛、無人機等設備中是至關重要的安全保障。因此獲取精確的姿態角數據非常關鍵。
目前在基于位置服務的應用導航系統中，計算姿態角的方法一般都是通過慣性測量組合系統測量后計算獲得，該慣性測量組合系統主要由加速度傳感器，陀螺儀，磁傳感器組合而成，其中加速度傳感、陀螺儀可以有效計算設備的翻轉角和俯仰角，而磁傳感器可用來計算設備的偏航角，通過獲得翻轉角、俯仰角以及偏航角這三個數據就可以確定設備的空間姿態。但采用慣性測量組合系統測量與計算姿態角時，會因器件受到各種各樣的影響導致計算誤差較大。這些影響包括：加速度傳感器的制作工藝水平，設備振動，周圍環境對磁傳感器的電磁干擾，以及陀螺儀的誤差隨著時間逐漸積累等。因此，采用慣性測量組合系統測量后計算的方法在干擾嚴重的環境和特定的條件下會導致設備無法正常工作或產生無法容忍的誤差。還有就是一旦慣性測量組合系統發生系統失靈、儀器故障或誤差不可容忍等情況時就會導致基于位置服務的應用出現嚴重的偏差。
有鑒于此，有必要提供一種能準確、穩定地計算姿態角的方法及設備，以避免出現現有技術中采用慣性測量組合系統發生系統失靈、儀器故障或誤差不可容忍等情況時所導致基于位置服務的應用出現嚴偏差的問題。
技術實現要素：
本發明的目的在于提供一種利用信號接收器計算姿態角的方法及設備，用以解決現有技術中采用慣性測量組合系統發生系統失靈、儀器故障或誤差不可容忍等情況時所導致基于位置服務的應用出現嚴重偏差的技術問題。
本發明提供一種利用信號接收器計算姿態角的方法，包括以下步驟：
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中：
在東北方向為參考坐標軸的坐標系內，設置任意時刻時移動設備姿態角的仰俯角、翻轉角、偏航角分別為
則任意時刻移動設備位置矩陣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接收器的偽距測量方法可知，
其中，
與偽距測量類似，載波相位的觀測量可表示為
其中
s22依據所述載波相位的觀測量建立各gps接收器之間的差分模型；具體來說，對于gps接收器j與衛星s間的載波相位觀測量可表示為
因此，在多臺gps接收器i，j同時觀測的情況下，對于同一顆衛星s，可得多gps接收器間差分模型如下：
其中，
根據如圖2中gps接收器i，j之間的空間位置關系，可得
其中ρij表示gps接收器i到gps接收器j的連線向量，
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接收器間差分模型：
其中，
同理，在t2時刻的gps接收器i，j間的差分模型為：
s32建立各gps接收器在所述t2時刻與所述t1時刻之間的雙差分模型；具體來說，由于在沒有周調產生的情況下，
其中，
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為連續時間點，
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。
本發明的利用信號接收器計算姿態角的設備，在采用慣性測量組合系統發生系統失靈、儀器故障或誤差不可容忍等情況時，自動切換利用信號接收器計算姿態角的方式，避免移動設備如無人飛行器、無人駕駛汽車等在移動時出現交通安全事故。
以上僅為本發明的優選實施方式而已，并不用于限制本發明，對于本領域的技術人員來說，本發明可以有各種更改和變化。凡在本發明的精神和原則之內，所作的任何修改、等同替換、改進等，均應包含在本發明的保護范圍之內。
- 還沒有人留言評論。精彩留言會獲得點贊！