Установка изображена на рис. 1. Платформа имеет одну фиксированную точку и две точки, в которых закрепляются шаговые двигатели. Стабилизация работает на принципе P-регулятора. Шаговые двигатели производят смещение только вдоль вертикальной оси. Данная модель симулирует работу двигателей и подсчитывает количество шагов, необходимых до стабилизации платформы. Модель работает корректно при углах Эйлера меньше
Рис. 1. Схема установки до и после поворота. Оранжевым цветом изображена платформа. Углы Эйлера: крен –
Программа состоит из 3 классов: EulerToMistake, MistakeToOffset, OffsetToEngine. Алгоритм работы:
- На вход класса EulerToMistake подаются углы Эйлера, которые описывают поворот платформы. Далее считается ошибка: разность между углами Эйлера и положением равновесия.
- Далее ошибка из класса EulerToMistake подается на вход класса MistakeToOffset. Метод computeOffset вычисляет смещения, которые нужно задать двум двигателям. Логика основана на принципе P-регулятора.
- Далее вызывается класс OffsetToEngine, на вход которого подаются смещения для двигателей и текущая ошибка. В классе OffsetToEngine симулируется работа двигателей и после смещения двигателей вычисляется ошибка, которая передается обратно классу MistakeToOffset. Затем процесс происходит рекурсивно до тех пор, пока платформа не стабилизируется или наоборот, начнет раскачиваться все сильнее.
Конструктор:
EulerToMistake(vector<double> euler_coords_t) – на вход подаются углы Эйлера;
Методы:
void computeMistake() – метод, вычисляющий ошибку;
vector<double> getEulerCoords() const – метод, возвращающий углы Эйлера;
vector<double> getMistake() const – метод, возвращающий ошибку;
Поля:
vector<double> euler_coords - углы Эйлера, {крен, тангаж}, положительное направление углов выбирается по правилу правой руки;
vector<double> mistake - ошибка, {крен, тангаж};
В нашем случае задаются два угла: крен и тангаж, рыскание отсутствует. Крен – поворот плоскости относительно оси
Конструктор:
MistakeToOffset(vector<double> mistake_t, double P_t, double I_t, double D_t, int step, ostream& output_t) – на вход подаются ошибка, коэффициенты P, I, D, шаг, поток вывода;
Методы:
void computeOffset()– метод, вычисляющий смещение моторов;
vector<double> getOffset() const – метод, возвращающий смещение моторов;
vector<double> getMistake() const – метод, возвращающий ошибку;
Поля:
vector<double> mistake - ошибка, {крен, тангаж};
double P, I, D - коэффициенты PID-регулятора;
vector<double> offset - смещение на двух моторах;
ostream& output – поток вывода;
int step – шаг стабилизации;
В нашем случае задаются
Это и есть P-регулятор. Например, если заданы крен =
Конструктор:
OffsetToEngine(vector<double> offset_t, vector<double> old_mistake) – на вход подаются смещение и текущая ошибка;
Методы:
void computeMatrix(vector<double> mistake) – метод, вычисляющий матрицу поворота;
void OffsetToMistakeModel(vector<double> old_mistake) – метод, вычисляющий ошибку после смещения моторов (модель);
void OffsetToMistake() – метод, вычисляющий ошибку после смещения моторов (истинная ошибка, полученная с датчика; метод пока реализован);
vector<double> getMistake() const – метод, возвращающий ошибку;
Matrix getMatrix() const – метод, возвращающий матрицу поворота;
Поля:
vector<double> offset - смещение на двух моторах;
vector<double> new_mistake - ошибка, {крен, тангаж};
Matrix matrix – матрица поворота;
Алгоритм работы модели:
- Задаем изначальное положение двух точек, к которым крепятся двигатели, так, чтобы они с началом координат составляли равносторонний треугольник (рис.1). Длина радиус векторов единичная. Задаем нормаль к платформе в состоянии равновесия.
- Вычисляем матрицу поворота, которая поворачивает систему координат, связанную с платформой в положении равновесия, вместе с платформой, т.е. новая система координат будет повернута так, как и платформа.
Матрица поворота вычисляется через произведение двух матриц поворота: матрица
где
- Преобразуем радиус-векторы двух точек в соответствии с поворотом плоскости. Теперь точки у нас находятся там, где и должны быть, если платформа отклонилась от положения равновесия.
Преобразование векторов задается по формуле:
где
- Задаем смещение двух точек по оси
$\large Z$ , так как моторы могут двигаться только в этом направлении. После этого нормируем радиус-векторы двух точек, так как их длина изначально единичная. Из-за нормировки, реальное смещение меньше, чем то, что мы изначально задали, но логики процесса это не меняет. При углах меньше$\large 30°$ смещение корректно, так равносторонний треугольник остается равносторонним. - Вычисляем через векторное произведение нормаль новой плоскости, построенной уже по двум смещенным радиус-векторам.
- Зная нормаль, находим новые углы Эйлера. Здесь алгоритм пока работает только для углов от
$\large -90°$ до$\large 90°$ .
Новые углы Эйлера находятся из решения обратной задачи:
где
- Новые углы Эйлера возвращаются в класс MistakeToOffset и процесс продолжается рекурсивно, пока платформа не стабилизируется.
Новые углы Эйлера возвращаются в класс MistakeToOffset и процесс продолжается рекурсивно, пока платформа не стабилизируется.
Начальные данные:
Вывод в консоль:
Начальные данные: крен = 30 градусов, тангаж = 30 градусов
P = 0.1
Стабилизация прошла успешно! Количество шагов: 47
P = 0.25
Стабилизация прошла успешно! Количество шагов: 16
P = 0.5
Стабилизация прошла успешно! Количество шагов: 6
P = 0.75
Стабилизация прошла успешно! Количество шагов: 8
P = 1
Стабилизация прошла неудачно!
P = 2
Стабилизация прошла неудачно!
Начальные данные:
Вывод в консоль:
Начальные данные: крен = 18 градусов, тангаж = 12 градусов
P = 0.1
Стабилизация прошла успешно! Количество шагов: 39
P = 0.25
Стабилизация прошла успешно! Количество шагов: 14
P = 0.5
Стабилизация прошла успешно! Количество шагов: 5
P = 0.75
Стабилизация прошла успешно! Количество шагов: 7
P = 1
Стабилизация прошла неудачно!
P = 2
Стабилизация прошла неудачно!