Использование твердотельных гироскопов в ракетомоделях

SashaMaks> Этот алгоритм помню, пробовал, ничего не вышло.

Ну, это надо мне допаять свою плату и попробовать в натуре.

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

Кстати, с этом случае можно проверить, к чему ведёт несинхронность считывания угловых скоростей и ускорений.
Ну и шумов можно намешать.

Xan> Чтоб в начале приключений исключить шумы и дрейф мемсов, а только проверить алгоритм.
Xan> Кстати, с этом случае можно проверить, к чему ведёт несинхронность считывания угловых скоростей и ускорений.
Xan> Ну и шумов можно намешать.

Не все сразу, а данные все у меня сохраняются и записываются.

Пока все просто, ракета стоит на столе, никаких земных шаров, широт, скоростей нет.

То берём вот это?:

1. X_x = cos( 0);  X_y = sin( 0);  X_z = sin( 0);          // Ракета стоит на северном полюсе,
2. Y_x = sin( 0);  Y_y = cos( 0);  Y_z = sin( 0);          // оси ракеты совпадают с осями
3. Z_x = sin( 0);  Z_y = sin( 0);  Z_z = cos( 0);          // абсолютной системы.

У меня это будет записано так в месте обнуления и задания начальных значений переменных:

1. ComRocket.XX := Vector(Cos(0.0), Sin(0.0), Sin(0.0));
2. ComRocket.YY := Vector(Sin(0.0), Cos(0.0), Sin(0.0));
3. ComRocket.ZZ := Vector(Sin(0.0), Sin(0.0), Cos(0.0));

Дальше, у меня идёт запоминание нуля в простейшем виде и вероятно оно совпадает с твоим кодом?

1.             //Сопряжение осей акселерометра
2.             ComRocket.FA.X := Buffer.Data[0] * 0.000598550415039;//м/с^2
3.             ComRocket.FA.Y := Buffer.Data[2] * 0.000598550415039;
4.             ComRocket.FA.Z := Buffer.Data[1] * 0.000598550415039;
5.             //Сопряжение осей гироскопа
6.             ComRocket.VWFull.X := Buffer.Data[4] * 0.0010652644360317;//рад/с
7.             ComRocket.VWFull.Y := Buffer.Data[5] * 0.0010652644360317;
8.             ComRocket.VWFull.Z := Buffer.Data[3] * 0.0010652644360317;
9.             //Подсчёт среднеарифметического за L_NULLс.
10.             If ComRocket.I_NULL < ComRocket.L_NULL then begin
11.               IntegralVector(ComRocket.SA, ComRocket.FA);
12.               IntegralVector(ComRocket.SW, ComRocket.VWFull);
13.               Inc(ComRocket.I_NULL);
14.             end else
15.             //Запоминание начального положения
16.             If ComRocket.I_NULL = ComRocket.L_NULL then begin
17.               ComRocket.SA := MultiVectors(ComRocket.SA, -1.0/ComRocket.L_NULL);
18.               ComRocket.SW := MultiVectors(ComRocket.SW, -1.0/ComRocket.L_NULL);
19.               Inc(ComRocket.I_NULL);
20.             end else begin

Потом корректируем начальное положение:

1. x = (float)gyro_x - zero_x;
2. y = (float)gyro_y - zero_y;
3. z = (float)gyro_z - zero_z;

или у меня:

1. //Корректировка начального положения
2. IntegralVector(ComRocket.FA, ComRocket.SA);
3. IntegralVector(ComRocket.VWFull, ComRocket.SW);

Всё ли так?

Это и есть те самые коэффициенты для управления?

1. k_x = 0.00030517;                                       // значения "с потолка" для
2. k_y = 0.00030517;                                       // наладки программы.
3. k_z = 0.00030517;

Вероятно их пока можно исключить из кода?
То эта запись:

1. x = k_x * x * dt;
2. y = k_y * y * dt;
3. z = k_z * z * dt;

будет:

1. x = x * dt;
2. y = y * dt;
3. z = z * dt;

А это у меня уже тоже есть:

1. //Вычисление мгновенного углового перемещения
2. ComRocket.FW := MultiVectors(ComRocket.VWFull, ComRocket.dT);

Вот это что такое?

1.     if (BEFORE_START)
2.        {
3.        if (x > 0.0)     zero_x += 0.00001;      // Настройка нулей перед стартом.
4.        else             zero_x -= 0.00001;
5.        if (y > 0.0)     zero_y += 0.00001;
6.        else             zero_y -= 0.00001;
7.        if (z > 0.0)     zero_z += 0.00001;
8.        else             zero_z -= 0.00001;
9.        }

