Задачу сделать такую платформу мне подкинул мой шеф в качестве упражнения по зарабатыванию денег головой. Денег не заработал, но интересно было… Итак, необходимо обеспечить стабильность малогабаритной и легкой видеокамеры типа видеорегистратора, установленной на легковом автомобиле. Сначала решил поискать готовые решения в интернете, но нашел только серьезные платформы авиационного назначения без ценника: [1], [2], [3], [4]. А также видеоролик про создание системы активной стабилизации радиоуправляемой авиамодели и про реинженеринг-адаптацию игрового манипулятора Nintendo для управления веб-камерой с помощью авиамодельных сервоприводов.

На основе этих идей и стал сочинять систему. По результатам одновременного сочинительства головой и руками теперь могу разделить решение задачи на части:

Относительно медленные колебания с частотой, близкой к собственной частоте подвески легкового автомобиля, но большой амплитуды — и есть главная цель компенсации. Эти колебания нужно компенсировать активным или пассивным гироскопом, т. к. необходимы большие угловые перемещения платформы на угол ±5..15 градусов (максимум), когда авто перекатывается с борта на борт после несимметричного наезда на неровность типа "лежачего полицейского". При такой качке нестабилизированная длиннофокусная камера (с малым углом поля зрения), смотрящая вбок, из-за качки может "видеть" смещение картинки на величину больше размера кадра, поэтому обычная для видеокамер система электронной стабилизации изображения, основанная на избыточности матрицы, не справится с задачей стабилизации. Единственная возможность — поворачивать всю камеру или сильно двигать сенсор внутри камеры или группу линз внутри объектива.

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

Самую низкочастотную составляющую качки, связанную с изменением наклона всего дорожного полотна (езда по горному серпантину, езда по холмам), решил учитывать и компенсировать с помощью трехосевого акселерометра с большим периодом осреднения его показаний для исключения влияния тряски.

Заманчивую идею решить задачу №1 с помощью классического карданного пассивного гироскопа пришлось отбросить в связи с трудностью решения проблемы медленного, но верного ухода "нуля" такой системы. Пришлось бы делать много подвижных соединений электрических проводов приводов моторов, да и просто не хотелось связываться с точной механикой. Что-то слышав про твердотельные гироскопы и умение планшетов, телефонов и других гаджетов определять ускорение и угол наклона, решил пошерстить интернет и — о чудо! — нашел [5] и [6]. Оказывается, прогресс шагнул очень далеко, и теперь есть дешевые твердотельные трехосевые акселерометры, микросхемы размером в несколько миллиметров, способные измерять вектор ускорения, а также полноценные измерители угловых скоростей — гироскопы! Есть и более продвинутые модели с цифровым интерфейсом ([7]), с полным набором датчиков (трехосевой гироскоп, трехосевой акселерометр и магнитный компас) и процессором обработки первичных данных, причем всё это счастье весит без проводов 7 граммов!

Микроплата акселерометра
Микроплата гироскопа
Рис. 1. Платы акселерометра и гироскопа

Эти устройства, в отличие от более дорогих и сложных лазерных старших собратьев, работают на принципах, максимально близких к работе нашего вестибулярного аппарата. "Интересны и другие системы на основе МЭМС-акселерометров, например гироскопы. Одно такое устройство имеет никелевое кольцо шириной около 6 мкм и внешним диаметром 1 мм, изготовленное на кремниевой основе. Кольцо крепится на подложке с помощью полукруглых подпорок, сходящихся в его центре. По внешнему диаметру кольца расположены электроды. На половину из них подают постоянное смещение, которое при вращении вызывает вибрацию кольца. Лепесток вибрации располагается по внутренней окружности кольца. Его амплитуду и, следовательно, скорость вращения определяют по величине изменения емкости конденсаторов, формируемых другими электродами, расположенными между питающими контактами. Разрешение таких устройств — 0,01 градус/с в полосе 10 Гц". [8]

