Обработка данных MEMS-акселерометра

MEMS-акселерометры — очень дешёвые устройства, но к сожалению они имеют сравнительно высокий шум, а также теряют калибровку по осям. Поэтому принятый сигнал нужно обработать — профильтровать и привести к стандартным осям. Аппноут AN3192 на датчик LSM303DLH рассказывает о хорошем методе калибровки — провести измерения известных ускорений по осям и вычислить коррекционную матрицу. Реализуем этот алгоритм на языке R!

Измерения

Всё, что нам нужно сделать — поставить плату в разные положения (пузом вверх, пузом вниз, ну и так же для двух остальных осей) и измерить все три ускорения в течение 5-10 секунд. Я проводил по 300 измерений в каждом положении. Таким образом мы получили «наблюдаемые значения» (кстати, это терминология из фильтра Калмана, который ещё впереди). Они довольно шумные, но самое главное что они в «неправильной» системе координат. Почему она неправильная? Потому что каждый датчик имеет:

• ненулевое смещение: меряем «0» — а он выдает не «0»
• неединичный масштаб: к примеру, измерили 0 и 1 м/с2 — а разница между выданными значениями не равна 1.

Важнее даже не равенство масштаба единице, а разный масштаб по разным осям. Куча факторов влияет на эти отклонения — температура, удары, старение, просто случайное отклонение, напряжение питания. Устранить их очень сложно, гораздо проще измерить их влияние и скомпенсировать его. Если мы сейчас возьмём датчик и будем им крутить в воздухе — все значения лягут на эллипсоид. Смещение значений датчика даст сдвиг эллипсоида по осям, а разный масштаб приведёт к разным длинам полуосей эллипсоида. Но самое поганое — датчик может быть установлен на плату не точно горизонтально, и тогда эллипсоид будет ещё и повёрнут.

Теория

Вряд ли есть необходимость в сильном углублении в теорию, скажу лишь что нам необходимо вычислить коррекционную матрицу вида

в которой диагональ (ACC_11, ACC_22, ACC_33) показывает масштаб по трём осям, нижняя строка (ACC_10, ACC_20, ACC_30) показывает сдвиг по осям, а остальные значения компенсируют неточность пайки датчика (поворот эллипсоида вокруг осей). Сделать это проще всего с помощью метода наименьших квадратов, который сводится к простой формуле:

Мда, без вывода эта формула смотрится просто магически. Суть метода — подбор таких коррекционных значений, которые сводят к минимуму сумму квадратов отклонений всех экспериментальных точек от теоретических. Матрица w — это реальные значения, к которым мы стремимся. К примеру, в положении «пузом вверх» это [0, 0, 1].

Калибровка

Итак, я подготовил скрипт на R, который сначала получает данные «известных» ускорений. len — количество точек в калибровочных измерениях, acquire — функция забирания данных с COM-порта, она принимает название (номер) порта.

len=300;
calib=array(0, c(3, 3, len, 6))
acquire=function(com, len, transp) { temp=array(as.double(scan(file = com, n = 9*len, quiet = "true", skip = 1)), c(3, 3, len)); if(transp) { for(i in 1:len) temp[,,i]=t(temp[,,i]) }; return(temp); }

Нужно последовательно собрать данные в 6 положениях платы:

1. Пузом вверх
2. Пузом вниз
3. Боком (на котором разъём SWD) вверх (юзерский USB около стола)
4. Боком вниз (т.е. разъём SWD около стола)
5. Низом (там, где компас со светодиодами) вверх
6. Низом вниз

В каждом положении выполняйте соответствующую строчку из этого списка:

calib[,,,1]=acquire(4, len, TRUE)
calib[,,,2]=acquire(4, len, TRUE)
calib[,,,3]=acquire(4, len, TRUE)
calib[,,,4]=acquire(4, len, TRUE)
calib[,,,5]=acquire(4, len, TRUE)
calib[,,,6]=acquire(4, len, TRUE)

Калибровочные данные готовы. По ним проводится расчёт матрицы X, и она выводится в консоль.

w=rbind(t(rbind(calib[3,,,1],1)), t(rbind(calib[3,,,2],1)), t(rbind(calib[3,,,3],1)), t(rbind(calib[3,,,4],1)), t(rbind(calib[3,,,5],1)), t(rbind(calib[3,,,6],1)))
y=rbind(t(array(c(0,0,1), c(3,len))), t(array(c(0,0,-1), c(3,len))), t(array(c(0,1,0), c(3,len))), t(array(c(0,-1,0), c(3,len))), t(array(c(1,0,0), c(3,len))), t(array(c(-1,0,0), c(3,len))));
x=solve(t(w)%*%w)%*%t(w)%*%y

В моём случае получилась такая коррекционная матрица:

