Skip to content

Глава 6. STEWART – платформа ​

6.1. Обзор платформы Стюарта ​

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

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

image241

Типовая модель платформы Стюарта

6.2. Обратная задача кинематики ​

Для запуска платформы необходимо научиться решать обратную (инверсную) кинематическую задачу. Исходя из требуемой позиции, в которую необходимо переместить платформу (например, для удержания в равновесии объекта, находящегося на платформе) требуется определить величины углов, на которые необходимо повернуть фланцы сервоприводов, связанные с тягами манипулятора, чтобы установить его платформу в правильное положение. Платформа является одним из видов параллельного манипулятора, имеющим шесть степеней свободы и координаты, по которым она может передвигаться - линейное перемещение по осям X, Y, Z, а также углы поворота: крен (Θ), тангаж (φ) и рысканье (ψ).

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

Решение обратной задачи необходимо начать с размещения в центре основания точки отсчета системы координат с осями х, у, z. В таком случае, платформа будет иметь свою подвижную систему координат х’, у’, z’. Платформа определяется с помощью трех поступательных перемещений вдоль осей х, у, z относительно основания. Три угла поворота вокруг осей определяют ориентацию платформы по отношению к основанию: поворот на угол ψ вокруг оси z, поворот на угол Θ вокруг оси у, поворот на угол φ вокруг оси х.

Рассмотрим поворот относительно оси z на угол ψ. Тогда матрица вращения Rz(y) имеет вид:

Аналогично, имеем матрицу вращения для поворота вокруг оси у на угол Θ:

Eqn45

Eqn46

Eqn49

Рассмотрение i-той тяги платоформы Стюарта

Для третьего поворота на угол φ вокруг оси х имеем:

Полная матрица вращения платформы по отношению к основанию определяется по следующей формуле:

Теперь рассмотрим i-тую тягу платформы Стюарта (Рис.6.2).

Координаты qi точки соединения верхней точки Pi относительно системы координат основания задаются уравнением:

image247

Eqn47

Eqn50

Здесь вектор 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-ого стержня,

α – угол между рычагом сервопривода и горизонталью,

β – угол между рычагом сервопривода и осью х

После некоторых преобразований имеем:

где

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

Eqn53

Eqn52

Eqn54

Eqn51

Схема расчета угла поворота

вала сервопривода

image250

Eqn57

Найдем проекции точек b и p на оси X и Y:

Найдем матрицу поворота:

Найдем вектор l1:

Найдем длину вектора l1:

Рассчитаем оставшиеся компоненты для конечной формулы:

Eqn60

Eqn55

Eqn59

Eqn58

Eqn56

Небольшая ошибка присутствует из-за округлений и неточности измерения базовой высоты в 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.

image259

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

image260

Схема подключения сервоприводов и их 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); // движение сервоприводов

Координаты вводятся в миллиметрах. Значения можно вводить через любые знаки препинания, но в правильном порядке и разделяя их пробелом.

В результате, код управляющей программы будет выглядеть следующим образом:

Eqn57

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.