Справочная информация

В последнее время смартфоны становятся все более популярными с каждым днем, поэтому я думаю, что у большинства людей есть смартфон. Смартфон имеет множество датчиков, таких как гироскоп, акселерометр, GPS и так далее. Используя эти датчики, многие приложения могут собирать информацию о поведении пользователя. Исходя из этого обстоятельства, я также думаю, что хочу разработать приложение, использующее датчик.
Но у меня нет Macbook и устройства Android. Поэтому для разработки приложения для смартфона мне придется купить Android-устройство или Macbook. С другой стороны, у меня есть jetson nano, поэтому я решил купить датчик BNO055, предоставленный adafruit, и разработать программное обеспечение с использованием BNO055 и jetson nano.
Я инженер-программист в автомобильной промышленности, поэтому это хороший проект по разработке системы регистрации данных с использованием BNO055 и jetson nano.

Оглавление

1.Требования
2.Датчик BNO055 9DoF
3.Проводка
4.Кодирование
5.Лабораторные испытания
6.Фактическое испытание автомобиля
7.Обсуждение
8.Заключение
9.Ссылка

1. Требования

• python 3.x
• adafruit_bno055
• busio
• плата
• потоковая передача
• время
• numpy
• pandas
• matplotlib
• os
• IPython
• ipywidgets
• дататайм
• Jetson nano B02 4GB
• Adafruit BNO055
• Провода для подключения компьютера и датчика

2. Датчик BNO055 9DoF

BNO055 может измерять вращательное движение по крену, тангажу и рысканию с помощью гироскопа и ускорение вперед, вбок и вертикально с помощью акселерометра. Также этот датчик может измерять данные о направлении с помощью компаса.[1]

• Абсолютная ориентация (вектор Эйлера, 100 Гц)
• Данные об ориентации по трем осям на основе сферы 360°.
• Абсолютная ориентация (кватернион, 100 Гц)
• Вывод кватерниона по четырем точкам для более точного манипулирования данными
• Вектор угловой скорости (100 Гц)
• Скорость «вращения» по трем осям в рад/с
• Вектор ускорения (100 Гц)
• Ускорение по трем осям (гравитация + линейное движение) в м/с^2
• Вектор напряженности магнитного поля (20 Гц)
• Трехосевое измерение магнитного поля в микротеслах (мТл)
• Вектор линейного ускорения (100 Гц)
• Три оси данных линейного ускорения (ускорение минус сила тяжести) в м/с^2
• Вектор гравитации (100 Гц)
• Три оси гравитационного ускорения (за вычетом любого движения) в м/с^2
• Температура (1 Гц)
• Температура окружающей среды в градусах Цельсия

Внешний вид BNO055 можно посмотреть по ссылкам ниже.
★Adafruit BNO055 внешний вид [2]

Этот датчик небольшой, и adafruits предоставляет библиотеку для python, которая называется Adafruits_CircuitPython. Библиотека, которая была закодирована на C-lang, также предоставляется ими.

★Adafruit_CircuitPython_BNO055[3]

★Adafruit_BNO055[4]

Кроме того, вы можете посмотреть демонстрацию по этой ссылке
★Adafruit BNO055 Demo[5]

3. Подключение

Для передачи данных от BNO055 к Jetson nano, необходимо соединить их с помощью GPIO.
Информация о GPIO Jetson nano находится здесь.
★Расположение контактов J41 Jetson nano[6]

Я подключил Jetson nano и BNO055 следующим образом.

4. Кодирование

Сначала я поделюсь своим кодом, опубликованным на GitHub.
measurement_system_app

Этот код реализован в Jupyter Notebook и кратко описывает каждую функцию.

4-1. Структура

Структура этой системы выглядит следующим образом.

• measurement_system_app.ipynb: Основной код
• Model/measurement.py: Функции для измерения данных с помощью BNO055
• Model/signalprocessing.py: Набор инструментов для обработки сигналов

