pyTroykaIMU

Библиотека на Python 3 для Raspberry Pi, позволяющая управлять IMU-сенсором на 10 степеней свободы (Troyka-модуль) от Амперки.

IMU-сенсор (Inertial measurement unit — Инерционное измерительное устройство) — позволяет определить положение вашего девайса в пространстве. IMU-сенсор включает в себя:

• Гироскоп, определяющий угловую скорость вокруг собственных осей X, Y, Z.
• Акселерометр, определяющий величину ускорения свободного падения по осям X, Y, Z.
• Компас, определяющий углы между собственными осями сенсора X, Y, Z и силовыми линиями магнитного поля Земли
• Барометр, определяющий атмосферное давление, высоту над уровнем моря и температуру

Подключение

Подключение библиотеки для работы с I2C шиной SMBus

Наберите в командной строке Python

import smbus

Если интерпретатор выдал ошибку, то необходимо в установить дополнительную библиотеку через менеджер пакетов. Наберите в терминале:

pip install smbus

Так же необходимо включить в Raspberry Pi шину через настройки:

• Запустите меню raspi-config через терминал

  sudo raspi-config

• Включите I2C

• Подключение модуля

Для подключения большего большего количества Troyka модулей очень удобно использовать, например Troyka Pad

Пример использования

from madgwickahrs import MadgwickAHRS
from pytroykaimu import TroykaIMU

imu = TroykaIMU()

# Чтобы пример работал корректно, нужно предварительно получить 
# калибровочную матрицу для вашего модуля
# Как это сделать описано в разделе "Калибровка магнитометра"
calibration_matrix = [[0.983175, 0.022738, -0.018581],
                      [0.022738, 0.942140, -0.022467],
                      [-0.018581, -0.022467, 1.016113]]

# raw measurements only
bias = [962.391696, -162.681348, 11832.188828]

imu.magnetometer.calibrate_matrix(calibration_matrix, bias)

filter = MadgwickAHRS(beta=1, sampleperiod=1/256)

while True:
    filter.update(imu.gyroscope.read_radians_per_second_xyz(),
                  imu.accelerometer.read_gxyz(),
                  imu.magnetometer.read_calibrate_gauss_xyz())
    data = filter.quaternion.to_angle_axis()

    dataencode = str(data).encode('utf-8')
    if dataencode:
        print(data)

Состав библиотеки

• Трёхосный гироскоп L3G4200D покажет скорость вращения относительно собственных осей X, Y и Z
• Трёхосный магнитометр/компас LIS3MDL покажет напряженность магнитного поля относительно собственных осей. Это поможет определить направление на Север
• Трёхосный акселерометр LIS331DLH покажет ускорение относительно собственных осей X, Y и Z. Это поможет определить направление к центру Земли
• Барометр LPS331AP покажет атмосферное давление и поможет вычислить высоту над уровнем моря.

Калибровка магнитометра

TroykaIMU оснащен магнитометром lis3mdl, который существенно расширяет возможности определения положения в пространстве. Но для точности данных, любой магнитометр нужно калибровать и желательно в тех же условиях в которых он будет эксплуатироваться "на месте". На магнитометр действуют искажения которые принято делить на Hard Iron и Soft Iron. Ниже приведена картинка, иллюстрирующая суть этих искажений.

Hard Iron источники искажений магнитного поля воздействуя на магнитометр, придают некоторое смещение измеряемым значениям. Ликвидировать такое искажение очень просто: достаточно увеличить или уменьшить получаемые от прибора значения на величину смещения. Это смещение обозначают вектором bias.

Soft Iron искажение уже более сложное для вычислений и для его нивелирования вычисляется калибровочная матрица, которая позволяет преобразовать множество значений от прибора очень близко с сферойду. В нашей библиотеке эта матрица называется calibration_matrix.

Для вычисления этих параметров существуют различные методики. В нашем методе, на Raspberry Pi мы запустим сервер данных, чтобы на другом компьютере можно было получить значения измерений, записать их в файл, а затем с помощью программы magneto вычислить калибровочные параметры. Скачать программу можно по адресу тут https://sites.google.com/site/sailboatinstruments1/home

MatLab желательно использовать один из последних версий под управлением MacOS/Windows. Программу Magneto в MacOS можно запустить из под Wine.

