Clever Geek Handbook

ウェイポイントの配列を使用したセンサー読み取り値のシミュレーション





出版構造







慣性航法センサーで機能するアルゴリズムをデバッグするには、これらの同じセンサーの読み取り値をシミュレートする必要がある場合があります。 たとえば、特定の状況をシミュレートするウェイポイントのデバッグシーケンスがあります。 機能があるGPSトラックを使用することも、逆に機能を持たないGPSトラックを使用することもできます。 私の場合、フィールドテストの結果はありますが、ボードの準備はまだできていません（生産段階）-何かする必要があります。





ロール予約



移動するとき、3Dポイントは移動軸に対する体の位置に関する情報を提供しないことにすぐ注意してください。 スレッドがパスのポイント間に引き伸ばされ、オブジェクトがビーズであると想像すると、スレッドに沿って明確に移動し、移動軸の周りを自由に回転できます。 速度ベクトルを動きの方向として使用し、結果として警告の予約を記憶します。







GPSトラックの準備



ソースデータとしてGPSトラックがある場合は、最初に準備する必要があります。 既存のファイルを、データを取得できる形式に変換する必要があります。 GPXに変換しました（内部はXMLであるため）。 



<trkpt lat="12.345678" lon="87.654321"> <ele>839</ele> <time>2013-05-09T11:24:28.776Z</time> <extensions> <mytracks c="194.9" s="9.25" /> </extensions> </trkpt> . . . <trkpt lat="12.345678" lon="87.654321"> <ele>837</ele> <time>2013-05-09T11:24:31.779Z</time> <extensions> <mytracks c="195.7" s="8.68" /> </extensions> </trkpt>


次に、使用可能なデータベース（MySQLなど）を取得し、プレートを作成して、結果のXMLからのデータを入力します。 XML形式は異なる場合があります-主なことは、緯度、経度、高度、および時間を見つけることです。 最初のラベル、たとえば「xml_src」を作成します。 ロードを容易にするために、すべての列は文字列になっています。







データを少しとかします。 便宜上、「ポイント」などの2つ目のプレートを作成します。 MySQLのコード： 

 insert into points (lat,lon,h,dt) SELECT cast(xx.lat AS DECIMAL(11, 6)) , cast(xx.lon AS DECIMAL(11, 6)) , cast(xx.ele AS DECIMAL(11, 6)) , cast(replace(replace(xx.`time`, "T", " "), "Z", "") AS DATETIME) FROM xml_src AS xx;


その結果、次のことができます。





次に、緯度と経度をメートルに変換します（ 「Entertaining Surveying」の記事を参照するか、MSSQLのデータベースツールを使用するか、 ShortestLineToメソッドを参照してください）。 次のようにメートルに変換します。 最初の点の座標はX = 0、Y = 0であると考えられます。最初の点を基準にして後続の各点の座標を考慮します。 最初にポイント間の距離を垂直方向に、次に水平方向にメートルで決定します。 距離を計算する関数は、記事「MySQLの地理的ポイント間の距離の決定」にあります。







時間を秒単位で変換して、最初の行で0秒になるようにします（残りから最初の行の値を減算するだけです）。



MySQLのクエリ
便宜上、距離をメートル単位で返す2つのポイントの座標に3番目のラベル「track」と関数「geodist」を作成します。 

 FUNCTION geodist(src_lat DECIMAL(9, 6), src_lon DECIMAL(9, 6), dst_lat DECIMAL(9, 6), dst_lon DECIMAL(9, 6) ) RETURNS decimal(11,3) DETERMINISTIC BEGIN SET @dist := 6371000 * 2 * asin(sqrt( power(sin((src_lat - abs(dst_lat)) * pi() / 180 / 2), 2) + cos(src_lat * pi() / 180) * cos(abs(dst_lat) * pi() / 180) * power(sin((src_lon - dst_lon) * pi() / 180 / 2), 2) )); RETURN @dist; END


次に、開始点を選択し、リクエストでその座標を使用します。 このポイントから次の各垂直および水平までの距離をメートルで測定します。 



 INSERT INTO track (x, y, z, dt) SELECT if(42.302929 > pp.lat, 1, -1) * geodist(42.302929, 18.891985, pp.lat, 18.891985) , if(18.891985 > pp.lon, -1, 1) * geodist(42.302929, 18.891985, 42.302929, pp.lon) , h , dt FROM points AS pp;




その結果、次のことができます。





