6自由度并联拉线写字机器人实现写字功能网站首页 技术交流

6自由度并联拉线写字机器人实现写字功能

Robotway 2024-06-17 10:25:04

简介6自由度并联拉线写字机器人实现写字功能

1. 功能说明

本文示例将实现R287样机6自由度并联拉线写字机器人写字（机器时代）的功能。

该机器人有两部分：绘图机构、走纸机构。绘图机构由6个舵机模块近似正六边形位置分布，共同控制位于中心的画笔；还具备一个走纸机构，走纸机构是由一个大圆周舵机驱动的。

2. 6自由度并联拉线写字机器人逆解算法

该6自由度并联拉线写字机器人的运动控制采用逆运动更容易一些，下面我们将对其逆运动算法进行介绍。我们先确定该6自由度并联拉线写字机器人的位置，通过建立坐标系的方法确定位置。这里我们选择在6自由度并联拉线写字机器人外一点建立一个直角坐标系，Z轴范围——笔架上下接线间距60，坐标系Z轴0点为7X11平板平面，这里面我们需要求解出每个舵机转动角度与画笔位置的关系：

各舵机坐标（注意这里面的Z轴坐标是以实际作用到舵机上的为准）

1（97，55，-10）

2（25，200，50）

3（97，345，-10）

4（302，345，50）

5（375，200，-10）

6（302，55，50）

中心点（x,y,z）；目标点（xt,yt,zt）；舵机半径 radius——24.0

中心点到每个舵机的距离：

$basic\_dist=sqrt{(x-x_i)^2+(y-y_i)^2+(z-z_i)^2}$

目标点到每个舵机的距离：

$dist=sqrt{(x_t-x_i)^2+(y_t-y_i)^2+(z_t-z_i)^2}$

中心点到目标点舵机需要转动的角度（弧长公式）：

$degree=90.0+frac{(dist-basic\_dist) imes 180}{M\_PI imes radius} +0.5$

其中90.0为笔在中心时舵机初始角度（这个很重要，舵机角度安装一定要注意），M_PI=3.1415926，0.5用于五入（为了补充运动过程中无法避免的损耗产生的运动误差）。

3. 电子硬件

本实验中采用了以下硬件：

主控板	Basra主控板（兼容Arduino Uno）‍
扩展板	Bigfish2.1扩展板‍
电池	7.4V锂电池
其它	画笔、画笔套管、连线、纸张等

电路连接说明： 舵机连接：按圆盘顺时针方向，舵机位置依次对应Bigfish扩展板的D4, D7, D11,D3, D8, D12

4. 功能实现

编程环境：Arduino 1.8.19

下面提供一个6自由度并联拉线写字机器人写字（机器时代）的参考例程（servo_writing.ino）：

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-04-20 https://www.robotway.com/

  ------------------------------*/

/*

* 舵机连接，按圆盘顺时针舵机位置依次对应Bigfish：4, 7, 11, 3, 8, 12

* 书写范围：dx --> 50; dy --> 40

*/

#include <Arduino.h>

#include <avr/pgmspace.h>

#include "model.h"

#include "words.h"

#include <Servo.h>


#define SQU 0

#define JI 1

#define QI 2

#define SHI 3

#define DAI 4


Servo myServo[SERVO_NUM];     //笔筒控制舵机数组

Servo myServo1;               //走纸舵机

Servo myServo2;               //压纸舵机

int servo_port[SERVO_NUM] = {4,7,11,3,8,12};   //笔筒控制舵机引脚   

int words_num[10] = {};   


Point squ[] = {};              //字体

Point ji[] = {};

Point qi[] = {};

Point shi[] = {};

Point dai[] = {};   


int pos_x = 0, pos_y = 1, pos_z = 2;


boolean pos_test = false;      //位置测试, 真为测试


void setup() {

  Serial.begin(9600);

  myServo1.attach(16);         //走纸舵机引脚

  myServo2.attach(17);         //压纸舵机引脚

  myServo1.write(88);          //走纸停

  pressServoDown();            //压纸

 

  if(pos_test) posTest();      //坐标位置测试

  wordsArrayLength();          //计算flash中存储的字体数组长度

  delay(1000);

}


void loop() {                  //写字

//   writing(SQU);

  writing(JI);                 //机

  writing(QI);                 //器

  writing(SHI);                //时

  writing(DAI);                //代

  while(1){};

}