Измеренные данные будут сохранены в папке data.

4-2. класс измерения

Этот класс предназначен для получения данных от BNO055. connect() служит для установления соединения с датчиком BNO055, а get_data() служит для выборки данных из BNO055. Обратите внимание, что BNO055 выдает эйлеровы углы в порядке рысканья, крена и тангажа.
Поэтому эта функция сохраняет углы Эйлера, перестраивая их в порядке Roll, Pitch и Yaw на основе системы координат транспортного средства ISO.
Также в этом классе есть функция, которая может вычислить угол Эйлера с помощью кватерниона. Это важный момент при использовании BNO055. BNO055 может напрямую выводить угол Эйлера, но ошибка может быть большой для вашего проекта.

• LSB является 16-битным, поэтому он склонен к возникновению ошибок.
• Значение Yaw нестабильно, если угол крена и тангажа больше ±45deg.

Для преодоления недостатков выгодно использовать кватернион, потому что кватернион может выражать все вращения.

Более подробную информацию можно найти в этом видео.
Эйлер (блокировка кардана) объяснение[7]

import os
path = os.getcwd()
from collections import deque
import numpy as np
import adafruit_bno055
import busio
import board

class MeasBNO055:

    def __init__(self, BNO_UPDATE_FREQUENCY_HZ=10, INTERVAL=1000):

        self.COLUMNS = ["Time","euler_x", "euler_y", "euler_z", "gyro_x", "gyro_y", "gyro_z", "gravity_x", "gravity_y", "gravity_z",
                        "linear_accel_x", "linear_accel_y", "linear_accel_z","accel_x", "accel_y", "accel_z",
                        "quaternion_1", "quaternion_2", "quaternion_3", "quaternion_4", 
                        "calibstat_sys", "calibstat_gyro", "calibstat_accel", "calibstat_mag",
                        "Roll", "Pitch", "Yaw"]

        self.BNO_UPDATE_FREQUENCY_HZ = BNO_UPDATE_FREQUENCY_HZ
        self.exepath = path + '/measurement_system'
        self.datapath = path + '/data'


        self.INTERVAL = INTERVAL
        self.INIT_LEN = self.INTERVAL // self.BNO_UPDATE_FREQUENCY_HZ

        self.Time_data = deque(np.zeros(self.INIT_LEN))# Time
        self.linear_accel_x_data = deque(np.zeros(self.INIT_LEN))# linear_accel_x
        self.linear_accel_y_data = deque(np.zeros(self.INIT_LEN))# linear_accel_y
        self.linear_accel_z_data = deque(np.zeros(self.INIT_LEN))# linear_accel_z
        self.gyro_x_data = deque(np.zeros(self.INIT_LEN))# gyro_x
        self.gyro_y_data = deque(np.zeros(self.INIT_LEN))# gyro_y
        self.gyro_z_data = deque(np.zeros(self.INIT_LEN))# gyro_z

        self.assy_data = None

        self.i2c, self.bno = self.connect()


    def connect(self):
        i2c = busio.I2C(board.SCL, board.SDA)# Access i2c using SCL and SDA
        bno = adafruit_bno055.BNO055_I2C(i2c)# Connect BNO055. Default: NDOF_MODE(12)
        #self.bno.use_external_crystal = True
        print("Established connection with BNO055")
        return i2c, bno

    def get_data(self, bno):
        '''
        Get data from new value from BNO055

        '''    
        euler_z, euler_y, euler_x= [val for val in bno.euler]# Euler angle[deg] (Yaw, Roll, Pitch) -> euler_x,euler_y,euler_z = (Roll, Pitch, Yaw)
        euler_x = (-1.0) * euler_x
        euler_y = (-1.0) * euler_y
        euler_z = euler_z - 180.0# 180deg offset
        gyro_x, gyro_y, gyro_z = [val for val in bno.gyro]# Gyro[rad/s]
        gravity_x, gravity_y, gravity_z = [val for val in bno.gravity]# Gravitaional aceleration[m/s^2]
        linear_accel_x, linear_accel_y, linear_accel_z = [val for val in bno.linear_acceleration]# Linear acceleration[m/s^2]
        accel_x, accel_y, accel_z = [val for val in bno.acceleration]# Three axis of acceleration(Gravity + Linear motion)[m/s^2]
        quaternion_1, quaternion_2, quaternion_3, quaternion_4 = [val for val in bno.quaternion]# Quaternion
        calibstat_sys, calibstat_gyro, calibstat_accel, calibstat_mag = [val for val in bno.calibration_status]# Status of calibration
        roll, pitch, yaw = self.calcEulerfromQuaternion(quaternion_1, quaternion_2, quaternion_3, quaternion_4)# Cal Euler angle from quaternion
        return euler_x, euler_y, euler_z, gyro_x, gyro_y, gyro_z, gravity_x, gravity_y, gravity_z,
                linear_accel_x, linear_accel_y, linear_accel_z, accel_x, accel_y, accel_z,
                quaternion_1, quaternion_2, quaternion_3, quaternion_4,
                calibstat_sys, calibstat_gyro, calibstat_accel, calibstat_mag,
                roll, pitch, yaw

    def calcEulerfromQuaternion(self, _w, _x, _y, _z):
        sqw = _w ** 2
        sqx = _x ** 2
        sqy = _y ** 2
        sqz = _z ** 2
        COEF_EULER2DEG = 57.2957795131
        yaw = COEF_EULER2DEG * (np.arctan2(2.0 * (_x * _y + _z * _w), (sqx - sqy - sqz + sqw)))# Yaw
        pitch = COEF_EULER2DEG * (np.arcsin(-2.0 * (_x * _z - _y * _w) / (sqx + sqy + sqz + sqw)))# Pitch
        roll = COEF_EULER2DEG * (np.arctan2(2.0 * (_y * _z + _x * _w), (-sqx - sqy + sqz + sqw)))# Roll
        return roll, pitch, yaw

