roboforum.ru

В качестве датчика положения я использовал широко распространенный модуль GY-521. Ардуинщикам он точно знаком. Но для них данная статья будет не интересна – все калибровки уже кто-то придумал и написал библиотеку. Мне же было интересно дойти своим умом. Я изучил много способов калибровки, сейчас я постараюсь объяснить идею и ее реализацию. Сразу скажу, что я еще только начал вникать в тему, поэтому информация будет дозированной. Кроме того, я не расписываю, как именно работать с этим модулем – про это информации много, а вот про калибровку – мало.
Калибровка акселерометра.
У акселерометра есть три пары регистров, в которых находится информация о величине проекции вектора ускорения свободного падения на ортогональные оси. Вот с этими данными мы и будем работать. Предварительно датчик настраивается на получение информации через равные промежутки времени (в моем случае через 5 мс, 200 раз в секунду), а также задается диапазон измеряемых ускорений (в моем случае ± 2g).
Очень желательно, чтобы датчик на роботе располагался коллинеарно осям робота, и был при этом жестко закреплен. В процессе калибровки нам понадобится ставить робота на ровную поверхность (как в нормальном положении, так и вверх ногами), класть его на левый бок, на правый бок, на перед и на зад. Желательно, чтобы конструкция робота это позволяла. Идеальная форма робота при этом – параллелепипед. Сразу скажу, что калибровать датчик надо именно на роботе и после этого не трогать. Любое его изменение положения на роботе после калибровки – делает калибровку бессмысленной.
Я написал вспомогательную программу для получения данных от датчика, с очень сильным усреднением (по 256 отсчетам). Данные приведены в таблице.
Положение робота AccelX AccelY AccelZ
Нормальное FF2A FDDA 4454
На левом боку C0E0 FE01 0132
Вверх ногами 02A8 FDFF C278
На правом боку 40FC FE6B 053C
На морде 92F9 3ED5 0493
На заду 01BA BE67 0254

Рассмотрим положения «нормальное» и «вверх ногами». Это влияет только на ось Z. Ось направлена вверх, в покое мы имеем 1g, а это должно быть число 4000 (извините, но нашему брату асемблерщику милее шестнадцатиричная система). Перевернутое положение должно дать -4000, или С000. Видим, что отклонение несимметрично. Можно было бы этим заморочиться, и кроме смещения ввести множитель. Но я в этом не вижу смысла – нельзя гнаться за истиной, не опираясь на нее же. Мои данные получены на столешнице стола, если я буду повторять много раз эксперимент – я получу близкие, но разные данные. Так что считаем, что диапазон измерения датчика все-таки верный, и ищем в калибровке только смещение.
Итак, алгоритм определения смещения.
Определяем, сколько надо отнять от каждого значения, чтобы получить идеал, и сходимся на половине.
От первого числа надо отнять 454, от второго – 278. Среднее между ними 366.

Переходим к оси Y. Видим наши числа 3ED5 и BE67. Тут уже надо прибавлять. К первому прибавить 4000 – 3ED5 = 12B. Ко второму надо прибавить C000 – BE67 = 199. Среднее между ними 162.
Переходим к оси X. Видим наши числа C0E0 и 40FC. Тут надо снова отнимать, от первого E0, от второго FC. Среднее EE.
Для универсальности смещение всегда будем прибавлять, а само смещение хранить в дополнительном коде. Тогда наши смещения:
X: FF12
Y: 0162
Z: FC9A
Проверка.
X: C0E0 + FF12 = BFF2; 40FC + FF12 = 400E
Y: 3ED5 + 0162 = 4037; BE67 + 0162 = BFC9
Z: 4454 + FC9A = 40EE; C278 + FC9A = BF12

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

Калибровка гироскопа
Теория калибровки очень проста – если датчик положения не вращается вообще, то по всем трем осям должно быть нулевое значение.
Я рассмотрел много примеров калибровки, некоторые товарищи для этого даже делают стенды с вращающимися осями. Это слишком сложно и труднодоступно. Проще всего калибровать неподвижный датчик.
Для начала я хотел поступить как с акселерометром – накопить показаний, усреднить, определить смещение и жить с ним. Трудности начались сразу с накоплением. Оказывается, нельзя просто взять и складывать числа в дополнительном коде, а потом разделить их на количество. Переполнение тут играет важную роль. Пример с двумя числами: -1 и +5. Чему равно среднее? +2. Теперь делаем это по-программистски). FFFF + 0005 = 10004. Делим на 2, получаем 8002 (-32766). В общем, я нашел метод для усреднения, здесь его не описываю, потому что это не помогло.
Я получил среднее значения от датчика по трем осям за 256 отсчетов, за поправки взял эти значения с противоположным знаком – смотрю на коды с датчика, ну да, они близки к нулю. После этого я перешел к получению угла от гироскопа. Меня интересует пока только одна ось, вдоль которой робот будет «падать», вперед – скорость со знаком «плюс», назад –скорость со знаком «минус».
Теперь немного теории о том, как получить угол от гироскопа. Сам по себе гироскоп выдает скорость вращения в градусах в секунду. В идеале, если нет вращения, то скорость равна нулю, ноль и выдает. Изменить угол без вращения нельзя. Чтобы получить угол от гироскопа, надо брать его значения через равные известные промежутки времени, умножать на это время, а произведения суммировать. У меня настроено на получение данных через каждые 5 мс. Если я буду показания умножать на 5, а результат накапливать, то я получу какое-то число. Как из него достать градусы? Мы знаем, что диапазон датчика в кодах (–32768 … +32767), а в скоростях ± 250°/с. Если наше накопленное число умножить на 250, а потом поделить (сдвигами) на 32768, то получим градусы, умноженные на 1000 (ну не люблю я дробные числа, а вот точность люблю!). Подробный алгоритм такой:
– беру сырое данное и прибавляю к нему смещение;
– беру модуль полученного числа и запоминаю знак;
– умножаю модуль на 5, получаю результат;
– беру накопитель (не менее трех байтов, я взял 4);
– если знак положительный – результат прибавляю к накопителю;
– если знак отрицательный – результат вычитаю из накопителя.
Тут можно было пару раз заморочиться (например, использовать знаковое умножение, или результат перевести в дополнительный код после беззнакового умножения, а потом только складывать), но на результат это не влияет. Алгоритмов можно придумать очень много.
Реализовал я этот алгоритм, поставил робота спокойно и стал смотреть на накопитель. А он и правда сидит и что-то копит, да быстро так. И тут я понял, что мой метод получения смещения – мягко говоря, не очень. Новый алгоритм получения калибровочной поправки для гироскопа – экспериментальный. Написал технологичку, чтобы менять поправку на ходу, и стал искать ее методом дихотомии. Шагов за 5 дошел до оптимальной поправки. Критерием оптимальности является минимальная скорость изменения состояния накопителя. Ну а заодно наглядно увидел, что такое «дрейф гироскопа».