下面是我的一些思路
机械臂总体结构图:
下面是总体的建模部分, 由于机械臂自由度不多,运用几何法可以更好的理解:
具体的思路大致如上,由于本人表达能力不强并且时间有限,没有具体一一介绍了,几何法结合代码能很快理解,如过有不懂的地方可以问我。
加上实现的视频效果:
VID20240326211602
给定目标点的位置便能求出对应的舵机角度,并且转换成总线舵机能够识别的pwm。 舵机需要270度舵机,利用上位机控制舵机应该在500到2500之间,1500为中间位置。
下面是相关的stm32串口发送代码:
#include "stm32f10x.h" // Device header
#include "Delay.h"
#include <math.h>
#define PI 3.1415926
#include <stdio.h>
#include "USART3.h"
#include "OLED.h"
//uint8_t action[]="{#000Pservo_pwm[0]T1000!#001Pservo_pwm[1]T2000!#002Pservo_pwm[2]T2000!} ";
//uint8_t action[]="{#000P1502T1000!#001P1700T2000!#002P1700T2000!}";
char action1[256];
char action2[256];
char action3[256];
double servo_angle[4] = {0, 0, 0, 0};//存放角度
int servo_pwm[4] = {0, 0, 0, 0};//舵机pwm
double L0;
double L1;
double L2;
double L3;
uint8_t a[]="{#255P1500T1000!}";
int kinematics_analysis(double x, double y, double z, double Alpha) {
x *= 10;
y *= 10;
z *= 10;//扩大方便计算
double theta6 = 0.0;
if(x == 0 && y!= 0)
theta6 = 0.0;
else if(x > 0 && y == 0)
theta6 = 90;
else if(x < 0 && y == 0)
theta6 = -90;
else
theta6 = atan(x/y)*180.0/PI;//舵机云台旋转
// x,x坐标斜边
y = sqrt(x * x + y * y);
y =y- L3 * cos(Alpha * PI / 180.0);
z = z - L0 - L3 * sin(Alpha * PI / 180.0);
if (z < -L0) return 1;
if (sqrt(y * y + z * z) > (L1 + L2)) return 2;
double ccc = acos(y / sqrt(y * y + z * z));
double bbb = (y * y + z * z + L1 * L1 - L2 * L2) / (2 * L1 * sqrt(y * y + z * z));
if (bbb > 1 || bbb < -1) return 3;
double theta5 = ccc * (z < 0 ? -1 : 1) + acos(bbb);//一号舵机弧度
theta5 *= 180.0 / PI;//一号多机角度
if (theta5 > 180.0 || theta5 < 0.0) return 4;
double aaa = -(y * y + z * z - L1 * L1 - L2 * L2) / (2 * L1 * L2);
if (aaa > 1 || aaa < -1) return 5;
double theta4 = acos(aaa);//二号舵机弧度
theta4 = 180.0 - theta4 * 180.0 / PI;//角度
if (theta4 > 135.0 || theta4 < -135.0) return 6;
double theta3 = -Alpha +theta5 - theta4;//三号舵机
if (theta3 > 90.0 || theta3 < -90.0) return 7;
servo_angle[0] = theta6;
servo_angle[1] = 90-theta5;
servo_angle[2] = theta4;
servo_angle[3] = theta3;
servo_pwm[0] = (int)(1500 - 2000.0 * servo_angle[0] / 270.0);
servo_pwm[1] = (int)(1500 + 2000.0 * servo_angle[1] / 270.0);
servo_pwm[2] = (int)(1500 + 2000.0 * servo_angle[2] / 270.0);
servo_pwm[3] = (int)(1500 + 2000.0 * servo_angle[3] / 270.0);//pwm波
return 0;
}
int kinematics_move(double x, double y, double z) {
int a=0;int b=0;int c=0;double max=-1;double min=100;
int flag =0;
double best_alpha=0;
// if(y < 0)
// return 0;
for (int i = -90; i <= 90; i++) {
if (kinematics_analysis(x, y, z, i)==0)
{a++;}}//得到可行解数目
int alpha_list[a];//存放舵机可行解
for (int i = -90; i <=90; i++) {
if (kinematics_analysis(x, y, z, i)==0)
{alpha_list[b]=i;b++;}}
if (a!=0)
{ if (y > 215)
{for(c=0;c<=a;c++)
if(abs(alpha_list[c])<min) best_alpha =alpha_list[c]; min=abs(alpha_list[c]);
//取出可行解最大值
}
if (y <= 215)
{for(c=0;c<=a;c++)
if(alpha_list[c]>max) best_alpha =alpha_list[c];max=abs(alpha_list[c]);
}
flag = 1;
}
if (flag) {
kinematics_analysis(x, y, z, best_alpha);
// printf("%d,%d,%d,%d",servo_pwm[0],servo_pwm[1],servo_pwm[2],servo_pwm[3]);
//********* pwm打印输出,每次为四个舵机对应的pwm***********
// return 1;
}
return 0;
}
int main() {
USART3_Init();
OLED_Init();
L0 =100;L1=89;L2=73;L3=130;
//L0 =82;L1=88;L2=73;L3=145;
L0 = L0 * 10;
L1 = L1 * 10;
L2 = L2 * 10;
L3 = L3 * 10;//运动学参数设置
USART3_SendString(a);
Delay_ms(2000);
kinematics_move(0.0, 130.0, 150.0);
snprintf(action1, sizeof(action1), "{#000P%dT1000!#001P%dT2000!#002P%dT2000!#003P%dT1500!}",
servo_pwm[0],
servo_pwm[1],
servo_pwm[2],
servo_pwm[3] );
OLED_ShowSignedNum(1,1, servo_pwm[0], 4);
OLED_ShowSignedNum(2,1, servo_pwm[1], 4);
OLED_ShowSignedNum(3,1, servo_pwm[2], 4);
OLED_ShowSignedNum(4,1, servo_pwm[3], 4);
USART3_SendString(action1);
Delay_ms(4000);
kinematics_move(50.0, 150.0, 150.0);
snprintf(action2, sizeof(action2), "{#000P%dT1000!#001P%dT2000!#002P%dT2000!#003P%dT1500!}",
servo_pwm[0],
servo_pwm[1],
servo_pwm[2],
servo_pwm[3] );
OLED_ShowSignedNum(1,1, servo_pwm[0], 4);
OLED_ShowSignedNum(2,1, servo_pwm[1], 4);
OLED_ShowSignedNum(3,1, servo_pwm[2], 4);
OLED_ShowSignedNum(4,1, servo_pwm[3], 4);
USART3_SendString(action2);
Delay_ms(4000);
kinematics_move(-50.0, 100.0, 100.0);
snprintf(action3, sizeof(action3), "{#000P%dT1000!#001P%dT2000!#002P%dT2000!#003P%dT1500!}",
servo_pwm[0],
servo_pwm[1],
servo_pwm[2],
servo_pwm[3] );
OLED_ShowSignedNum(1,1, servo_pwm[0], 4);
OLED_ShowSignedNum(2,1, servo_pwm[1], 4);
OLED_ShowSignedNum(3,1, servo_pwm[2], 4);
OLED_ShowSignedNum(4,1, servo_pwm[3], 4);
USART3_SendString(action3);
Delay_ms(4000);
return 0;
}