Ниже приведены координаты датчика BNO055.

Также ниже приведена система координат транспортного средства ISO[8].

4-3. класс measurement_control

Этот класс для реализации интерактивной работы с использованием ipwidget на Jupyter notebook.
После выполнения этого класса появятся кнопки «Start/Stop», «Save» и «Calib».

• Старт/Стоп: запуск и остановка измерения
• Сохранить: Сохранить данные после предварительной обработки (меню предварительной обработки — смещение и фильтр низких частот).
• Calib: Подтверждение статуса калибровки. Эта функция может использоваться только в том случае, если измерение не начато.

5. Лабораторный тест

Цель этого теста — подтвердить следующие моменты.

• Система координат работает правильно или нет.
• Проверить точность измерений с помощью приложения, работающего на iphone.

5-1. Метод тестирования

Я оценил свой код, используя phyphox[9], который является приложением для смартфона.
Это приложение может измерять многие физические величины с помощью смартфона, очень полезное для меня.
Я установил датчик на iphone следующим образом.

После этого я перемещаю смартфон в каждом направлении координат. Форма сигнала — синусоида. Порядок движения: вперед / вбок / по вертикали / по крену / по тангажу / по рысканию.
Обратите внимание, что координаты датчика и координаты автомобиля не совпадают. Поэтому мой инструмент преобразует координаты датчика в систему координат автомобиля типа ISO. Частота дискретизации составляет 10 Гц.

Система координат iphone приведена ниже.

5-2. Результат лабораторных испытаний

Вот результат лабораторных испытаний. Синяя линия показывает данные приложения для iphone, а оранжевая — разработанного мной приложения для регистрации данных. Желтая линия — состояние калибровки BNO055, 3 — полностью откалиброван. Это значение просто справочное.