Пока пропускаю...

SashaMaks> Вот это что такое?
SashaMaks> Пока пропускаю...

Дальше сложнее.
Вот это непонятно зачем и какие данные должны быть в этих коэффициентах, если не с потолка?

1. float   k_xy, k_xz, k_yx, k_yz, k_zx, k_zy;             // Поправки на неотогональность осей гироскопов.
2. k_xy = 0.001;
3. k_xz = 0.002;
4. k_yx = 0.003;
5. k_yz = 0.004;
6. k_zx = 0.005;
7. k_zy = 0.006;
8. x += k_xy * y + k_xz * z;                       // Поправка на взаимное влияние осей гироскопов.
9.     y += k_yx * x + k_yz * z;
10.     z += k_zx * x + k_zy * y;

Косинусы с синусами:

1.               //Направляющие косинусы?
2.               ComRocket.Cos.X := Cos(ComRocket.FW.X);
3.               ComRocket.Cos.Y := Cos(ComRocket.FW.Y);
4.               ComRocket.Cos.Z := Cos(ComRocket.FW.Z);
5.               //Какие-то синусы?
6.               ComRocket.Sin.X := Sin(ComRocket.FW.X);
7.               ComRocket.Sin.Y := Sin(ComRocket.FW.Y);
8.               ComRocket.Sin.Z := Sin(ComRocket.FW.Z);
9.               //Что-то считаем дальше
10.               ComRocket.XXnew.X := ComRocket.Sin.Z * ComRocket.YY.X - ComRocket.Sin.Y * ComRocket.[color=red]ZZ.Y[/color]  + ComRocket.Cos.Y * ComRocket.XX.X;
11.               ComRocket.XXnew.Y := ComRocket.Sin.Z * ComRocket.YY.Y - ComRocket.Sin.Y * ComRocket.[color=red]ZZ.X[/color]  + ComRocket.Cos.Y * ComRocket.XX.Y;
12.               ComRocket.XXnew.Z := ComRocket.Sin.Z * ComRocket.YY.Z - ComRocket.Sin.Y * ComRocket.ZZ.Z  + ComRocket.Cos.Y * ComRocket.XX.Z;
13.  
14.               ComRocket.YYnew.X := -ComRocket.Sin.Z * ComRocket.YY.X + ComRocket.Sin.X * ComRocket.ZZ.X  + ComRocket.Cos.Z * ComRocket.YY.X;
15.               ComRocket.YYnew.Y := -ComRocket.Sin.Z * ComRocket.YY.Y + ComRocket.Sin.X * ComRocket.ZZ.Y  + ComRocket.Cos.Z * ComRocket.YY.Y;
16.               ComRocket.YYnew.Z := -ComRocket.Sin.Z * ComRocket.YY.Z + ComRocket.Sin.X * ComRocket.ZZ.Z  + ComRocket.Cos.Z * ComRocket.YY.Z;
17.  
18.               ComRocket.ZZnew.X := ComRocket.Sin.Y * ComRocket.ZZ.X - ComRocket.Sin.X * ComRocket.ZZ.X  + ComRocket.Cos.X * ComRocket.ZZ.X;
19.               ComRocket.ZZnew.Y := ComRocket.Sin.Y * ComRocket.ZZ.Y - ComRocket.Sin.X * ComRocket.ZZ.Y  + ComRocket.Cos.X * ComRocket.ZZ.Y;
20.               ComRocket.ZZnew.Z := ComRocket.Sin.Y * ComRocket.ZZ.Z - ComRocket.Sin.X * ComRocket.ZZ.Z  + ComRocket.Cos.X * ComRocket.ZZ.Z;
21.  
22.               ComRocket.XX := ComRocket.XXnew;
23.               ComRocket.YY := ComRocket.YYnew;
24.               ComRocket.ZZ := ComRocket.ZZnew;

Я опустил приближенный расчёт тригонометрии пока. Вроде все правильно переписал?

То, что выделил красным, так и должно быть?

ComRocket.XXnew.X := ComRocket.Sin.Z * ComRocket.YY.X - ComRocket.Sin.Y * ComRocket.ZZ.Y + ComRocket.Cos.Y * ComRocket.XX.X;
ComRocket.XXnew.Y := ComRocket.Sin.Z * ComRocket.YY.Y - ComRocket.Sin.Y * ComRocket.ZZ.X + ComRocket.Cos.Y * ComRocket.XX.Y;
ComRocket.XXnew.Z := ComRocket.Sin.Z * ComRocket.YY.Z - ComRocket.Sin.Y * ComRocket.ZZ.Z + ComRocket.Cos.Y * ComRocket.XX.Z;

