5自由度并联机械臂实现搬运功能
本帖最后由 机器谱 于 2023-4-20 08:59 编辑1. 功能说明
本文示例将实现R306样机5自由度并联机械臂搬运牛奶到指定地点的功能。该机械臂由1个5自由度并联机械臂和1个单轴丝杠平台构成，机械臂通过并联的方式同时控制同一个端点的运动。其驱动系统采用大扭矩舵机和小扭矩舵机、执行末端为夹持机构。
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgwIDZoQYojMTa5gEwpxg4_RE!450x450.png.webphttps://28846868.s21i.faiusr.com/3/ABUIABADGAAggITZoQYomYmO1QQwsgQ4uwI.gif.webp
2. 并联机械臂机械系统
并联机械臂与串联机械臂每个独立的关节模块组成不同，并联机械臂是通过连杆组将多个驱动同时共同控制机械臂末端，如下图（四连杆并联机械臂）所示，可以分解为三个部分：
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgzYTZoQYo1qXHqgcwuwQ4nQU!300x300.png.webp
第一个部分是下图中蓝色的四连杆，舵机y作为驱动，舵机x固定不动，这时会形成一个FBCD四连杆结构，FB为机架，FD为驱动杆；
第二个部分是下图中红色的四连杆，舵机x作为驱动，舵机y固定不动，这时会形成一个ABCD四连杆，其中AD作为机架，AB作为驱动杆，BC作为传动杆，CD作为随动杆；
第三部分为DGHI平行四连杆，这个部分没有驱动，主要作用是保证执行端HI保持一个方向；
综上可知CD杆为第一部分四连杆和第二部分四连杆共同控制的杆件。
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAg14XZoQYouZG_nwIw3wY41QI.png.webp
3. 并联机械臂运动学算法
当机械臂完成动作时，需通过舵机对L1和L2了两根连杆进行控制和调节，2个舵机的角度分别为θ1和θ2。为了方便分析，将机械臂简化进行分析。
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgv4bZoQYoh6K66gIwowI4zwI!300x300.png.webp
图中L1=AB，L2=BC，L3=CD，L4=DA，L5=AF，L6=DF，其中假设舵机x和舵机y的0°和180°的极限位置为AF杆方向，θ1和θ2分别为舵机x和舵机y的转动角度，其中L1、L2、L3、L5、L6都为已知尺寸（可用尺子量出）。
在这个模型中，并联机械臂的运动算法就是CD杆的运动轨迹，如果要求出CD杆的运动轨迹，实际是求出CD杆与DF杆的夹角∠CDF。
从图中可知：∠CDF=∠CDA+∠ADF；
∠CDA位于四连杆ABCD中，通过欧拉公式可以推导；∠ADF位于三角形ADF中，可以通过三角形余弦定理推导；
求解∠CDA，假设∠CAD=α：
根据欧拉公式展开得：
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgqIfZoQYo-tX9sAQwgQk4tgI!700x700.png.webp
从上面的公式中可知我们还需要求出L4和∠BAD；
L4位于三角形ADF中，可通过三角函数求解得出。在三角形ADF中需要知道两条相邻边长和该相邻边的夹角，其中L6和L5为已知量，所以：
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAg-ofZoQYorunbyQcw3gQ4Vg!400x400.png.webp
其中∠DFA=180°-θ2；
∠BAD=180°-θ1-∠DAF；
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgxIjZoQYogNLjdzCRCDh5!700x700.png.webp
同理可得：
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAg_IjZoQYogPqq0Qcw-wQ4RQ.png.webp
最终可解得：
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAg4InZoQYoyqCUtQUw-wQ4RQ.png.webp
计算机械臂的端点I的运动轨迹，可建立舵机x和舵机y转动中心连线为x轴的平面坐标系，如下图所示：
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgoIvZoQYom5fv-gEwigU4wgQ!450x450.png.webp
如上图所示，对D点和I点（x，y）作平面直角坐标系的投影，根据前面计算出的D点角度位置计算I点的运动坐标，最终可获得I（f（θ1，θ2），f（θ1，θ2））的关系。
4. 电子硬件
在这个示例中，采用了以下硬件，请大家参考：
主控板Basra主控板（兼容Arduino Uno）
扩展板Bigfish2.1扩展板
电池7.4V锂电池
电路连接：舵机接线：由上至下分别连接在Bigfish扩展板的D4、D7、D3、D8、D12接口。
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgko7ZoQYoltT7sQEw9gs4pQY!600x600.png.webp
5. 功能实现
上位机：Controller 1.0
下位机编程环境：Arduino 1.8.19
① 将参考例程（get_milk_new2.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-07 https://www.robotway.com/
------------------------------*/
#include <Servo.h>
#define servo_speed 120 //servo_speed
#define action_delay 1000 //action_delay
Servo myServo;
int servo_port={4, 7, 3, 8, 12}; //servo_pin
float value_init={115, 60, 156, 35, 75}; //servo_value_inital
int num = 10;
int servo_num = sizeof(servo_port)/sizeof(servo_port); //servo_pin length
boolean debug = false; //if(true) debug
void setup() {
Serial.begin(9600);
for(int i=0;i<servo_num;i++)
ServoGo(i,value_init);
delay(action_delay);
}
void loop() {
servo_move(115, 105, 160, 110, 20);
servo_move(140, 60, 160, 35, 75);
get_milk();
putMilk_near();
servo_move(115, 105, 160, 110, 20);
servo_move(140, 60, 160, 35, 75);
get_milk();
putMilk_far();
}
void ServoStart(int which)
{
if(!myServo.attached())myServo.attach(servo_port);
pinMode(servo_port, OUTPUT);
}
void ServoStop(int which)
{
myServo.detach();
digitalWrite(servo_port,LOW);
}
void ServoGo(int which , int where)
{
if(where!=200)
{
if(where==201) ServoStop(which);
else
{
ServoStart(which);
myServo.write(where);
}
}
ServoStart(which);
myServo.write(where);
}
void get_milk()
{
servo_move(115, 105, 160, 110, 20); //down
servo_move(160, 95, 160, 95, 40); //get
servo_move(160, 60, 160, 35, 75); //up
delay(action_delay);
servo_move(160, 60, 25, 35, 75); //turn
}
void putMilk_near()
{
//Serial.println("near");
servo_move(160, 60, 25, 60, 65); //down
servo_move(140, 60, 25, 60, 65); //put
servo_move(115, 60, 160, 35, 75);//reset
}
void putMilk_far()
{
//Serial.println("far");
servo_move(160, 100, 22, 56, 54); //down
servo_move(140, 103, 22, 56, 54); //put
servo_move(115, 60, 160, 35, 75); //reset
}
void servo_move(float value0, float value1, float value2, float value3, float value4)
{
//Serial.println("begin");
float value_arguments[] = {value0, value1, value2, value3, value4};
float value_delta;
for(int i=0;i<servo_num;i++)
{
value_delta = (value_arguments - value_init) / num;
}
for(int i=0;i<num;i++)
{
for(int k=0;k<servo_num;k++)
{
value_init = value_delta == 0 ? value_arguments : value_init + value_delta;
}
for(int j=0;j<servo_num;j++)
{
ServoGo(j,value_init);
}
delay(servo_speed);
}
delay(action_delay);
}
② 双击打开Controller 1.0.exe。将波特率与串口设置好，同时留下4、7、3、8、12几个舵机串口，调试舵机角度。具体操作步骤可参考 如何驱动模拟舵机-Controller 1.0b软件的使用 。
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAg-pHZoQYouea4tAIwtgo4ogc!600x600.png.webp
③ 将调试好的舵机角度写到对应例程（get_milk_new2.ino）中的位置；这里以几个角度为例，大家可尝试自己更改角度使其动作看起来更流畅。
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAguJLZoQYolITkNDCEBTiFCA!800x800.png.webp
6. 资料下载
资料内容：
①搬运-例程源代码
②搬运-样机3D文件
③Controller1.0b资料包
资料下载地址：https://www.robotway.com/h-col-206.html
想了解更多机器人开源项目资料请关注 机器谱网站 https://www.robotway.com
页:
[1]