次に、定期的にポイントの座標を取得する必要があります。 2番目のポイントが最初のポイントから4秒後に現れ、3番目のポイントが2秒後に現れ、次のポイントが1秒後に現れたという事実は私たちには合わない。 センサーをシミュレートしますか？ そして、定期的に値を測定します。



等間隔でポイントの座標を取得するには、補間を使用します。 補間ツールとして、1次元の3次スプラインを使用します。 Excelを手元に置いて、マクロを作成しました（ネタバレ参照）。 この段階で、各「センサー」が機能する周波数を決定します。 たとえば、すべての「センサー」は1秒あたり10回値を返します。 つまり、測定間隔は0.1秒です。

VBの3次スプライン 
 Public Sub interpolate() '------------------------ Dim i As Integer Const start_n As Integer = 0 Const n As Integer = 1718 Dim src_x(n) As Double Dim src_y(n) As Double Dim spline_x(n) As Double Dim spline_a(n) As Double Dim spline_b(n) As Double Dim spline_c(n) As Double Dim spline_d(n) As Double For i = start_n To n - 1 spline_x(i) = Application.ActiveWorkbook.ActiveSheet.Cells(i + 1, 1).Value spline_a(i) = Application.ActiveWorkbook.ActiveSheet.Cells(i + 1, 2).Value src_x(i) = spline_x(i) src_y(i) = spline_a(i) Next spline_c(0) = 0 Dim alpha(n - 1) As Double Dim beta(n - 1) As Double Dim a As Double Dim b As Double Dim c As Double Dim F As Double Dim h_i As Double Dim h_i1 As Double Dim z As Double Dim x As Double alpha(0) = 0 beta(0) = 0 For i = start_n + 1 To n - 2 h_i = src_x(i) - src_x(i - 1) h_i1 = src_x(i + 1) - src_x(i) If (h_i = 0) Or (h_i1 = 0) Then MsgBox ("!  " + CStr(i + 1) + "     X!   !") Exit Sub End If a = h_i c = 2 * (h_i + h_i1) b = h_i1 F = 6 * ((src_y(i + 1) - src_y(i)) / h_i1 - (src_y(i) - src_y(i - 1)) / h_i) z = (a * alpha(i - 1) + c) alpha(i) = -b / z beta(i) = (F - a * beta(i - 1)) / z Next spline_c(n - 1) = (F - a * beta(n - 2)) / (c + a * alpha(n - 2)) For i = n - 2 To start_n + 1 Step -1 spline_c(i) = alpha(i) * spline_c(i + 1) + beta(i) Next For i = n - 1 To start_n + 1 Step -1 h_i = src_x(i) - src_x(i - 1) spline_d(i) = (spline_c(i) - spline_c(i - 1)) / h_i spline_b(i) = h_i * (2 * spline_c(i) + spline_c(i - 1)) / 6 + (src_y(i) - src_y(i - 1)) / h_i Next '------------------------------- ' my Dim dx As Double Dim j As Integer Dim k As Integer Dim y As Double row_num = 1 For x = 0 To 3814 Step 0.1 i = 0 j = n - 1 Do While i + 1 < j k = i + (j - i) / 2 If x <= spline_x(k) Then j = k Else i = k End If Loop dx = x - spline_x(j) y = spline_a(j) + (spline_b(j) + (spline_c(j) / 2 + spline_d(j) * dx / 6) * dx) * dx Application.ActiveWorkbook.ActiveSheet.Cells(row_num, 3).Value = x Application.ActiveWorkbook.ActiveSheet.Cells(row_num, 4).Value = y row_num = row_num + 1 Next End Sub




ペアで内挿：

  1. 時間-X
  2. 時間-Y
  3. 時間-Z






結果は、「センサー」が配置されている「オブジェクト」のポイントの座標を持つテーブルです。 ポイント間の時間間隔は0.1秒です。 特定の点の座標の出現時間は、式t = n / 10によって計算されます。ここで、nは行番号です。







ベクトルの配列からクリロフ・オイラー角を取得する方法



以前の記事から飛行機の機首の動きを見てみましょう。









鼻を意味する点の座標は等しい：





3つのターンすべてを定義しましょう。 これを行うには、最初の回転の軸vと、軸の周りの最初の回転alfの角度を取得します。 ウェイポイントをベクトルの頂点とします。 次に、隣接するベクトルを乗算することで軸が取得されます。



v12 = v1 * v2 =（1; 0; 0）*（0; 1; 0）=（0; 0; 1）



