Усовершенствование калибровки датчика ИМУ для сегмента алгоритма голени

В настоящее время я работаю над проектом, который направлен на интеграцию системы захвата движения IMU для передачи вибротактильной обратной связи через вибромоторы. Проект ориентирован на бегунов и может быть использован как для повышения производительности, так и для предотвращения травм. В данный момент я сосредоточен на реализации IMU для получения необходимых параметров, в данном случае – тибиальной акселерации. Я работал над алгоритмом для калибровки сенсора относительно сегмента, и у меня уже есть некоторые результаты, в надежности которых я не совсем уверен. Я надеялся, что кто-то сможет дать мне совет или сказать, если я упускаю что-то важное.

Некоторая релевантная информация:

• Сенсоры, которые я использую, это Movella Dot, которые уже предоставляют информацию об ориентации сенсора с использованием фильтра Калмана.

• Я размещаю сенсор на медиальной передней части голени (костной части).

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

Вот что у меня есть на данный момент:

import numpy as np
import pandas as pd
from scipy import signal
from scipy.spatial.transform import Rotation as R
from sklearn.decomposition import PCA
from sklearn.preprocessing import StandardScaler
import matplotlib.pyplot as plt

# Фильтр Баттерворта
def filtering(fs, cutoff, data):
    order = 2
    b, a = signal.butter(order, (2 * cutoff / fs), btype="low")
    filt_data = signal.filtfilt(b, a, data, axis=0)
    return filt_data

# Извлечение данных акселерометра, гироскопа и кватернионов
def get_data(filename):
    data = pd.read_csv(filename, delimiter=",", header=0)

    acc_data = data[['Acc_X', 'Acc_Y', 'Acc_Z']].values / 9.81  # Преобразование в м/с^2
    gyr_data = data[['Gyr_X', 'Gyr_Y', 'Gyr_Z']].values * (np.pi / 180)  # Преобразование в рад/с
    quaternions = data[['Quat_W', 'Quat_X', 'Quat_Y', 'Quat_Z']].values

    return acc_data, gyr_data, quaternions

# Поворот вектора с использованием кватерниона
def rotate_vector(quaternion, vector):
    r = R.from_quat(quaternion)  # Кватернион (w, x, y, z)
    rotated_vector = r.apply(vector)
    return rotated_vector

# Статическая калибровка: выравнивание по оси Z (НЕ НОРМАЛИЗОВАНО)
def static_calibration(filename):
    [acc_data, gyr_data, quaternions] = get_data(filename)

    fs = 120  # Частота дискретизации
    N = len(acc_data)

    # Поворот данных акселерометра в глобальную систему координат с использованием кватернионов
    a_global = np.zeros((N, 3))
    for i in range(120, N-1):
        q = quaternions[i]  # Получение кватерниона для этого образца
        a_global[i, :] = rotate_vector(q, acc_data[i, :])  # Поворот с помощью кватерниона

    # Фильтрация повернутых данных акселерометра
    a_global_filtered = filtering(fs, 15, a_global)

    # Оценка гравитационного вектора во время статического вертикального положения
    Tstart, Tend = 0, N  # Заменить на фактические индексы статического периода
    gravity_vector = np.mean(a_global_filtered[Tstart:Tend, :], axis=0)
    gravity_magnitude = np.linalg.norm(gravity_vector)
    sensor_z_axis = gravity_vector

    return sensor_z_axis, a_global_filtered, gyr_data

# Функциональная калибровка: Поворот сырых данных и применение PCA для определения оси X (НЕ НОРМАЛИЗОВАНО)
def functional_calibration(filename):
    [acc_data,_, quaternions] = get_data(filename)

    fs = 120
    N = len(acc_data)

    # Поворот данных акселерометра в глобальную систему координат с использованием кватернионов
    a_global = np.zeros((N, 3))
    for i in range(120, N-1):
        q = quaternions[i]  # Получение кватерниона для этого образца
        a_global[i, :] = rotate_vector(q, acc_data[i, :])  # Поворот с помощью кватерниона

    # Фильтрация повернутых данных акселерометра
    a_global_filtered = filtering(fs, 15, a_global)

    # Теперь применяем PCA к повернутым данным акселерометра, чтобы определить ось X
    scaler = StandardScaler()
    a_global_scaled = scaler.fit_transform(a_global_filtered)

    pca = PCA(n_components=1)
    pca.fit(a_global_scaled)
    sensor_x_axis = pca.components_[0]
    #sensor_x_axis /= np.linalg.norm(sensor_x_axis)  # Нормализация

    return sensor_x_axis, a_global_filtered