Тренд всех значений физики хороший. Кажется, что в определенной степени его можно использовать для анализа управляемости автомобиля.
Особенно хорошо видны данные гироскопа.
С другой стороны, разница в форме ускорения между оранжевой и синей линиями может быть разницей влияния на ускорение от движения гироскопа.
Например, если можно поместить датчик в центр тяжести, то боковое ускорение не будет зависеть от скорости крена, но реализовать это сложно. Поэтому боковое ускорение обычно зависит от скорости крена. Существуют некоторые методы компенсации, но они трудоемки.

Здесь представлены углы поворота в то же время, что и на предыдущих графиках (данные со смещением).

Также эти графики отличаются от вышеприведенных графиков.

Угол поворота, рассчитанный с помощью кватерниона, намного лучше, чем значение, которое непосредственно выводится из BNO055 как угол Эйлера.

6. Фактический тест автомобиля

В качестве следующего шага я измерил поведение автомобиля с помощью разработанного мной регистратора данных и phyphox.
Вот результат измерений.
Синяя линия показывает данные приложения для iphone, а оранжевая — разработанного мной регистратора данных.

6-1. Обзор сравнения (частота дискретизации: 10 Гц)

• Особенно скорость рысканья совпадает с данными phyphox.
• Абсолютное значение всех сигналов не совпадает между iphone и BNO055. Но тенденция всех сигналов хорошая.

6-2. Сравнение в точке 3

Эта точка является перекрестком, поэтому включает в себя начало движения автомобиля со скорости 0 км/ч и поворот.

• Абсолютное значение не совпадает между данными iphone и BNO055 в отношении ускорения вперед. Но тенденция хорошая
• Тенденция других сигналов хорошая
• Видно движение по углу продольного наклона во время ускорения.
• Эти графики также показывают боковое движение, движение по крену и рысканье при прохождении поворотов.

7. Обсуждение

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

Эта система может измерять данные с частотой дискретизации до 40 Гц. Но BNO055 имеет возможность измерять с частотой дискретизации 100 Гц[1].
Также phyphox может измерять гироскоп и ускорение с частотой выборки 100 Гц[10].
Поэтому я решил, что эта проблема связана с методом реализации.

7-1. Дополнительный эксперимент о времени выполнения

Проблема с частотой дискретизации связана с функцией get_data цикла while в методе запуска.
Цикл while управляет тактовыми импульсами этой системы на основе частоты выборки, но если время выполнения функции get_data() больше времени тактового импульса, то это приводит к падению выборки.

Структуру цикла while можно рассматривать как последовательную связь получения данных каждого датчика BNO055.

Поэтому я исследовал время, затрачиваемое на получение данных каждого датчика.
Для исследования прошедшего времени я использовал time.time(). Я также оценил время выполнения time.time(), как и время выполнения получения данных датчика.

Скажем, get_data() требует 0.021[s] для завершения всего процесса, это означает, что данное приложение не может реализовать измерение с частотой дискретизации 100 Гц.

С другой стороны, время выполнения каждого процесса для получения данных датчиков, таких как эйлер, гироскоп гравитации и т.д., составляет около 0.0055[с].
Поэтому, если get_data() может реализовать параллельную обработку для получения данных сенсора, эта проблема может быть решена.

В моем проекте функция get_data() просто получает физические данные с датчика. Каждая функция для получения данных занимает 0.0054 секунды в худшем случае. Также эти функции не включают процесс ввода/вывода.

7-2. Исследование параллельной обработки

Я оценил время выполнения приведенного ниже метода.
Допустим, время выполнения одного процесса, который обращается к данным датчика, например, гироскопа, составляет 0,005 секунды.
Я оценил:
[тайм-аут DIL составляет 0,005 секунды].

• одиночный процесс
• потоковая обработка 7 функций
• потоковая обработка 4 функций
• параллельная обработка 7 функций

[таймаут DIL составляет 0,0001 секунды]

• одиночный процесс
• потоковая обработка 7 функций
• потоковая обработка 4 функций
• параллельная обработка 7 функций

