Глава 6. STEWART – платформа
6.1. Обзор платформы Стюарта
STEWART - платформа (платформа Стюарта) представляет собой один из видов параллельного манипулятора, имеющего 6 степеней свободы. Отличительной особенностью данного вида манипуляторов является компоновка его сервоприводов – все сервопривода располагаются в одной плоскости тремя парами, что приводит к созданию шести тяг на шарнирных соединителях (Рис. 6.1).
Рабочий орган такого манипулятора чаще всего представляется в виде горизонтальной платформы, на котором размещается объект. За счет большого количества тяг становится возможно точное и быстрое изменение положения находящегося на платформе объекта в пространстве. Вследствие чего, платформы Стюарта используются в задачах, где необходимо точное позиционирование объекта в пространстве.

Типовая модель платформы Стюарта
6.2. Обратная задача кинематики
Для запуска платформы необходимо научиться решать обратную (инверсную) кинематическую задачу. Исходя из требуемой позиции, в которую необходимо переместить платформу (например, для удержания в равновесии объекта, находящегося на платформе) требуется определить величины углов, на которые необходимо повернуть фланцы сервоприводов, связанные с тягами манипулятора, чтобы установить его платформу в правильное положение. Платформа является одним из видов параллельного манипулятора, имеющим шесть степеней свободы и координаты, по которым она может передвигаться - линейное перемещение по осям X, Y, Z, а также углы поворота: крен (Θ), тангаж (φ) и рысканье (ψ).
Манипулятор состоит из двух основных частей: неподвижной плоскости – базы, и подвижной плоскости – платформы. На базе находятся шесть сервоприводов, к фланцам которых прикреплены рычаги. В свою очередь, усилие от рычагов передается на тяги через шарнирные соединители, которые так же имеют шарнирное соединение с подвижной частью манипулятора.
Решение обратной задачи необходимо начать с размещения в центре основания точки отсчета системы координат с осями х, у, z. В таком случае, платформа будет иметь свою подвижную систему координат х’, у’, z’. Платформа определяется с помощью трех поступательных перемещений вдоль осей х, у, z относительно основания. Три угла поворота вокруг осей определяют ориентацию платформы по отношению к основанию: поворот на угол ψ вокруг оси z, поворот на угол Θ вокруг оси у, поворот на угол φ вокруг оси х.
Рассмотрим поворот относительно оси z на угол ψ. Тогда матрица вращения Rz(y) имеет вид:
Аналогично, имеем матрицу вращения для поворота вокруг оси у на угол Θ:



Рассмотрение i-той тяги платоформы Стюарта
Для третьего поворота на угол φ вокруг оси х имеем:
Полная матрица вращения платформы по отношению к основанию определяется по следующей формуле:
Теперь рассмотрим i-тую тягу платформы Стюарта (Рис.6.2).
Координаты qi точки соединения верхней точки Pi относительно системы координат основания задаются уравнением:


![]()
Здесь вектор T описывает линейное перемещение начала координат платформы по отношению к системе координат основания, pi – является вектором, определяющим координаты точки соединения Pi относительно системы координат платформы.
Аналогичным образом задается длина i-ой тяги в следующем виде:
где bi – вектор, определяющий координаты нижней точки Bi соединения тяги. С помощью 6 уравнений определяют длины 6 тяг, таким образом, определяется положение и ориентация платформы.
Далее необходимо определить угол поворота вала сервопривода. При нахождении формулы использовались следующие обозначения (Рис. 6.3):
а – длина рычага сервопривода,
Аi – точки соединения рычага с нижней точкой стержня i-го сервопривода с координатами а = [xa, ya, za] в системе координат основания,
Вi – точки вращения центра рычага сервопривода с координатами b = [xb, yb, zb] в системе основания,
Pi – точки соединения верхнего шарнира стержня с платформой, с координатами р = [xр, yр, zр] в системе координат платформы,
S – длина стержня,
li = длина i-ого стержня,
α – угол между рычагом сервопривода и горизонталью,
β – угол между рычагом сервопривода и осью х
После некоторых преобразований имеем:
где
При расчете объявим все константы и приведем платформу в базовое положение, при котором все рычаги будут стоять параллельно базе.



![]()
Схема расчета угла поворота
вала сервопривода


Найдем проекции точек b и p на оси X и Y:
Найдем матрицу поворота:
Найдем вектор l1:
Найдем длину вектора l1:
Рассчитаем оставшиеся компоненты для конечной формулы:


![]()