角度は次のように計算されます。 

 Public Function vectors_angle(v1 As TVector, v2 As TVector) As Double v1 = normal(v1) v2 = normal(v2) vectors_angle = Application.WorksheetFunction.Acos(v1.x * v2.x + v1.y * v2.y + v1.z * v2.z) End Function


Alf12 =90。次に、取得した軸と角度に基づいてクォータニオンを作成します。 

 Public Function create_quat(rotate_vector As TVector, rotate_angle As Double) As TQuat rotate_vector = normal(rotate_vector) create_quat.w = Cos(rotate_angle / 2) create_quat.x = rotate_vector.x * Sin(rotate_angle / 2) create_quat.y = rotate_vector.y * Sin(rotate_angle / 2) create_quat.z = rotate_vector.z * Sin(rotate_angle / 2) End Function


最初のクォータニオンを取得します。



（w = 0.7071; x = 0; y = 0; z = 0.7071）



四元数から回転成分を取得します： 

  sqw = w * w sqx = x * x sqy = y * y sqz = z * z bank = atan2(2 * (w * x + y * z), 1 - 2 * (sqx + sqy)) altitude = Application.WorksheetFunction.Asin(2 * (w * y - x * z)) heading = atan2(2 * (w * z + x * y), 1 - 2 * (sqy + sqz))


最初のターンを決定した結果：コース= 90、ピッチ= 0、ロール= 0。



残りのターンでは、最初のターンの後、平面のローカル座標系がグローバル座標系と一致しなくなったため、カウントできません。 2番機の機首位置から3番までの2番めのターンを決定するには、最初に1番めのターンをキャンセルする必要があります。つまり、レポートシステムを元の場所に戻し、新しいベクターを展開します。 このためには、最初のターンの逆四元数を取得し、それをベクトルNo. 2およびNo. 3に適用する必要があります（逆四元数と四元数によるベクトルの回転を取得します- 記事を参照 ）。

最初の回転の逆四元数（Z軸に沿った反時計回りの回転）：



（w = 0.7071; x = 0; y = 0; z = -0.7071）



この四元数を適用した後、2番目と3番目のベクトルは等しくなります。

v2 =（x = 1; y = 0; z = 0）

v3 =（x = 0; y = 0; z = -1）







ローカル座標系をグローバル座標系と組み合わせると、上記の方法で2番目の四元数と2番目の回転の成分を計算できます。



（w = 0.7071; x = 0; y = 0; z = -0.7071）、

ヘディング= 0、ピッチ= 90、ロール= 0。



次に、ローカル座標系の完全な広がりを覚えておく必要があります。 これを行うには、個別のクォータニオンを作成し、ターンのクォータニオンをそれに乗算します。





次に、一連の完全なターンの四元数から、レポートシステムを結合するための逆四元数を取得します。



まとめると。 動きの方向を示すベクトルのリストでターンのコンポーネントを取得するには、次を実行する必要があります。 



 '  " " q_mul.w = 1 q_mul.x = 0 q_mul.y = 0 q_mul.z = 0 For row_n = M To N '     v1.x = Application.ActiveWorkbook.ActiveSheet.Cells(row_n - 1, 1).Value v1.y = Application.ActiveWorkbook.ActiveSheet.Cells(row_n - 1, 2).Value v1.z = Application.ActiveWorkbook.ActiveSheet.Cells(row_n - 1, 3).Value '     v2.x = Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 1).Value v2.y = Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 2).Value v2.z = Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 3).Value '        q_inv = myMath.quat_invert(q_mul) '    v1 = myMath.quat_transform_vector(q_inv, v1) '    '        (length(v1); 0; 0) –    v2 = myMath.quat_transform_vector(q_inv, v2) '    '     r12 = myMath.vecmul(v1, v2) '   alf = myMath.vectors_angle(v1, v2) '     q12 = myMath.create_quat(r12, alf) '         '    ypr = myMath.quat_to_krylov(q12) Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 4).Value = ypr.heading Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 5).Value = ypr.altitude Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 6).Value = ypr.bank '     q_mul = myMath.quat_mul_quat(q_mul, q12) '   Next


このExcelのマクロでは、ベクトルは最初の3列から読み取られます。 結果は4、5、6列に書き込まれます。



ジャイロスコープシミュレーション



ご存知のように、入力としてのポイントの座標は適切ではありません-速度ベクトルが必要です。



準備されたデータから速度と加速度を取得するのは簡単です。ポイント座標テーブルの隣接するライン間の差は速度であり、速度の隣接するライン間の差は加速度です。 測定の頻度を覚えておく必要があります。 この場合、1秒あたり10の「測定」があります。 これは、たとえば、メートル/秒の速度を取得するには、対応するセルの値に10を掛ける必要があることを意味します。







