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 положениях платы:
- Пузом вверх
- Пузом вниз
- Боком (на котором разъём SWD) вверх (юзерский USB около стола)
- Боком вниз (т.е. разъём SWD около стола)
- Низом (там, где компас со светодиодами) вверх
- Низом вниз
В каждом положении выполняйте соответствующую строчку из этого списка:
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. Начинаем проект квадрокоптера
- Выбор деталей
- Заказ деталей на Hobbyking.com
- Управление положением квадрокоптера
- Получение данных с MEMS-акселерометра
- Калибровка и обработка сигнала MEMS-акселерометра
из всей статьи осилил только “мохнатая штука”