Это код для оценки.

import time
from time import perf_counter
import sys

import random

import numpy as np
from concurrent.futures import ThreadPoolExecutor, ProcessPoolExecutor

class BNO055:
    def __init__(self):
        self.val = 1
        self.name = 'bno055'


def wait_process(wait_sec):
    until = perf_counter() + wait_sec
    while perf_counter() < until:
        pass
    return

def get_euler(waittime):
    wait_process(waittime)
    x, y, z = 0.1, 0.1, 0.1
    return x, y, z

def get_gyro(waittime):
    wait_process(waittime)
    x,y,z = 2,2,2
    return x,y,z

def get_accel(waittime):
    wait_process(waittime)
    x, y, z = 1, 1, 1
    return x, y, z

def get_liner_acc(waittime):
    wait_process(waittime)
    x, y, z = 10, 10, 10
    return x, y, z

def get_gravity(waittime):
    wait_process(waittime)
    x, y, z = 5, 5, 5
    return x, y, z

def get_quaternion(waittime):
    wait_process(waittime)
    w, x, y, z = 0.1, 0.1, 0.1, 0.1
    return w, x, y, z

def get_calib_status(waittime):
    wait_process(waittime)
    w, x, y, z = 3, 3, 3, 3
    return w, x, y, z




def single_process(waittime):
    start = time.time()
    get_gyro(waittime)
    end = time.time()
    print('elepsed_time of single:{:.6f}[s]'.format(end-start))

def stream_process(waittime = 0.005):
    start = time.time()
    ex, ey, ez = get_euler(waittime)
    gx, gy, gz = get_gravity(waittime)
    ax, ay, az = get_accel(waittime)
    lax, lay, laz = get_liner_acc(waittime)
    rx, ry, rz = get_gyro(waittime)
    qw, qx, qy, qz = get_quaternion(waittime)
    a, b ,c, d = get_calib_status(waittime)
    end = time.time()
    print('elepsed_time of stream:{:.6f}[s]'.format(end-start))
    return ex, ey, ez, gx, gy, gz, ax, ay, az, lax, lay, laz, rx, ry, rz, qw, qx, qy, qz, a, b, c, d

def stream_process_less(waittime = 0.005):
    start = time.time()
    lax, lay, laz = get_liner_acc(waittime)
    rx, ry, rz = get_gyro(waittime)
    qw, qx, qy, qz = get_quaternion(waittime)
    a, b ,c, d = get_calib_status(waittime)
    end = time.time()
    print('elepsed_time of stream less:{:.6f}[s]'.format(end-start))
    return lax, lay, laz, rx, ry, rz, qw, qx, qy, qz, a, b, c, d
def get_all_data(params, waittime=0.005):
    bno = params[0]
    sensor = params[1]
    #print(bno)
    if sensor == 'euler':
        x, y, z = get_euler(waittime)
        return x, y, z

    elif sensor == 'gyro':
        x, y, z = get_gyro(waittime)
        return x,y,z

    elif sensor == 'linear_acc':
        x,y,z=get_liner_acc(waittime)
        return x,y,z

    elif sensor == 'gravity':
        x, y, z = get_gravity(waittime)
        return x,y,z

    elif sensor == 'acceleration':
        x, y, z =  get_accel(waittime)
        return x, y, z

    elif sensor == 'quaternion':
        w, x, y, z = get_quaternion(waittime)
        return w, x, y, z
    elif sensor == 'calib_status':
        a, b, c, d = get_calib_status(waittime)
        return a, b, c, d
    else:
        pass



