文章目錄
- 姿态更新
-
- 地球自轉角速度和牽連角速度
- 更新姿态
- 速度更新
-
- 比力方程
- PSINS源碼
- 雙子樣假設
- 速度更新算法
- 劃槳誤差補償算法
- PSINS源碼
- 位置更新
-
- 計算公式
- PSINS源碼
姿态更新
捷聯慣導數值更新算法通常可劃分為姿态、速度和位置更新三部分﹐姿态更新算法是核心,其求解精度對整個捷聯慣導的精度起着決定性的作用。目前主流的姿态更新求解方法是，先使用陀螺角增量的多子樣采樣計算等效旋轉矢量,補償轉動不可交換誤差,再使用等效旋轉矢量計算姿态更新四元數。
地球自轉角速度和牽連角速度
在考慮n系相對于i系的旋轉時，一定要考慮這兩部分:地球自轉引起的n系旋轉，以及慣導系統在地球表面附近移動因地球表面彎曲而引起的n系旋轉
更新姿态
姿态更新可采用四元數連乘來實作
也可由嚴老師書中提到的方向餘弦矩陣計算得到
速度更新
比力方程
比力方程是在地球表面附近進行慣性導航解算的基本方程
上式後兩項可統稱為有害加速度。比力方程式表明,隻有在加速度計輸出中扣除有害加速度後,才能獲得運載體在導航系下的幾何運動加速度,對加速度積分一次可得速度，再積分一次可得位置。是以,比力方程是慣導解算的基本方程。
PSINS源碼
嚴老師在PSINS中也給出了相應的代碼，計算了有害加速度
function eth = ethupdate(eth, pos, vn)
% Update the Earth related parameters, much faster than 'earth'.
%
% Prototype: eth = ethupdate(eth, pos, vn)
% Inputs: eth - input earth structure array
% pos - geographic position [lat;lon;hgt]
% vn - velocity
% Outputs: eth - parameter structure array
%
% See also ethinit, earth.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 23/05/2014
if nargin==2, vn = [0; 0; 0]; end
eth.pos = pos; eth.vn = vn;
eth.sl = sin(pos(1)); eth.cl = cos(pos(1)); eth.tl = eth.sl/eth.cl;
eth.sl2 = eth.sl*eth.sl; sl4 = eth.sl2*eth.sl2;
sq = 1-eth.e2*eth.sl2; RN = eth.Re/sqrt(sq);
eth.RNh = RN+pos(3); eth.clRNh = eth.cl*eth.RNh;
eth.RMh = RN*(1-eth.e2)/sq+pos(3);
% eth.wnie = [0; eth.wie*eth.cl; eth.wie*eth.sl];
eth.wnie(2) = eth.wie*eth.cl; eth.wnie(3) = eth.wie*eth.sl;
% eth.wnen = [-vn(2)/eth.RMh; vn(1)/eth.RNh; vn(1)/eth.RNh*eth.tl];
eth.wnen(1) = -vn(2)/eth.RMh; eth.wnen(2) = vn(1)/eth.RNh; eth.wnen(3) = eth.wnen(2)*eth.tl;
% eth.wnin = eth.wnie + eth.wnen;
eth.wnin(1) = eth.wnie(1) + eth.wnen(1); eth.wnin(2) = eth.wnie(2) + eth.wnen(2); eth.wnin(3) = eth.wnie(3) + eth.wnen(3);
% eth.wnien = eth.wnie + eth.wnin;
eth.wnien(1) = eth.wnie(1) + eth.wnin(1); eth.wnien(2) = eth.wnie(2) + eth.wnin(2); eth.wnien(3) = eth.wnie(3) + eth.wnin(3);
% eth.gn = [0;0;-eth.g];
eth.g = eth.g0*(1+5.27094e-3*eth.sl2+2.32718e-5*sl4)-3.086e-6*pos(3); % grs80
eth.gn(3) = -eth.g;
% eth.gcc = eth.gn - cros(eth.wnien,vn); % Gravitational/Coriolis/Centripetal acceleration
% eth.gcc = [ eth.wnien(3)*vn(2)-eth.wnien(2)*vn(3); % faster than previous line
% eth.wnien(1)*vn(3)-eth.wnien(3)*vn(1);
% eth.wnien(2)*vn(1)-eth.wnien(1)*vn(2)+eth.gn(3) ];
eth.gcc(1) = eth.wnien(3)*vn(2)-eth.wnien(2)*vn(3);
eth.gcc(2) = eth.wnien(1)*vn(3)-eth.wnien(3)*vn(1);
eth.gcc(3) = eth.wnien(2)*vn(1)-eth.wnien(1)*vn(2)+eth.gn(3);
雙子樣假設
本篇博文以雙子樣算法為例，推導速度更新方程。
雙子樣假設:[
tk-2,tk]
時段内角速度觀測量和比力觀測量均
随時間線性變化
。如圖2所示，在兩個采樣區間内，将實際的角速度和比力(黑色曲線)近似為随時間線性變化(曲線簡化為直線，紅色虛線，黑色曲線下的積分面積為慣性傳感器實際輸出的角增量或速度增量)。上式表明，線性假設的系數 a、b、c、d不但決定了角速度和比力的大小，同時還決定了其方向，也即線性假設也考慮了角速度向量和比力向量的方向變化。
如下是雙子樣假設下的角速度和比力随時間變化函數
速度更新算法
速度更新中，有兩項積分非常重要，一個是比力積分項，另一個就是重力/哥式積分項
那麼可推導出雙子樣速度更新公式：
對于中間時刻(前一時刻和目前時刻的中間時刻)的速度求解，可采用線性外推方法：
對位置也可進行如上外推。
劃槳誤差補償算法
下式描述的運動與現實生活劃船中的劃槳運動狀況非常相似：一方面槳繞船身的橫軸做往複角運動,另一方面船身連帶船槳沿縱軸做間歇性加速線運動。可見,劃船過程中船槳同時存在周期性的角運動和線運動,是以形象地稱下式所描述的運動為劃槳運動(
sculling motion
)。
推導可得N子樣劃槳誤差補償算法：
PSINS源碼
嚴老師同樣在源碼中給出了這個算法：
if coneoptimal==0
scullm = [0, 0, 0];
else
scullm = 1/12*(cros(glv.wm_st,vm)+cros(glv.vm_st,wm)); glv.wm_st = wm; glv.vm_st = vm;
end
else
vmm = sum(vm,1);
if coneoptimal==0
sm = glv.cs(n-1,1:n-1)*vm(1:n-1,:);
scullm = (cros(cm,vm(n,:))+cros(sm,wm(n,:)));
else % else: using polynomial fitting sculling compensation method
scullm = scullpolyn(wm, vm);
end
end
其中
sm = glv.cs(n-1,1:n-1)*vm(1:n-1,:);
scullm = (cros(cm,vm(n,:))+cros(sm,wm(n,:)));
這一塊代碼，為N子樣劃槳誤差補償算法的實作
位置更新
計算公式
可采用嚴老師書中的公式計算
也可參考牛老師的講義中的公式
但需要注意的是，不同的公式，對應的不同的b系得定義，嚴老師定義東北天為正，牛老師定位北東地為正
PSINS源碼
嚴老師在源碼中也給出了相應得矩陣形式
ins.Mpv = [0, 1/ins.eth.RMh, 0; 1/ins.eth.clRNh, 0, 0; 0, 0, 1];
ins.MpvCnb = ins.Mpv*ins.Cnb; ins.Mpvvn = ins.Mpv*ins.vn;