Для ознакомления с новыми MEMS-технологиями я выбрал варианты двухосевого гироскопа и трехосевого акселерометра, адаптированные друзьями технологии Arduino для применения трясущимися руками большого паяльника. Производители этих плат так и пишут в сопроводиловке, что микросхема установлена на миниатюрную плату и снабжена своим стабильным источником питания для облегчения интеграции в любительские системы. Далее всё как обычно: изучение даташитов и привязка к понравившемуся мозгу-микроконтроллеру STM32F103. Понятно, что в качестве управляющего микроконтроллера можно использовать любой из приглянувшихся современных МК.

Плата двухосевого гироскопа со всеми выводами показана на рис. 1 и на рис. 5. Питание на нее 5 В "Vin" подается от того же блока питания, которым питался МК. Два аналоговых выхода гироскопа "X" и "Y", несущих информацию о скорости вращения платы вокруг осей OX и OY, соединяются с условными входами АЦП МК "Vox" и "Voy" (рис. 5). Т. к. выходы "X" и "Y" однополярные, то состоянию покоя гироскопа соответствует некоторое среднее значение выходного сигнала ( 1,23 В). Поэтому помимо информационных сигналов используется еще и фактическое значение опорного напряжения ("Vref", рис. 1 и 5), поданное на третий вход АЦП МК для программного учета дрейфа. Также обязательно надо соединить землю гироскопа "GND" и землю каналов АЦП МК. В ходе отладки программы МК оказалось, что состояние покоя гироскопа на выходах "Х" и "Y" соответствует существенно различным потенциалам, отличным от заявленного 1,23 В, что неприятно и требует учета свойств каждого экземпляра гироскопа при программировании. Обращаю ваше внимание на особенность микросхемы этого гироскопа: ее аналоговые выходы являются абсолютными, т. е. не масштабируются в зависимости от уровня напряжения на входе "3.3V" (т. к. есть дополнительный встроенный блок стабилизации питания внутри самой микросхемы). Таким образом, значения напряжений на выходах в зависимости от скоростей вращения гиродатчика должны вычисляться как: X[В] = VREF + SOxVOX,

Y[В] = V

REF + SOxVOY,

где S

O — абсолютная чувствительность в единицах [В/(°/с)], а VOX и VOY — скорости вращения гиродатчика вокруг соответствующих осей в единицах [°/с]

В качестве исполнительных механизмов привода платформы я, как только отказался от пассивного гироскопа, сразу же решил использовать авиамодельные рулевые машинки. Критериями выбора машинок для первого опытного варианта системы были:

быстродействие

мощность

низкая стоимость в ущерб водозащищенности

Рис. 2. Сервопривод SM-S4471M
Напряжение питания 4,8—6,0 В Время поворота на угол 60° 0,10 с при напряжении питания 4,8 В 0,07 с при напряжении питания 6,0 В Крутящий момент 8,8 кгс·см при напряжении питания 4,8 В 9,7 кгс·см при напряжении питания 6,0 В Угол поворота ±60° Размеры 40,3x20,2x37,2 мм Масса 63 г

В итоге я выбрал цифровые машинки с моторами с полым ротором, применяющиеся для привода механизма управления шагом винта на радиоуправляемых вертолетах (рис. 2, [9]). К этой машинке прилагается внушительный набор пластиковых качалок, однако было бы лучше иметь одну, но металлическую. Я предполагал,что для обеспечения независимого управления положением платформы по двум осям придется делать карданный шарнир самому, но совершенно случайно на сайте любителей робототехники нашел уже готовый, хоть и хлюпенький, карданный шарнир, специально адаптированный к постановке в него двух рулевых машинок. Этот шарнир (рис. 3, [10]) используется для изготовления кистевого сустава робота и прекрасно подошел для моих целей (рис. 4), тем более что под рукой нашелся микроподшипник, идеально подошедший для поддержки консольной части шарнира (в моем наборе не оказалось подшипника, видного на рис. 3).

