关于逆运动学解算总线舵机stm32串口发送的实现

下面是我的一些思路

机械臂总体结构图:

下面是总体的建模部分， 由于机械臂自由度不多，运用几何法可以更好的理解：

        具体的思路大致如上，由于本人表达能力不强并且时间有限，没有具体一一介绍了，几何法结合代码能很快理解，如过有不懂的地方可以问我。

加上实现的视频效果：

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;
}