Небольшая ошибка присутствует из-за округлений и неточности измерения базовой высоты в 200 мм. Из-за конструкции в дальнейшем перед нечетными углами поворота необходимо ставить знак “- ” для вращения в противоположную сторону.
Расчет для оставшихся пяти сервоприводов аналогичен.
6.3. Устройство платформы Стюарта
Инструкция по сборке робота, а также исходные файлы конструктивных элементов под лазерную резку находятся в свободном доступе и доступны для скачивания с официального сайта appliedrobotics.ru из раздела «Учебные материалы». Сборка платформы Стюарта представляет собой размещение 6 сервоприводов на базе, установке на их фланцы рычагов и соединение подвижной платформы с рычагами через тяги с использованием шарнирного соединения. Перед установкой рычага необходимо перевести сервопривода в нулевое положение и присоединить рычаг параллельно базе. Также необходимо обратить внимание, что соседние рычаги располагаются навстречу друг другу в базовом положении. Тяги должны располагаться, как показано на рисунке 6.4.
В качестве управляющего контроллера используется Arduino-подобная аппаратная платформа OpenCM, установленная на плате расширения 485 Expansion Board, либо же на плате расширения STEM Board. Питание обеспечивается либо при помощи стандартного LiPo аккумулятора, выдающего номинальное напряжение в 11,1 В, либо через сетевой адаптер питания. Соединение сервоприводов рекомендуется выполнить последовательно и подключить получившуюся цепь к плате расширения STEM Board как показано на Рис. 6.5.
Перед тем как приступить к разработке управляющей программы, необходимо уточнить ID сервоприводов, установленных на манипуляторе. ID сервоприводов указаны на следующей схеме, приведенной на рисунке 6.5.

Предлагаемая к сборке платформа Стюарта

