Список использованных источников

1. Фомин Н.В. Системы управления электроприводов: учеб. пособие / Н.В. Фомин. - Магнитогорск: Изд-во Магнитогорск. гос. техн. ун-та им. Г.И. Носова, 2012. – 293с.

2. Бурдаков С.Ф. Элементы теории роботов: учебное пособие / - Ленинградский политехнический институт имени М.И.Калинина, 1986 – 88с.

3. Бурдаков С.Ф., Дьяченко В.А., Тимофеев А.Н. Проектирование манипуляторов, промышленных роботов и роботизированных комплексов.Учебное пособие.-М.: Высш.школа, 1986.-264 с

4. Пол Р. Моделирование, планирование траекторий и управление движением робота-манипулятора. М.: Наука, 1976. – 104 с

5. Зенкевич С. Л., Ющенко А. С. Основы управления манипуляционными роботами. 2-е изд. — М.: Изд-во МГТУ им.Н. Э. Баумана, 2004. — 480 с.

Приложение а

Программа управления манипулятором РТК – AR-RTK-ML-02, реализованная на C++ для исполнения контроллером OpenCM9.04

#include <DynamixelWorkbench.h>

#define DEVICE_NAME "3"

#define BAUDRATE 1000000

DynamixelWorkbench dxl_wb;

//Кинематические параметры манипулятора

float L1[3] = {0, 0, 0.15}; //длина первого звена

float L2[3] = {0, 0, 0.21};

float L3[3] = {0.135, 0, 0};

float L4[3] = {0.065, 0, 0};

float L5[3] = {0.105, 0, 0};

float LTCP[3] = {0, 0, -0.000}; //длина рабочего инструмента

float LL2 = sq(L2[0])+sq(L2[1])+sq(L2[2]);

float LL3 = sq(L3[0])+sq(L3[1])+sq(L3[2]);

float xyz_present[3]; //координаты текущего положения TCP манипулятора

float xyz_goal[3]; //целевые координаты TCP

int32_t goal_position[6] = {2048, 2048, 2048, 2048, 512, 300}; //целевое положение приводов

int32_t present_position[6] = {0, 0, 0, 0, 0, 0}; //текущее положение приводов

int32_t present_load[6] = {0, 0, 0, 0, 0, 0}; //текущая нагрузка приводов (max: 1023)

int32_t angle_max[6] = {4086, 3167, 3080, 2995, 0, 666}; //задание максимальных допустимых значений углов

int32_t angle_min[6] = {10, 697, 875, 857, 1014, 0}; //задание минимальных допустимых значений углов

int32_t speed_max[6] = {25, 25, 25, 25, 25, 50}; //задание установившейся скорости (1 ~ 0.11rpm; max: 1024 ~ 114rpm)

int32_t torque_max[6] = {300, 300, 300, 300, 300, 100}; //задание максимального момента (max: 1023)

int32_t delay_time = 0; //задержка ответа привода (1 - 2мкс; max: 254 - 0.508мс)

int tm = 0;

const uint8_t handler_index = 0;

uint8_t dxl_id[6] = {1,2,3,4,5,6}; //перечисление ID всех приводов (1 - поворот платформы; ... 6 - зажим схвата)

float temp_track;

float vel_track = 0.005;

float step_track = 0.005;

int32_t goal_velocity[6];

void setup(){

Serial.begin(57600);

const char *log;

bool result = false;

uint16_t model_number = 0;

result = dxl_wb.init(DEVICE_NAME, BAUDRATE, &log);

for (int i = 0; i < 6; i++)

{

result = dxl_wb.ping(dxl_id[i], &model_number, &log);

if (result == false)

{

dxl_wb.ping(dxl_id[i], &model_number, &log);

}

result = dxl_wb.jointMode(dxl_id[i], 0, 0, &log);

result = dxl_wb.itemWrite(dxl_id[i],"Return_Delay_Time", delay_time, &log);

result = dxl_wb.itemWrite(dxl_id[i],"Moving_Speed", speed_max[i], &log);

result = dxl_wb.itemWrite(dxl_id[i],"I_Gain", 64, &log);

result = dxl_wb.itemWrite(dxl_id[i],"CW_Angle_Limit", angle_min[i], &log);

result = dxl_wb.itemWrite(dxl_id[i],"CCW_Angle_Limit", angle_max[i], &log);

result = dxl_wb.itemWrite(dxl_id[i],"Max_Torque", torque_max[i], &log);

}

tm = millis();

}

void loop()

{

const char *log;

bool result = false;

if(millis()-tm<5000){

xyz_goal[0] = 0.3;

xyz_goal[1] = 0.2;

xyz_goal[2] = 0.2;

}else if(millis()-tm<10000){

xyz_goal[0] = 0.1;

xyz_goal[1] = -0.3;

xyz_goal[2] = 0.2;

}else{

tm = millis();

}

inverse_kinematic(goal_position, xyz_goal[0], xyz_goal[1], xyz_goal[2]);

result = sync_write(goal_position, "Goal_Position");

result = sync_read(present_position, "Present_Position");

forward_kinematic(xyz_present, present_position[0], present_position[1], present_position[2], present_position[3], present_position[4]);

temp_track = step_track/sqrt(sq(xyz_goal[0]-xyz_present[0])+sq(xyz_goal[1]-xyz_present[1])+sq(xyz_goal[2]-xyz_present[2]))+1;

inverse_kinematic(goal_velocity, xyz_present[0]*temp_track, xyz_present[1]*temp_track, xyz_present[2]*temp_track);

for(int i=0; i<6; i++){

goal_velocity[i] = round(abs(goal_velocity[i]-present_position[i])*vel_track/step_track);

if(goal_velocity[i]>100) goal_velocity[i] = 100;

if(goal_velocity[i]<1) goal_velocity[i] = 1;

}

result = sync_write(goal_velocity, "Moving_Speed");

}