SashaMaks> То, что выделил красным, так и должно быть?
SashaMaks> ComRocket.XXnew.X := ComRocket.Sin.Z * ComRocket.YY.X - ComRocket.Sin.Y * ComRocket.ZZ.Y + ComRocket.Cos.Y * ComRocket.XX.X;
SashaMaks> ComRocket.XXnew.Y := ComRocket.Sin.Z * ComRocket.YY.Y - ComRocket.Sin.Y * ComRocket.ZZ.X + ComRocket.Cos.Y * ComRocket.XX.Y;
SashaMaks> ComRocket.XXnew.Z := ComRocket.Sin.Z * ComRocket.YY.Z - ComRocket.Sin.Y * ComRocket.ZZ.Z + ComRocket.Cos.Y * ComRocket.XX.Z;

Нет, это явная апучатка.
Исправил в файле.

SashaMaks> Это и есть те самые коэффициенты для управления?

Нет. Это просто перевод прочитанных из мемсов значений в радианы/с2.

В этом файле только вычисление положения и ориентации.
Управление — отдельная прога, не готовая.

Всё, убегаю на работу, потом.

Xan> Нет. Это просто перевод прочитанных из мемсов значений в радианы/с2.

Понятно, это уже есть.

Датчики у меня сейчас настроены на 2G и на 2000гр./с, насколько можно верить такой их калибровке? Может у них есть погрешность и они не точно вписываются в указанный диапазон по значениям? Как ее определить тогда?

SashaMaks> Датчики у меня сейчас настроены на 2G и на 2000гр./с, насколько можно верить такой их калибровке? Может у них есть погрешность и они не точно вписываются в указанный диапазон по значениям? Как ее определить тогда?

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

Гироскопы лучше всего крутить на платформе, на которой можно точно фиксировать количество оборотов. Не скорость вращения, а просто угол.

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

Более тонкая проверка — делать этот оборот (или несколько) с разной скоростью. От самой низкой, до максимальной.
Это даст нелинейность гироскопов.
Нужно ли будет учитывать нелинейность в ракете — фиг знает.

Xan> Нет. Это просто перевод прочитанных из мемсов значений в радианы/с2.

Опять апучатка — радианы в секунду, конечно.


SashaMaks> Дальше, у меня идёт запоминание нуля в простейшем виде и вероятно оно совпадает с твоим кодом?

У меня перед стартом, вместо "усреднить за большой интервал", сделано "подкручивать маленькими шагами, чтоб получился ноль".

if (BEFORE_START)
{
if (x > 0.0)
zero_x += 0.00001; // Настройка нулей перед стартом.
else
zero_x -= 0.00001;
...
}


SashaMaks> Потом корректируем начальное положение:

Нет, у меня сначала делается промежуточное значение

x = (float)gyro_x - zero_x;

значение int из гиры превратилось во float (и минус ноль),
а потом, с учетом преобразования в радианы/с, превратилось в угол за квант времени

x = k_x * x * dt;

Это потому что у меня привычка сложные вычисления расписывать в несколько простых строчек. От древних компиляторов осталась.


SashaMaks> Вот это непонятно зачем и какие данные должны быть в этих коэффициентах, если не с потолка?

float k_xy, k_xz, k_yx, k_yz, k_zx, k_zy; // Поправки на неотогональность осей гироскопов.
k_xy = 0.001;
...
x += k_xy * y + k_xz * z; // Поправка на взаимное влияние осей гироскопов.
...

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


SashaMaks> Косинусы с синусами:
...
SashaMaks> Я опустил приближенный расчёт тригонометрии пока. Вроде все правильно переписал?

Тут у меня глаза разбежались.

Xan> Например, поворачиваешь ровно на один оборот, и смотришь, что тебе покажет интегрирование.

Да, тут можно сделать точную калибровку, а с акселерометром я до сих пор думаю, как бы это по-проще сделать.

Xan> Нужно ли будет учитывать нелинейность в ракете — фиг знает.

Много чего еще нужно будет выяснить, что-то может и отпадет, если где-то уже будет большая ошибка, но это потом...

Xan> У меня перед стартом, вместо "усреднить за большой интервал", сделано "подкручивать маленькими шагами, чтоб получился ноль".

Так это дольше будет и нисколько не точнее.

Xan> Нет, у меня сначала делается промежуточное значение
Xan> x = (float)gyro_x - zero_x;
Xan> значение int из гиры превратилось во float (и минус ноль),

