arduino三自由度机械臂

三自由度机械臂设计报告

我们的机械臂参照人体小臂的结构：手肘处两个自由度（一个水平方向一个垂直方向），手腕处一个（垂直方向）。按照题目要求在30*30的坐标系内我们将（0,15）设为底座放置点（0,0）为机械臂初始位置。由此可知机械臂需达到的最远距离为15*√5，考虑到需要有螺钉固定的重合距离暂定臂长为：大臂长20cm，小臂长15cm。且参考模型的机械结构暂定用双臂。按照最初设计安装好之后，我们发现所购买的舵机并不能带动这么长的臂长，于是我们将臂长改成10cm+12cm并将双臂减少为单臂。该方案能实现半径4cm左右的圆的绘制，找点的误差在0.5-1cm左右。

一．找坐标

设底盘水平方向的舵机角度为s，手肘处垂直舵机角度为θ1，手腕处角度为θ2。确定坐标时先根据输入的（X，Y）得s=arctan（x/(y-15)）。

可以列出方程式组ρ=acosθ1+bcosθ2

△h=asinθ1+bsinθ2

θ3=θ1+θ2

解得θ1=arcsin((ρ²﹢△h²＋a²-b²)/(2a√(△h²+ρ²)))-arctan(ρ/△h)

θ2=arcsin((asinθ1-△h)/b)

θ3=θ1+θ2

（其中a=10cm，b=12cm，△h=3cm）

二．画圆

方案一：

圆可以分为两部分的配合而组成的。垂直自由度舵机的来回划线运动及底盘水平自由度舵机的左右旋转运动当水平舵机转到设定最大值的时间与垂直舵机划线划到中点的时间相同时就能得到一个椭圆，而当左右转动到设定的最大值之间的距离与划线的距离相等时就构成了一个圆。

我们先将圆划分为四部分如下：

调试程序后发现s的变化速率也是变化的。于是加上红色两条线使水平方向线分为4份

利用找点的公式确定五个交点各自对应的θ1,s值，再各自进行相减分别算出四段运动相对应角度变化的平均速率。

该方案的难点在于时间的合理搭配及s的速率补偿划分

方案二：

根据圆心的坐标在坐标里找圆周上一系列的点在将其连线构成圆。

该方案思路较简单清晰但容易造成误差的累积，对舵机的精度要求较高，且对之前找坐标的算法精确性要求很高。而且有个严肃的问题斜着的两点不是直线移过去的而是折线，所以定点需特别相近，可以选择建立一个数据库进行点数据的调用达到目的。

综合考虑了以上两种方案的优缺点及现有的硬件条件，我选择了方案一。

三．主要程序

#include

//定义舵机引脚

Servo myservo1;

Servo myservo2;

Servo myservo3;

//定义极坐标

float x=15.0,y=10.0,r=4;

float l;

float s,jiao1,jiao2;

float rd=57.3;

//按钮定义

int BUTTON1 = 9;

int BUTTON2 = 8;

void setup() {

myservo1. attach(3);

myservo2. attach(5);

myservo3.attach(6);

pinMode(BUTTON1,INPUT);

pinMode(BUTTON2,INPUT);

}

void loop() {

l=sqrt(sq(x)+sq(y-15.0));

//分情况讨论得s

if(y<15)

s=atan(x/(15.0-y))*rd;

else if(y==15)

s=90;

else

s=atan(x/(15.0-y))*rd+180;

if(digitalRead(BUTTON1)==HIGH)//找点

{FD();

myservo1.write(s);

delay(30);

myservo2.write(jiao1*rd);

delay(30);

myservo3.write(jiao2*rd);

delay(30);

}

else if(digitalRead(BUTTON2)==HIGH)//画圆

huayuan();

}

void FD()//计算角1角2

{

float number1,number2,number3;

number2=atan(l/3.0);

number1=(sq(l)-35.0)/(20.0*sqrt(9+sq(l)));

number1=3.14-asin(number1);

jiao1=(number1-number2);//计算角1弧度值

number3=(10.0*sin(jiao1)-3.0)/12.0;

jiao2=(asin(number3)+jiao1);//角2弧度值

}

void huayuan()

{

float number1,number2,number3,number4,number5,number6; int i;

float jiao11,jiao12,jiao13;

float du1=3.25,du2=1;

FD();

number1=jiao1;

l=l-r;

FD();

jiao11=jiao1;

l=l+r/2.0;

FD();

jiao12=jiao1;

l=l+r;

FD();

jiao13=jiao1;

l=l+r/2.0;

FD();

myservo1.write(s);

myservo2.write(jiao1*rd);

myservo3.write(jiao2*rd);

number2=((jiao13-jiao1)/5.0);

number3=((number1-jiao13)/5.0);//前半圆 number5=(jiao12-number1)/5.0;

number6=(jiao11-jiao12)/5.0;//后半圆

for(i=0;i<20;i++)

{ if(i<5)

{ s=s-du1;

jiao1=jiao1+number2;

}

else if(i>=5&&i<10)

{s=s-du2;

jiao1=jiao1+number3;

}

else if(i>=10&&i<15)

{ s=s+du2;

jiao1=jiao1+number5;

}

else if(i>=15)

{s=s+du1;

jiao1=jiao1+number6;

}

number4=(10.0*sin(jiao1)-3.0)/12.0; jiao2=(asin(number4)+jiao1);

myservo1.write(s);

myservo2.write(jiao1*rd);

myservo3.write(jiao2*rd);

delay(100);

}

for(i=19;i>=0;i--)

{

if(i>=15)

{s=s+du1;