//Определение координат рабочей точки по углам привода

void forward_kinematic(float xyz[3], float q1, float q2, float q3, float q4, float q5) {

//Приведение из си углов приводами в СИ

q1 = (q1-2048)/2048*3.1416;

q2 = -(q2-2252)/2048*3.1416;

q3 = -(q3-2048)/2048*3.1416;

q4 = -(q4-2048)/2048*3.1416;

q5 = (q5-512)/512*3.1416;

//Повороты систем координат с накоплением длин

xyz[0] = LTCP[0];

xyz[1] = LTCP[1];

xyz[2] = LTCP[2];

xyz[0] += L5[0];

xyz[1] += L5[1];

xyz[2] += L5[2];

rot_coordinate(xyz, q5, 1);

xyz[0] += L4[0];

xyz[1] += L4[1];

xyz[2] += L4[2];

rot_coordinate(xyz, q4, 2);

xyz[0] += L3[0];

xyz[1] += L3[1];

xyz[2] += L3[2];

rot_coordinate(xyz, q3, 2);

xyz[0] += L2[0];

xyz[1] += L2[1];

xyz[2] += L2[2];

rot_coordinate(xyz, q2, 2);

xyz[0] += L1[0];

xyz[1] += L1[1];

xyz[2] += L1[2];

rot_coordinate(xyz, q1, 3);

}

//Определения углов приводов манипулятора по целевым координатам рабочей точки

void inverse_kinematic(int32_t q[6], float x, float y, float z) {

float Q[6];

float xyz4[3];

if(x==0)

{

if(y==0)

{

Q[0] = round((q[0]-2048)*3.1416/2048);

}else{

Q[0] = 3.1416/2;

}

}else

{

Q[0] = atan(y/x);

}

xyz4[0] = x-cos(Q[0])*(L4[0]+L5[0]+LTCP[0])+sin(Q[0])*(L4[1]+L5[1]+LTCP[1]);

xyz4[1] = y-sin(Q[0])*(L4[0]+L5[0]+LTCP[0])-cos(Q[0])*(L4[1]+L5[1]+LTCP[1]);

xyz4[2] = z-L4[2]-L5[2]-LTCP[2];

float LL23 = sq(xyz4[0]-L1[0])+sq(xyz4[1]-L1[1])+sq(xyz4[2]-L1[2]);

float fi_L23_x = asin((L1[2]-xyz4[2])/sqrt(LL23));

Q[2] = 3.1416/2-acos((LL2+LL3-LL23)/(2*sqrt(LL2*LL3)));

Q[1] = 3.1416/2+fi_L23_x-asin(sqrt(LL3/LL23)*sin(3.1416/2-Q[2]));

Q[3] = -Q[1]-Q[2];

Q[4] = 0;

Q[5] = 0; // схват: 0 - полностью сжат; 1 - полностью открыт

//Приведение углов к системе измерения углов в приводах манипулятора

q[0] = int32_t(round(Q[0]*2048/3.1416)+2048);

q[1] = int32_t(-round(Q[1]*2048/3.1416)+2252);

q[2] = int32_t(-round(Q[2]*2048/3.1416)+2048);

q[3] = int32_t(-round(Q[3]*2048/3.1416)+2048);

q[0] -= (q[0]/4096)*4096;

q[1] -= (q[1]/4096)*4096;

q[2] -= (q[2]/4096)*4096;

q[3] -= (q[3]/4096)*4096;

//q[4] = int(round(Q[4]*512/3.1416+512));

//q[5] = int(round(Q[5]*376+290));

}

//r - ось вращения (1 - x, 2 - y, 3 - z)

void rot_coordinate(float m[3], float q, int r) {

if(r==1){

float tmp=m[1];

m[1] = (m[1]*cos(q)-m[2]*sin(q));

m[2] = tmp*sin(q)+m[2]*cos(q);

}

if(r==2){

float tmp=m[0];

m[0] = m[0]*cos(q)+m[2]*sin(q);

m[2] = -tmp*sin(q)+m[2]*cos(q);

}

if(r==3){

float tmp=m[0];

m[0] = m[0]*cos(q)-m[1]*sin(q);

m[1] = tmp*sin(q)+m[1]*cos(q);

}

}

bool sync_write(int32_t arr[6], char *cont_name)

{

bool result = 1;

bool rez = 1;

const char *log;

for(int i=0; i<6; i++){

result = dxl_wb.itemWrite(dxl_id[i], cont_name, arr[i], &log);

if (result == false)

{

result = dxl_wb.itemWrite(dxl_id[i], cont_name, arr[i], &log);

}

}

return rez;

}

bool sync_read(int32_t arr[6], char *cont_name)

{

bool result = 1;

bool rez = 1;

const char *log;

for(int i=0; i<6; i++){

result = dxl_wb.itemRead(dxl_id[i], cont_name, &arr[i], &log);

if (result == false)

{

result = dxl_wb.itemRead(dxl_id[i], cont_name, &arr[i], &log);

}

}

return rez;

}