Рис. 3. Карданный шарнир

Из машинки выходит трехжильный провод (рис. 2): земля (черный/коричневый или просто темный), питание +5..6 В (красный) и третий (желтый/белый/цветной) — линия управления углом поворота привода. Управляются выбранные машинки стандартным ШИМ-сервосигналом с уровнями потенциалов ТТЛ или выше, я использовал амплитуду сигнала в 5 В. Данные машинки имеют максимальный рабочий угол отклонения ±60°. Тогда их угол в зависимости от длительности управляющего сервосигнала можно рассчитать по следующей формуле (1): Угол поворота привода в градусах = 60x(t–0,001)/0,001

где t — длительность управляющего импульса в диапазоне от 0,001 до 0,002 с

Выбранные машинки "умеют" обрабатывать управляющий ШИМ-сигнал с повышенной частотой следования управляющих импульсов — больше, чем стандартные 50 Гц. Я использовал 300 импульсов в секунду, и машинки их нормально отрабатывали. Питание машинкам подается от дополнительного авиамодельного блока питания BEC с возможностью выбора 5 В или 6 В на выходе. Большее напряжение обеспечивает большее быстродействие машинок (выбранные машинки допускают питание 6 В, хоть и заметно теплеют от этого). Питание МК и машинок, чтобы не связываться с помехами, я сразу же развел, полностью исключив их связь: BEC питает только машинки с помощью двух двухбаночных LiPo-аккумуляторов.

Про МК писать подробно смысла не вижу, т. к. вряд ли кто-то будет работать с тем же МК, что и я. А общие принципы уже перечислены, и остается добавить только, что питание силовой части и мозгов из-за импульсного характера нагрузки и импульсной же природы блоков питания надо разводить подальше друг от друга всеми силами. В этой системе плата МК питалась от кроссплаты Фрактал со встроенным блоком питания, а тот — от трехбаночного авиамодельного LiPo-аккумулятора. Хотя с успехом можно было питаться и от USB-порта ноутбука, на который одновременно велась и видеозапись c установленной на гироплатформу веб-камеры, но не хотелось обматываться проводами при полевых испытаниях.

Рис. 4. Двухосевая гироплатформа с веб-камерой

Т. к. на первом этапе не ставилось задачи минимизировать габариты блока управления, то были использованы стандартные платы комплекта Фрактал [11]: вполне компактная плата MCU32 с МК STMF103, кроссплата расширения и клеммный адаптер. Всё это, кроме МК, лишнее в конечном изделии. В полевых испытаниях участвовал вариант системы без акселерометра, только с двухосевым гиродатчиком. В предыдущем исполнении удалось вполне успешно освоить приведение гиросистемы в действительно горизонтальное положение на основе показаний трехосевого акселерометра, и в программе МК этот кусочек имеется. Однако для первых полевых испытаний я упростил схему, отказавшись временно от арретирования платформы в горизонт, заменив на возврат платформы в положение "параллельно полу автомобиля" путем медленного возврата машинок в нейтральное положение.

Программирование МК выполнено в среде IAR на языке C++. За "болванку" программы взят один из примеров типа "GettingStarted", поставляемый с демо-версией среды IAR С++ [12].

Модель функционирования системы

Двухосевой датчик угловых скоростей 1 (рис. 5) устанавливается на автомобиле и, соответственно, ориентируется осью OX вдоль направления движения, а осью OY — вдоль пола или крыши. Эти датчики фиксируют качку авто вокруг двух осей: в авиационных терминах — по тангажу и по крену (направление осей указано на микроплате гиродатчика с противоположной от микросхемы стороны).

Рис. 5. Схема подключения элементов