def pararel_process(bno, n):
    start = time.time()
    sensors = ['euler', 'gyro', 'gravity', 'acceleration', 'linear_acc', 'quaternion', 'calib_status']
    #sensors = ['gyro', 'linear_acc', 'quaternion', 'calib_status']#  [(2, 2, 2), (10, 10, 10), (0.1, 0.1, 0.1, 0.1), (3, 3, 3, 3)]
    result = []
    #print(bno)
    #params = [bno, sensors]
    bno_list = [bno for i in range(len(sensors))]
    params = zip(bno_list,sensors)
    with ThreadPoolExecutor(max_workers=n) as e:
        ret = e.map(get_all_data, params, chunksize=1)
    sms_multi = [r for r in ret]
    for res in sms_multi:
        for i in range(len(res)):
            result.append(res[i])

    end = time.time()
    print('elepsed_time of pararel:{:.6f}[s]'.format(end - start))


import multiprocessing
def multiproc(bno):
    start = time.time()
    sensors = ['gyro', 'linear_acc', 'quaternion', 'calib_status']
    result = []
    print(bno)
    # params = [bno, sensors]
    bno_list = [bno for i in range(len(sensors))]
    params = zip(bno_list, sensors)
    with multiprocessing.Pool() as pool:
        ret = pool.map(get_all_data, params, chunksize=1)
    sms_multi = [r for r in ret]
    for res in sms_multi:
        for i in range(len(res)):
            result.append(res[i])#gyro/linear_accel/quat/calib_status
    end = time.time()
    print('elepsed_time:{:.6f}[s]'.format(end - start))
if __name__ == '__main__':


    waittime = 0.005
    bno = BNO055()
    worker = 7

    print(sys.getswitchinterval())  # GIL timeout default is 5ms
    single_process(waittime)# Single
    stream_process(waittime)# Stream processing
    stream_process_less(waittime)  # Stream processing with reducing access of sensor value
    pararel_process(bno, n=worker) # parallel processing using ThreadPoolExecutor
    print('----------Change DIL timeout---------------')
    sys.setswitchinterval(0.0001)# 0.1ms
    print(sys.getswitchinterval())
    single_process(waittime)# Single
    stream_process(waittime)# Stream processing
    stream_process_less(waittime)  # Stream processing with reducing access of sensor value
    pararel_process(bno, n=worker) # parallel processing using ThreadPoolExecutor

    #multiproc(bno)# multi process

Вот результат.

Согласно этой таблице, если таймаут DIL равен 0,005 секунды, то применение параллельной обработки не дает преимущества.
С другой стороны, если таймаут DIL установлен как 0.00001сек, параллельная обработка выгодна для сокращения времени выполнения.

После эксперимента я реализовал функцию параллельной обработки с помощью ThreadPoolExecutor для get_data(), но get_data() стала часто возвращать NULL. Кроме того, порядок данных не совпадал.

С другой стороны, на самом деле мне не нужен эйлеров угол, полученный из bno.euler. Потому что эта система вычисляет угол Эйлера, используя кватернион. Также мне нужно только линейное ускорение. Поэтому количество обращений к данным можно уменьшить с 7 до 4. Тогда get_data() станет быстрее. Влияние изменения таймаута DIL незначительно.
Уменьшив канал данных измерений, эта система смогла измерять данные с частотой дискретизации 50 Гц.
Частота выборки от 30 Гц до 50 Гц достаточна для анализа динамики автомобиля.

8. Заключение

• Я реализовал систему регистрации данных, используя Jetson nano и BNO055.
• Измеренные данные могут быть использованы для анализа динамики автомобиля.
• Кватернион полезен для расчета угла Эйлера.
• Практическая максимальная частота дискретизации для измерений составляет 50 Гц.
• Эту систему можно улучшить в плане более быстрого выполнения.

9. Ссылка

[1] Обзор BNO055
[2] Внешний вид Adafruit BNO055
[3] Adafruit_CircuitPython_BNO055
[4] Adafruit_BNO055
[5] Adafruit BNO055 Demo
[6] Jetson nano J41 Pin layout
[7] Эйлер (фиксация кардана) объяснение
[8] Системы координат в Automated Driving Toolbox
[9] phyphox
[10] phyphox База данных датчиков