Имитация показаний датчиков с помощью массива точек пути

Структура публикации

• Оговорка про крен
• Подготовка 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'. Все столбцы для простоты загрузки делаем строковыми.

Немного причешем данные. Для удобства создадим вторую табличку, например 'points'. Код для 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;

В результате имеем следующее:

Затем переводим широту и долготу в метры (см. статью «Занимательная геодезия» или используем средства БД, например в MSSQL см. метод ShortestLineTo). Конвертировать в метры можно следующим образом. Мы считаем что координаты первой точки равны X = 0, Y = 0. Координаты каждой последующей точки считаем относительно первой. Определяем расстояние между точками сначала по вертикали, затем по горизонтали в метрах. Функция для рассчёта растояния есть в статье «Определение расстояния между географическими точками в MySQL».

Время переводим в секунды так, чтобы в первой строке получилось 0 секунд (просто вычитаем значение первой строки из остальных).

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;

В результате имеем следующее:

Теперь нам нужно получить координаты точки в равные промежутки времени. То, что вторая точка появились через 4 секунды после первой, третья через 2 секунды, а следующая через секунду – нас так не устраивает. Мы же собираемся имитировать датчики? А они измеряют значения в равные промежутки времени.

Для получения координат точки в равные промежутки времени используем интерполяцию. В качестве инструмента интерполяции используем одномерный кубический сплайн. У меня под рукой был Excel, я в нём макрос написал (см. спойлер). На этом этапе мы решаем с какой частотой будет работать каждый «датчик». Например, все «датчики» будут давать значения 10 раз в секунду. То есть интервал между измерениями равен 0,1 секунды.

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 — это номер строки.

Как из массива векторов получить углы Крылова-Эйлера

Возьмём перемещение носа самолёта из моей предыдущей статьи:

Координаты точки, означающей нос равны:

Давайте определим все три поворота. Для этого получим ось первого вращения 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 и №3 (получение обратного кватерниона и поворот вектора кватернионом – см. в статье).
Обратный кватернион первого разворота (поворот против часовой вдоль оси Z):

(w=0,7071; x=0; y=0; z=-0,7071)

После применения данного кватерниона второй и третий вектора равны:
v2 = (x=1; y=0; z=0)
v3 = (x=0; y=0; z=-1)

Когда локальная система координат совмещена с глобальной, можно вышеописанным способом вычислить второй кватернион и компоненты второго поворота:

(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) ' разворот вектора кватернионом
                                         ' в этом примере для всех шагов получим (1; 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-я вектора считываются с первых трёх столбиков. Результат пишется в 4, 5, 6 столбик.

Имитация показаний гироскопа

Как вы понимаете, координаты точек в качестве входных данных не годятся — нам нужны вектора скорости.

Получить скорость и ускорение из подготовленных данных легко – разница между соседними строчками таблицы координат точек – это скорость, разница между соседними строчками скорости – это ускорение. Только нужно помнить про частоту замеров. В данном случае у нас 10 «измерений» в секунду. Это значит, чтобы получить, например, скорость в метрах в секунду, нужно значение соответствующей ячейки умножить на 10.

Берём вектора скорости и получаем компоненты поворота.

Теперь про гироскоп. Гироскоп показывает угловую скорость. Поэтому берём разницу между соседними значениями углов и получаем угловую скорость.

Кроме получения угловой скорости, нужно ещё прибавить шум. В шуме гироскопа присутствует два ощутимых компонента — собственный шум датчика (равномерное распределение) и влияние вращения земли (если ваш «датчик» не в космосе). Уровень собственного шума датчика описан в спецификации (изучаем datasheet на имитируемый датчик). Мой датчик имеет разрешение 16 бит и имеет 4 режима измерения. У каждого режима свой максимум: у первого ±250º/сек,… у четвёртого ±2000º/сек. Чувствительность для первого режима 131 LSB/(º/s). Чтобы посчитать это в градусах, нужно воспользоваться формулой:

Чувствительность = Чувствительность_LSB * Максимум / Разрешение = 131 * ±250 / (2 ^ 16) = 131 * ±250 / 65536 = ±0,49972534 º/сек.

То есть величина шума в пределах одного градуса. Формула для Excel-я:
=(СЛЧИС()-0,5)/N, где N — это число имитируемых «измерений» в секунду.

Земля вращается со скоростью 15 градусов в час. Это 0,0041667 градусов в секунду, что сильно меньше погрешности измерения. Забавы ради можно рассчитать и это.

Предположим, что ось вращения Земли совпадает с осью Z. Предположим также, что тело находится на экваторе и ось Y тела ориентировано строго на север (по касательной). Тогда ось Х тела совпадает с касательной направления вращения. В этом случае ось вращения Земли и тела вместе с ним совпадает с осью Z тела. Зрительно представим смещение по широте нашего тела к северу. Тогда ось вращения повернётся вокруг оси Х по часовой стрелке на число градусов, равное значению новой широты. При смещении в северное полушарие — это знак плюс, в южное — знак минус. Когда тело свободно ориентировано в пространстве, нам поможет вектор ускорения свободного падения — g.