Так это и есть корректировка начального положения. Перевод в флоат не актуален для кода на делфи. У меня данные уже приобразованы, как и перевод единиц.

Xan> Но это надо проверять на каждой собранной самоделке.

Я думаю будут другие места для больших ошибок, которые просто не дадут этого заметить.

Xan> Тут у меня глаза разбежались.

Вроде не так много написано)))
По сравнению с кодом ПРОПЕПа - это сущая ерунда)))
Но я не спешу, сейчас я далеко от компа и все равно ничего не напишу и не испытаю, только завтра...

SashaMaks> Да, тут можно сделать точную калибровку, а с акселерометром я до сих пор думаю, как бы это по-проще сделать.

Я думаю использовать центрифугу стиралки (с горизонтальной осью), в моей до 170 g можно получить.
Там будут колебания ускорения на +1 и —1 g, так что в каждом опыте будут получаться две точки, дающие наклон кривой.

SashaMaks> Так это дольше будет и нисколько не точнее.

Помехозащищённость больше. Ну и я в весах постоянно так делаю — привычка!

SashaMaks> Перевод в флоат не актуален для кода на делфи.

Я нарывался на случаи, когда компилятор "забывал" делать автоматическое преобразование типов. Так что в ответственных случаях всегда ему напоминаю.
Ну и из железа мемсов читается int, так что (в моей проге) это надо делать.

Xan>> Но это надо проверять на каждой собранной самоделке.
SashaMaks> Я думаю будут другие места для больших ошибок, которые просто не дадут этого заметить.

Выкинуть всегда успеется.

Xan>> Тут у меня глаза разбежались.
SashaMaks> Вроде не так много написано)))

Попробую в другое время.
Что-то производительность мозгов какая-то низкая.

Xan> Возможно, в некоторых мемсах корректировка уже делается во внутренних мозгах индивидуальными заводсками настройками.

Делается. Но остаётся заметный, для ракетомоделей, остаток. Для "большой" ракеты - тем более заметный. Например, у моего любимого ADIS16445, нормируется перекос осей на уровне не более 0,05 градуса между осями, и на уровне 0,5 градуса между осью и корпусом. И это у гироскопа ценой 500 долларов!

Xan> Тогда в проге это не нужно.

Нужно. Больше того, поскольку внутренняя калибровка делает это весьма странным образом, а МЕМС-гироскопы плохо чувствуют малые угловые скорости, то встроенная калибровка может влиять на точность неожиданным образом.

Xan> Что для меня было удивительным.
Xan> До этого считал, что чем жёстче, тем лучше.

Перерегулирование — плохая идея

Xan> В общем, надо обязательно держать k1 в приличном диапазоне.
Xan> И вот бы научиться его вычислять на ходу по поведению системы.
Xan> Может быть надо смотреть на спектр колебаний. И если там много ВЧ, то уменьшать k1.

Я бы всё же посоветовал смотреть не только на ВЧ, а на спектр вообще, и давить резкие пики режекторным фильтром. Причём пики можно посчитать заранее, как раз через матмодель ракеты.

Xan> Только как это сделать за первую четверть периода?

Разумеется, никак. Для того, чтобы убедиться в наличии колебаний нужен хотя бы один полный период.

Решил привести свою симуляцию к единичным единицам!

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

Сигнал = — k1 * Angle — k2 * (Angle — Old_Angle) / dt

Единичный сигнал даёт единичное угловое ускорение.

Вот 4 картинки для k1 = 0.4, 0.2, 0.1, 0.05
k2 = 2 * sqrt(k1) — более лучшие не подбирал.

Ракете пинком придаётся единичная угловая скорость (красный)
и угол начинает линейно с этой скоростью расти (синий)
через секунду до машинки доходит сигнал управления и она мгновенно поворачивается (желтый)
Скорость начинает падать к нулю.
Ещё через секунду до машинки доходит сигнал, учитывающий изменение скорости, на линии получается излом.
Синие графики справа — АЧХ в герцах. ФЧХ поленился.
Вообще, на мой взгляд, отклик гораздо информативнее, чем ЧХ.

Xan> k2 = 2 * sqrt(k1) — более лучшие не подбирал.

Вот посмотрел, при каких k1 кривулька будет спадать без колебаний — без перехода через нуль (или с выбросом меньше тысячной).
Апериодически.

И подобрал лучшие k2.
Точнее, подбирал коэффициент при корне:

k2 = Coef * sqrt(k1)

Ampl — это размер первого выброса на графике.