Схема подключения сервоприводов и их ID
Для уточнения и установки ID сервоприводов можно использовать стандартные примеры из Arduino IDE с установленной поддержкой платы OpenCM9.04. Для этого необходимо открыть два примера под названием: “o_Find_Dynamixel” и “c_ID_Change”. (Файл => Примеры => OpenCM9.04 => DynamixelWorkbench). С помощью примера “o_Find_Dynamixel” можно узнать текущее ID сервопривода следующим образом: сервопривод подключается к плате, к нему подключается питание, после чего на контроллер загружается пример. После чего в Мониторе порта сможем увидеть информацию о текущем значении ID, которое необходимо запомнить.
Теперь необходимо открыть второй пример - “c_ID_Change”. В оглавлении, при необходимости, нужно изменить эти строки:
#define DXL_ID 1 // 1 – это текущее значение ID сервопривода
#define NEW_DXL_ID 2 // 2 – это значение, на которое требуется поменять ID
Помимо этого, в обоих случаях необходимо поменять скорость “общения” платы со входами DYNAMIXEL.
#define BAUDRATE 57600 // изменяем значение на 1 000 000
Таким образом, устанавливаются необходимые ID для сервоприводов.
Для обеспечения безопасной работы, рекомендуется перед запуском определить, в каком положении находится вал сервопривода в положении 0°.
6.4. Разработка управляющей программы
В модели данного робота в качестве управляющего контроллера используется Arduino-подобный контроллер OpenCM. Для работы с ним используется среда разработки Arduino IDE.
Для работы с Dynamixel подключаем библиотеку и инициализируем переменные:
cpp
#include <DynamixelWorkbench.h>
DynamixelWorkbench dxl_wb;После чего подключаем библиотеку для работы с математическими выражениями
cpp
#include <math.h>Для обмена данных по протоколу Dynamixel в коде программы необходимо указать аппаратную шину – другими словами, порты на плате, к которым будет выполнено подключение сервоприводов.
cpp
#define Device_Name “1” – подключение к порту на плате OpenCM9.04
#define Device_Name “2” – подключение через USB переходник или через Bluetooth
#define Device_Name “3” – подключение к плате расширения OpenCM 485 Expansion BoardВ рассматриваемом варианте требуется выбрать 3 вариант.
Указываем скорость обмена данными через порт подключения:
cpp
#define BAUDRATE 1000000Далее необходимо указать ID сервоприводов, которыми необходимо управлять. Все это делается в коде программы до функции setup() при помощи следующих команд:
cpp
#define DXL_ID1 1
#define DXL_ID2 2
#define DXL_ID3 3
#define DXL_ID4 4
#define DXL_ID5 5
#define DXL_ID6 6 // Объявили ID сервоприводов манипулятора (1-6)Инициализируем геометрические данные робота в качестве глобальных переменных. Они нам необходимы для расчетов. В рассматриваемой модели робота инициализируем следующие переменные:
cpp
float s = 211;
float a = 35;
float b = 100;
float p = 110;
float fi_b = 0.61; // Угол поворота для точки b
float fi_p = 0.13; // Угол поворота для точки р
float bx, by, px, py;
float R[3][3];
float alfa1 = 0;
float beta1 = 0;
float gama1 = 0;
float x1;
float y_1;
float z1;// высота поднятия платформы: 200 - сред значение, 170 - макс
float angle0, angle1, angle2, angle3, angle4, angle5, angle6;Функция setup()
Создаем переменные model_number и присваиваем им номер сервопривода. После производим подключение всех сервоприводов манипулятора.
Вызываем функцию инициализации:
cpp
dxl_wb.init(DEVICE_NAME, BAUDRATE, &log);
Serial.begin(1000000);
const char *log ;
uint16_t model_number1 = 1;
uint16_t model_number2 = 2;
uint16_t model_number3 = 3;
uint16_t model_number4 = 4;
uint16_t model_number5 = 5;
uint16_t model_number6 = 6;
dxl_wb.ping(DXL_ID1, &model_number1, &log);
dxl_wb.ping(DXL_ID2, &model_number2, &log);
dxl_wb.ping(DXL_ID3, &model_number3, &log);
dxl_wb.ping(DXL_ID4, &model_number4, &log);
dxl_wb.ping(DXL_ID5, &model_number5, &log);
dxl_wb.ping(DXL_ID6, &model_number6, &log);
Настроим режим работы сервоприводов для манипулятора:
dxl_wb.jointMode(DXL_ID1, 20, 20, &log);
dxl_wb.jointMode(DXL_ID2, 20, 20, &log);
dxl_wb.jointMode(DXL_ID3, 20, 20, &log);
dxl_wb.jointMode(DXL_ID4, 20, 20, &log);
dxl_wb.jointMode(DXL_ID5, 20, 20, &log);
dxl_wb.jointMode(DXL_ID6, 20, 20, &log);Где первое значение 20 является скоростью, а второе значение 20 – ускорение.
Теперь рассмотрим действие каждой функции отдельно.
Функция matrix(float alfa, float beta, float gama)
В данной функции находится матрица поворота, которая соответствует уравнению:
cpp
R[0][0] = cos(alfa)*cos(beta);
R[1][0] = -sin(alfa)*cos(gama) + cos(alfa)*sin(beta)*sin(gama);
R[2][0] = sin(alfa)*sin(gama) + cos(alfa)*sin(beta)*cos(gama);
R[0][1] = sin(alfa)*cos(beta);
R[1][1] = cos(alfa)*cos(gama) + sin(alfa)*sin(beta)*sin(gama);
R[2][1] = -cos(alfa)*sin(gama) + sin(alfa)*sin(beta)*cos(gama);
R[0][2] = -sin(beta);
R[1][2] = cos(beta)*sin(gama);
R[2][2] = cos(beta)*cos(gama);Функция angle(float fi_b, float fi_p, float x, float y, float z)
При помощи данной функции можно определить угол поворота для i-го рычага
cpp
bx = b*cos(fi_b); // Нахождение проекции точек b и р
by = b*sin(fi_b);
px = p*cos(fi_p);
py = p*sin(fi_p);
float A = R[0][0]*px + R[1][0]*py; // Нахождение вектора li
float B = R[0][1]*px + R[1][1]*py;
float C = R[0][2]*px + R[1][2]*py ;
A = A - bx + x;
B = B - by + y;
C = C + z;
float l = sqrt(A*A + B*B + C*C); // Нахождение длины вектора li
float L = l*l - s*s + a*a; // Определение неизвестных коэффициентов
float M = 2*a*l;
float N = 2*a*(cos(fi_b)*(px - bx) + sin(fi_b)*(py - by));
angle0 = -(asin(L/(sqrt(M*M + N*N))) - atan(N/M)); // Нахождение угла поворотаФункция set_pos(float p1 , float p2, float p3, float p4, float p5, float p6, int del)
В данной функции осуществляется задание угла поворота вала двигателя на манипуляторе
cpp
dxl_wb.goalPosition(DXL_ID1,p1);
dxl_wb.goalPosition(DXL_ID2,p2);
dxl_wb.goalPosition(DXL_ID3,p3);
dxl_wb.goalPosition(DXL_ID4,p4);
dxl_wb.goalPosition(DXL_ID5,p5);
dxl_wb.goalPosition(DXL_ID6,p6);
delay(del);Функция loop()
Теперь рассмотрим функцию loop(), которая представляет собой бесконечный цикл. Через нее можно корректировать положения робота в реальном времени. С помощью консоли пользователь может вручную вводить координаты, которые будет занимать робот.
cpp
if (Serial.available() > 0 ) // Функция, проверяющая наличие символов, введенных в консоль
{
x1 = Serial.parseInt(); // Присваивание глобальной переменной первое целочисленное значение – координату Х
y_1 = Serial.parseInt(); // Координата Y
z1 = Serial.parseInt(); // Координата Z
alfa1 = Serial.parseInt(); // Угол поворота вводится в градусах
beta1 = Serial.parseInt();
gama1 = Serial.parseInt();
Serial.print(«X = « ); // Вывод координат, введённых пользователем
Serial.println(x1);
Serial.print(«Y = « );
Serial.println(y_1);
Serial.print(«Z = « );
Serial.println(z1);
Serial.print(«Alfa = « );
Serial.println(alfa1);
Serial.print(«Beta = « );
Serial.println(beta1);
Serial.print(«Gamma = « );
Serial.println(gama1);
alfa1 = alfa1*3.14/180; // Перевод градусы в радианы
beta1 = beta1*3.14/180;
gama1 = gama1*3.14/180;
matrix(alfa1, beta1, gama1);// Расчет матрицы
angle(0.61, 0.13, x1, y_1, z1); // Расчет угла поворота для первого двигателя
angle1 = angle0;
angle(1.48, 1.95, x1, y_1, z1); // Расчет угла поворота для второго двигателя
angle2 = -angle0;
angle(2.7, 2.22, x1, y_1, z1);
angle3 = angle0;
angle(3.58, 4.04, x1, y_1, z1);
angle4 = -angle0;
angle(4.8, 4.31, x1, y_1, z1);
angle5 = angle0;
angle(5.6, 6.13, x1, y_1, z1);
angle6 = -angle0;
Serial.print(«Angle #1: « ); // Вывод углов поворота
Serial.println(angle1);
Serial.print(«Angle #2: « );
Serial.println(angle2);
Serial.print(«Angle #3: « );
Serial.println(angle3);
Serial.print(«Angle #4: « );
Serial.println(angle4);
Serial.print(«Angle #5: « );
Serial.println(angle5);
Serial.print(«Angle #6: « );
Serial.println(angle6);
set_pos(angle1, angle2, angle3, angle4, angle5, angle6, 50); // движение сервоприводовКоординаты вводятся в миллиметрах. Значения можно вводить через любые знаки препинания, но в правильном порядке и разделяя их пробелом.
В результате, код управляющей программы будет выглядеть следующим образом:

cpp
#include <DynamixelWorkbench.h>
#include <math.h>
#if defined(__OPENCM904__)
#define DEVICE_NAME «3» //Dynamixel on Serial3(USART3) <-OpenCM 485EXP
#elif defined(__OPENCR__)
#define DEVICE_NAME «»
#endif
#define BAUDRATE 1000000
#define DXL_ID1 1
#define DXL_ID2 2
#define DXL_ID3 3
#define DXL_ID4 4
#define DXL_ID5 5
#define DXL_ID6 6
DynamixelWorkbench dxl_wb;
float s = 211;
float a = 35;
float b = 100;
float p = 110;
float fi_b = 0.61;
float fi_p = 0.13;
float bx, by, px, py;
float R[3][3];
float alfa1 = 0;
float beta1 = 0;
float gama1 = 0;
float x1;
float y_1;
float z1;
float angle0, angle1, angle2, angle3, angle4, angle5, angle6;
void set_pos( float p1 , float p2, float p3, float p4, float p5, float p6, int del)
{
dxl_wb.goalPosition(DXL_ID1,p1);
dxl_wb.goalPosition(DXL_ID2,p2);
dxl_wb.goalPosition(DXL_ID3,p3);
dxl_wb.goalPosition(DXL_ID4,p4);
dxl_wb.goalPosition(DXL_ID5,p5);
dxl_wb.goalPosition(DXL_ID6,p6);
delay(del);
}
void matrix(float alfa, float beta, float gama)
{
R[0][0] = cos(alfa)*cos(beta);
R[1][0] = -sin(alfa)*cos(gama) + cos(alfa)*sin(beta)*sin(gama);
R[2][0] = sin(alfa)*sin(gama) + cos(alfa)*sin(beta)*cos(gama);
R[0][1] = sin(alfa)*cos(beta);
R[1][1] = cos(alfa)*cos(gama) + sin(alfa)*sin(beta)*sin(gama);
R[2][1] = -cos(alfa)*sin(gama) + sin(alfa)*sin(beta)*cos(gama);
R[0][2] = -sin(beta);
R[1][2] = cos(beta)*sin(gama);
R[2][2] = cos(beta)*cos(gama);
}
void angle(float fi_b, float fi_p, float x, float y, float z)
{
bx = b*cos(fi_b);
by = b*sin(fi_b);
px = p*cos(fi_p);
py = p*sin(fi_p);
float A = R[0][0]*px + R[1][0]*py;
float B = R[0][1]*px + R[1][1]*py;
float C = R[0][2]*px + R[1][2]*py ;
A = A - bx + x;
B = B - by + y;
C = C + z;
float l = sqrt(A*A + B*B + C*C);
float L = l*l - s*s + a*a;
float M = 2*a*l;
float N = 2*a*(cos(fi_b)*(px - bx) + sin(fi_b)*(py - by));
angle0 = -(asin(L/(sqrt(M*M + N*N))) - atan(N/M));
}
void setup()
{
const char *log ;
Serial.begin(1000000);
uint16_t model_number1 = 1;
uint16_t model_number2 = 2;
uint16_t model_number3 = 3;
uint16_t model_number4 = 4;
uint16_t model_number5 = 5;
uint16_t model_number6 = 6;
dxl_wb.init(DEVICE_NAME, BAUDRATE, &log);
dxl_wb.ping(DXL_ID1, &model_number1, &log);
dxl_wb.ping(DXL_ID2, &model_number2, &log);
dxl_wb.ping(DXL_ID3, &model_number3, &log);
dxl_wb.ping(DXL_ID4, &model_number4, &log);
dxl_wb.ping(DXL_ID5, &model_number5, &log);
dxl_wb.ping(DXL_ID6, &model_number6, &log);
dxl_wb.jointMode(DXL_ID1, 20, 20, &log);
dxl_wb.jointMode(DXL_ID2, 20, 20, &log);
dxl_wb.jointMode(DXL_ID3, 20, 20, &log);
dxl_wb.jointMode(DXL_ID4, 20, 20, &log);
dxl_wb.jointMode(DXL_ID5, 20, 20, &log);
dxl_wb.jointMode(DXL_ID6, 20, 20, &log);
}
void loop()
{
if (Serial.available() > 0 ) {
x1 = Serial.parseInt();
y_1 = Serial.parseInt();
z1 = Serial.parseInt();
alfa1 = Serial.parseInt();
beta1 = Serial.parseInt();
gama1 = Serial.parseInt();
Serial.print(«X = « );
Serial.println(x1);
Serial.print(«Y = « );
Serial.println(y_1);
Serial.print(«Z = « );
Serial.println(z1);
Serial.print(«Alfa = « );
Serial.println(alfa1);
Serial.print(«Beta = « );
Serial.println(beta1);
Serial.print(«Gamma = « );
Serial.println(gama1);
alfa1 = alfa1*3.14/180;
beta1 = beta1*3.14/180;
gama1 = gama1*3.14/180;
matrix(alfa1, beta1, gama1);
angle(0.61, 0.13, x1, y_1, z1);
angle1 = angle0;
angle(1.48, 1.95, x1, y_1, z1);
angle2 = -angle0;
angle(2.7, 2.22, x1, y_1, z1);
angle3 = angle0;
angle(3.58, 4.04, x1, y_1, z1);
angle4 = -angle0;
angle(4.8, 4.31, x1, y_1, z1);
angle5 = angle0;
angle(5.6, 6.13, x1, y_1, z1);
angle6 = -angle0;
Serial.print(«Angle #1: « );
Serial.println(angle1);
Serial.print(«Angle #2: « );
Serial.println(angle2);
Serial.print(«Angle #3: « );
Serial.println(angle3);
Serial.print(«Angle #4: « );
Serial.println(angle4);
Serial.print(«Angle #5: « );
Serial.println(angle5);
Serial.print(«Angle #6: « );
Serial.println(angle6);
set_pos(angle1, angle2, angle3, angle4, angle5, angle6, 50);
}Таким образом, была решена обратная задача кинематики для платформы Стюарта, а также написана управляющая программа для реализации задачи на физической модели с микроконтроллером OpenCM.