速度ベクトルを取得し、回転成分を取得します。







ジャイロスコープについて。 ジャイロスコープは角速度を示します。 等間隔のローカル座標系におけるターンのコンポーネント-これは角速度です。



角速度の取得に加えて、ノイズも追加する必要があります。 ジャイロスコープのノイズには、センサー固有のノイズ（均一な分布）と地球の回転の影響（「センサー」が宇宙にない場合）の2つの実体的な要素があります。 センサーのノイズレベルは仕様に記載されています（シミュレートされたセンサーのデータシートを調べます）。 センサーの解像度は16ビットで、4つの測定モードがあります。 各モードには独自の最大値があります：最初の±250º/ s、... 4番目の±2000º/ s。 最初のモードの感度は131 LSB /（º/ s）です。 これを度単位で計算するには、次の式を使用する必要があります。



感度= Sensitivity_LSB *最大/解像度= 131 *±250 /（2 ^ 16）= 131 *±250/65536 =±0.49972534º/秒



つまり、ノイズの量は1度以内です。 Excelの式：

=（RAND（）-0.5）/ N、ここでNは1秒あたりのシミュレートされた「測定」の数です。



地球は1時間あたり15度の速度で回転します。 これは1秒あたり0.0041667度であり、測定誤差よりもはるかに小さくなっています。 あなたのための楽しみはこれを計算できます。



地球の回転軸がZ軸と一致すると仮定し、体が赤道にあり、体のY軸が厳密に北（接線方向）に向けられているとします。 次に、ボディのX軸は回転の接線方向と一致します。 この場合、地球の回転軸とそれを伴う物体は、物体のZ軸と一致します。 北への私たちの体の幅の変化を視覚化します。 次に、回転軸は、新しい緯度の値に等しい度数だけ時計回りにX軸を中心に回転します。 北半球にシフトすると-これはプラス記号で、南に-マイナス記号です。 身体が空間内で自由に方向付けられている場合、自由落下加速ベクトルgが役立ちます。







ローカル座標系で地球の回転軸を計算するには、次のものが必要です。

  1. ベクトルgに北コンパス読み取りベクトルを乗算します。
  2. 結果のベクトルと角度に基づいてクォータニオンを作成します。 角度の値として、緯度を取ります。
  3. コンパスベクトル「北」を結果のクォータニオンに展開します。




各測定時の惑星による私たちの体の回転の値を取得するには、次のようにします。

  1. ソースデータの現在の緯度を使用して、惑星の回転軸のベクトルを計算します。
  2. 最後の測定以降に惑星がどれだけ回転したかを計算します。 この場合、1秒間に10回測定すると、0.0041667 / 10 = 0.00041667になります。
  3. 軸ベクトルとピボット角度に基づいてクォータニオンを作成します。
  4. Uターンの3つのコンポーネント（コース、ピッチ、ロール）を取得し、「センサー」の測定値に追加します。




一般的に、最後にテーブルを取得します。





最後の3列は「ジャイロ測定値」です。



自由落下加速度ベクトルと北方向



子午線の接線と方向「南」→「北」と一致する単位ベクトルCがあるとします。 グローバル座標系のY軸をベクトルC =（0、1、0）と一致させます。 そして、加速度ベクトルgはZ軸に沿って、ただし反対方向に向けられます。 g =（0、0、-1）。 その後、H =（1、0、0）として最初の瞬間に物体の速度ベクトルを明示的に指定すると、速度ベクトルの回転に基づいて取得されたすべてのターンは、ベクトルgおよびCの回転にも適用されます。これらのベクトルは、逆四元数によって回転する必要があります。 四元数の逆数。速度ベクトル（q_mul）の完全な回転をすべて蓄積します。