# Вычислить ось Y для обеспечения ортогональности и правосторонней системы
def gram_schmidt(sensor_x_axis, sensor_z_axis):
    # Применить процесс Грама-Шмидта для получения ортогональных векторов
    sensor_x_axis_corrected = sensor_x_axis - (np.dot(sensor_x_axis, sensor_z_axis)/np.dot(sensor_z_axis, sensor_z_axis)) * sensor_z_axis
    
    # Вычислить ось Y как векторное произведение Z и X, чтобы обеспечить правостороннюю систему
    sensor_y_axis = np.cross(sensor_x_axis_corrected, sensor_z_axis)
    
    # Получить ортонормированные векторы, нормализуя
    sensor_x_axis_norm = sensor_x_axis_corrected / np.linalg.norm(sensor_x_axis_corrected) 
    sensor_y_axis_norm = sensor_y_axis / np.linalg.norm(sensor_y_axis)
    sensor_z_axis_norm = sensor_z_axis / np.linalg.norm(sensor_z_axis)
    
    return sensor_x_axis_norm, sensor_y_axis_norm, sensor_z_axis_norm

# Объединить оси в матрицу преобразования и выровнять данные
def apply_transformation(a_global_filtered, gyr_data, transformation_matrix):
    aligned_acc = np.dot(a_global_filtered, transformation_matrix.T)
    aligned_gyr = np.dot(gyr_data, transformation_matrix.T)
    return aligned_acc, aligned_gyr

# Главная функция для выравнивания данных
def align_tibia_data(static_file, functional_file):
    sensor_z_axis, static_acc, gyr_data = static_calibration(static_file) # Ось Z нормализована
    sensor_x_axis, functional_acc = functional_calibration(functional_file)
    tibia_x_axis_norm, tibia_y_axis_norm, tibia_z_axis_norm = gram_schmidt(sensor_x_axis, sensor_z_axis)

    print(f"X & Z: {np.dot(tibia_z_axis_norm, tibia_x_axis_norm)}\nX & Y: {np.dot(tibia_y_axis_norm, tibia_x_axis_norm)}\nZ % Y: {np.dot(tibia_y_axis_norm, tibia_z_axis_norm)}")
    # Матрица преобразования
    transformation_matrix = np.vstack([tibia_x_axis_norm, tibia_y_axis_norm, tibia_z_axis_norm])

    # Выравнивание данных акселерометра и гироскопа
    aligned_acc, aligned_gyr = apply_transformation(functional_acc, gyr_data, transformation_matrix)

    plot_acceleration(functional_acc, aligned_acc)
    
    return aligned_acc, aligned_gyr, transformation_matrix


def plot_acceleration(a_global, aligned_acc, fs = 120):
    time = np.arange(len(a_global)) / fs

    # Построение графика сырых и выровненных ускорений
    fig, axs = plt.subplots(3, 1, figsize=(10, 8))

    for i, axis in enumerate(['X', 'Y', 'Z']):
        axs[i].plot(time, a_global[:, i], label=f'Сырой {axis}')
        axs[i].plot(time, aligned_acc[:, i], label=f'Выровненный {axis}')
        axs[i].set_xlabel('Время (с)')
        axs[i].set_ylabel(f'Ускорение {axis} (м/с^2)')
        axs[i].legend()
        axs[i].grid(True)

    plt.tight_layout()
    plt.show()


# Вычислить и построить PTA (пиковая тибиальная акселерация) во времени
def plot_pta(aligned_tibia_acc, fs=120):
    # Вычислить временной массив
    time = np.arange(len(aligned_tibia_acc)) / fs
    
    # Вычислить PTA (норму каждого вектора ускорения в каждый момент времени)
    pta = np.linalg.norm(aligned_tibia_acc, axis=1)
    
    # Построение графика PTA во времени
    plt.figure(figsize=(10, 6))
    plt.plot(time, pta, label="PTA (м/с^2)", color="b")
    plt.xlabel('Время (с)')
    plt.ylabel('PTA (м/с^2)')
    plt.title('Пиковая тибиальная акселерация во времени')
    plt.grid(True)
    plt.legend()
    plt.show()
    
    return pta

# Главное выполнение
static_calibration_file = r"\test 16-10\standing still upright.csv"
functional_calibration_file = r"\test 16-10\walking slow calibration.csv"
aligned_acc, _, transformation_matrix = align_tibia_data(static_calibration_file, functional_calibration_file)

test_data_file = r"\test 16-10\test data.csv"
acc_data, _, quaternions = get_data(test_data_file)
global_acc = np.array([rotate_vector(q, acc) for q, acc in zip(quaternions, acc_data)])  # Применить поворот кватерниона
global_filtered_acc = filtering(120, 15, global_acc)

# Выровнять тестовые данные в тибиальной системе отсчетов
aligned_tibia_acc = np.dot(global_filtered_acc, transformation_matrix.T) - [0, 0, 9.81]

# Построить график тибиального ускорения во времени
tibial_acceleration = plot_pta(aligned_tibia_acc)