Для вычисления оси вращения Земли в локальной системе координат нужно:

1. Умножить вектор g на вектор показания компаса «на север».
2. Создать кватернион на основе получившегося вектора и угла. В качестве значения угла берём широту.
3. Развернуть получившимся кватернионом вектор компаса «на север».

Для получения значений разворота нашего тела планетой в момент каждого замера делаем:

1. Вычисляем вектор оси вращения планеты используя текущую широту из исходных данных.
2. Вычисляем на сколько повернулась планета с момента последнего измерения. В нашем случае при измерении 10 раз в секунду это будет 0,0041667 / 10 = 0,00041667.
3. Строим кватернион на основе вектора оси и угла разворота.
4. Получаем три компоненты разворота (курс, тангаж, крен) и прибавляем к показаниям «датчика».

В общем в итоге всего получим таблицу:

Последние три столбца — это «показания гироскопа».

Вектор ускорения свободного падения и направление «на север»

Предположим, что есть единичный вектор С, который совпадает с касательной к меридиану и направлением «юг» -> «север». Пусть ось Y нашей глобальной системы координат совпадает с вектором С = (0, 1, 0). А вектор ускорения свободного падения g направлен вдоль оси Z, но в противоположную сторону. g = (0, 0, -1). Тогда, если явно указать вектор скорости тела в начальный момент времени как Н = (1, 0, 0), то все развороты, полученные на основе вращений вектора скорости будут применимы и для вращений векторов g и С. Вращать эти вектора нужно обратным кватернионом. Обратный от кватерниона, который накапливает все совершённые повороты вектора скорости (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

g.x = 0
g.y = 0
g.z = -1

nord.x = 0
nord.y = 1
nord.z = 0

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
    
    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 = g.x
    Application.ActiveWorkbook.ActiveSheet.Cells(row_n - 1, 8).Value = g.y
    Application.ActiveWorkbook.ActiveSheet.Cells(row_n - 1, 9).Value = g.z
    
    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

Первые три столбца — вектор скорости, с явно указанным направлением в первой точке. Это входные данные. Затем три столбца — расчётные углы поворота. Далее вектор направления «на север» и вектор ускорения свободного падения. И наконец три столбика — это имитируемые показания гироскопа.

Имитация показаний акселерометра, компаса и барометра

Так как у нас всё движение происходит вдоль вектора скорости, то и ускорение у нас направлено так же вдоль вектора скорости — это значит вдоль оси X. Показания датчика ускорения складываются из трёх ощутимых компонентов: ускорение свободного падения, шумы и собственно ускорение тела:

a = (x = length(a), 0, 0);
A = a + (9,8 / N) * g + rnd,
где a — расчётный по входным данным вектор ускорения,
N — количество имитируемых измерений в секунду,
g — вектор ускорения свободного падения,
rnd — шум.
Величина шума снова зависит от имитируемого датчика. Для расчёта смотрим в спецификацию и используем ту же формулу:

Чувствительность = Чувствительность_LSB * Максимум / Разрешение = 16,384 * ±2 / 65536 = ±0,0005 g = ±0,0049 м/с2.

Тогда можно немножко ухудшить результат и выразить имитируемые показания датчика ускорения простыми формулами:
Ax = length(a) + gx * 9,8 / N + random(0,01) — 0,005
Ay = gy * 9,8 / N + random(0,01) — 0,005
Az = gz * 9,8 / N + random(0,01) — 0,005

Диапазон измерений компаса в спецификациях дают в Теслах. Магнитное поле Земли будет на уровне 0,00005 T = 50 uT. Посчитаем чувствительность по той же формуле:

Чувствительность = 15uT * ±4800uT / 65536 = ±1uT.

Это даёт разброс в пределах 4%. Поскольку показания компаса так или иначе приводятся к вектору «на север», то в качестве показаний компаса можно просто взять уже рассчитанный вектор и «ухудшить» каждую его составляющую на 4%:
mx = Cx + random(2) — 1
my = Cy + random(2) — 1
mz = Cz + random(2) — 1.

С барометром всё ещё проще. Их показания приводятся к высоте в метрах, разброс обычно в пределах метра (см. спецификацию, бывает меньше). Если нужны прям показания датчика в Па — см. барометрическую формулу. А так

h = Z + random(1) — 0,5.

Вот собственно и всё. Кому понравилось — не забывайте ставить плюсики. Если хотите статью на тему обратной задачи (от датчиков к точкам пути) — пишите в комментариях.

Автор: 1div0

Источник