回転角を計算する手順に数行を追加するだけで十分です。 これは次のようになります。 

 Public Sub calc_all() Dim v1 As myMath.TVector Dim v2 As myMath.TVector Dim r12 As myMath.TVector Dim m12 As myMath.TMatrix Dim q12 As myMath.TQuat Dim q_mul As myMath.TQuat Dim q_inv As myMath.TQuat Dim ypr As myMath.TKrylov Dim alf As Double Dim row_n As Long Dim g As myMath.TVector Dim nord As myMath.TVector q_mul.w = 1 q_mul.x = 0 q_mul.y = 0 q_mul.z = 0 For row_n = 3 To 38142 v1.x = Application.ActiveWorkbook.ActiveSheet.Cells(row_n - 1, 1).Value v1.y = Application.ActiveWorkbook.ActiveSheet.Cells(row_n - 1, 2).Value v1.z = Application.ActiveWorkbook.ActiveSheet.Cells(row_n - 1, 3).Value v2.x = Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 1).Value v2.y = Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 2).Value v2.z = Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 3).Value gx = 0 gy = 0 gz = -1 nord.x = 0 nord.y = 1 nord.z = 0 q_inv = myMath.quat_invert(q_mul) v1 = myMath.quat_transform_vector(q_inv, v1) v2 = myMath.quat_transform_vector(q_inv, v2) g = myMath.quat_transform_vector(q_inv, g) nord = myMath.quat_transform_vector(q_inv, nord) r12 = myMath.vecmul(v1, v2) alf = myMath.vectors_angle(v1, v2) q12 = myMath.create_quat(r12, alf) ypr = myMath.quat_to_krylov(q12) Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 4).Value = ypr.heading Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 5).Value = ypr.altitude Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 6).Value = ypr.bank Application.ActiveWorkbook.ActiveSheet.Cells(row_n - 1, 7).Value = gx Application.ActiveWorkbook.ActiveSheet.Cells(row_n - 1, 8).Value = gy Application.ActiveWorkbook.ActiveSheet.Cells(row_n - 1, 9).Value = gz Application.ActiveWorkbook.ActiveSheet.Cells(row_n - 1, 10).Value = nord.x Application.ActiveWorkbook.ActiveSheet.Cells(row_n - 1, 11).Value = nord.y Application.ActiveWorkbook.ActiveSheet.Cells(row_n - 1, 12).Value = nord.z q_mul = myMath.quat_mul_quat(q_mul, q12) Next End Sub


最初の3列は速度ベクトルで、方向は最初のポイントで明確に示されています。 これは入力です。 次に3列-推定回転角度。 次に、方向ベクトル「北」と重力の加速度ベクトル。 そして最後に、3つの列がジャイロ読み取り値をシミュレートしています。







加速度計、コンパス、気圧計の読み取り値のシミュレーション



速度ベクトルに沿ったすべての動きがあるため、加速度も速度ベクトルに沿って方向付けられます-これはX軸に沿った方向を意味します。加速度センサーの読み取り値は、重力加速度、ノイズ、および適切な身体加速度の3つの知覚可能なコンポーネントで構成されています：



a =（x =長さ（a）、0、0）;

A = a +（9.8 / N）* g + rnd、

ここで、aは入力データから計算された加速度ベクトル、

Nは1秒あたりのシミュレートされた測定の数です。

gは重力の加速度ベクトル、

rndはノイズです。

ノイズの量は、シミュレートされたセンサーによっても異なります。 計算のために、仕様を調べ、同じ式を使用します。



感度= Sensitivity_LSB *最大/解像度= 16.384 *±2/65536 =±0.0005 g =±0.0049 m / s2。



次に、結果をわずかに悪化させ、単純な式で加速度センサーのシミュレーション読み取り値を表現できます。

Ax =長さ（a）+ gx * 9.8 / N +ランダム（0.01）-0.005

Ay = gy * 9.8 / N +ランダム（0.01）-0.005

Az = gz * 9.8 / N +ランダム（0.01）-0.005



仕様のコンパス測定範囲はテスラで示されています。 地球の磁場は、0.00005 T = 50 uTのレベルになります。 同じ式で感度を計算します：



感度= 15uT *±4800uT / 65536 =±1uT。



これにより、約4％のスプレッドが得られます。 コンパスの読み取り値は何らかの方法で「北」ベクトルに削減されるため、既に計算されたベクトルをコンパスの読み取り値として取得し、各コンポーネントを4％だけ「悪化」させることができます。

mx = Cx +ランダム（2）-1

my = Cy +ランダム（2）-1

mz = Cz +ランダム（2）-1。



バロメーターを使えば、さらに簡単です。 それらの測定値はメートル単位の高さになり、スプレッドは通常1メートル以内になります（仕様を参照してください。 Paで直接センサーを読み取る必要がある場合は、 気圧式を参照してください。 そしてそう



h = Z +ランダム（1）-0.5



それだけです。 誰がそれを好き-プラスを置くことを忘れないでください。 逆問題（センサーからウェイポイントまで）のトピックに関する記事が必要な場合は、コメントを記入してください。


More articles:


All Articles