1 — двухосевой гиродатчик, 2 — мозг, установленный на кроссплате с собственным импульсным блоком питания, 3 — трехбаночный LiPo-аккумулятор питания мозга и гиродатчика, 4 — сервомашинки для управления камерой по крену и тангажу, 5 — два последовательно соединенных двухбаночных LiPo-аккумулятора для питания сервомашинок, 6 — импульсный блок питания сервомашинок

Сигналы от датчика в аналоговом виде поступают в микроконтроллер, который выполняет предварительную фильтрацию сигналов, чтобы удалить из сигнала высокочастотную пульсационную составляющую. В результате получаются значения двух компонент окружной скорости вращения: VOX [°/с] — вокруг оси OX, и VOY — вокруг OY. Затем рассчитываются значения текущих углов наклона авто по формулам: УголOX(i+1)= УголOX(i)+VOXxДt,

Угол

OY(i+1)= УголOY(i)+VOYxДt,

где Дt — шаг по времени, равный в данном случае Дt=1/300 с, а индексы (i) и (i+1) соответствуют значениям переменных на предыдущем и следующем шаге по времени

Далее все просто: надо наклонить камеру на эти же углы, но с противоположным знаком, и тогда платформа выровняется. Т. к. эта процедура будет повторяться 300 раз в секунду, то и выглядеть это будет так, как будто камера наклоняется навстречу качке авто почти без задержки. Наклон камеры на эти углы осуществляется подачей микроконтроллером управляющих сигналов (упр1 и упр2, рис. 5) на каждую из двух машинок одновременно по двум каналам, а длительность сервосигналов при этом вычисляется по обратной (1) формуле.

При реализации этой простой схемы имеется несколько нюансов:

необходимо определить, какому уровню сигнала от каждого канала гиродатчика соответствует состояние покоя, и учесть это при вычислении скоростей VOX и VOY;

необходимо приводить машинки к нулевому положению.

Я это сделал, введя постоянную составляющую скорости приближения к среднему положению. Но пришлось выбирать значение этой скорости не слишком большим, чтобы, возвращая машинки в "ноль", все же давать системе стабилизации работать. C другой стороны, маленькое значение этой скорости может оказаться недостаточным из-за отклонения сигнала покоя от измеренного при наладке системы. Пришлось даже ввести алгоритм учета убегания напряжения покоя. Подробности реализации этих алгоритмов в данном случае не важны, т. к. каждый, кто захочет эту систему повторить или использовать в своих разработках, будет вынужден осваивать программирование МК, после чего описанные мною трудности покажутся цветочками :). Однако привожу два основных фрагмента программы МК, отвечающих за первичную обработку сигналов от гиродатчика и за формирование управляющих сигналов.

/* Фрагмент настройки последовательной выборки для 6 каналов первого из двух модулей АЦП МК STM32F103: */

/* ADC1 configuration ------------------------------------------------------*/

/* ADC1 regular channels configuration */

ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 1, ADC_SampleTime_55Cycles5);

ADC_RegularChannelConfig(ADC1, ADC_Channel_11, 2, ADC_SampleTime_55Cycles5);

ADC_RegularChannelConfig(ADC1, ADC_Channel_12, 3, ADC_SampleTime_55Cycles5);

ADC_RegularChannelConfig(ADC1, ADC_Channel_13, 4, ADC_SampleTime_55Cycles5);

ADC_RegularChannelConfig(ADC1, ADC_Channel_14, 5, ADC_SampleTime_55Cycles5);

ADC_RegularChannelConfig(ADC1, ADC_Channel_15, 6, ADC_SampleTime_55Cycles5);

ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;

ADC_InitStructure.ADC_ScanConvMode = ENABLE;

ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;

ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;

ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;

ADC_InitStructure.ADC_NbrOfChannel = 6;

ADC_Init(ADC1, &ADC_InitStructure);

/* Процедура обработки прерывания по окончанию преобразования 6 каналов АЦП1: */

void ADC_IRQHandler(void)