> x
[,1]           [,2]           [,3]
[1,]  9.681204e-04   6.032174e-06  -3.443519e-06
[2,]  1.435535e-05  -9.796286e-04   1.303020e-05
[3,] -8.583223e-06  -1.976647e-06   9.610175e-04
[4,]  9.075191e-03  -1.673895e-02   5.152653e-02

Использование

Откалибровав датчик, вы можете использовать матрицу x для коррекции данных акселерометра — просто допишите к пришедшему вектору 1 и умножьте его на x. Точно так же можно умножить несколько векторов — составьте из них матрицу (с «1» в 4 столбце), умножьте её на x и получите матрицу с корректными векторами. Однако, калибровка будет очень точна только ближайшие полчаса. Потом она будет просто точна. Теперь, для проверки, нужно собрать данные реального эксперимента — покрутите плату в руках, как можно равномернее и без ускорений.

len_exp=3000;
data=acquire(4, len_exp, TRUE)

Можно увеличить количество измерений, я использовал 30 тысяч, но это довольно долго и мучительно. Также я провожу очистку данных от выбросов (критерий 3*sigma)

data3_clean=array(0, c(3,3,len_exp));
acc1=cbind(t(data3[3,,]), 1)%*%x; r=acc1[,1]%$@~*!G4;:%#`2+acc1[,2]%$@~*!G4;:%#`2+acc1[,3]%$@~*!G4;:%#`2; mean(r); sd(r)/(len%$@~*!G4;:%#`0.5)/mean(r)*100; mean=mean(r); sd=sd(r); j=0; for(i in 1:len_exp) if(abs(r[i]-mean)<3*sd) {j=j+1; data3_clean[,,j]=data3[,,i]}; j; data3_clean=array(data3_clean, c(3,3,j)); acc=cbind(t(data3_clean[3,,]), 1)%*%x library(rgl) plot3d(acc[,1], acc[,2], acc[,3], col="red", size=3, type="l", lwd=1)

Нужна библиотека rgl, её можно взять в CRAN. В результате отобразится картина вектора g. У меня получилась такая мохнатая штука:

Перед калибровкой

После калибровки

А ещё вы увидите среднее значение длины вектора (в моём случае — 1.006 — очень близко к 1) и среднего отклонения в процентах (у меня — 0.7%, это прекрасное значение для эксперимента «на коленках»). Ещё раз, весь скрипт:

len=300;
calib=array(0, c(3, 3, len, 6))
calib[,,,1]=acquire(4, len, TRUE)
calib[,,,2]=acquire(4, len, TRUE)
calib[,,,3]=acquire(4, len, TRUE)
calib[,,,4]=acquire(4, len, TRUE)
calib[,,,5]=acquire(4, len, TRUE)
calib[,,,6]=acquire(4, len, TRUE)
w=rbind(t(rbind(calib[3,,,1],1)), t(rbind(calib[3,,,2],1)), t(rbind(calib[3,,,3],1)), t(rbind(calib[3,,,4],1)), t(rbind(calib[3,,,5],1)), t(rbind(calib[3,,,6],1)))
y=rbind(t(array(c(0,0,1), c(3,len))), t(array(c(0,0,-1), c(3,len))), t(array(c(0,1,0), c(3,len))), t(array(c(0,-1,0), c(3,len))), t(array(c(1,0,0), c(3,len))), t(array(c(-1,0,0), c(3,len))))
x=solve(t(w)%*%w)%*%t(w)%*%y


len_exp=3000
data2_clean=array(0, c(3, 3, len_exp))
acc1=cbind(t(data2[3,,]), 1)%*%x
r=acc1[,1]%$@~*!G4;:%#`2+acc1[,2]%$@~*!G4;:%#`2+acc1[,3]%$@~*!G4;:%#`2
mean(r)
sd(r)/(len%$@~*!G4;:%#`0.5)/mean(r)*100
mean=mean(r)
sd=sd(r)
j=0
# убираем выбросы
for(i in 1:len_exp) if(abs(r[i]-mean)<3*sd) {j=j+1; data2_clean[,,j]=data2[,,i]}
data2_clean=array(data2_clean, c(3,3,j))
acc=cbind(t(data2_clean[3,,]), 1)%*%x
plot3d(acc[,1], acc[,2], acc[,3], col="red", size=3, type="l", lwd=1)

Мы не делали фильтрацию, это и не было нашей задачей. Здесь мы только устраняли систематическую погрешность, а случайную погрешность будет устранять фильтр Калмана на следующем шаге.

**Статьи цикла:

** 1. Начинаем проект квадрокоптера

1. Выбор деталей
2. Заказ деталей на Hobbyking.com
3. Управление положением квадрокоптера
4. Получение данных с MEMS-акселерометра
5. Калибровка и обработка сигнала MEMS-акселерометра