void setPos(Point pos) {                               

  static const float _basic_dists[kActuatorCount] = {

    distance(kInitialPoint, kActuatorOrigins[0]),

    distance(kInitialPoint, kActuatorOrigins[1]),

    distance(kInitialPoint, kActuatorOrigins[2]),

    distance(kInitialPoint, kActuatorOrigins[3]),

    distance(kInitialPoint, kActuatorOrigins[4]),

    distance(kInitialPoint, kActuatorOrigins[5])

  };

  int degree[kActuatorCount] = {};

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

  {

    float dist = distance(pos, kActuatorOrigins[i]);

    float deg = 90.0 + (dist - _basic_dists[i]) /

                   kActuatorRadius / M_PI * 180.0;

    degree[i] = floor(deg+0.5);

  }

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

  {

    ServoGo(i, map(degree[i], 0, 180, 700, 2100));

  }

}


void writeLine(Point a, Point b, unsigned long t) {

  static const int dt = 50;

  unsigned long k = t / dt;

  float dx = (b.x - a.x) / (float)k;

  float dy = (b.y - a.y) / (float)k;

  Point p = a;

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

  {

    setPos(p);

    delay(dt);

    p.x += dx;

    p.y += dy;

  }

}


void ServoStart(int which)

{

  if(!myServo[which].attached())myServo[which].attach(servo_port[which]);

  pinMode(servo_port[which], OUTPUT);

}



void ServoStop(int which)

{

  myServo[which].detach();

  digitalWrite(servo_port[which],LOW);

}


void ServoGo(int which , int where)

{

    ServoStart(which);

    myServo[which].writeMicroseconds(where);

}


void posTest(){

//    setPos({175, 185, 20});   //最小坐标

//    delay(1000);

    setPos({200, 200, 50});   //中间坐标,确定笔的位置时可将位置设置为中间坐标，打开开关然后机械调整舵机角度直到笔筒在中间位置即可

    delay(1000);

//    setPos({225, 225, 20});   //最大坐标

//    delay(1000);


    while(1){delay(10);};

}


void wordsArrayLength(){

   words_num[0] = sizeof(squArray) / sizeof(squArray[0]) / 3;

   words_num[1] = sizeof(jiArray) / sizeof(jiArray[0]) / 3;             //机

   words_num[2] = sizeof(qiArray) / sizeof(qiArray[0]) / 3;             //器

   words_num[3] = sizeof(shiArray) / sizeof(shiArray[0]) / 3;           //时

   words_num[4] = sizeof(daiArray) / sizeof(daiArray[0]) / 3;           //代

}


void readProgmem(int p, Point a, Point b, int ary[]){

    a.x = pgm_read_word_near( ary + p * 3 + pos_x) + 175;

    a.y = pgm_read_word_near( ary + p * 3 + pos_y) + 185;

    a.z = pgm_read_word_near( ary + p * 3 + pos_z);

   

    b.x = pgm_read_word_near( ary + (p + 1) * 3 + pos_x) + 175;

    b.y = pgm_read_word_near( ary + (p + 1) * 3 + pos_y) + 185;

    b.z = pgm_read_word_near( ary + (p + 1) * 3 + pos_z);

   

    writeLine(a, b, 500);


//    Serial.print(a.x);  

//    Serial.print("_");

//    Serial.print(a.y);

//    Serial.print(" ");

//    Serial.print(b.x);  

//    Serial.print("_");

//    Serial.println(b.y);

}


void writing(int which){

  for(int i=0;i<words_num[which] - 1;i++){

    switch(which){

        case 0:

          readProgmem(i, squ[i], squ[i+1], squArray);

        break;

        case 1:

          readProgmem(i, ji[i], ji[i+1], jiArray);

        break;

        case 2:

          readProgmem(i, qi[i], qi[i+1], qiArray);

        break;

        case 3:

          readProgmem(i, shi[i], shi[i+1], shiArray);

        break;

        case 4:

          readProgmem(i, dai[i], dai[i+1], daiArray);

        break;

    }

    pos_x = 0;

    pos_y = 1;

    pos_z = 2;

  }

  setPos({200, 200, 50});

  delay(1000);

  pressServoUp();

  delay(100);

  positionSwitch();

  pressServoDown();

}


void positionSwitch(){                //走纸函数

    myServo1.write(80);

    delay(500);

    myServo1.write(88);

    delay(100);

}


void pressServoDown(){                //压杆落

  myServo2.write(70);  

}


void pressServoUp(){                  //压杆抬

  myServo2.write(75);  

}