机械臂学习——舵机的速度控制、坐标变换、DH模型、正运动学解、逆运动学解_舵机速度可以调节吗

机械臂学习

前言

舵机工作原理：


通过单片机产生PWM信号控制舵机。

一、舵机的速度控制

1.舵机概述

工作电压：4.8v-6v
舵机的驱动是比较容易的，当我们使用单片机控制的时候，通过输出50HZ（20ms的周期）的PWM，控制PWM脉宽调节舵机的转角。舵机的转角与脉宽（高电平时间）存在一一对应关系，如果控制舵机转到某一角度，就改变输出脉宽即可，比如从1ms到1.5ms。那么如何进行舵机的速度控制呢？

2.代码实现

float Position_PID1 (float Encoder,float Target)
{ 	
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=Target-Encoder;                                  //计算偏差
	 Integral_bias+=Bias;	                                 //求出偏差的积分
	 Pwm=Position_KP*Bias/100+Position_KI*Integral_bias/100+Position_KD*(Bias-Last_Bias)/100; //位置PID控制器
	 Last_Bias=Bias;                                       //保存上一次偏差
	 return Pwm;                                           //增量输出
}
1
2
3
4
5
6
7
8
9

P=5、I=0、D=2

二、机械臂的坐标变换

1.坐标变换

坐标系是根据DH法建立的

2.坐标的几何概述

单位向量——模等于1的向量。由于是非零向量，单位向量具有确定的方向。

由坐标系OXaYa绕Z轴旋转得到新的坐标系OXbYb



如果三维连续旋转变换就会复制很多，当然神奇的DH模型会解决这些问题。

三、DH模型和正运动学解

给每个关节都附上单独的坐标系之后：


机械臂DH参数表：



正运动学解并不能在单片机上执行，所以我们需要逆运动学几何法来在单片机上运行。

四、逆运动学解

1.逆运动学几何法

方程：




这样就完成了逆运动学的计算。

2.代码实现

void Kinematic_Analysis(float x,float y,float Beta,float Alpha) 
{
float m,n,k,a,b,c,theta1,theta2,theta3,s1ps2;
 m=l2*cos(Alpha)-x; 
 n=l2*sin(Alpha)-y;
 k=(l1*l1-l0*l0-m*m-n*n)/2/l0;
 a=m*m+n*n; b=-2*n*k; c=k*k-m*m;
 theta1=(-b+sqrt(b*b-4*a*c))/2/a; 
 theta1=asin(theta1)*180/PI;
  if(theta1>90)
  theta1=90; 
  if(theta1<-90)
  theta1=-90;
  k=(l0*l0-l1*l1-m*m-n*n)/2/l1; 
  a=m*m+n*n; 
  b=-2*n*k;
  c=k*k-m*m; 
  s1ps2=(-b-sqrt(b*b-4*a*c))/2/a; 
  s1ps2=asin(s1ps2)*180/PI;
  if(s1ps2>90)
  theta2=90;
  if(s1ps2<-90)
  theta2=-90;
  theta2=s1ps2-theta1; 
  if(theta2>90)
  theta2=90; 
  if(theta2<-90)
  theta2=-90;
  theta3=Alpha*180/PI-theta1-theta2; 
  if(theta3>90)
  theta3=90; 
  if(theta3<-90)
  theta3=-90;
  Target1 = 750-(Beta)*Ratio;
  Target2 = 735+(theta1-90)*Ratio; 
  Target3 = 717-(theta2)*Ratio; 
  Target4 = 702-(theta3)*Ratio;; 
  Target5 = 750; 
 }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39