{

/* Clear ADC1 EOC pending interrupt bit */

ADC_ClearITPendingBit(ADC1, ADC_IT_EOC);

/* Предварительно была настроена прямая пересылка результатов преобразования АЦП в память — в массив ADC1ConvertedValueA[0..5] */

/* Первые 3 канала — это компоненты ускорения X, Y и Z акселерометра. */

/* Здесь происходит примитивная фильтрация входных данных: значение измеренной величины накапливается суммированием в переменных ADC1ConvertedValueX,..Y,..Z, а количество измерений сохраняется в переменной nADC1ConvertedValue: */

nADC1ConvertedValue++;

ADC1ConvertedValueX= ADC1ConvertedValueX+ADC1ConvertedValueA[0];

ADC1ConvertedValueY= ADC1ConvertedValueY+ADC1ConvertedValueA[1];

ADC1ConvertedValueZ= ADC1ConvertedValueZ+ADC1ConvertedValueA[2];

/* Следующие 2 канала — входы от гиродатчика: */

ADC1ConvertedValueUX= ADC1ConvertedValueUX+ADC1ConvertedValueA[3];

ADC1ConvertedValueUY= ADC1ConvertedValueUY+ADC1ConvertedValueA[4];

/* Последний, 6-й канал — вход Vref от гиродатчика: */

ADC1ConvertedValueVrefg= ADC1ConvertedValueVrefg+ADC1ConvertedValueA[5];

}

/* Процедура обработки прерывания от таймера — 300 раз в секунду для выработки управляющих воздействий: */

void TIM4_IRQHandler(void)

{

TIM_ClearITPendingBit(TIM4, TIM_FLAG_Update);

Vrefg=ADC1ConvertedValueVrefg/nADC1ConvertedValue;/* осреднение по всем выборкам АЦП, которые успели произойти за 1/300 секунды */

ADC1ConvertedValueVrefg=0;

Ucurd=ADC1ConvertedValueUX/nADC1ConvertedValue;

ADC1ConvertedValueUX=0;

Ucurd=(Ucurd-Vrefg)*3.3/4096.0; /* пересчет данных АЦП в вольты с учетом середины сигнала = Vref */

aux1=Ucurd/0.0025; /* пересчет, с учетом чувствительности, в [градус/сек] */

aux=aux+(aux1-aux0);/* попытка учесть уплывание нуля путем вычисления отличия показаний прошлого замера aux0 от текущего aux1*/

auxmidl=auxmidl*0.999+aux*0.001;/* вычисление "медленного среднего" — также для учета уплывания нуля; возможно, это лишнее, но так работает… */

cornx=cornx-(aux-auxmidl)/300.0;/* расчет текущего угла наклона авто вокруг оси ОХ в градусах по значению скорости поворота и известному шагу по времени Дt=1/300 с */

aux0=aux1;

/*Далее — ограничение предельных углов, чтоб камера в стекло авто не билась :-) :*/

if(cornx>45.0){cornx=45.0;}

if(cornx<-45.0){cornx=-45.0;}

/*А вот далее — медленное приведение сервомашинки по оси ОХ к нулю, а точнее — к мертвой зоне ±1 градус: */

if(cornx>1.0){cornx=cornx-0.04;}

if(cornx<-1.0){cornx=cornx+0.04;}

/*Аналогично для тангажа: */

Ucurd=ADC1ConvertedValueUY/nADC1ConvertedValue;

ADC1ConvertedValueUY=0;

Ucurd=(Ucurd-Vrefg)*3.3/4096.0;

auy1=Ucurd/0.0025;

auy=auy+(auy1-auy0);

auymidl=auymidl*0.999+auy*0.001;

corny=corny-(auy-auymidl)/300.0;

auy0=auy1;

if(corny>60.0){corny=60.0;}

if(corny<-60.0){corny=-60.0;}

if(corny>1.0){corny=corny-0.04;}

if(corny<-1.0){corny=corny+0.04;}

nADC1ConvertedValue=0; /* для нового периода осреднения */

/* Далее — расчет длительности управляющего импульса ШИМ: 1 мс = "поворот на угол –60 градусов", 2 мс = "поворот на угол +60 градусов". Предварительно таймер, вырабатывающий импульсы, был настроен на частоту счета 12 МГц, поэтому импульс длиной 0,001 с соответствует 12000 тиков таймера. Общий период T=1/300 с соответствует 40000 тиков.*/

powerY=6000.0+corny*100.0; /* эта величина меняется в диапазоне 0..12000, а знак "+" или "–" здесь надо подбирать в зависимости от ориентации сервомашинок. */

pic=12000+(u16)powerY; /* эта величина меняется в диапазоне 12000..24000 */

if(pic<12000){pic=12000;}

if(pic>24000){pic=24000;}

TIM4->CCR1 = pic; /* загрузка в регистр таймера длительности управляющего импульса, со следующего цикла таймера это значение вступит в силу */

/*Аналогично для второй оси:*/

powerX=6000.0-cornx*100.0;

pic=12000+(u16)powerX;

if(pic<12000){pic=12000;}

if(pic>24000){pic=24000;}

TIM4->CCR2 = pic;

}