Для сбора значений на вашей Raspberry Pi с подключенным IMU модулем замустите скрипт сервера калибровочных данных pyIMUCalibrationDataServer.py Также скачайте и установите на вашем компьютере MatLab для сбора значений и подготовки их в файл. Для этого в MatLab загрузите скрипт magneto.m После запуска скрипта в MatLab нужно некоторое время покрутить IMU датчик в руках(примерно 4 минуты) в разных плоскостях. MatLab будет визуализировать этот процесс и в идеале нужно получить облако значений похожее на сферойд.

Все данные сохраняются в файл calibration.txt, его и надо загрузить в программу magneto для расчета каллибровочных коэффициентов.

Калибровку нужно производить в единицах сенсора RAW.

Если возникли сложности, пройдем все поэтапно.

1. Загрузим с GitHub последнюю версию библиотеки в каталог с проектами. В каталоге calibration нужно запустить скрипт pyIMUCalibrationDataServer.py Если в консоли мы увидели 'waiting for connection...', то все идет как надо, сервер данных у нас заработал и перед переходом на рабочий компьютер узнаем IP адрес вашего микрокомпьютера Raspberry Pi с запущенным скриптом. Cервер выдает данные в RAW единицах датчика, и отправляет их по TCP/IP клиентской программе на другом компьютере с предустановленным заранее програмным обеспечением MatLab и magneto.

2. В каталоге pyTroykaIMU/calibration/ лежит скрипт для MatLab, который служит для сбора данных по сети с сервера Raspberry Pi, но перед запуском его нужно настроить. В параметре HOST укажите IP адрес сервера вашей Raspberry Pi.

   % Enter Your Raspberry Pi pyIMUCalibrationDataServer host
   HOST = '192.168.0.76';

3. IMU датчик должен быть на достаточно длинном кабеле и иметь надежное соединение, так как для сбора 3000 замеров, его придется вращать в разных плоскостях. Уберите всевозможные предметы, создающие дополнительные искажения магнитного поля в области вращения IMU сенсора, на расстояние не менее полуметра от датчика. Это акустические системы, блоки питания а также некоторые мониторы.

4. После того как все условия обеспечены и "железо" правильно настроено нажимаем на Run в MatLab и даблюдаем за постоением сферообразного облака в 3D графике. Вращать необходимо неспеша, и желательно в разных плоскостях. И если все сделано правильно. мы увидим окно "fugure1" и в каталоге со скрипптом MatLab будет необходимый файл calibration.txt

   

5. Далее нам понадобиться программа magneto. Выше указан адрес, по которому ее можно скачать бесплатно. В поле «Raw magnetic measurements» выбираем файл с исходными данными. В поле «Norm of Magnetic or Gravitational field» вводим величину магнитного поля Земли в точке нашей дислокации. Узнать это можно на разных WEB сервисах. В PyTroykaIMU есть библиотека igrf12py позволяющая самостоятельно расчитать эту величину в любой точке Земли. Мы воспользуемся этим сервисом http://www.geomag.bgs.ac.uk

   

Значение 49923 наноТесла переводим в Гауссы из соотношения 1 Тесла = 10 000 Гаусс. Получим общую напряженнность равную 0,49923 Гаусс. Окончательно 0,49923 умножаем на 6842 это множитель чувствительности сенсора для перевода из Гауссов в RAW единицы, получаем значение 3415,73166

Даже можно не узнавать величину напряженности в вашей местности, а подогнать этот коэффициент так чтобы значения главной диагонали были близки к единице. Это не повлияет на точность определения значений датчика так как матрица изменит лишь свой масштаб.

6. Жмем кнопку Calibrate и получаем:
   • значения смещения по всем трем осям: Combined bias (b);
   • и матрицу масштаба и ортогонализации: Correction for combined scale factors, misalignments and soft iron (A-1).

calibration_matrix = [[0.983175, 0.022738, -0.018581],
                      [0.022738, 0.942140, -0.022467],
                      [-0.018581, -0.022467, 1.016113]]

bias = [962.391696, -162.681348, 11832.188828]

После использования калибровочных значений

IGRF12py

Реализация класса стантартного международного геомагнитного аналитического поля (IGRF) позволяющего более эффективно использовать магнитометр на TroykaIMU модуле.

Про использования данных пожно почитать, например тут:

• http://geologyandpython.com/igrf.html
• http://geomag.nrcan.gc.ca/calc/calc-en.php

Обратная связь

По вопросам о работе библиотеки или обнаруженным ошибкам писать на selidimail@gmail.com