В предложенной реализации фильтра Махони чему соответствую компоненты вектора q
- User
В предложенной реализации фильтра Махони чему соответствую компоненты вектора q
- Assistant
В фильтре Махони используются кватернионы для представления ориентации объекта в трёхмерном пространстве. Кватернион обычно обозначается как ( q = (q_0, q_1, q_2, q_3) ), где:
- ( q_0 ) – скалярная часть кватерниона.
- ( q_1, q_2, q_3 ) – векторная часть кватерниона, соответствующая осям x, y и z.
Кватернионы используются для описания поворотов и ориентации, поскольку они избегают проблем, связанных с потерей точности и особенностями, как это может происходить при использовании других математических описаний, таких как Эйлеровы углы. В контексте фильтра Махони кватернионы позволяют плавно и стабильно отслеживать ориентацию объекта, к примеру, в таких приложениях, как инерциальная навигация или стабилизация платформы.
- User
Фильтр Махони для ориентации объекта по данным с гироскопа и акселерометра на питон
- Assistant
Фильтр Махони — это алгоритм, который используется для расчета ориентации объекта с использованием данных от гироскопа и акселерометра. Он использует кватернионы для устойчивого представления ориентации. В этой реализации, фильтр будет корректировать данные гироскопа с учетом ускорения, измеренного акселерометром, чтобы минимизировать дрейф гироскопа.
Вот пример простой реализации фильтра Махони на Python:
import numpy as np class MahonyAHRS: def __init__(self, sample_freq=100.0, kp=2.0, ki=0.0): self.sample_freq = sample_freq # Частота дискретизации self.kp = kp # Коэффициент пропорциональности self.ki = ki # Коэффициент интеграции self.q = np.array([1.0, 0.0, 0.0, 0.0]) # Начальная ориентация кватерниона self.integral_fb = np.array([0.0, 0.0, 0.0]) # Интегральная ошибка def update(self, gx, gy, gz, ax, ay, az): # Нормализуем ускорение norm = np.sqrt(ax * ax + ay * ay + az * az) if norm == 0: return # Ускорение равно 0, невозможно нормализовать ax /= norm ay /= norm az /= norm # Вычисляем промежуточные величины q1, q2, q3, q4 = self.q v2q1, v2q2, v2q3 = 2 * q1, 2 * q2, 2 * q3 v4q1, v4q2, v4q3, v4q4 = 4 * q1, 4 * q2, 4 * q3, 4 * q4 v8q2, v8q3 = 8 * q2, 8 * q3 # Вычисляем вектор ошибки f1 = v2q2 * q4 - v2q1 * q3 - ax f2 = v2q1 * q2 + v2q3 * q4 - ay f3 = 1.0 - v2q2 * q2 - v2q3 * q3 - az # Вычисляем градиентный спуск grad = np.array([ v4q1 * f1 + v2q3 * f2, v4q2 * f1 + v4q3 * f2 + v8q2 * f3, v4q1 * f2 - v2q4 * f1 + v8q3 * f3, -v2q1 * f1 + v4q4 * f2 ]) # Нормализуем градиент norm = np.linalg.norm(grad) grad /= norm # Интегральная часть управления self.integral_fb += self.ki * grad / self.sample_freq # Корректируем данные гироскопа gx += self.kp * grad[0] + self.integral_fb[0] gy += self.kp * grad[1] + self.integral_fb[1] gz += self.kp * grad[2] + self.integral_fb[2] # Интеграл от скорости накапливается в кватернионе q_dot = 0.5 * np.array([ -q2 * gx - q3 * gy - q4 * gz, q1 * gx + q3 * gz - q4 * gy, q1 * gy - q2 * gz + q4 * gx, q1 * gz + q2 * gy - q3 * gx ]) # Интегрируем для получения нового состояния кватерниона self.q += q_dot / self.sample_freq self.q /= np.linalg.norm(self.q) # Нормализуем кватернион # Пример использования ahrs = MahonyAHRS(sample_freq=100.0) for _ in range(100): # Пример использования в цикле ahrs.update(gx=0.01, gy=0.02, gz=0.03, ax=0.0, ay=0.0, az=1.0) print(ahrs.q)Этот код реализует фильтр Махони для оценки ориентации на основе данных синхронизированных инерциальных датчиков. Переменные
gx,gy,gzпредставляют собой угловые скорости, измеренные гироскопом, аax,ay,az— ускорения, измеренные акселерометром. Ориентация возвращается в виде кватернионаself.q. - User
По данным с акселерометра и гироскопа необходимо точно оценить ускорение с учетом разложения вектора сила тяжести по оясм с учетом данных гироскопа. Нужна программа на питоне
- Assistant
Для точной оценки ускорения, с учетом компенсации влияния гравитации, на основе данных с акселерометра и гироскопа, необходимо использовать фильтр для оценки ориентации объекта. В данном контексте можно использовать фильтр Махони или фильтр Комплементарной, чтобы сначала оценить ориентацию, а затем использовать ее для компенсации гравитации.
Вот пример программы на Python, которая использует фильтр Махони для оценки ориентации и компенсирует гравитацию:
import numpy as np class MahonyAHRS: def __init__(self, sample_freq=100.0, kp=2.0, ki=0.0): self.sample_freq = sample_freq # Частота дискретизации self.kp = kp self.ki = ki self.q = np.array([1.0, 0.0, 0.0, 0.0]) self.integral_fb = np.array([0.0, 0.0, 0.0]) def update(self, gx, gy, gz, ax, ay, az, dt): # Normalize acceleration norm = np.sqrt(ax * ax + ay * ay + az * az) if norm == 0: return ax /= norm ay /= norm az /= norm q1, q2, q3, q4 = self.q # Gradient descent algorithm corrective step f1 = 2.0 * (q2 * q4 - q1 * q3) - ax f2 = 2.0 * (q1 * q2 + q3 * q4) - ay f3 = 2.0 * (0.5 - q2 * q2 - q3 * q3) - az j11or24 = 2.0 * q3 j12or23 = 2.0 * q4 j13or22 = 2.0 * q1 j14or21 = 2.0 * q2 j32 = 2.0 * j14or21 j33 = 2.0 * j11or24 # Gradient calculation grad_f = np.array([ j14or21 * f2 - j11or24 * f3, j12or23 * f3 - j33 * f1, j32 * f1 - j13or22 * f2 ]) norm = np.linalg.norm(grad_f) grad_f /= norm # Integrate rate of change of quaternion self.integral_fb += self.ki * grad_f * dt gx += self.kp * grad_f[0] + self.integral_fb[0] gy += self.kp * grad_f[1] + self.integral_fb[1] gz += self.kp * grad_f[2] + self.integral_fb[2] # Integrate to yield quaternion q_dot = 0.5 * np.array([ -q2 * gx - q3 * gy - q4 * gz, q1 * gx + q3 * gz - q4 * gy, q1 * gy - q2 * gz + q4 * gx, q1 * gz + q2 * gy - q3 * gx ]) self.q += q_dot * dt self.q /= np.linalg.norm(self.q) def get_gravity(self): q1, q2, q3, q4 = self.q g_x = 2 * (q2 * q4 - q1 * q3) g_y = 2 * (q1 * q2 + q3 * q4) g_z = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4 return np.array([g_x, g_y, g_z]) def get_linear_acceleration(self, ax, ay, az): gravity = self.get_gravity() return np.array([ax, ay, az]) - gravity # Пример использования ahrs = MahonyAHRS(sample_freq=100.0) # Допустим, есть поток данных от датчиков: for _ in range(100): # Этот цикл имитирует чтение данных gx, gy, gz = 0.01, 0.02, 0.03 # Данные гироскопа (в радианах/секунду) ax, ay, az = 0.0, 0.0, 1.0 # Данные акселерометра (в g) dt = 1.0 / 100.0 # Период дискретизации ahrs.update(gx, gy, gz, ax, ay, az, dt) linear_acceleration = ahrs.get_linear_acceleration(ax, ay, az) print("Linear Acceleration:", linear_acceleration)В этом примере:
- Мы сначала обновляем состояние фильтра, используя данные акселерометра и гироскопа.
- Затем мы рассчитываем жесткий вектор гравитации из текущего кватерниона ориентации.
- Наконец, мы вычитаем рассчитанную гравитацию из измеренного ускорения для оценки линейного ускорения, исключая влияние гравитации.
Обратите внимание, что в реальных приложениях для фильтрации более шумных данных вы можете также использовать фильтрацию высоких частот или другие методы увеличения устойчивости.
- User
В таком случае накапливается ускорение и данные не точны
- Assistant
Если в процессе работы системы наблюдается накопление ошибок в оценке ускорения, такое явление обычно связано с дрейфом интеграции ошибок гироскопа и шумами в данных сенсоров. Для улучшения точности и уменьшения накопления ошибок можно рассмотреть несколько подходов:
-
Калибровка сенсоров: Убедитесь, что акселерометры и гироскопы правильно откалиброваны. Неточные данные с сенсоров могут привести к накапливающимся ошибкам.
-
Фильтрация данных: Используйте фильтры, такие как комплементарный, Калмана или усовершенствованные версии алгоритма Махони, которые умеют сглаживать данные и уменьшать влияние шума.
-
Регулярная коррекция: Добавьте дополнительные методы коррекции (например, используя данные от других сенсоров, таких как GPS или магнитометр), чтобы периодически корректировать оценку ориентации и ускорения.
-
Улучшение модели: Увеличьте сложность модели, например, добавьте дополнительные состояния в фильтр, которые будут учитывать дрейф гироскопа или случайные ошибки.
Давайте рассмотрим, как можно применить более сложный подход, используя фильтр Калмана для улучшенной оценки линейного ускорения. Это достаточно обширная тема, но здесь приведён адаптированный пример:
import numpy as np from scipy.spatial.transform import Rotation as R class KalmanFilter: def __init__(self, dt, process_noise_var, measurement_noise_var): self.dt = dt # Состояния: [угловая скорость, ускорение] self.state = np.zeros(6) # Ковариационная матрица self.covariance = np.eye(6) * 0.1 # Матрица перехода состояния self.F = np.eye(6) # Матрица измерений self.H = np.zeros((3, 6)) self.H[:, 3:] = np.eye(3) # Процесс и измерение ковариаций self.Q = np.eye(6) * process_noise_var self.R = np.eye(3) * measurement_noise_var def predict(self, gyro): gyro_scaled = np.array(gyro) * self.dt rotation = R.from_rotvec(gyro_scaled).as_matrix() # Обновляем матрицу перехода состояния для ускорения acc_rotation_matrix = np.eye(6) acc_rotation_matrix[3:, 3:] = rotation self.F = acc_rotation_matrix # Предсказание нового состояния self.state = self.F @ self.state self.covariance = self.F @ self.covariance @ self.F.T + self.Q def update(self, measured_acc): y = measured_acc - self.H @ self.state S = self.H @ self.covariance @ self.H.T + self.R K = self.covariance @ self.H.T @ np.linalg.inv(S) self.state = self.state + K @ y self.covariance = (np.eye(6) - K @ self.H) @ self.covariance def get_linear_acceleration(self): return self.state[3:] # Пример использования KalmanFilter: sample_freq = 100.0 kf = KalmanFilter(dt=1.0/sample_freq, process_noise_var=0.001, measurement_noise_var=0.01) # Основной поток данных for _ in range(100): gx, gy, gz = 0.01, 0.02, 0.03 # Данные гироскопа, радиан/секунда ax, ay, az = 0.0, 0.0, 1.0 # Данные акселерометра, g # Обновляем оценку данных kf.predict([gx, gy, gz]) kf.update([ax, ay, az]) # Получаем линейное ускорение linear_acceleration = kf.get_linear_acceleration() print("Linear Acceleration:", linear_acceleration)Этот пример использует упрощённый фильтр Калмана для оценки линейного ускорения с учетом ориентации, основанной на данных гироскопа. Фильтр Калмана помогает улучшить точность оценки, объединяя прогнозируемую динамику системы и реальное измеренное состояние.
Для продвинутого моделирования следует более внимательно выбирать параметры фильтра и ковариационные матрицы, а также учитывать возможные дополнительные источники данных для получения оптимальных результатов.
-
- User
Данные выдаются с mpu6050. Обнови программу с учетом этого
- Assistant
Чтобы интегрировать данные с датчика MPU6050, необходимо использовать библиотеку, которая поддерживает сбор данных с этого сенсора. Одна из популярных библиотек для работы с MPU6050 в Python — это
mpu6050-raspberrypi, которая позволяет читать данные с акселерометра и гироскопа через интерфейс I2C. Вот пример, как можно использовать эту библиотеку совместно с фильтром Калмана для оценки линейного ускорения:- Установите библиотеку
mpu6050-raspberrypi, если вы еще этого не сделали:
pip install mpu6050-raspberrypi- Теперь создадим программу для работы с MPU6050 и фильтром Калмана:
import numpy as np from mpu6050 import mpu6050 from scipy.spatial.transform import Rotation as R import time class KalmanFilter: def __init__(self, dt, process_noise_var, measurement_noise_var): self.dt = dt self.state = np.zeros(6) # [угловая скорость, ускорение] self.covariance = np.eye(6) * 0.1 self.F = np.eye(6) self.H = np.zeros((3, 6)) self.H[:, 3:] = np.eye(3) self.Q = np.eye(6) * process_noise_var self.R = np.eye(3) * measurement_noise_var def predict(self, gyro): gyro_scaled = np.array(gyro) * self.dt rotation = R.from_rotvec(gyro_scaled).as_matrix() acc_rotation_matrix = np.eye(6) acc_rotation_matrix[3:, 3:] = rotation self.F = acc_rotation_matrix self.state = self.F @ self.state self.covariance = self.F @ self.covariance @ self.F.T + self.Q def update(self, measured_acc): y = measured_acc - self.H @ self.state S = self.H @ self.covariance @ self.H.T + self.R K = self.covariance @ self.H.T @ np.linalg.inv(S) self.state = self.state + K @ y self.covariance = (np.eye(6) - K @ self.H) @ self.covariance def get_linear_acceleration(self): return self.state[3:] # Установка параметров sample_freq = 100.0 kf = KalmanFilter(dt=1.0/sample_freq, process_noise_var=0.001, measurement_noise_var=0.01) # Инициализация датчика sensor = mpu6050(0x68) try: while True: accel_data = sensor.get_accel_data() gyro_data = sensor.get_gyro_data() ax, ay, az = accel_data['x'], accel_data['y'], accel_data['z'] gx, gy, gz = np.radians([gyro_data['x'], gyro_data['y'], gyro_data['z']]) # Перевод в радианы kf.predict([gx, gy, gz]) kf.update([ax, ay, az]) linear_acceleration = kf.get_linear_acceleration() print("Linear Acceleration:", linear_acceleration) time.sleep(1.0 / sample_freq) except KeyboardInterrupt: print("Measurement stopped by user")Основные моменты:
-
Интерфейс I2C: Чтобы использовать датчик MPU6050, ваш микрокомпьютер (например, Raspberry Pi) должен поддерживать I2C и иметь соответствующие подключения.
-
Настройка библиотеки: Пример использует
mpu6050-raspberrypiдля доступа к данным акселерометра и гироскопа. Убедитесь, что адрес I2C указан правильно. По умолчанию адрес для MPU6050 —0x68. -
Образец кода: Он читает данные с датчика, передает их в фильтр Калмана и периодически выводит оценивание линейного ускорения с учетом компенсированной гравитации.
Перед использованием убедитесь, что датчик корректно подключен и настроена соответствующая конфигурация I2C. Кроме того, фильтр может нуждаться в тонкой настройке коэффициентов, чтобы обеспечить оптимальную производительность в зависимости от конкретного применения и условий эксплуатации.
- Установите библиотеку
- User
Есть программа, реализующая оценку ускорения с датчика MPU6050 через фильтр Махони. Однако ускорение накапливается. Если пошевелить датчик, то она покажет ускорение. При остановке датчика покажет ноль, а затем если опять пошевелить датчик, она покажет ускорение, но с учетом последнего ускорение от предыдущего шевеления. Как это исправить? Код программы: import numpy as np from scipy import signal from mpu6050 import mpu6050 import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation import time
def low_pass_filter(new_data, prev_filtered_data, alpha=0.95): return alpha * new_data + (1 - alpha) * prev_filtered_data
class MahonyFilter: def init(self, kp=2.0, ki=0.0): self.q = np.array([1.0, 0.0, 0.0, 0.0]) # Инициализация кватерниона self.kp = kp self.ki = ki self.integral_error = np.zeros(3)
def update(self, gyro, accel, dt): accel_norm = np.linalg.norm(accel) if accel_norm == 0: return accel = accel / accel_norm v = np.array([2*(self.q[1]*self.q[3] - self.q[0]*self.q[2]), 2*(self.q[0]*self.q[1] + self.q[2]*self.q[3]), 2*(0.5 - self.q[1]*self.q[1] - self.q[2]*self.q[2])]) error = np.cross(v, accel) self.integral_error += self.ki * error * dt gyro = gyro + self.kp * error + self.integral_error q_dot = 0.5 * np.array([ 0.0, -gyro[0], -gyro[1], -gyro[2], gyro[0], 0.0, gyro[2], -gyro[1], gyro[1], -gyro[2], 0.0, gyro[0], gyro[2], gyro[1], -gyro[0], 0.0 ]).reshape((4, 4)).dot(self.q) self.q += q_dot * dt self.q /= np.linalg.norm(self.q) def get_quaternion(self): return self.qdef calibrate_sensors(sensor, samples=100): gyro_offsets = [] accel_offsets = []
for _ in range(samples): data = sensor.get_all_data() gyro_offsets.append(np.radians([data[1]['x'] * 250 / 16384, data[1]['y'] * 250 / 16384, data[1]['z'] * 250 / 16384])) accel_offsets.append([data[0]['x'] / 16384, data[0]['y'] / 16384, data[0]['z'] / 16384]) time.sleep(0.01) gyro_offset = np.mean(gyro_offsets, axis=0) accel_offset = np.mean(accel_offsets, axis=0) - np.array([0, 0, 9.81]) return gyro_offset, accel_offsetdef detect_stationary(gyro, accel, gyro_threshold=np.radians(0.15), accel_threshold=0.03): gyro_magnitude = np.linalg.norm(gyro) accel_magnitude = np.linalg.norm(accel) - 9.81 # Исключаем эффект гравитации # ~ print(accel_magnitude) return gyro_magnitude < gyro_threshold and abs(accel_magnitude) < accel_threshold
def main(): sensor = mpu6050(0x68)
# Калибровка gyro_offset, accel_offset = calibrate_sensors(sensor) # Инициализация velocity = np.zeros(3) mahony = MahonyFilter() prev_filtered_gyro = np.zeros(3) prev_filtered_accel = np.zeros(3) dt = 0.01 fig, ax = plt.subplots() ax.set_xlim(0, 100) line, = ax.plot(np.zeros(100), label="Velocity") velocity_data = [] accel_data = [] def update_plot(frame): nonlocal prev_filtered_gyro, prev_filtered_accel, velocity data = sensor.get_all_data() gyro_raw = np.array(np.radians([data[1]['x'] * 250 / 16384, data[1]['y'] * 250 / 16384, data[1]['z'] * 250 / 16384])) - gyro_offset accel_raw = np.array([data[0]['x'] / 16384, data[0]['y'] / 16384, data[0]['z'] / 16384]) - accel_offset # ~ print('Raw accel') # ~ print(accel_raw) print('Raw gyro') print(gyro_raw) # Фильтрация данных filtered_gyro = low_pass_filter(gyro_raw, prev_filtered_gyro) filtered_accel = low_pass_filter(accel_raw, prev_filtered_accel) # ~ filtered_gyro = gyro_raw # ~ filtered_accel = accel_raw print('filtered_accel: ', filtered_accel) if detect_stationary(filtered_gyro, filtered_accel): velocity = np.zeros(3) # Обнуляем скорость, если устройство неподвижно net_accel = np.zeros(3) # ~ filtered_gyro = np.array([0, 0, 0]) # ~ filtered_accel = np.array([0, 0, 0]) else: # Обновление фильтра Махони mahony.update(filtered_gyro, filtered_accel, dt) # Получение ориентации и компенсация гравитации q = mahony.get_quaternion() # ~ g_estimate = 2 * np.array([ # ~ q[1] * q[3] - q[0] * q[2], # ~ q[0] * q[1] + q[2] * q[3], # ~ 0.5 - q[1]*q[1] - q[2]*q[2] # ~ ]) * 9.81 w = q[0] x = q[1] y = q[2] z = q[3] g_estimate = np.array([ [1 - 2*y**2 - 2*z**2, 2*x*y - 2*z*w, 2*x*z + 2*y*w], [2*x*y + 2*z*w, 1 - 2*x**2 - 2*z**2, 2*y*z - 2*x*w], [2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x**2 - 2*y**2] ]) @ np.array([0, 0, 9.81]) net_accel = filtered_accel - g_estimate # Обновление скорости velocity += net_accel * dt velocity_data.append(np.linalg.norm(velocity)) accel_data.append(np.linalg.norm(net_accel)) print('net_accel') print(net_accel) print() if len(velocity_data) > 100: velocity_data.pop(0) if len(net_accel) > 100: accel_data.pop(0) line.set_ydata(accel_data) line.set_xdata(np.arange(len(accel_data))) # ~ line.set_ydata(velocity_data) # ~ line.set_xdata(np.arange(len(velocity_data))) prev_filtered_gyro = filtered_gyro prev_filtered_accel = filtered_accel print() print('prev values:') print(prev_filtered_accel) print(prev_filtered_gyro) print() ax.set_ylim(min(accel_data)-3, max(accel_data)+5) ax.set_xlim(0, len(accel_data)+5) return line, ani = FuncAnimation(fig, update_plot, blit=True, interval=dt*1000) plt.legend() plt.show()if name == "main": main()
- Assistant
Замечено, что в вашем коде происходит накопление ошибок в скорости, что может быть связано с несколькими вопросами:
-
Неточная калибровка и дрифт: Даже после калибровки гироскопов и акселерометров могут возникать небольшие ошибки, которые складываются с течением времени.
-
Выход за пределы при определении состояния покоя: Убедитесь, что вы правильно обнуляете скорость, когда устройство находится в покое.
-
Накопление небольших ошибок в фильтре: Малейшая систематическая ошибка в расчетах, если ее своевременно не корректировать, также будет склонна быстро накапливаться.
Давайте рассмотрим улучшения, которые помогут уменьшить накопление ошибок в этом контексте:
-
Обновление обнуления скорости: Обнуляйте скорость только в случае уверенного определения состояния покоя, что предотвращает накопление погрешностей.
-
Улучшите фильтрацию данных: Убедитесь, что фильтрация данных выходного сигнала достаточно агрессивна, чтобы удалить дрифт.
Вот обновленный код с рекомендациями по улучшению:
import numpy as np from scipy import signal from mpu6050 import mpu6050 import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation import time def low_pass_filter(new_data, prev_filtered_data, alpha=0.95): return alpha * new_data + (1 - alpha) * prev_filtered_data class MahonyFilter: def __init__(self, kp=2.0, ki=0.0): self.q = np.array([1.0, 0.0, 0.0, 0.0]) # инициализация кватерниона self.kp = kp self.ki = ki self.integral_error = np.zeros(3) def update(self, gyro, accel, dt): accel_norm = np.linalg.norm(accel) if accel_norm == 0: return accel = accel / accel_norm v = np.array([ 2*(self.q[1]*self.q[3] - self.q[0]*self.q[2]), 2*(self.q[0]*self.q[1] + self.q[2]*self.q[3]), 2*(0.5 - self.q[1]*self.q[1] - self.q[2]*self.q[2]) ]) error = np.cross(v, accel) self.integral_error += self.ki * error * dt gyro = gyro + self.kp * error + self.integral_error q_dot = 0.5 * np.array([ -self.q[1] * gyro[0] - self.q[2] * gyro[1] - self.q[3] * gyro[2], self.q[0] * gyro[0] + self.q[2] * gyro[2] - self.q[3] * gyro[1], self.q[0] * gyro[1] - self.q[1] * gyro[2] + self.q[3] * gyro[0], self.q[0] * gyro[2] + self.q[1] * gyro[1] - self.q[2] * gyro[0] ]) self.q += q_dot * dt self.q /= np.linalg.norm(self.q) def get_gravity_vector(self): q = self.q g = np.array([ 2 * (q[1]*q[3] - q[0]*q[2]), 2 * (q[0]*q[1] + q[2]*q[3]), q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3] ]) return g def calibrate_sensors(sensor, samples=100): gyro_offsets = [] accel_offsets = [] for _ in range(samples): data = sensor.get_all_data() gyro_offsets.append([ np.radians(data[1]['x'] / 131.0), np.radians(data[1]['y'] / 131.0), np.radians(data[1]['z'] / 131.0) ]) accel_offsets.append([ data[0]['x'] / 16384, data[0]['y'] / 16384, data[0]['z'] / 16384 ]) time.sleep(0.01) gyro_offset = np.mean(gyro_offsets, axis=0) accel_offset = np.mean(accel_offsets, axis=0) accel_offset[2] -= 1 # компенсировать гравитацию return gyro_offset, accel_offset def detect_stationary(gyro, accel, gyro_threshold=np.radians(0.15), accel_threshold=0.03): gyro_magnitude = np.linalg.norm(gyro) accel_magnitude = np.linalg.norm(accel) - 1.0 # Исключаем эффект гравитации return gyro_magnitude < gyro_threshold and abs(accel_magnitude) < accel_threshold def main(): sensor = mpu6050(0x68) # Калибровка gyro_offset, accel_offset = calibrate_sensors(sensor) # Инициализация переменных velocity = np.zeros(3) mahony = MahonyFilter() prev_filtered_gyro = np.zeros(3) prev_filtered_accel = np.zeros(3) dt = 0.01 fig, ax = plt.subplots() ax.set_xlim(0, 100) ax.set_ylim(-0.2, 0.2) line, = ax.plot(np.zeros(100), label="Velocity") velocity_data = [] def update_plot(frame): nonlocal prev_filtered_gyro, prev_filtered_accel, velocity data = sensor.get_all_data() gyro_raw = np.array([ np.radians(data[1]['x'] / 131.0) - gyro_offset[0], np.radians(data[1]['y'] / 131.0) - gyro_offset[1], np.radians(data[1]['z'] / 131.0) - gyro_offset[2] ]) accel_raw = np.array([ data[0]['x'] / 16384, data[0]['y'] / 16384, data[0]['z'] / 16384 ]) - accel_offset filtered_gyro = low_pass_filter(gyro_raw, prev_filtered_gyro, alpha=0.8) filtered_accel = low_pass_filter(accel_raw, prev_filtered_accel, alpha=0.8) if detect_stationary(filtered_gyro, filtered_accel): velocity = np.zeros(3) # Обнуляем скорость, если устройство неподвижно else: # Обновление фильтра Махони mahony.update(filtered_gyro, filtered_accel, dt) # Получение ориентации и компенсация гравитации g_estimate = mahony.get_gravity_vector() net_accel = filtered_accel - g_estimate # Обновление скорости velocity += net_accel * dt velocity_data.append(np.linalg.norm(velocity)) if len(velocity_data) > 100: velocity_data.pop(0) line.set_ydata(velocity_data) line.set_xdata(np.arange(len(velocity_data))) prev_filtered_gyro = filtered_gyro prev_filtered_accel = filtered_accel return line, ani = FuncAnimation(fig, update_plot, blit=True, interval=dt*1000) plt.legend() plt.show() if __name__ == "__main__": main()Объяснения улучшений:
- Реализация
get_gravity_vector: Более точно рассчитывает вектор гравитации из текущей ориентации, определенной фильтром Махони. - Обнуление скорости при покое: Только когда уверены, что устройство действительно в состоянии покоя, будет сбрасываться скорость.
- Улучшенная фильтрация: Ускорения и угловые скорости проходят через несколько сглаживающих фильтров, чтобы уменьшить шумы.
Этот код направлен на сокращение накопления ошибок, гарантируя, что система обнуляет накопленные ошибки, когда определяет, что устройство неподвижно. Вы можете добавить другие стратегии фильтрации и коррекции ошибок в зависимости от специфики применения и данных.
-
- User
Мне нужно рисовать не скорость, а ускорение
- Assistant
Если вам нужно отобразить линейное ускорение, а не скорость, в вашей системе, вы можете сделать это, просто изменив код, чтобы сохранять и отображать линейное ускорение после компенсации гравитации. Это даст вам визуализацию изменений ускорения при движении датчика.
Вот обновленный код, который отображает линейное ускорение вместо скорости:
import numpy as np from scipy import signal from mpu6050 import mpu6050 import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation import time def low_pass_filter(new_data, prev_filtered_data, alpha=0.95): return alpha * new_data + (1 - alpha) * prev_filtered_data class MahonyFilter: def __init__(self, kp=2.0, ki=0.0): self.q = np.array([1.0, 0.0, 0.0, 0.0]) # Инициализация кватерниона self.kp = kp self.ki = ki self.integral_error = np.zeros(3) def update(self, gyro, accel, dt): accel_norm = np.linalg.norm(accel) if accel_norm == 0: return accel = accel / accel_norm v = np.array([ 2*(self.q[1]*self.q[3] - self.q[0]*self.q[2]), 2*(self.q[0]*self.q[1] + self.q[2]*self.q[3]), 2*(0.5 - self.q[1]*self.q[1] - self.q[2]*self.q[2]) ]) error = np.cross(v, accel) self.integral_error += self.ki * error * dt gyro = gyro + self.kp * error + self.integral_error q_dot = 0.5 * np.array([ -self.q[1] * gyro[0] - self.q[2] * gyro[1] - self.q[3] * gyro[2], self.q[0] * gyro[0] + self.q[2] * gyro[2] - self.q[3] * gyro[1], self.q[0] * gyro[1] - self.q[1] * gyro[2] + self.q[3] * gyro[0], self.q[0] * gyro[2] + self.q[1] * gyro[1] - self.q[2] * gyro[0] ]) self.q += q_dot * dt self.q /= np.linalg.norm(self.q) def get_gravity_vector(self): q = self.q g = np.array([ 2 * (q[1]*q[3] - q[0]*q[2]), 2 * (q[0]*q[1] + q[2]*q[3]), q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3] ]) return g def calibrate_sensors(sensor, samples=100): gyro_offsets = [] accel_offsets = [] for _ in range(samples): data = sensor.get_all_data() gyro_offsets.append([ np.radians(data[1]['x'] / 131.0), np.radians(data[1]['y'] / 131.0), np.radians(data[1]['z'] / 131.0) ]) accel_offsets.append([ data[0]['x'] / 16384, data[0]['y'] / 16384, data[0]['z'] / 16384 ]) time.sleep(0.01) gyro_offset = np.mean(gyro_offsets, axis=0) accel_offset = np.mean(accel_offsets, axis=0) accel_offset[2] -= 1 # компенсировать гравитацию return gyro_offset, accel_offset def detect_stationary(gyro, accel, gyro_threshold=np.radians(0.15), accel_threshold=0.03): gyro_magnitude = np.linalg.norm(gyro) accel_magnitude = np.linalg.norm(accel) - 1.0 # Исключаем эффект гравитации return gyro_magnitude < gyro_threshold and abs(accel_magnitude) < accel_threshold def main(): sensor = mpu6050(0x68) # Калибровка gyro_offset, accel_offset = calibrate_sensors(sensor) # Инициализация mahony = MahonyFilter() prev_filtered_gyro = np.zeros(3) prev_filtered_accel = np.zeros(3) dt = 0.01 fig, ax = plt.subplots() ax.set_xlim(0, 100) ax.set_ylim(-2, 2) line, = ax.plot(np.zeros(100), label="Linear Acceleration (m/s²)") accel_data = [] def update_plot(frame): nonlocal prev_filtered_gyro, prev_filtered_accel data = sensor.get_all_data() gyro_raw = np.array([ np.radians(data[1]['x'] / 131.0) - gyro_offset[0], np.radians(data[1]['y'] / 131.0) - gyro_offset[1], np.radians(data[1]['z'] / 131.0) - gyro_offset[2] ]) accel_raw = np.array([ data[0]['x'] / 16384, data[0]['y'] / 16384, data[0]['z'] / 16384 ]) - accel_offset filtered_gyro = low_pass_filter(gyro_raw, prev_filtered_gyro, alpha=0.8) filtered_accel = low_pass_filter(accel_raw, prev_filtered_accel, alpha=0.8) # Обновление фильтра Махони mahony.update(filtered_gyro, filtered_accel, dt) # Получение ориентации и компенсация гравитации g_estimate = mahony.get_gravity_vector() net_accel = filtered_accel - g_estimate # Сохранение линейного ускорения accel_data.append(np.linalg.norm(net_accel)) if len(accel_data) > 100: accel_data.pop(0) line.set_ydata(accel_data) line.set_xdata(np.arange(len(accel_data))) prev_filtered_gyro = filtered_gyro prev_filtered_accel = filtered_accel return line, ani = FuncAnimation(fig, update_plot, blit=True, interval=dt*1000) plt.legend() plt.show() if __name__ == "__main__": main()Объяснение:
- Мы отрисовываем линейное ускорение (
accel_data), оставляя отфильтрованные данные акселерометра от долгосрочного вклада гравитации (предоставляемого кватернионным фильтром Махони). - Графика будет представлять изменение линейного ускорения в течение времени, что дает более точное понимание, как устройство движется в пространстве.
- Убедитесь, что датчик надежно калиброван перед использованием, это существенно влияет на качество компенсированных оценок ускорения.
- Мы отрисовываем линейное ускорение (
- User
Почему при компенсации ускорения свободного падения вычитается единица, а не 9.8?
- Assistant
В программах и расчетах, связанных с акселерометрами, величина 1 часто используется для представления ускорения свободного падения (гравитационного ускорения), и эта величина измеряется в g — размерной единице ускорения, равной 9.81 м/с².
По умолчанию, многие акселерометры возвращают данные в виде величины, нормированной на единицу g. Это значит, что если акселерометр неподвижен и его чувствительная ось направлена вдоль гравитационного поля, сенсор покажет значение, близкое к ±1, что соответствует ±9.81 м/с² в более привычных для нас единицах измерения (метры в секунду в квадрате).
Таким образом, если вы вычитаете единицу в ваших расчетах, это соответствует корректировке ускорения свободного падения в тех данных, которые представлены в g. Если ваши данные акселерометра уже нормализованы на 1g, то вычитание 1 обнуляет гравитационное ускорение. Если же они представлены в м/с², необходимо вычесть 9.81.
В вашем предыдущем коде, данные акселерометра делились на 16384 (что соответствует количеству единиц в гравитации для 16-битного акселерометра), чтобы нормализовать показания в g. Это также связано с тем, что вычитание единицы выглядело бы более подходящим в этой ситуации. Если ваши данные сразу в м/с², то вы должны вычитать 9.81 для правильной компенсации.
- User
Чему равно среднее значение ускорения при движении человека?
- Assistant
Среднее ускорение человека при движении сильно зависит от вида активности и скорости выполнения. Разные виды движений, такие как ходьба, бег, прыжки или езда на велосипеде, имеют разные профили ускорения. Вот некоторые приблизительные значения:
-
Ходьба: При ходьбе ускорения обычно находятся в пределах от 0.5 до 1.5 м/с². В моменты, когда ноги касаются земли или поднимаются, могут наблюдаться небольшие пики ускорения.
-
Бег: Ускорения при беге могут варьироваться от 1.5 до 5 м/с² или даже выше, в зависимости от интенсивности и скорости бега.
-
Прыжки: В момент отталкивания и приземления ускорение может кратковременно достигать значительно больших значений, вплоть до 10 м/с² и более.
-
Езда на велосипеде: Это зависит от скорости и усилий, но часто ускорения на велосипеде ниже, чем при беге, и могут находиться в пределах 0.5-2 м/с², за исключением моментов начала движения или резкого торможения.
Эти значения — лишь ориентиры и могут существенно варьироваться в зависимости от личных особенностей физической активности, условий движения и использования различных устройств для измерения.
-
- User
MPU6050 sensor.get_all_data() в каких единицах измерения выдает значения
- Assistant
Сенсор MPU6050 выдает сырые данные, которые представляют собой 16-битные значения из регистров акселерометра и гироскопа. Эти значения необходимо преобразовать в физические величины с соответствующими единицами через деление на коэффициенты чувствительности.
Акселерометр
-
Сырой выход: 16-битное целочисленное значение от -32768 до 32767.
-
Единицы физической величины: Обычно измеряется в "g" (ускорение свободного падения), где 1g = 9.81 м/с².
-
Чувствительность: Наиболее распространенные уровни настраиваемой чувствительности для акселерометра в MPU6050 — ±2g, ±4g, ±8g, и ±16g. Например, если акселерометр установлен на ±2g:
- Для диапазона ±2g: 1g соответствует 16384 (т.е. 32768 / 2) единицам.
- Чтобы преобразовать в "g", значения делятся на 16384, если настроено на ±2g.
Гироскоп
-
Сырой выход: 16-битное целочисленное значение от -32768 до 32767.
-
Единицы физической величины: Обычно измеряется в градусах в секунду (°/s).
-
Чувствительность: Наиболее распространенные настроенные уровни чувствительности для гироскопа — ±250, ±500, ±1000, ±2000 °/s:
- Для диапазона ±250 °/s: 1 °/s соответствует примерно 131 единице (т.е. 32768 / 250).
- Чтобы преобразовать в °/s, значения делятся на эти коэффициенты.
Пример преобразования
Когда вы получите данные с MPU6050 через
sensor.get_all_data(), вы, по сути, получите сырые значения, которые нужно преобразовать, чтобы привести их к физически значимым показателям.Пример:
data = sensor.get_all_data() accel_raw = data[0] # Сырые данные акселерометра gyro_raw = data[1] # Сырые данные гироскопа # Предполагаем, что акселерометр настроен на ±2g и гироскоп на ±250 °/s # Преобразование акселерометра в g accel_g = { 'x': accel_raw['x'] / 16384.0, 'y': accel_raw['y'] / 16384.0, 'z': accel_raw['z'] / 16384.0 } # Преобразование гироскопа в °/s gyro_dps = { 'x': gyro_raw['x'] / 131.0, 'y': gyro_raw['y'] / 131.0, 'z': gyro_raw['z'] / 131.0 }Полученные преобразованные значения можно использовать для анализа или дальнейших вычислений, таких как оценка ориентации или измерение линейного ускорения.
-
- User
Как изменить подпись к графику в приведенной мной программе?
- Assistant
Чтобы изменить подпись к графику в программе, вы можете изменить параметры для функции визуализации
matplotlib. В вашем коде вы используетеmatplotlibдля отображения данных. Вот места, где можно изменить подписи:-
Изменение подписи к графику: Если вы хотите изменить текстовое описание, отображаемое для линии на графике, вам нужно изменить
labelпараметр в функцииplot. -
Изменение заголовка графика: Чтобы задать заголовок для всего графика, используйте функцию
plt.title. -
Изменение подписи к осям: Вы можете изменить описание для обоих осей с помощью
plt.xlabelиplt.ylabel.
Пример обновленного кода с изменением подписи графика:
import numpy as np from scipy import signal from mpu6050 import mpu6050 import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation import time # ... ваш существующий код ... def main(): sensor = mpu6050(0x68) # Инициализация и калибровка gyro_offset, accel_offset = calibrate_sensors(sensor) mahony = MahonyFilter() prev_filtered_gyro = np.zeros(3) prev_filtered_accel = np.zeros(3) dt = 0.01 fig, ax = plt.subplots() # Установите пределы осей ax.set_xlim(0, 100) ax.set_ylim(-2, 2) # Установите подписи ax.set_title("Линейное ускорение с MPU6050") # Заголовок графика ax.set_xlabel("Время (кадры)") ax.set_ylabel("Ускорение (m/s²)") line, = ax.plot(np.zeros(100), label="Линейное Ускорение") # Измените подпись линии accel_data = [] def update_plot(frame): nonlocal prev_filtered_gyro, prev_filtered_accel data = sensor.get_all_data() gyro_raw = np.array([ np.radians(data[1]['x'] / 131.0) - gyro_offset[0], np.radians(data[1]['y'] / 131.0) - gyro_offset[1], np.radians(data[1]['z'] / 131.0) - gyro_offset[2] ]) accel_raw = np.array([ data[0]['x'] / 16384, data[0]['y'] / 16384, data[0]['z'] / 16384 ]) - accel_offset filtered_gyro = low_pass_filter(gyro_raw, prev_filtered_gyro, alpha=0.8) filtered_accel = low_pass_filter(accel_raw, prev_filtered_accel, alpha=0.8) mahony.update(filtered_gyro, filtered_accel, dt) g_estimate = mahony.get_gravity_vector() net_accel = filtered_accel - g_estimate accel_data.append(np.linalg.norm(net_accel)) if len(accel_data) > 100: accel_data.pop(0) line.set_ydata(accel_data) line.set_xdata(np.arange(len(accel_data))) prev_filtered_gyro = filtered_gyro prev_filtered_accel = filtered_accel return line, ani = FuncAnimation(fig, update_plot, blit=True, interval=dt*1000) plt.legend() plt.show() if __name__ == "__main__": main()Обратите внимание на изменения в функциях
ax.set_title,ax.set_xlabel, иax.set_ylabel, а также на настройкуlabelв функцииplot, чтобы изменить подпись линии. Эти изменения позволят вам задавать собственные заголовки и описание осей для вашей визуализации данных. -
- User
А если нужно добавить текст на график снизу так чтобы на каждый кадр он менялся, выводя текущее ускорение
- Assistant
Чтобы добавить текст, который будет обновляться в каждом кадре анимации и отображать текущее ускорение, вы можете использовать объект
textизmatplotlib. Этот объект можно обновлять в функцииupdate_plot, как и линию графика. В данном случае текст будет отображать текущее значение линейного ускорения в каждом кадре.Вот пример, как это можно сделать:
import numpy as np from mpu6050 import mpu6050 import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation import time def low_pass_filter(new_data, prev_filtered_data, alpha=0.95): return alpha * new_data + (1 - alpha) * prev_filtered_data class MahonyFilter: def __init__(self, kp=2.0, ki=0.0): self.q = np.array([1.0, 0.0, 0.0, 0.0]) self.kp = kp self.ki = ki self.integral_error = np.zeros(3) def update(self, gyro, accel, dt): accel_norm = np.linalg.norm(accel) if accel_norm == 0: return accel = accel / accel_norm v = np.array([ 2*(self.q[1]*self.q[3] - self.q[0]*self.q[2]), 2*(self.q[0]*self.q[1] + self.q[2]*self.q[3]), 2*(0.5 - self.q[1]*self.q[1] - self.q[2]*self.q[2]) ]) error = np.cross(v, accel) self.integral_error += self.ki * error * dt gyro = gyro + self.kp * error + self.integral_error q_dot = 0.5 * np.array([ -self.q[1] * gyro[0] - self.q[2] * gyro[1] - self.q[3] * gyro[2], self.q[0] * gyro[0] + self.q[2] * gyro[2] - self.q[3] * gyro[1], self.q[0] * gyro[1] - self.q[1] * gyro[2] + self.q[3] * gyro[0], self.q[0] * gyro[2] + self.q[1] * gyro[1] - self.q[2] * gyro[0] ]) self.q += q_dot * dt self.q /= np.linalg.norm(self.q) def get_gravity_vector(self): q = self.q g = np.array([ 2 * (q[1]*q[3] - q[0]*q[2]), 2 * (q[0]*q[1] + q[2]*q[3]), q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3] ]) return g def calibrate_sensors(sensor, samples=100): gyro_offsets = [] accel_offsets = [] for _ in range(samples): data = sensor.get_all_data() gyro_offsets.append([ np.radians(data[1]['x'] / 131.0), np.radians(data[1]['y'] / 131.0), np.radians(data[1]['z'] / 131.0) ]) accel_offsets.append([ data[0]['x'] / 16384, data[0]['y'] / 16384, data[0]['z'] / 16384 ]) time.sleep(0.01) gyro_offset = np.mean(gyro_offsets, axis=0) accel_offset = np.mean(accel_offsets, axis=0) accel_offset[2] -= 1 # компенсировать гравитацию return gyro_offset, accel_offset def main(): sensor = mpu6050(0x68) # Калибровка gyro_offset, accel_offset = calibrate_sensors(sensor) # Инициализация mahony = MahonyFilter() prev_filtered_gyro = np.zeros(3) prev_filtered_accel = np.zeros(3) dt = 0.01 fig, ax = plt.subplots() # Установите пределы осей ax.set_xlim(0, 100) ax.set_ylim(-2, 2) # Установите подписи ax.set_title("Линейное ускорение с MPU6050") ax.set_xlabel("Время (кадры)") ax.set_ylabel("Ускорение (m/s²)") line, = ax.plot(np.zeros(100), label="Линейное Ускорение") accel_text = ax.text(0.5, -2.5, "", transform=ax.transAxes, fontsize=12, ha='center') # добавление текста ниже оси accel_data = [] def update_plot(frame): nonlocal prev_filtered_gyro, prev_filtered_accel data = sensor.get_all_data() gyro_raw = np.array([ np.radians(data[1]['x'] / 131.0) - gyro_offset[0], np.radians(data[1]['y'] / 131.0) - gyro_offset[1], np.radians(data[1]['z'] / 131.0) - gyro_offset[2] ]) accel_raw = np.array([ data[0]['x'] / 16384, data[0]['y'] / 16384, data[0]['z'] / 16384 ]) - accel_offset filtered_gyro = low_pass_filter(gyro_raw, prev_filtered_gyro, alpha=0.8) filtered_accel = low_pass_filter(accel_raw, prev_filtered_accel, alpha=0.8) mahony.update(filtered_gyro, filtered_accel, dt) g_estimate = mahony.get_gravity_vector() net_accel = filtered_accel - g_estimate current_acceleration = np.linalg.norm(net_accel) accel_data.append(current_acceleration) if len(accel_data) > 100: accel_data.pop(0) line.set_ydata(accel_data) line.set_xdata(np.arange(len(accel_data))) accel_text.set_text(f"Текущее ускорение: {current_acceleration:.2f} m/s²") # обновление текста prev_filtered_gyro = filtered_gyro prev_filtered_accel = filtered_accel return line, accel_text ani = FuncAnimation(fig, update_plot, blit=True, interval=dt*1000) plt.legend() plt.show() if __name__ == "__main__": main()Примечания по коду:
ax.textиспользуется для создания текста под графиком. Параметрtransform=ax.transAxesгарантирует, что текст отрисовывается в координатах относительно области осей, а не данных.ha='center'центрирует текст по горизонтали.- Текст обновляется в каждом кадре посредством функции
set_text, гдеcurrent_acceleration— это величина текущего линейного ускорения.
- User
Как написать в строке м/с^2
- Assistant
Чтобы отобразить единицы измерения ( \text{м/с}^2 ) (метры в секунду в квадрате) в текстовой строке в Python, вы можете использовать Unicode для правильного представления степени. В частности, символ
²имеет код\u00B2.Вот как можно отобразить текст с этими единицами, включая квадрат:
"м/с²"Этот код корректно отобразит "м/с²". Однако, если вам нужно это сделать в строковом отображении в
matplotlibили где-либо еще, этот же подход будет работать. Например:plt.ylabel("Ускорение (м/с²)")Символ
²представляет собой один знак Unicode и корректно отобразится в большинстве современных систем и текстовых рендереров, включая графические интерфейсы вmatplotlib.