Описанная система довольно легко была доведена до работоспособного состояния, потому что сразу же было решено датчики устанавливать непосредственно на авто, а не на камере, что позволило совсем избавиться от решения проблем с положительной обратной связью. Вроде, естественным решением было бы установить датчики прямо на камере и решать более общую задачу возвращения платформы с датчиками в состояние покоя. Тогда корректирующие воздействия напрямую получаются из текущих показаний датчиков. Получилась бы аналогия человеческой системы поддержания тела в равновесии, когда мы просто стоим на двух ногах, например, в движущемся вагоне поезда метро. Но такая задачка будет, пожалуй, на порядок сложнее описанной из-за самовозбуждения колебаний в системе датчик—привод. На первом этапе опытов с акселерометром я попробовал прижать акселерометр прямо к машинке — и сразу же заработал самоподдерживающиеся колебания привода, который своими рывками повторно возбуждал акселерометр. Такой же опыт с гиродатчиком дал более обнадеживающий результат: возбудить самоподдерживающиеся колебания системы удалось только один раз, и то при промежуточном уровне разряда батарей.

Испытания

Для полевых испытаний я установил платформу с приводами и веб-камерой на одолженный у друга автомобильный держатель с присоской, слегка его разобрав (рис. 4). Чтобы не возиться с наружной установкой камеры, решили прикрепить ее изнутри авто на боковое стекло задней двери и направили вбок через салон авто так, чтобы в поле зрения камеры попадала рама окна задней двери и пейзаж за ней. Такая установка камеры позволила фиксировать, насколько хорошо камера держит горизонт. На записанных видеороликах видно, что при включенном стабилизаторе изображение пейзажа за окном вполне стабильно, а рамка двери прыгает в кадре — значит, стабилизатор работает.

Вид на установку при работе в полевых условиях

Стабилизатор выключен

Стабилизатор включен

По результатам испытаний возникли следующие мысли:

Для применений такого стабилизатора на автомобиле вполне достаточно хода привода ±5°, а у меня из-за прямого привода от машинок получался ход ±60°.

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

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

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

Благодарности:

Тимофею Киянскому за интересное задание и полевые испытания;

Андрею Тарасову за "болванку" и помощь при программировании разработанного им удобного MCU32;

Максиму Вальбе за держатель с присоской, оторванный от любимого авто;

Владимиру Родионову за поддержку и добрую помощь в написании статьи.

Интересные ссылки по теме статьи:

"Бешеный" трикоптер на Arduino

Использование акселерометра 1

Использование акселерометра 2

Неадекватная система стабилизации полета…

Источник