5 章 慣 性 航 法 複 合 演 算 処 理 5- 処 理 フロー 慣 性 航 法 複 合 演 算 処 理 の 全 体 処 理 フローを 図 5-- に 示 す GPS 観 測 データの 取 得 取 得 取 得 できず RK-GPS IMU 観 測 データの 取 得 慣 性 航 法 演 算 誤 差 方 程 式 の 導 出 位 置 オフセット 処 理 アンビギュイティの 状 態 決 定 未 決 定 oosel ouled 方 式 htl ouled 方 式 フィルタ 変 数 の 引 継 ぎ( 切 換 え 方 式 ) 拡 張 カルマンフィルタ 慣 性 航 法 誤 差 の 補 正 図 5-- 複 合 航 法 の 処 理 フロー 45
5- 座 標 系 の 定 義 慣 性 航 法 複 合 演 算 処 理 の 説 明 を 行 う 前 に 本 ソフトウェアで 用 いる 座 標 系 について 説 明 する 移 動 体 の 位 置 ( 緯 度 経 度 高 度 ) 姿 勢 角 及 び 方 位 角 を 慣 性 航 法 演 算 によって 求 めるために は 座 標 軸 の 設 定 が 重 要 である これらは 基 本 的 に 右 手 系 の 直 交 デカルト 座 標 系 であるもの とし 座 標 軸 を (X Y Z) で 表 す () I 座 標 系 ( X Y Z ) I I I Inertal Frame I 座 標 系 は 地 球 の 質 量 中 心 を 原 点 にとり Z I 軸 は 北 極 点 の 方 向 を 正 とする 地 軸 上 に X I 軸 とY I 軸 を 赤 道 面 にとり X I 軸 は 遠 い 星 方 向 ( 春 分 点 の 方 向 とする 場 合 もある) を 向 いている ものとする arth-entered arth-fed Frame 座 標 系 は 地 球 と 共 に 回 転 し 4 時 間 ( 正 確 には 恒 星 日 ) ごとに I 座 標 系 に 一 致 する Z 軸 は Z I 軸 と 等 しく 北 極 点 の 方 向 を 正 とする 地 軸 上 に X 軸 はグリニッジ 子 午 線 と 赤 道 面 の 交 点 の 方 向 を 正 とし Y 軸 を X 軸 を 含 む 赤 道 面 上 で Z X 軸 と 右 手 系 を 成 す 方 向 ( 東 経 9 [de]) に 定 めたものである 座 標 系 は 緯 度 経 度 など 移 動 体 の 位 置 を 表 すために 用 いられ る () 座 標 系 ( X Y Z ) ocall eel Frame 座 標 系 は 移 動 体 の 現 在 位 置 を 原 点 とする 局 地 水 平 座 標 系 である 北 向 きを X 軸 東 向 きをY 軸 とし 下 向 きに Z 軸 をとる ND (North-ast-Down) 座 標 系 である この 座 標 系 は 移 動 体 の 速 度 を 表 すために 用 いられる (3) 座 標 系 ( X Y Z ) omuter Frame 座 標 系 は 計 算 機 内 に 作 り 出 される 座 標 系 で 計 算 機 座 標 系 やワンダーアジマス 座 標 系 (Wander-Amuth Frame) と 呼 ばれる 座 標 系 と 同 様 移 動 体 の 現 在 位 置 を 原 点 とする 北 向 きを X 軸 西 向 きをY 軸 鉛 直 上 向 きを Z 軸 とした 右 手 系 を Z 軸 まわりに wander (4) 座 標 系 ( X Y Z ) anle と 呼 ばれる 角 度 α だけ 回 転 させたものとして 定 義 される wander anle α は 真 の 北 方 向 に 対 して 西 向 きを 正 とするような 角 度 である 航 法 座 標 系 として 座 標 系 を 用 いないのは 座 標 系 の X 軸 は 絶 えず 北 を 向 いているため 移 動 体 が 高 緯 度 で 東 向 きに 移 動 する 場 合 Z 軸 まわりには 座 標 系 を 保 つような 大 きな 回 転 角 が 必 要 になるためである このような 問 題 を 避 けるために 真 の 北 向 きからのずれを 考 慮 した 座 標 系 を 定 義 して 信 頼 性 を 向 上 させている 図 5-- に 座 標 系 を 図 5-- に 局 地 水 平 面 内 での wander anle と 座 標 系 の 関 係 を 示 す 46
図 5-- および 座 標 系 (5) 座 標 系 ( X Y Z ) 図 5-- 局 地 水 平 面 内 での wander anle と および 座 標 系 od Frame 座 標 系 は 移 動 体 の 質 量 中 心 を 原 点 にとる 座 標 系 で 機 体 座 標 系 と 呼 ばれる これは 車 両 の 先 頭 方 向 を X 軸 (ロール 軸 roll as) 車 体 の 右 側 をY 軸 (ピッチ 軸 tch as) X -Y 平 面 に 対 して 垂 直 下 向 きを Z (ヨー 軸 aw as) の 正 方 向 として 表 す 加 速 度 計 ジャイロ は 車 体 に 直 接 固 定 して 設 置 され 座 標 系 の 座 標 軸 に 沿 った 加 速 度 角 速 度 を 検 知 する 図 5--3 に 座 標 系 と 移 動 体 に 搭 載 された 加 速 度 計 ジャイロを 模 式 的 に 示 す 47
図 5--3 座 標 系 と 加 速 度 計 ジャイロ 5-3 慣 性 航 法 演 算 INS の 慣 性 航 法 演 算 (ストラップダウン 方 式 )の 算 出 内 容 を 以 下 に 示 す () 機 能 IMU データを 用 いて 位 置 速 度 姿 勢 方 位 を 算 出 する () アルゴリズム ) 初 期 化 処 理 慣 性 航 法 演 算 を 行 うために まず 座 標 系 から 座 標 系 への 座 標 変 換 行 列 の 初 期 値 およ び 座 標 系 から 座 標 系 への 座 標 変 換 行 列 の 初 期 値 を 求 める 座 標 系 から 座 標 系 への 座 標 変 換 行 列 の 初 期 値 を 求 めるために ロール 角 Φ ピッチ 角 Θ よりクォータニオンの 初 期 化 を 行 う クォータニオンの 初 期 値 は となる 得 られた ( w ) めると Θ Φ w cos cos Θ Φ cos sn Θ Φ sn cos Θ Φ sn sn を 用 いて 座 標 系 から 座 標 系 への 変 換 行 列 (5-3-) (5-3-) (5-3-3) (5-3-4) を 求 48
w ( w ) ( w ) ( ) ( ) ( ) ( ) w w w w w w (5-3-5) となる 座 標 系 から 座 標 系 への 変 換 行 列 は 緯 度 経 度 の 初 期 値 に 加 え 求 めた wander anle の 初 期 値 を 用 いて cosα sn λ cosϕ snα snϕ snα sn λ cosϕ cosα snϕ cosλ cosϕ cosα sn λ snϕ snα cosϕ snα sn λ snϕ cosα cosϕ cosλ snϕ cosα cosλ snα cos λ sn λ (5-3-6) となる その 他 に 航 法 演 算 を 始 める 準 備 として 座 標 変 換 行 列 を 用 いて 地 球 の 重 力 モデル の 計 算 ( 後 述 の 手 順 ) 参 照 )を 行 う 必 要 がある 上 記 の 初 期 化 処 理 を 完 了 させた 後 図 5-3- に 示 す 慣 性 航 法 演 算 を 行 い 得 られた INS 位 置 及 び 速 度 を 基 に カルマンフィルタを 適 用 した 複 合 航 法 演 算 が 行 われる 加 速 度 センサ (3 軸 ) 座 標 変 換 速 度 計 算 コリオリ 補 正 重 力 補 正 相 対 角 速 度 計 算 姿 勢 角 方 位 角 方 向 余 弦 行 列 計 算 初 期 位 置 初 期 方 位 角 初 期 姿 勢 角 INS 位 置 計 算 真 方 位 角 計 算 INS 速 度 INS 位 置 クォータニオン 更 新 ジャイロ (3 軸 ) 補 正 角 速 度 計 算 自 転 角 速 度 計 算 図 5-3- 慣 性 航 法 演 算 ブロック 図 ) 航 法 演 算 開 始 アライメントが 完 了 し 移 動 体 が 運 用 モードに 移 行 すると 同 時 に 慣 性 航 法 演 算 が 開 始 され る ここで INS の 航 法 演 算 間 隔 を Δ とする 49
5 3) 角 速 度 前 処 理 移 動 体 の Δ 間 の 姿 勢 変 化 に 伴 う 角 速 度 の 座 標 系 でのふるまいは 地 球 自 転 角 速 度 Ωと 相 対 角 速 度 ρの 補 正 を 行 って (5-3-7) となる 次 に これを 座 標 系 の 3 軸 成 分 に 変 換 すると (5-3-8) が 得 られる 4) 角 速 度 処 理 ジャイロが 時 刻 t に 出 力 する 角 速 度 を とする このとき 演 算 間 隔 Δ で 生 ずる 移 動 体 の 角 変 動 量 ( ) s rad Δ / は を 用 いて (5-3-9) となる 5) 座 標 変 換 行 列 の 更 新 と 姿 勢 角 算 出 まず 式 (5-3-9) で 算 出 された 角 速 度 を 用 いて クォータニオンの 時 間 更 新 を 行 う 更 新 式 は (5-3-) となる ただし とする 更 新 されたパラメータを 式 (5-3-5) へ 代 入 し の 更 新 を 行 う 更 新 された より 移 動 体 の 姿 勢 角 を 求 める ロール 角 Φ ピッチ 角 Θ およびヨー 角 Ψはそれぞれ (5-3-) ( ) ( ) ( ) Δ Ω Δ Ω Δ Ω ρ ρ ρ ( ) ( ) t ( ) t ( ) t ( ) ( ) () ( ) () ( ) Δ Δ Δ t t t t t t () ( ) ( ) ( ) ( ) q t q t q t t q Ω & w q & & & & & ( ) Ω w q ( ) Φ tan w w
5 (5-3-) (5-3-3) となる 6) 加 速 度 変 換 処 理 加 速 度 計 から 直 接 出 力 された 座 標 系 の 加 速 度 を 座 系 の 加 速 度 へ 変 換 する すなわち (5-3-4) となる 7) 速 度 補 正 演 算 座 標 系 の 速 度 を 算 出 する この 際 座 標 系 の 加 速 度 に 加 え 地 球 の 自 転 角 速 度 Ω 相 対 角 速 度 ρ 重 力 加 速 度 が 必 要 となる 速 度 補 正 演 算 の 式 は (5-3-5) となる 8) 相 対 角 速 度 算 出 移 動 体 の 座 標 系 の 速 度 より 相 対 角 速 度 は (5-3-6) (5-3-7) (5-3-8) となる ただし ( ) { } w Θ sn ( ) Ψ tan w w a a a a a a ( ) ( ) ( ) ( ) ( ) ( ) Ω Ω Ω Ω Ω Ω a a a ρ ρ ρ ρ ρ ρ & & & ( ) α α tan tan m m R R R R R ( ) α α tan tan m m R R R R R ( ) ( ) 3 sn λ e e a R m λ sn e a R h R ρ h R ρ ρ
とし h: 高 度 a : 地 球 の 赤 道 面 での 長 半 径 e: 離 心 率 とする 9) 座 標 変 換 行 列 の 更 新 と 位 置 の 算 出 手 順 k で 求 めた 相 対 角 速 度 ρを 用 いて 座 標 系 から 座 標 系 への 変 換 行 列 を 更 新 すると ( t Δ ) { I Ω( t ) Δ} ( t ) k k k (5-3-9) となる ただし Ω ( ) t k ρ ρ ρ ρ ρ ρ とする 更 新 された より 緯 度 λ 経 度 ϕ wander anleα について を 得 る ただし λ ϕ tan α tan sn 3 3 3 33 3 (5-3-) (5-3-) (5-3-) 3 3 3 3 33 とする ) 地 球 自 転 角 速 度 の 算 出 移 動 体 の 現 在 緯 度 λ wander anle λ より 地 球 自 転 角 速 度 の 座 標 系 における 3 軸 成 分 は Ω Ω cos λ cosα (5-3-3) Ω Ω cos λ snα (5-3-4) Ω (5-3-5) Ω sn λ となる ただし Ω 7.959 5 [rad/sec] とする ) 地 球 重 力 モデルの 計 算 5
更 新 された 座 標 変 換 行 列 より 座 標 系 における 重 力 成 分 は n h 33 3 n h 33 3 となる ただし h: 高 度 [ft] とし 4 ( l 33 l 33 ) { ( ) h h } h h 33 h3 3.87686[ ft / s.69389 l l h h h n 7.47483 8 9.67 6.49 3 6.85.69 8 4 5 ] (5-3-6) (5-3-7) (5-3-8) とする ) 地 球 モデルの 計 算 移 動 体 の 現 在 緯 度 λ wander anleα が 算 出 されたので 地 球 モデル すなわち 座 標 系 での 曲 率 半 径 は R RRm R R ( tan α ) m tan α (5-3-9) R R R R m m ( tan α ) R tan α (5-3-3) となる ただし R m a( e ) 3 ( e sn λ) a e sn λ とし h: 高 度 a : 地 球 の 赤 道 面 での 長 半 径 e: 離 心 率 とする 以 上 3) ~ ) の 処 理 が 繰 り 返 される R 53
5-4 誤 差 方 程 式 の 算 出 慣 性 航 法 における 誤 差 をカルマンフィルタで 補 正 するための 誤 差 方 程 式 を 算 出 する () 機 能 慣 性 航 法 における 誤 差 方 程 式 を 算 出 する () アルゴリズム INS の 航 法 誤 差 は 慣 性 センサから 得 られる 物 理 量 を 航 法 演 算 の 基 準 となる 演 算 座 標 系 ( 座 標 系 )での 値 に 座 標 変 換 を 行 う 際 に 生 じると 考 え その 座 標 変 換 行 列 の 誤 差 は INS の 誤 差 源 と 上 記 で 述 べた 初 期 誤 差 により 生 じると 考 える したがって 局 地 水 平 面 座 標 系 ( 座 標 系 )から 座 標 系 への 座 標 変 換 行 列 において 方 位 角 誤 差 による 航 法 誤 差 への 影 響 を 考 慮 する なお INS の 誤 差 源 は 加 速 度 センサ ジャイロの 誤 差 のみと 仮 定 する また 航 法 誤 差 は 位 置 誤 差 速 度 誤 差 姿 勢 角 誤 差 方 位 角 誤 差 を 考 え アライメントにおける 方 位 角 誤 差 は 方 位 角 に 相 当 する wander anle の 誤 差 とする 以 下 では ) 位 置 誤 差 ) 速 度 誤 差 3) 姿 勢 角 誤 差 4) 方 位 角 誤 差 5) 慣 性 センサの 誤 差 に 分 けて 誤 差 方 程 式 の 導 出 をおこなう ) 位 置 誤 差 方 程 式 INS 航 法 演 算 において 緯 度 経 度 は 座 標 変 換 行 列 から 求 める よって 位 置 誤 差 ( δ r )は 座 標 系 と 座 標 系 との 座 標 変 換 行 列 の 誤 差 δ と 考 えることができる しかし 移 動 アラ イメントの 前 提 条 件 を 考 慮 すると 方 位 角 誤 差 (δα )が 大 きいためにα の 回 転 によって 求 められ る 座 標 系 ( 座 標 系 )が 真 の 座 標 系 に 比 べて 大 きくずれていると 考 えられる したがって 大 きな 誤 差 を 含 んだ 座 標 変 換 行 列 を 局 地 水 平 面 が X Y 軸 まわりに 微 小 な 傾 きを 持 ち Z 軸 まわりに 大 きくずれた 座 標 系 に 変 換 する 座 標 変 換 行 列 としてモデル 化 する なお 演 算 によ って 求 まるものは 真 とするものは その 誤 差 はδ として 表 す そこで は { δ } ( δ ) I ( r ) (5-4-) と 考 えることができる ここで ( δ r ) はベクトル δr [ δr δr ] の 要 素 によって 決 ま る 歪 対 称 行 列 である 次 に 式 (5-4-)よりδ は 54
δ δ { δ } { } ( ) I ( r ) δ I ( δr ) ( δr ) (5-4-) となる ここで 式 (5-4-)において 位 置 誤 差 の 要 因 となる 行 列 を 位 置 誤 差 行 列 とし snδα ( cosδα ) をγ β として 表 すとδ は δ ( β)cos α ( γ)sn α ( β)sn α ( γ)cos α ( β ) δr ( γ) δr (5-4-3) ( β)sn α ( γ)cos α ( β)cos α ( γ)sn α ( β ) δr ( γ) δr δr δr となる ここで 式 (5-4-3)の 両 辺 を 微 分 すると δ& & & ( ρ/ ) { & ( ρ/ )} (5-4-4) となる ここで ( ρ / ) は 座 標 系 における( 上 添 字 ) 座 標 系 に 対 する 座 標 系 ( 下 添 字 ) の 相 対 角 速 度 ベクトルの 要 素 で 決 まる 歪 対 称 行 列 とする 次 に δ を 以 下 のように 考 えると δ / ρ/ / I r / ( ρ ) ( ) ( ρ ){( δ )( ( δ ))} ( ρ ) となる よって 式 (5-4-4)と 式 (5-4-5)より 位 置 誤 差 行 列 は { } & ( ρ ) ( ρ ) ( δ ) ( δ ) ( ρ ) / / I r / / / / ( ρ ) ( ρ )( ) ( ρ ) となり 位 置 に 関 する 誤 差 方 程 式 を 導 くことができた ここで ρ ρ ρ ρ ρ ρ ρ ρ ρ ρ / / ρ ρ ρ ρ (5-4-5) (5-4-6) (5-4-7) であり これまでの 座 標 変 換 行 列 と 同 様 に ( δρ ) ( ρ ) ( ρ ) として 考 えることができるので 式 (5-4-6)を 展 開 することで {( ) ( ) }{( ) ( ) } r { r r } δr& δr β ρ γ ρ β δr γ δr ρ δ ( β ) δ ( γ) δ ( γ) ρ ( β) ρ ( β ) δρ (5-4-8) 55
{( ) ( ) }{( ) ( ) } r { r r } δr& δr β ρ γ ρ β δr γ δr ρ δ ( β ) δ ( γ) δ ( γ) ρ ( β) ρ ( β ) δρ となる ただし ストラップダウン 方 式 では ρ とする また δρ δρ δρ δρ δ R h ( R h) δ R h ( R h) δh δh として 考 えることができるので δr& δr ( β ) ρ ( γ) ρ ( β ) δr ( γ) δr { }{ } δ ρ δ δ β δ γ δ δ ρ δh ( γ) ( β) ρ R h ( R h) は {( ) ( ) } h r r r R h ( R h) δ δh ( β ) ur R h ( R h) δr& δr ( β ) ρ ( γ) ρ ( β ) δr ( γ) δr { }{ } δ ρ δ δ β δ γ δ δ ρ δh ( γ) ( β) ρ R h ( R h) {( ) ( ) } h r r r R h ( R h) δ R h ( R h) δh ( β ) ur (5-4-9) (5-4-) (5-4-) (5-4-) (5-4-3) となる また Z 軸 方 向 の 位 置 誤 差 ( 高 度 誤 差 )は 次 式 のようになる δ h& δ u (5-4-4) r 以 上 (5-4-) (5-4-3) (5-4-4) 式 が 位 置 誤 差 に 関 する 方 程 式 となる ただし u r u r u はモデル 化 されていない 誤 差 を 表 すもので ここでは 互 いに 無 相 関 な 平 均 分 散 U r U r r Ur の 正 規 性 白 色 雑 音 と 仮 定 する ) 速 度 誤 差 方 程 式 INS 速 度 を 求 める 基 本 方 程 式 から & f ( ρ Ω ) (5-4-5) として 移 動 体 の 速 度 が 求 まる ここで f は secfc force と 呼 ばれるもので 重 力 を 除 いた 慣 性 系 に 対 する 移 動 体 に 生 じる 加 速 度 のことであり は 重 力 加 速 度 を 表 す また Ω は 地 球 自 転 角 速 度 ベクトル Ω ( [ ]) Ω を 座 標 系 で 表 したものである しかしながら 演 算 速 度 は 姿 勢 角 方 位 角 の 初 期 値 を 決 定 しないため 誤 差 を 含 んでいるものと 考 えられるので 速 56
度 誤 差 を 考 慮 する 際 の 誤 差 を 含 んだ secfc force( f )と 重 力 加 速 度 ( )は 次 のように 表 すこ とができる f [ I ( δθ )] f δ f (5-4-6) δ (5-4-7) ただしδ f は 加 速 度 センサ 誤 差 であり δ f b とする また 式 (5-4-7)の 座 標 系 にお ける 重 力 は [ ] のベクトルで 表 現 できる また δ は 真 の 座 標 系 に 対 し 微 小 な 角 度 誤 差 δθ δθ がある 場 合 に 発 生 する 重 力 加 速 度 誤 差 として 考 えると δθ ( ) δθ δθ δ δθ δθ δθ δθ (5-4-8) として X Y 軸 それぞれの 重 力 加 速 度 誤 差 は δθ δθ として 表 すことができる な お δα 軸 に 関 しては 重 力 加 速 度 誤 差 は 発 生 しない これより 移 動 アライメント 時 の 演 算 速 度 は [ I ( δθ )] f b δ { } ( ρ δρ ) ( Ω δω ) ( δ ) (5-4-9) として 考 えることができる 以 上 より 移 動 アライメント 時 の 速 度 誤 差 δ & は δ & & f δθ b ( δρ δω ) ( ρ Ω ) δ ( δρ δω ) δ δ (5-4-) と 示 すことができる これより X 軸 速 度 誤 差 について 整 理 すると δ& f δθ b ( δρ δω )( δ ) ( ρ Ω ) δ (5-4-) ( δρ δω )( δ ) Ω δ δθ となる ここでZ 軸 の 相 対 角 速 度 誤 差 は(5-4-3) 式 より 求 まり X Y 軸 の 相 対 角 速 度 誤 差 は 式 (5-4-) (5-4-)のようになる また 後 述 の 式 (5-4-6)より 地 球 自 転 角 速 度 誤 差 δω を 式 (5-4-)に 代 入 すれば δ δ& δh Ω { ( β)sn α ( γ)cosα} R h ( R h) Ω { ( β ) δr ( γ) δr } ( δ ) Ω δr Ω δr δθ ( ρ Ω ) (5-4-) { δθ ( ρ Ω ) d ( δ ) ( ρ Ω ) δ Ω δ } f δθ b δθ u を 得 る 同 様 にしてY Z 軸 の 速 度 誤 差 も 導 出 し 以 下 の 式 を 得 る 57
δ δ& δh Ω { ( β)cos α ( γ)snα} ( R h) R h Ω { ( β ) δr ( γ) δr } ( δ ) Ω δr Ω δr δθ ( ρ Ω ) { δθ ( ρ Ω ) d ( δ ) ( ρ Ω ) δ Ω δ f δθ b δθ u } (5-4-3) δ δ& δh Ω { ( β)sn α ( γ)cosα} R h ( R h) Ω { ( β ) δr ( γ) δr } ( δ ) δ δh Ω { ( β)cos α ( γ)snα} R h ( R h) Ω { ( β ) δr ( γ) δr } ( δ ) ( ρ Ω ) δ ( ρ Ω ) δ f δθ f δθ b u (5-4-4) 以 上 式 (5-4-) (5-4-3) (5-4-4)が 速 度 誤 差 に 関 する 方 程 式 となる ただし u u u はモデル 化 されていない 誤 差 を 表 すもので ここでは 互 いに 無 相 関 な 平 均 分 散 U U U の 正 規 性 白 色 雑 音 と 仮 定 する 3) 姿 勢 角 誤 差 方 程 式 姿 勢 角 誤 差 は 座 標 系 と 座 標 系 の 関 係 を 与 える 角 度 の 誤 差 として 考 える すなわち ロー ル 角 ピッチ 角 ヨー 角 のずれを 姿 勢 角 誤 差 δθ [ δθ δθ δθ ] として 考 える ただし 前 述 したように Z 軸 まわりの Yaw 角 誤 差 δθ は wander anle 誤 差 δα に 比 べて 小 さいの で 無 視 する そこで 姿 勢 角 誤 差 は 座 標 系 を 局 地 水 平 に 保 つための 地 球 に 対 する 相 対 角 速 度 地 球 自 転 角 速 度 の 補 正 誤 差 そしてジャイロのバイアス 誤 差 により 生 ずるとし δθ& δρ δω δθ d (5-4-5) / I/ を 姿 勢 角 の 誤 差 方 程 式 とする ここで はジャイロのバイアス 誤 差 とし d [ d d d ] とする また Ω ( [ Ω Ω Ω ] ) は 座 標 系 で 表 した 地 球 自 転 角 速 度 であり d 58
δ δω Ω Ω Ω Ω Ω Ω Ω (5-4-6) Ω {( β)cos α ( γ)sn α} {( β ) δr ( γ) δr } {( β)sn α ( γ)cos α} {( β ) δr ( γ) δr } ( δr snα δr cosα) Ω Ω Ω Ω Ω となる また δθ ρ Ω δθ I/ δθ ρ Ω δθ ρ Ω (5-4-7) Ω δθ Ω δθ ( δθ ) ( ρ Ω ) δθ ( ρ Ω ) δθ となる これより 式 (5-4-6) (5-4-7)を 式 (5-4-5)に 代 入 することにより 姿 勢 角 誤 差 方 程 式 を 得 る {( )cos ( )sn } {( ) r ( ) r } δθ& Ω β α γ α Ω β δ γ δ δ h Ω δθ d u ( R ) h δ h R θ (5-4-8) {( ) r ( ) r } {( )sn ( )cos } δθ& Ω β δ γ δ Ω β α γ α δ h Ω δθ d u ( R ) h δ h R θ (5-4-9) δθ& δρ ( δ r snα δ r cos α) Ω ( ρ Ω ) δθ ( ρ Ω ) δθ d (5-4-3) ただし u θ はモデル 化 されていない 誤 差 を 表 すもので ここでは 互 いに 無 相 関 な 平 u θ 均 分 散 U θ の 正 規 性 白 色 雑 音 と 仮 定 する U θ 4) 方 位 角 誤 差 方 程 式 方 位 角 誤 差 に 関 する 誤 差 方 程 式 は 位 置 誤 差 方 程 式 と 同 様 に 式 (5-4-6)から 導 くことができる ここで 位 置 誤 差 行 列 を 59
( β)cos α ( γ)sn α ( β)sn α ( γ)cos α ( β ) δr ( γ) δr ( β)sn α ( γ)cos α ( β)cos α ( γ)sn α ( β ) δr ( γ) δr δr δr A ( β ) δr ( γ) δr A ( β ) δr ( γ) δr δr δr (5-4-3) とする ここで 式 (5-4-6)の 左 辺 & を 導 出 するにあたり A& & は A& & αβ ( )sn α ( & β)cos α & αγ ( )cos α( & γ)snαsd & α{( β)sn α ( γ)cos α} ( & β)cos α ( & γ)snα (5-4-3) & α ( & β)cos α ( & γ)snα & & α( β)cos α ( & β)sn α & α( γ)sn α ( & γ)cosα & α{( β)cos α ( γ)sn α} ( & β)sn α ( & γ)cosα (5-4-33) & αa ( & β)sn α ( & γ)cosα となる また の 右 辺 は ( ) 考 えることができることより ρ とし Z 軸 の 相 対 角 速 度 ρ は 方 位 角 の 時 間 微 分 要 素 α& と {( ) ( ) } A& ρ ρ β δr γ δr {( )sn ( )cos } δρ β α γ α ρ δ r {( ) ( ) } & α ρ β δr γ δr {( )sn ( )cos } δρ β α γ α ρ δ r (5-4-34) {( ) ( ) } & Aρ ρ β δr γ δr { ( )cos ( )sn } δρ β α γ α ρ δ r {( ) ( ) } A& α ρ β δr γ δr { ( )cos ( )sn } δρ β α γ α ρ δ r (5-4-35) となる ここで 式 (5-4-34)の 両 辺 に cosα を 式 (5-4-35)の 両 辺 に snα を 乗 じ 互 いを 差 し 引 くことにより { } & β ρ ( βδ ) r ( γδ ) r ( γδρ ) δρ δr (5-4-36) を 得 る また 同 様 にして 式 (5-4-34)の 両 辺 に snα を 式 (5-4-35)の 両 辺 に cosα を 乗 じ 互 い を 加 えることにより { } & γ ρ ( β ) δr ( γ) δr ( β ) δρ ρ δr (5-4-37) を 得 る 一 方 δρ は 式 (5-4-3)より 求 められるので 式 (5-4-36) (5-4-37)に 代 入 すると 6
{( ) r ( ) r } { r r & β ρ β δ γ δ ( δ snα δ cos α) Ω ( ρ Ω ) δθ ( ρ Ω ) δθ d ( γ) δρ δr u β { } & γ ρ ( β ) δr ( γ) δr ( δr snα δr cos α) Ω ( ρ Ω ) δθ ( ρ Ω ) δθ d ( β ) δ ρ δh δr u R h ( R h) { } γ } (5-4-38) (5-4-39) 以 上 式 (5-4-38) (5-4-39)が 方 位 角 誤 差 に 関 する 方 程 式 となる ただし u β u γ はモデル 化 されていない 誤 差 を 表 すもので ここでは 互 いに 無 相 関 な 平 均 分 散 U β U γ の 正 規 性 白 色 雑 音 とする また それら β γ を 用 いて 方 位 角 誤 差 δα は tan snδα δα (5-4-4) (cos δα ) として 求 めることができる 5) 慣 性 センサの 誤 差 モデル 加 速 度 センサ ジャイロの 誤 差 は 共 に 強 い 自 己 相 関 をもつ 量 と 考 えられる したがって 慣 性 センサの 誤 差 を 各 軸 成 分 がそれぞれ 指 数 関 数 で 表 されるような 自 己 相 関 を 持 つ 有 色 雑 音 とし て 取 り 扱 い 次 のマルコフ 過 程 としてモデル 化 し 以 下 のように 表 現 する b b u (5-4-4) b τ b b b u (5-4-4) b τb b b u (5-4-43) b τ b d d u (5-4-44) d τ d d d u (5-4-45) d τ d d d u (5-4-46) d τ d ただし ub K ud は 互 いに 無 相 関 で 平 均 分 散 U K U り τ b τ d は 相 関 時 間 である b d の 正 規 性 白 色 雑 音 であ 6
5-5 位 置 オフセット 処 理 実 環 境 において 慣 性 航 法 複 合 演 算 を 実 施 する 際 GPS アンテナと IIM の 座 標 の 違 いによ るオフセットを 考 慮 しなくてはならない 慣 性 航 法 複 合 演 算 における oosel ouled 方 式 は GPS の 位 置 速 度 と IMU の 位 置 速 度 を 用 いて 観 測 量 を 計 算 するため GPS IMU それぞ れの 座 標 を 統 一 する 必 要 がある ここでは IMU と GPS アンテナの 設 置 箇 所 による 座 標 の 違 い( 位 置 オフセット)に 対 し 補 正 を 行 う 手 法 について 説 明 する () 機 能 GPS アンテナと IMU の 位 置 オフセット 量 から GPS の 位 置 を IMU の 位 置 へ 補 正 する () アルゴリズム 位 置 オフセットの 概 略 図 を 図 5-5- に 示 す :GPS アンテナ :IMU 図 5-5- IMU と GPS アンテナの 物 理 的 オフセット ( 左 図 : 上 から 見 た 図 右 図 : 横 から 見 た 図 ) ) 初 期 設 定 Τ IMU の NU 座 標 を[ ] とし ロール 角 Ψ ピッチ 角 Ξ 方 位 角 Φがそれぞれ [de] であった 場 合 の GPS アンテナ 箇 所 における 物 理 的 オフセット 量 を 求 める 水 平 方 向 に 対 して 東 方 向 に m[m] 南 方 向 に n[m] 鉛 直 高 さ 方 向 に h[m]とすると GPS アンテナの NU 座 標 は Τ Τ [ m n h] となる また 静 止 している 状 態 で IMU に 角 速 度 [ ] だけ 印 加 される Τ と 機 体 座 標 系 での IMU の 速 度 は [ ] であり 角 速 度 による GPS アンテナにおける 速 度 は s ( : 外 積 )となる 6
条 件 IMU の 座 標 : GPS の 座 標 : Τ [ ] [ m n h] Τ m h n n 図 5-5- オフセットパラメータの 詳 細 ( 左 図 : 上 方 より 見 た 図 右 図 : 側 面 より 見 た 図 ) ) GPS アンテナの 座 標 からの IMU 座 標 での GPS 位 置 の 算 出 IMU における 真 の 角 度 (ロール 角 Ψ ピッチ 角 Ξ 方 位 角 Φ)を 用 いて 機 体 座 標 ( ) から 局 地 平 面 (NU) 座 標 ( )への 座 標 変 換 は (5-5-) となる ここで は 機 体 座 標 ( )から 局 地 水 平 面 (ND) 座 標 ( )への 座 標 変 換 行 列 であり 以 下 の 式 で 表 される cosθcosφ snψsnθcosφ cosψsnφ cosψsnθcosφ snψsnφ cosθsnφ snψsnθsnφ cosψcosφ cosψsnθsnφsnψcosφ (5-5-) snθ snψcosθ cosψcosθ よって IMU 座 標 における GPS 座 標 は 物 理 的 オフセット 量 ( )( n m h )を 用 いて となる XI XG XG Y I Y G Y G Z I Z G Z G (5-5-3) 3) GPS アンテナでの 速 度 から IMU 座 標 での GPS 速 度 の 算 出 GPS アンテナでの 速 度 [ N U ] Τ が 与 えられているとき IMU 座 標 における GPS アンテ ナの 速 度 [ N U Τ ] は IMU の 角 速 度 と 物 理 的 オフセット 量 により 以 下 の 関 係 式 となる 63
n N N N m U U U h (5-5-4) 5-6 oosel couled 方 式 慣 性 航 法 複 合 演 算 において 慣 性 航 法 演 算 の 誤 差 を 補 正 する 手 法 の oosel couled 方 式 に ついて 説 明 する () 機 能 oosel couled 方 式 のモデル 式 を 算 出 する () アルゴリズム 5-4 項 において INS の 航 法 誤 差 に 関 する 方 程 式 が 得 られ oosel couled 方 式 の INS 航 法 誤 差 を 表 5-6- に 示 す 慣 性 航 法 複 合 演 算 における oosel couled 方 式 は 表 5-6- に 示 した INS の 航 法 誤 差 を GPS の 位 置 と 速 度 を 用 いて 推 定 し 補 正 する 表 5-6- oosel couled 方 式 の 状 態 変 数. δ r : X 軸 まわりの 位 置 誤 差 9. b : X 軸 加 速 度 バイアス. δ r : Y 軸 まわりの 位 置 誤 差. b : Y 軸 加 速 度 バイアス 3. δ : X 軸 方 向 の 速 度 誤 差. b : Z 軸 加 速 度 バイアス 4. δ : Y 軸 方 向 の 速 度 誤 差. d : X 軸 ジャイロバイアス 5. δθ : X 軸 まわりの 姿 勢 角 誤 差 3. d : Y 軸 ジャイロバイアス 6. δθ : Y 軸 まわりの 姿 勢 角 誤 差 4. d : Z 軸 ジャイロバイアス 7. δ h : Z 軸 方 向 の 高 度 誤 差 5. γ : snδα 8. δ : Z 軸 方 向 の 速 度 誤 差 6. β : (cosδα ) oosel couled 方 式 は 図 5-6- に 示 すような 構 成 であり GPS の 測 位 結 果 ( 位 置 速 度 ) を 用 い 表 5-6- の 各 変 数 を 推 定 する 以 下 では oosel couled 方 式 における 状 態 方 程 式 お よび 観 測 方 程 式 を 示 す 64
INS INS 位 置 速 度 位 置 速 度 姿 勢 方 位 センサ 誤 差 補 正 値 カルマン フィルタ RK-GPS RK-GPS 位 置 速 度 図 5-6- oosel couled 方 式 のシステム 構 成 ) 状 態 方 程 式 (oosel 方 式 ) 表 5-6- に 示 した INS の 位 置 誤 差 速 度 誤 差 姿 勢 角 誤 差 方 位 角 誤 差 及 び 慣 性 センサ のバイアス 誤 差 を 状 態 変 数 とし 状 態 ベクトルを 次 のように 定 義 する [ δ r δr δr δ δ δθ δθ δh b b b d d d γ β] したがって を 用 いると 連 続 時 間 の 状 態 方 程 式 は & () t f% ( () t t) u () t (5-6-) の 形 で 表 すことができる ただし 関 数 f% は 5-4 項 に 示 した INS の 航 法 誤 差 に 関 する 方 程 式 から 定 まる 既 知 のベクトル 関 数 である.また u () t は 6 次 元 の 正 規 性 白 色 雑 音 ベクトルであ り u () t [ u () t u () t u () t u () t u () t u () t u () t u () t r r θ θ h u () t u () t u () t u () t u () t u () t u () t u ()] t b b b d d d u [ ( t )] γ β u [ ( tu ) ( t)] U ( t) { da U ( t) U ( t) U ( t) U ( t) U ( t) r r θ U () t U () t U () t U () t U () t θ h b b U ( t) U ( t) U ( t) U ( t) U ( t) U ( t) b d d d とする ここで 式 (5-6-)を 観 測 データである GPS 信 号 の 更 新 間 隔 Δt でオイラー 近 似 し 離 散 化 すると ( k ) ( k) lm f% ( ( k) k) u( k) (5-6-) Δt Δ t γ β } 65
( k ) ( k) f% ( ( k) k) Δ t q ( k) f( ( k) k) q ( k) となり 離 散 時 間 系 の 状 態 方 程 式 が 得 られる ただし q ( k) u ( k) Δ t f ( ( k) k) ( k) f% ( ( k) k) Δt である また q ( k) は 平 均 が 共 分 散 行 列 (5-6-3) q [ ( kq ) ( k)] Q( k) であるような 正 規 性 白 色 雑 音 ベクトルである { da Q ( k) Q ( k) Q ( k) Q ( k) Q ( k) r r θ Q ( k) Q ( k) Q ( k) Q ( k) Q ( k) θ h b b Q ( k) Q ( k) Q ( k) Q ( k) Q ( k) Q ( k) b d d d γ β } ) 観 測 方 程 式 oosel couled 方 式 では GPS より 得 られる 位 置 速 度 と INS が 出 力 する 位 置 速 度 との 差 を 観 測 量 として 観 測 方 程 式 を 導 出 する すなわち INS演 算 誤 差 INS演 算 値 GPS観 測 量 として INS 演 算 誤 差 を 観 測 量 とすることを 考 える ただし GPS 位 置 は 座 標 系 GPS 速 度 は 座 標 系 で 観 測 されるため 上 記 の 関 係 に 当 てはめるには GPS 観 測 量 を 航 法 演 算 座 標 系 である 座 標 系 に 適 切 に 変 換 する 必 要 がある そこで 以 下 では 方 位 角 が 未 知 であるために 生 じる 座 標 変 換 の 誤 差 を 考 慮 した 位 置 速 度 観 測 量 について 述 べる ここで INS から 得 られる 諸 量 には GPS から 得 られる 諸 量 には を 各 変 数 の 右 肩 に 付 加 する 例 えば は INS から 得 られる 座 標 系 での X 軸 に 関 する 速 度 である また GPS データは 離 散 的 に 得 られ データ 更 新 間 隔 は Δt とする () 位 置 誤 差 観 測 量 INS と GPS より 得 られる 緯 度 λ と 経 度 ϕ の 差 を 位 置 誤 差 観 測 量 とする したがって この 位 置 誤 差 観 測 量 を 座 標 系 に 適 切 に 変 換 する まず 座 標 系 と 座 標 系 の 関 係 について 考 え る 座 標 系 での INS 演 算 の 緯 度 誤 差 Δ λ 経 度 誤 差 Δϕ は として GPS 位 置 観 測 量 を 基 準 とすることで 与 えられる Δ λ λ λ (5-6-4) Δ ϕ ϕ ϕ (5-6-5) 66
δ r 図 5-6- 緯 度 誤 差 と の 関 係 緯 度 誤 差 Δλ と δ r の 関 係 を 図 5-6- に 示 す Δλ により 移 動 体 の 真 の 位 置 O で 座 標 系 から 座 標 系 へ 座 標 変 換 を 行 うと O を 原 点 とする 座 標 系 ( X Y Z )が 構 成 される し かし この 座 標 系 は 実 際 の 位 置 O で 構 成 されるべき 座 標 系 とは 異 なる すなわち 緯 度 誤 差 が Δλ である 場 合 INS 演 算 によって 構 成 される 局 地 水 平 面 ( 座 標 系 )は 真 の 局 地 水 平 面 に 対 してY 軸 まわりに 関 して δ r だけ 傾 いてしまうことが 分 かる したがって 次 式 の 関 係 を 得 る Δ λ δ r (5-6-6) 次 に 経 度 誤 差 Δϕ が INS 演 算 によって 構 成 される 局 地 水 平 面 に 及 ぼす 影 響 について 考 える 緯 度 誤 差 の 場 合 と 同 様 に 経 度 誤 差 Δϕ により INS 演 算 によって 構 成 された 座 標 系 は 真 の 座 標 系 とは 異 なる( 図 5-6-3) Δϕ は X 軸 まわりの 回 転 角 であり 座 標 系 は 真 の 局 地 水 平 面 に 対 して X 軸 まわりに 関 して δ r だけ 傾 くことが 分 かる しかし X 軸 と X は 赤 道 上 以 外 は 平 行 な 関 係 ではないので Δ ϕ δ r の 関 係 は 成 り 立 たない したがって 経 度 誤 差 によ る δ r を 考 慮 する 際 Δϕ の 物 理 量 を 適 切 に 変 換 する 必 要 がある 図 5-6-4 に 示 すように δ r は 緯 度 λ を 用 いて 次 のように 表 すことができる δ r Δ ϕcosλ (5-6-7) ただし Δϕ による Z 軸 まわりの 誤 差 δ 定 する r は 方 位 角 誤 差 δα に 含 まれるものとして と 仮 67
図 5-6-3 座 標 系 と 真 の 座 標 系 の 関 係 δ r 図 5-6-4 経 度 誤 差 と の 関 係 以 上 より 座 標 系 での INS 演 算 の 位 置 誤 差 Δ λ Δϕ は 座 標 系 において δ r Δϕ cosλ δr Δλ δ r (5-6-8) と 示 され 更 に 座 標 系 に 変 換 すると δ r Δϕ cosλ δr Δλ δ r (5-6-9) と 表 すことができる 式 (5-6-9)をδα による 座 標 変 換 行 列 の 誤 差 δ を 考 慮 し 展 開 すると 68
δ r Δϕcosλ δr ( δ ) Δλ δ r cosα snα Δϕcosλ snα cosα λ Δ ( β)cos α ( γ)sn α ( β)sn α ( γ)cosα Δϕcosλ ( β)sn α ( γ)cos α ( β)cos α ( γ)snα λ Δ (5-6-) となる だだし γ sn δα β cosδα (5-6-) である したがって Δ ϕcosλcosα Δ λsn α δr [ Δϕcosλsnα Δλcos α] γ Δ [ ϕ cosλcosα Δλsn α] β Δ ϕcosλsnα Δλcosα δr [ Δ ϕcosλcosα Δλsn α] γ Δ [ ϕ cosλsnα Δλcos α] β となる これらをまとめ さらに GPS 観 測 雑 音 np np np を 考 慮 すると (5-6-) P r r ( ) ( ) P δr r r γ r r β n (5-6-3) P r r ( ) ( ) P δr r r γ r r β n (5-6-4) として 位 置 誤 差 に 関 する 観 測 量 を 得 る また 高 度 誤 差 δ h に 関 しては h h P δ h n P (5-6-5) となる () 速 度 誤 差 観 測 量 GPS 速 度 ( 座 標 系 )を 座 標 系 に 座 標 変 換 し INS 速 度 との 差 を 速 度 観 測 量 とする したが って 方 位 角 誤 差 δα を 考 慮 し 方 位 角 をα δα として 座 標 系 と 座 標 系 の 関 係 を 示 す( 図 5-6-5) 69
図 5-6-5 GPS 速 度 の 座 標 系 への 変 換 これより GPS 速 度 を 基 準 とすると INS 演 算 速 度 誤 差 は δ ( δ ) δ (5-6-6) と 表 すことができる ここで 位 置 誤 差 観 測 量 の 場 合 と 同 様 にγ β を 用 いて 展 開 すると δ cosα snα N δ snα cosα δ D ( β)cos α ( γ)sn α ( β)sn α ( γ)cosα ( β)sn α ( γ)cos α ( β)cos α ( γ)snα となる したがって N D (5-6-7) δ Ncosα snα N N ( cosα sn αβ ) ( snα cos αγ ) (5-6-8) δ N snα cosα N N ( snα cos αβ ) ( cosα sn αγ ) (5-6-9) D δ (5-6-) となる さらに GPS 観 測 雑 音 n n n を 考 慮 すると δ β γ n (5-6-) 7
δ β γ n (5-6-) n δ (5-6-3) と 表 すことができ 速 度 誤 差 の 観 測 量 を 得 る () 観 測 方 程 式 式 (5-6-3)~(5-6-5) 式 (5-6-)~(5-6-3)をまとめ ベクトル 行 列 表 現 すれば 時 刻 k で の 観 測 方 程 式 は と 表 すことができる ただし ( k) H ( k) ( k) n ( k) (5-6-4) ( k) [ ( k) ( k) ( k) ( k) ( k) ( k)] P P P n ( k) [ n ( k) n ( k) n ( k) n ( k) n ( k) n ( k)] P P P r r r r r r r r H ( k) 6 6 (5-6-5) であり 平 均 で 共 分 散 行 列 が [ n ( k) n ( k)] R ( k) のように 与 えられる 正 規 性 白 色 雑 音 と する 7
5-7 htl couled 方 式 慣 性 航 法 複 合 演 算 において 慣 性 航 法 演 算 の 誤 差 を 補 正 する 手 法 の htl couled 方 式 に ついて 説 明 する () 機 能 htl couled 方 式 のモデル 式 を 算 出 する () アルゴリズム GPS の 測 位 結 果 ( 位 置 速 度 )から 観 測 量 を 構 成 する oosel 方 式 に 対 し htl 方 式 は コード 擬 似 距 離 や 搬 送 波 位 相 積 算 値 から 直 接 観 測 量 を 構 成 し INS 航 法 誤 差 を 推 定 し 補 正 する 方 式 である したがって oosel 方 式 では 移 動 体 に 設 置 された 受 信 機 の 3 次 元 座 標 と 受 信 機 時 計 誤 差 を 得 るために 4 機 以 上 の 可 視 衛 星 が 必 要 であったが htl 方 式 ではコード 擬 似 距 離 搬 送 波 位 相 積 算 値 が 得 られれば 観 測 量 を 構 成 することができるため 機 以 上 の GPS 衛 星 が 観 測 できれば INS 航 法 誤 差 を 推 定 することが 可 能 である htl 方 式 のシステム 構 成 図 を 図 5-7- に 示 す 以 下 では コード 擬 似 距 離 搬 送 波 位 相 積 算 値 の 観 測 量 について 述 べ INS 演 算 誤 差 を 状 態 変 数 とした 慣 性 航 法 複 合 演 算 における 観 測 方 程 式 を 導 出 する また 以 下 では 簡 単 のために コード 擬 似 距 離 については /A コード 搬 送 波 位 相 積 算 値 については 波 のみを 用 いる 場 合 について 説 明 するが 波 やその 他 の 周 波 数 帯 についても 同 様 に 扱 うことができる INS INS 位 置 速 度 位 置 速 度 姿 勢 方 位 GPS GPS センサ 誤 差 補 正 値 カルマン フィルタ 搬 送 波 位 相 ドップラ 周 波 数 図 5-7- htl 方 式 のシステム 構 成 7
表 5-7- htl 方 式 における 状 態 変 数 ( λ は 帯 の 波 長 m は 受 信 衛 星 数 ). δ r : X 軸 まわりの 位 置 誤 差. d : X 軸 ジャイロバイアス. : Y 軸 ジャイロバイアス δ r : Y 軸 まわりの 位 置 誤 差 3. 3. δ : X 軸 方 向 の 速 度 誤 差 4. d : Z 軸 ジャイロバイアス 4. d δ : Y 軸 方 向 の 速 度 誤 差 5. γ : snδα 5. δθ : X 軸 まわりの 姿 勢 角 誤 差 6. β : (cosδα ) 6. δθ : Y 軸 まわりの 姿 勢 角 誤 差 7. λn ku. δ h : Z 軸 方 向 の 高 度 誤 差 M M 8. δ : Z 軸 方 向 の 速 度 誤 差 M M 9. b : X 軸 加 速 度 バイアス M M M. b : Y 軸 加 速 度 バイアス M M M : 整 数 値 バイアス( 距 離 の 単 位 ) M M. b : Z 軸 加 速 度 バイアス M m λn ku : 整 数 値 バイアス( 距 離 の 単 位 ) () 状 態 方 程 式 htl 方 式 における INS 航 法 誤 差 の 状 態 変 数 を 表 5-7- に 示 す 本 方 式 では oosel 方 式 時 の 未 知 量 に 加 え 表 5-7- 中 の 6 番 以 降 に 示 される 整 数 値 バイアス m N λn ku λn ku (5-7-) が 状 態 変 数 に 追 加 される すなわち htl 方 式 における 状 態 変 数 ベクトルは である いま N に 関 して N (5-7-) N ( k ) F N ( k) q ( k) (5-7-3) N N とモデル 化 する ただし FN は 既 知 の ( m ) ( m ) 行 列 qn は m 次 元 の 正 規 性 白 色 雑 音 である このとき htl 方 式 における 状 態 方 程 式 は ( ) ( ( ) ) ( ) k f k k q k ( k ) F ( k) ( k) N N q N N (5-7-4) となる 73
() コード 擬 似 距 離 および 搬 送 波 位 相 積 算 値 既 知 受 信 機 座 標 未 知 受 信 機 座 標 をそれぞれ [ k( k) k( k) k( k )] [ u( k) u( k) u( k)] と し 両 受 信 機 で 共 通 に m 個 の 衛 星 を 捉 え 基 準 衛 星 を とすると その 組 み 合 わせは 以 下 のよ うに 表 せる ( j) {()(3)(4) ( m)} このとき 独 立 な 二 重 差 は( m ) 組 得 られ コード 搬 送 波 位 相 の 観 測 方 程 式 はそれぞれ 以 下 のように 表 すことができる Δ A( k) h( η( k)) na( k) (5-7-5) Δ ( k) h( η( k)) N n ( k) (5-7-6) ただし Δ ( k) [ R ( k) R ( k) R ( k)] Δ 3 m A ku ku ku 3 m ( k) [ λϕ ( ) ( ) ( )] ku k λϕku k λϕku k η( k) [ u( k) u( k) u( k)] 3 m na ( k) [ na ku ( k) na ku ( k) na ku ( k)] 3 m n ( k) [ n ku ( k) n ku ( k) n ku ( k)] とし N を 整 数 値 バイアス λ を 搬 送 波 周 波 数 の 波 長 とする このとき 観 測 雑 音 ベクトル n A( k ) は 平 均 共 分 散 行 列 R n A ( k) は 平 均 共 分 散 行 列 R の 正 規 性 白 色 雑 音 と 仮 定 する ここで 下 添 え 字 のA は /A コード I は 帯 の 搬 送 波 を 意 味 する また ベク トル 関 数 h( η( k)) の 各 要 素 h ( η ( k )) ( j 3 m) は j j j h ( η( k)) { ρ ( k) ρ ( k)} { ρ ( k) ρ ( k)} j { u k u k ( ( k) ( k)) ( ( k) ( k)) ( ( k) ( k)) { u u u ( ( k) ( k)) ( ( k) ( k)) ( ( k) ( k)) k k k ( ( k) ( k)) ( ( k) ( k)) ( ( k) ( k)) j j j u u u j j k k k ( k)) ( ( k) ( k)) j ( k( ) ( )) ( k( ) } k } (5-7-7) となる この 場 合 式 (5-7-6)の 搬 送 波 位 相 積 算 値 Δ は 距 離 の 単 位 で 表 現 されていることに 注 意 する 式 (5-7-7)より ベクトル 関 数 h( η( k)) は 非 線 形 であるので 線 形 近 似 を 行 う そこで h( η( k)) を INS 演 算 値 のまわりでテーラー 展 開 し 次 の 項 までの 近 似 を 行 うことで INS 航 法 誤 差 における 観 測 方 程 式 を 導 出 する INS 位 置 ( 単 位 [m])を r ( k) [ ( k) ( k) ( k)] とする と h( η( k)) h( r ( k)) H% ( k)( η( k) r ( k)) (5-7-8) となる ただし 下 添 え 字 の は 地 手 系 直 交 座 標 系 である 座 標 系 を 意 味 する また 74
h( ( k)) Hk % η ( ) ( k) η η r ( k) ( k) h( η( k)) h( η( k)) h( η( k)) ( k) ( k) ( k) h ( η( k)) h ( η( k)) h ( η( k)) ( k) ( k) ( k) M M M h ( η( k)) h ( η( k)) h ( η( k)) m m m ( k) ( k) ( k) h ( η( k)) ( ( k) ( k)) j ( ( k) ( k)) ( k) ( k) j ρ ( k) j ρ h ( η( k)) ( ( k) ( k)) j ( ( k) ( k)) ( k) ( k) j ρ ( k) j ρ h ( η( k)) ( ( k) ( k)) j ( ( k) ( k)) ( k) ( k) j ρ ( k) j ρ (5-7-9) (5-7-) (5-7-) (5-7-) とする 式 (5-7-5) (5-7-6) (5-7-8)より 線 形 化 されたコード 擬 似 距 離 搬 送 波 位 相 積 算 値 の 観 測 方 程 式 ( k) ( ) ( ( )) A ΔA k h r k Hk % ( )( η( k) r ( k)) na( k) Hk % ( )( δ r ( k)) n ( k) A (5-7-3) ( k) ( k) h( ( k)) Hk % ( )( η( k) r ( k)) N n ( k) Hk % ( )( δ r ( k)) N n ( k) Δ r (5-7-4) を 得 る ただし δ r ( k) は 座 標 系 における 位 置 誤 差 [m]を 表 す ここで 式 (5-7-3) (5-7-4) の 観 測 方 程 式 を 慣 性 航 法 複 合 演 算 に 適 用 するため r ( k) を 航 法 演 算 座 標 系 である 座 標 系 の 位 置 誤 差 [rad]に 変 換 することが 必 要 である まず δ r を 座 標 系 における 位 置 誤 差 [rad]に 変 換 することを 考 える ただし 省 略 のため 時 刻 の 表 記 k を 省 略 して 表 記 する ここで 座 標 変 換 行 列 を 用 いて 座 標 系 における 位 置 誤 差 [m]に 変 換 すると δ δ r δ r δ δ (5-7-5) δ となる ただし 座 標 変 換 行 列 は 緯 度 λ 経 度 ϕ を 用 いて 75
cosϕ snϕ sn λ cos λ snϕ cosϕ cosλ snλ cosϕsn λ snϕ cosϕcos λ snϕsn λ cosϕ snϕcos λ cos λ sn λ (5-7-6) とする 次 に 弧 度 法 を 用 いて 式 (5-7-5)を 変 換 すると 以 下 のようになる ( R h) Δλ δ ( R h) ϕ r Δ (5-7-7) δ ただし R は 曲 率 半 径 であり Δ λ Δϕ はそれぞれ 緯 度 誤 差 経 度 誤 差 を 表 す ここで 緯 度 誤 差 経 度 誤 差 と 座 標 系 の 関 係 について 考 える まず 緯 度 誤 差 Δλ と δ r の 関 係 を 図 5-7- に 示 す Δλ により 移 動 体 の 真 の 位 置 O で 座 標 系 から 座 標 系 へ 座 標 変 換 を 行 うとO を 原 点 とする 座 標 系 ( X Y Z )が 構 成 される しかし この 座 標 系 は 実 Δλ である 場 合 際 の 位 置 O で 構 成 されるべき 座 標 系 とは 異 なる すなわち 緯 度 誤 差 が INS 演 算 によって 構 成 される 局 地 水 平 面 ( 座 標 系 )は 真 の 局 地 水 平 面 に 対 してY 軸 まわりに 関 して r だけ 傾 いてしまうことが 分 かる したがって 次 式 の 関 係 を 得 る δ Δ λ δ r (5-7-8) 図 5-7- 緯 度 誤 差 とδ の 関 係 r 76
図 5-7-3 座 標 系 と 真 の 座 標 系 の 関 係 ( 左 ) 経 度 誤 差 とδ r の 関 係 ( 右 ) 次 に 経 度 誤 差 Δϕ が INS 演 算 によって 構 成 される 局 地 水 平 面 に 及 ぼす 影 響 について 考 える 緯 度 誤 差 の 場 合 と 同 様 に 経 度 誤 差 Δϕ により INS 演 算 によって 構 成 された 座 標 系 は 真 の 座 標 系 とは 異 なる( 図 5-7-3( 左 )) Δϕ は X 軸 まわりの 回 転 角 であり 座 標 系 は 真 の 局 地 水 平 面 に 対 して X 軸 まわりに 関 して δ r だけ 傾 くことが 分 かる しかし X 軸 と X は 赤 道 上 以 外 は 平 行 な 関 係 ではないので Δ ϕ δ r の 関 係 は 成 り 立 たない したがって 経 度 誤 差 によるδ r を 考 慮 する 際 Δϕ の 物 理 量 を 適 切 に 変 換 する 必 要 がある 図 5-7-3( 右 )に 示 すように δ r は 緯 度 λ を 用 いて 次 のように 表 すことができる δ r Δ ϕcos λ (5-7-9) したがって 以 下 のような 関 係 が 得 られる したがって 式 (5-7-7)は δ r Δλ δ r Δ ϕ (5-7-) cos λ δ δ r ( R h) δ r ( R h) δ r δ r (5-7-) cos λ δ r となり δ r を 座 標 系 における 位 置 誤 差 [rad]に 変 換 することができた 更 に 座 標 系 に 変 換 するとδ は 以 下 のように 表 すことができる r δ r δr δr (5-7-) δr A 77
ただし A ( R h) R h cos λ (5-7-3) cosα snα snα cosα (5-7-4) である したがって 式 (5-7-3) 及 び 式 (5-7-4)は A ( k) Hk % ( ) ( k ) ( k ) ( k) δ ( ) ( ) A r k k Hk ˆ ( ) δ r ( k ) n ( k ) A (5-7-5) ( k) Hk % ( ) ( k ) ( k ) ( k) δ r ( k) N ε( k) Hk ˆ ( ) δ r ( k ) N n ( k ) A (5-7-6) となる ただし Hk ˆ ( ) の 各 要 素 は 省 略 のため 時 刻 の 表 記 k を 省 いて 以 下 のように 表 す Hk ˆ ( ) [ a( k) b( k) c ( k)] (5-7-7) a ( R h) h h ϕ ϕ α cos λ (5-7-8) h h h j j j ( R h) cosϕ snϕ snλ cosλ snα j j ( k) sn cos cos j b ( R h) h h ϕ ϕ α cos λ (5-7-9) h h h j j j ( R h) cosϕ snϕ sn λ cos λ cosα j j ( k) sn cos sn j c h h h ϕ ϕ λ λ j j j ( k) cos sn cos sn j (5-7-3) 78
() 観 測 方 程 式 以 上 より htl 方 式 の 慣 性 航 法 複 合 演 算 における 観 測 方 程 式 は ( k) H ( k) n ( k) A INS ( m ) ( m) A W ( k) H ( ) ( ) ( ) ( ) INS I m m k k N n (5-7-3) ただし である H INS a ( k) b( k) c ( k) a ( k) b ( k) c ( k) ( k) M M M a ( k) b ( k) c ( k) m m m ( m ) 4 ( m ) 9 (5-7-3) 以 上 より htl 方 式 の 場 合 は 式 (5-7-4) (5-7-3)に 拡 張 カルマンフィルタ 等 を 適 用 するこ とで INS の 誤 差 を 推 定 し 複 合 航 法 を 行 うことが 可 能 である (ⅳ) ドップラ 観 測 量 ここでは htl 方 式 の 複 合 航 法 システムにおいてドップラ 情 報 を 活 用 する 場 合 のシステ ムモデルについて 述 べる 衛 星 と 受 信 機 の 間 に 相 対 的 な 動 きがある 場 合 受 信 機 で 観 測 される 電 波 の 周 波 数 はドップラ 効 果 によって 変 化 する したがって 衛 星 から 送 信 される 搬 送 波 の 周 波 数 を f 衛 星 受 信 機 間 の 距 離 を r 電 波 の 伝 搬 速 度 をν s とすると 受 信 機 で 受 信 される 周 波 数 f R は ν s r& r& fr f f ν s ν s (5-7-33) となる ゆえに 送 信 周 波 数 と 受 信 周 波 数 の 差 は 次 のように 表 される ft r f f r& & (5-7-34) R ν s 一 般 に 受 信 機 では 離 散 的 に 観 測 が 行 われるため サンプリング 区 間 での 距 離 変 化 すなわち 平 均 のドップラシフト 量 が 観 測 される 実 際 には 受 信 機 衛 星 時 計 誤 差 電 離 層 対 流 圏 の 影 響 等 々が 加 わる したがって ドップラ 観 測 量 の 観 測 モデルは 式 (5-7-35)に 示 すような 擬 似 距 離 観 測 モデルを 時 間 微 分 して 求 めるのが 合 理 的 である λ ρu () t ru ( t t τu ) δiu () t δu () t c δtu() t δt ( t τu ) eu () t (5-7-35) ただし ru ( u ) ( u ) ( u ) δ I u : 電 離 層 屈 折 効 果 δ u : 対 流 圏 屈 折 効 果 δ t u : 受 信 機 時 計 誤 差 δ t : 衛 星 時 計 誤 差 e u : 雑 音 τ u : 衛 星 から 受 信 機 u への 電 波 伝 搬 時 間 である 上 式 を 微 分 して D とおくとドップラ 観 測 モデルは 次 のようになる 79
D δi& δ& c & & e& u r& u u u δtu δt u r& c t ε u δ u u (5-7-36) ただし u である ここで r& について 考 える εu δi& u δ& u cδ t& e& u (5-7-37) r r r r r r u u u u u u r& u & u & & u & & u & (5-7-38) u u u であるが すなわち ただし ru ( u ) ru ( u ) ru ( u ) r r r u u u u u u ru ( u ) ru ( u ) ru ( u ) r r r u u u r u u r s u (5-7-39) (5-7-4) (5-7-4) なる 関 係 がある したがって u u u u s (5-7-4) r u r u ru ru ru u u s u u u (5-7-43) u [ & u & u] s u & & & & (5-7-44) と 定 義 すると 式 (5-7-36)-(5-7-44)よりドップラ 観 測 式 は となる D c t& ε (5-7-45) s u u u δ u u (ⅴ) ドップラ 情 報 活 用 時 の 状 態 方 程 式 および 観 測 方 程 式 s 衛 星 の 速 度 は 航 法 メッセージより 既 知 であるので 式 (5-7-45)における 未 知 数 は 受 信 機 時 計 誤 差 の 変 化 率 cδ t& u である 受 信 機 時 計 の 精 度 は 使 用 する 受 信 機 に 依 存 するが ここでは 一 般 的 に 離 散 時 間 系 において cδtu( k ) FD cδtu( k) qd( k) (5-7-46) とモデル 化 できるものと 仮 定 する ただし F D は 既 知 の 係 数 u D は 正 規 性 白 色 雑 音 とする 8
したがって htl 方 式 でのドップラ 情 報 活 用 時 における 状 態 ベクトル および 状 態 方 程 式 は 次 のようになる D N cδ t u (5-7-47) ( ( ) ) ( ) ( k ) f k k q k ( ) N ( ) ( ) k F k N k N N q (5-7-48) cδ t u( k ) Fc D δt u( k) qd( k) 一 方 式 (5-7-45)に 示 した 速 度 は 基 本 的 に WGS-84 座 標 系 に 基 づいたものであるため 観 測 方 程 式 を 記 述 する 際 は 式 (5-7-)で 行 ったような 座 標 変 換 を 行 う 必 要 がある 前 述 のように 座 標 系 における INS 速 度 および INS 速 度 誤 差 の 間 には δ (5-7-49) ただし なる 関 係 がある また 座 標 変 換 行 列 δ δ δ δ (5-7-5) を 用 いると (5-7-5) u と 表 すことができ 式 (5-7-45) (5-7-5)および 式 (5-7-5)より D% D s u u u u δ ct& ε u δ u u δ u ε u cδ t& u (5-7-5) となる したがって 受 信 衛 星 数 を m とし m D D% u D% u (5-7-53) とすると htl 方 式 においてドップラ 情 報 を 利 用 する 場 合 の 観 測 方 程 式 は ( k) H ( ) ( ) ( ) ( ) A INS m m m k na k ( k) H INS I( m ) ( m ) m ( k) ( k) N n D( k) H ( ) ( ) DINS c ( m ) ( m ) m δtu k D k n (5-7-54) ただし 8
H DINS h h h D D D M M M M M M M m 8 (5-7-55) m m m h h h D D D ( k) h h h ( ) D D D u K m (5-7-56) m nd εu ε u (5-7-57) である 5-8 フィルタ 変 数 の 引 継 ぎ( 切 換 え 方 式 ) 慣 性 航 法 複 合 演 算 において 高 精 度 な 測 位 を 行 うために 5-6 節 と 5-7 節 で 説 明 した 複 合 方 式 の 効 率 的 な 切 換 えの 方 法 を 説 明 する () 機 能 oosel couled 方 式 と htl couled 方 式 のフィルタ 変 数 の 引 継 ぎを 行 う () アルゴリズム 運 用 時 の GPS 利 用 状 況 によって oosel couled 方 式 と htl couled 方 式 を 適 宜 切 換 えて 使 用 することによって より 効 果 的 な 複 合 航 法 を 行 う すなわち アンビギュイティが 決 定 している 場 合 :RK-GPS 方 式 により 高 精 度 な GPS 測 位 結 果 ( 位 置 速 度 )が 利 用 可 能 である アンビギュイティが 決 定 していない 場 合 :RK-GPS 方 式 では 十 分 な 精 度 の 測 位 結 果 が 得 られ ないが GPS 観 測 量 ( 擬 似 距 離 搬 送 波 位 相 ドッ プラ 等 )は 利 用 可 能 である GPS 衛 星 が 利 用 できない 場 合 :GPS が 全 く 利 用 できない の 3 つの 状 況 に 応 じて 表 5-8- に 示 すような 複 合 航 法 方 式 を 適 用 することとする 表 5-8- GPS 利 用 状 況 と 複 合 航 法 方 式 RK-GPS 利 用 状 況 RK-GPS 測 位 の 可 否 複 合 航 法 方 式 アンビギュイティ 決 定 oosel couled 方 式 アンビギュイティ 未 決 定 htl couled 方 式 使 用 不 可 慣 性 航 法 演 算 複 合 航 法 方 式 の 切 換 え 時 においては システムモデルの 変 更 が 必 要 となるため 拡 張 カルマ ンフィルタ 等 の 初 期 値 を 設 定 する 必 要 がある 以 下 にその 詳 細 を 示 す 8
() oosel couled 方 式 htl 方 式 の 切 換 え oosel couled 方 式 から htl couled 方 式 への 切 換 え 時 には 整 数 値 バイアス N N および 受 信 機 時 計 誤 差 cδ t u の 初 期 値 が 必 要 となる 例 えば 時 刻 j で 切 換 えが 行 われるとする と 時 刻 j までの 航 法 結 果 を 有 効 に 活 用 して 初 期 値 を 設 定 するために 以 下 のように N ˆ ( j j) N ˆ ( ) j j を 設 定 できる Nˆ ( ) ( ) % ( )( ˆ δ r ( )) (5-8-) j j j H j j j また cδ t u Nˆ ( j j ) ( j) H % ( j)( ˆ δ r ( j j)) (5-8-) に 関 しては 事 前 情 報 がないため 以 下 のように 設 定 する c ˆ δt ( j j ) (5-8-3) u () htl couled 方 式 oosel couled 方 式 の 切 換 え htl couled 方 式 から oosel couled 方 式 への 切 換 え 時 には 整 数 値 バイアス N N および 受 信 機 時 計 誤 差 cδ t u が 状 態 変 数 から 削 除 される したがって 新 たに 初 期 値 等 を 求 める 必 要 はなく 時 刻 j で 切 換 えが 行 われるとすると 時 刻 j での 予 測 推 定 値 を 初 期 値 として 使 用 することができる 83
5-9 拡 張 カルマンフィルタ 慣 性 航 法 演 算 処 理 における 拡 張 カルマンフィルタの 演 算 内 容 を 説 明 する () 機 能 拡 張 カルマンフィルタを 用 いて INS の 航 法 誤 差 を 推 定 する () アルゴリズム 拡 張 カルマンフィルタはシステムモデルに 含 まれる 非 線 形 関 数 を テーラー 展 開 による 線 形 化 によって 近 似 し カルマンフィルタのアルゴリズムを 適 用 するものである 拡 張 カルマンフ ィルタアルゴリズムを 以 下 に 示 す フィルタ 方 程 式 ˆk k f( ˆk k k) (5-9-) ˆkk ˆkk K k k Hkˆ kk (5-9-) カルマンゲイン Kk Pk k H k HkPk k Hk R k (5-9-3) 推 定 誤 差 共 分 散 行 列 P Fˆ P Fˆ Q k k k k k k k (5-9-4) P P K H P (5-9-5) kk kk k k kk ただし Fˆ ( k) は 次 式 とする Fˆ k f k k k ˆ kk (5-9-6) 初 期 値 ˆ ˆ (5-9-7) P Π { KK } da Π Π Π 6 (5-9-8) 以 上 のアルゴリズムより 得 られる 状 態 推 定 値 を INS の 演 算 の 各 ステップにおいて 補 正 する ことで 慣 性 航 法 複 合 演 算 を 実 現 する 84