...k1.. Coef Ampl
0.0075 1.89 4.72
0.0150 1.83 3.52
0.0300 1.73 2.73
0.0600 1.61 2.21
0.1200 1.56 1.84
0.1300 1.56 1.80

При k1 = 0.13 кривулька хоть и быстро спадает, но уже несколько раз колеблется вокруг нуля.

1.
Получается, что при изменении k1 на порядок, размах растёт всего в пару раз, примерно в степени 1/3.
Это значит, что k1 можно ставить поменьше, с большим запасом от критического.

2.
Когда симулировал машинку с плавным перемещением, для маленьких k1 у меня получались большие k2 (Coef > 2).
Надо посмотреть внимательнее на влияние машинки.

Xan> При k1 = 0.13 кривулька хоть и быстро спадает, но уже несколько раз колеблется вокруг нуля.
Xan> 1.
Xan> Получается, что при изменении k1 на порядок, размах растёт всего в пару раз, примерно в степени 1/3.

Размах - это только половина апельсина, важно ещё и полное время регулирования. Можно смотреть по интегралу от угла, так как он показывает боковое и нормальное рассеяние. Кроме того, надо учитывать поведение системы при постоянном возмущающем воздействии. Кажущаяся нереальность этого, на самом деле, реализуется в каждом полёте из-за области высотных ветров.

Xan> Это значит, что k1 можно ставить поменьше, с большим запасом от критического.

Тут ещё вот какая засада - если ракета длительное время летит не вдоль вектора скорости, появляются потери на управление - косинус угла между вектором скорости и вектором тяги. Чем меньше k1 - тем они больше, особенно, при максимальной программной скорости изменения угла тангажа.

Б.г.> Размах - это только половина апельсина, важно ещё и полное время регулирования.

Угу, площадь под кривой.

Но я уже говорил, что над этой управлялкой будет ещё одна, которая ей уставку задаёт.
А этой надо только быстрые отклонения от заданного убирать.

Б.г.> Тут ещё вот какая засада - если ракета длительное время летит не вдоль вектора скорости, появляются потери на управление - косинус угла между вектором скорости и вектором тяги.

Ну врядли ракета будет колебаться на пи пополам!

Б.г.>> Тут ещё вот какая засада - если ракета длительное время летит не вдоль вектора скорости, появляются потери на управление - косинус угла между вектором скорости и вектором тяги.
Xan> Ну врядли ракета будет колебаться на пи пополам!
Пи пополам - не пи пополам, а вот мы с Алексеем Мазуром моделировали трёхмерное выведение с учётом шума гироскопов, так получается, что орбиты с коррекцией по солнцу посредине траектории, считая по ХС, имеют матожидание высоты перигея на 10 км больше, чем без коррекции. Потому что часть энергии рассеивается на случайные повороты, вызванные накоплением ошибки гироскопа.
А 10 км в перигее могут быть очень критичны.

Б.г.> А 10 км в перигее могут быть очень критичны.

Это, примерно, косинус 2 градусов. Не много.

Чтоб ракета болталась на 2 градуса при машинке с задержкой 0.05 с (частота примерно 4 Гц) угловая скорость болтанки должна быть:

w = 2 * pi * 4 * 0.035 = 0.9 рад/с = 50 град/с

До 2000 град/с ещё далеко.
А 2000 как раз пи пополам даст!

Б.г.> Пи пополам - не пи пополам, а вот мы с Алексеем Мазуром моделировали трёхмерное выведение с учётом шума гироскопов,

Две симуляции: среднеквадратичное отклонение угла от k1.

Третья колонка — к прочитанному из гир значению прибавляется единичный шум. И это значение используется в вычислениях, что приводит к дерганью управляющего сигнала и руля. Что и раскачивает ракету
размах = k1^(+0.35)

Четвёртая колонка — к реальной угловой скорости прибавляется единичный шум. Это влияние атмосферы и вихляние вектора тяги качает ракету.
размах = k1^(-0.65)

1. k1 = 0.25       k2 = 1.60       0.149   23.53
2. k1 = 0.12       k2 = 1.56       0.093   33.89
3. k1 = 0.06       k2 = 1.61       0.069   50.37
4. k1 = 0.03       k2 = 1.73       0.054   77.05
5. k1 = 0.015      k2 = 1.83       0.043   124.3
6. k1 = 0.0075     k2 = 1.89       0.035   206.9

Ну, какие-то выводы надо бы сделать...
Но что-то я не соображу!

I.C.> Давно не видно новостей по системе управления.
Фото нового каркаса приборного отсека:

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