基于嵌入式控制以及EtherCat通讯的四轴SCARA机械臂----笨笨

阴阳怪气139

1592人浏览 · 2024-01-23 23:16:15

阴阳怪气139 · 2024-01-23 23:16:15 发布

一、前言：

时隔一整年，笔者打算趁闲暇之余对今年做过的一些有意思的东西进行一个记录总结。本次介绍的是如何手搓一个SCARA机械臂，之所以要叫其笨笨，是因为笔者也算是一个钢铁侠迷，自己也想像钢铁侠那样作出属于自己的笨笨机械手，所以也将其取名为笨笨，哈哈哈。 OK,闲话少说，开始正题。

二、组成结构：

如下图所示，为SCARE机器人的系统组成：

根据系统组成可以知道我这个机械臂包含了

（1）STM32F407ZET6控制板：

这款控制板是我在淘宝上买的，之所以要买这块控制板其主要原因是其价格便宜而且售后技术也挺到位的，只要你驱动器的资料完整一般来说很快就可以通讯成功，方便我的调试。如果有想自己做机器人的小伙伴可以考虑一下。当然我也不是在打广告，只是事实说话而且。

（2）汇川 IS620N伺服驱动器：

这是一款比较经典的国产的伺服驱动器，之所以要选用这一款驱动器，主要是因为我在咸鱼上淘的二手机械臂也是汇川的，所以得尽量做到配对。

（3）SCARA机械臂：

如图，这是一款在工厂自动化生产中比较常用的4轴SCARA机械臂了，SCARA机械臂相较于传统的多关节机械臂其自由度少，结构简单，成本低，水平方向灵活，垂直方向刚性强，适用于快速装配工作。可以利用其结构简单和平面特性来规划和验证算法的正确性，这也是我选择用SCARA机械臂的原因。

三、SCARA机器人运动学分析：

如图所示，机械臂运动学分析主要分为两个方面：在确定机械臂各连杆参数的基础上，正运动学是已知各运动关节变量，求出末端执行器的位置姿态；而逆运动学是已知机械臂将要到达的位置，然后求出各关节变量。

正解、逆解相辅相成才能使得机械臂按照我们想要的方式去运动。

3.1 机械臂运动学建模基础

(注：下面为基本的矩阵空间关系推导过程，如果不想看可以直接跳到 3.3机械臂正运动学分析和 3.4机械臂逆运动学分析 )

注：因为CSDN的公式编辑功能笔者觉得不太好用，所以以下公式均以图片形式代替

（1）平移坐标变换

假设某一空间中存在一点 p，在坐标系{B}中可用位置矢量来表示，由于坐标系{A}与坐标系{B}姿态相同而原点不同，则坐标系{B}相对于坐标系{A}可以用位置矢量来描述，如图2-1所示：

图3-1

此时点 p 在坐标系{A}中的描述就可以通过矢量加法表示：

式（3-1）

（2）旋转坐标变换

坐标系{A}与坐标系{B}原点位于同一位置而姿态不同，点 p 在坐标系{B}中的位置矢量为则坐标系{B}相对于坐标系{A}可以用旋转矢量来描述，如图2-1所示：

图3-2

此时点 p 在坐标系{A}中的描述可表示为：

式（3-2）

（3）复合坐标变换

若坐标系{B}与坐标系{A}原点与姿态均不相同，需要用到上述两者的复合方程来进行坐标变换。点 p 在坐标系 {A} 与{B}中的位置矢量分别为、,坐标系{B}的原点相对于坐标系{A}用位置矢量来描述，姿态相对于坐标系{A}用旋转矢量来描述，如图2-3所示：

此时点 p 在两坐标系中的转换可以表示为：

式（3-3）

将式（2-3）转化为齐次变换形式

式（3-4）

其中，，是1*3维的列向量，表示点p在两坐标系的位置描述：

式（3-5）

旋转矢量是一个3*3的矩阵：

式（3-6）

列矢量 AxB，AyB，AzB为坐标系{B}的坐标轴单位矢量相对于坐标系{A}的描述。

式（2-4）中，加上 1 后成为四维列向量，称为点的齐次坐标，仍记为，

得：

式（3-7）

4*4矩阵被称为齐次变换矩阵，包含了平移变换和旋转变换，可转换为：

式（3-8）

3.2 基于D-H法的SCARA机械臂运动学模型

SCARA机械臂由一系列的连杆通过移动和关节的旋转组成，如下图所示大致表示出了其连杆以及各关节局部的坐标对应关系：

SCARA机械臂实体

关节连杆示意图

其中：用表示连杆i,i-1,(i属于1，2，3，4轴)间位姿关系的变换矩阵，如表示连杆1在基坐标系中的变换矩阵。则连杆坐标系4与基坐标系之间的转换关系可表示为：

式（3-9）

SCARA机械臂连杆坐标系

D-H 建模法是由 Denavit 和 Hartenberg 提出的一种建立关节物理模型与空间位姿间关系的建模方法。对于D-H建模法，每个连杆i使用4个特征参数：

连杆长度 ：相邻关节 z 轴轴线间的垂直距离，即表示从沿轴到 的距离。
关节偏置量 :从沿轴到的距离
连杆扭角：从到的转角，沿的轴的指向为正
关节转角 :从到的转角，沿轴顺时针方向为正

表3-1 SCARA机械臂连杆参数表：

旋转关节 1、2、4 的关节变量为 θi，移动关节 3 的关节变量为 d3。

3.3 机械臂正运动学分析：

根据 SCARA 机器人的连杆特征参数和关节变量，可以得到变换矩阵：

式（3-10）

式（2-10）中：Rot(x, )代表轴正向转角，Trans( x, )代表沿着轴移动距离；Rot(z, θi)代表z轴正向转 θi 角，Trans(z, )代表沿 z轴移动距离

根据表2-1 以及式（2-10）式（2-8）可以得到以下正解结果，从而得出末端位姿的变换矩阵

式（3-11）

可以得出：

= ; 表示末端位姿的X坐标点

= ; 表示末端位姿的Y坐标点

; 表示 J3 （竖直杆）的行程

3.4 机械臂逆运动学分析

在笛卡尔空间轨迹进行规划的时候，需要对机械臂进行实时的逆运动学求解，现通过之前的正运动学分析已经得到了式（2-11）即位姿变换矩，然后通过乘以其逆矩阵分离出变量，最后根据代数分解求出所需变量：

具体过程如下：

（1）旋转关节1关节变量 θ1 ：

式（3-12）

经化简解方程组得：

式（3-13）

（2）求旋转关节 2 关节变量 θ2

同理可得：

式（3-14）

（3）求移动关节 3 关节变量 d3：

d3 = d1 - Pz 式（3-15）

（4）求旋转关节 4 关节变量 θ4：

式（3-16）

注：我的机械臂只使用了前 3 个变量，并没有对竖直杆的旋转作出要求，所以接下来的代码并没有关于关节4的部分。

四、EtherCAT通讯

4.1 EtherCAT通讯原理：

EtherCAT 网络采用一主多从式结构，其运行原理如图 4-1 所示。一个主站设备可连接多个从站设备，主站是通讯的发起者，从站只能被动回应而无法主动发起通讯，从而避免多个从站同时发送以太网帧导致的网络拥堵。在每个通讯周期中，主站根据协议创建以太网帧，该以太网帧中涵盖了各从站的寻址信息，数据信息等，然后主站将其依照拓扑结构顺序向下发送给各个从站[66]。从站在接收到数据帧时，只寻址与本从站节点相关信息，读取输入数据，将输出数据插入回数据报文继续传输到下一个从站，直至最后一个从站再从相反方向发送回去，通过第一个从站返回报文给主站，完成一个通信周期。整个通信周期只有数纳秒的延迟，从而保证了 EtherCAT 传输的实时性。

注：通讯方面并不是笔者的强项，我的机械臂所使用的通讯底层，实际上是由我所买的控制板的商家提供的，所以说细节方面笔者并不是很清楚，如果读者有这方面的通讯需要，笔者将32开发板的链接放在下方，可以自行去了解。

开发板链接：【淘宝】https://m.tb.cn/h.5qsHj6v4vqUx1HP?tk=5q5tWR2eae8 CZ0001 「EtherCAT主站开发板机器人机械臂开源控制器总线控制卡232CAN485」
点击链接直接打开或者淘宝搜索直接打开

五、准备工作

因为我的机械臂是淘来的二手，所以一些基本的参数需要自己手动去测定。因为此为由伺服电机组成的机械臂，所以我需要知道我的开发板触发的节拍数与实际的物理运动关系。

5.1：机械臂节拍数对应

这里笔者是使用六轴传感器来分别测出在一定的节拍数下每个轴实际的目标值与理论值之间的关系，以及所对应的旋转角度是多少。

如下图所示：

测试结果：

5.2：机械臂笛卡尔基坐标系建立

这里以机械臂底座为原点建立笛卡尔坐标系，这一步也很关键，坐标系的准确与否关系到你的取点的判断。

六、代码：

6.1：定时器中断：

void TIM3_IRQHandler(void)
{
	timer_10ms_cnt++;
	
	if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET) //����ж�
	{

		
		app_time_base += SYNC0TIME*1000;
		pdoTimeFlag = 1;
		if (dorun==0){	
		  ec_send_processdata();//��վ���ݷ���
		  ec_pdo_outframe();
		}
		else {
			ecat_app();
		}

		//LED2=!LED2;
		LocalTime+=10;//10ms����
		
	}
	TIM_ClearITPendingBit(TIM3,TIM_IT_Update);  //����жϱ�־λ
}

当我的控制板与机械臂建立连接之后，主要的处理过程将会在定时器中断中的ecat_app（）中进行，且此定时器中断每隔10ms触发一次，每触发一次记为一个节拍数。例如当我给了一个坐标点要机械臂去走，开发板会将这个点分为1000个节拍去走，也就是10秒走一个坐标点。

6.2 ecat_app（）：

int32_t cmd_pos_raw[4] = {0};
int32_t cmd_pos_raw_linear[4] = {0};
int32_t act_pos[4] = {0};
int32_t cur;
uint8_t flage_TargetPos = 0;
int32_t tick = 0,tick_1 = 0;

void ecat_app(void){

	static s16 i = 0;
	static u8 flag=0;
	u8 wkc;
	
			wkc=ec_receive_processdata(EC_TIMEOUTRET);
			//printf("%d ",wkc);
	
			cur_status = inputs1->StatusWord;//0x6041
			cur_status2 = inputs2->StatusWord;//0x6041
			cur_status3 = inputs3->StatusWord;//0x6041
			cur_status4 = inputs4->StatusWord;//0x6041 
		
							act_pos[0] =  inputs1->CurrentPosition;   //得到实际的总增量值
							act_pos[1] =  inputs2->CurrentPosition;
							act_pos[2] =  inputs3->CurrentPosition;
							act_pos[3] =  inputs4->CurrentPosition; 
	
	
			switch(startup_step)
			{
				case 1:
				  outputs1->ControlWord = 0x06;//0x6040
				  outputs2->ControlWord = 0x06;//0x6040
				  outputs3->ControlWord = 0x06;//0x6040
				  outputs4->ControlWord = 0x06;//0x6040
				  if(((cur_status==0x0631)||(cur_status==0x0231)||(cur_status==0x0221)||(cur_status==0x1221))
				  && ((cur_status2==0x0631)||(cur_status2==0x0231)||(cur_status2==0x0221)||(cur_status2==0x1221))
				  && ((cur_status3==0x0631)||(cur_status3==0x0231)||(cur_status3==0x0221)||(cur_status3==0x1221))
				  && ((cur_status4==0x0631)||(cur_status4==0x0231)||(cur_status4==0x0221)||(cur_status4==0x1221)))
						 startup_step=2;
		
			  case 2:
					outputs1->ControlWord = 0x07;
					outputs2->ControlWord = 0x07;
					outputs3->ControlWord = 0x07;
					outputs4->ControlWord = 0x07;
					if(((cur_status==0x633)||(cur_status==0x0233)||(cur_status==0x0223)||(cur_status==0x1223))
					&& ((cur_status2==0x633)||(cur_status2==0x0233)||(cur_status2==0x0223)||(cur_status2==0x1223))
					&& ((cur_status3==0x633)||(cur_status3==0x0233)||(cur_status3==0x0223)||(cur_status3==0x1223))
					&& ((cur_status4==0x633)||(cur_status4==0x0233)||(cur_status4==0x0223)||(cur_status4==0x1223)))
						 startup_step=3;
				case 3:
					outputs1->ControlWord = 0x0f;
					outputs2->ControlWord = 0x0f;
					outputs3->ControlWord = 0x0f;
					outputs4->ControlWord = 0x0f;
					if(((cur_status==0x1637)||(cur_status==0x1633)||(cur_status==0x1237))
					&& ((cur_status2==0x1637)||(cur_status2==0x1633)||(cur_status2==0x1237))
					&& ((cur_status3==0x1637)||(cur_status3==0x1633)||(cur_status3==0x1237))
					&& ((cur_status4==0x1637)||(cur_status4==0x1633)||(cur_status4==0x1237)))
						{
							if(init_state == 1)   //初始化完成后对增量清空一次，防止机械臂误动
							{
							cmd_pos_raw[0] =  inputs1->CurrentPosition;
							cmd_pos_raw[1] =  inputs2->CurrentPosition;
							cmd_pos_raw[2] =  inputs3->CurrentPosition;
							cmd_pos_raw[3] =  inputs4->CurrentPosition; 
							}
							
						startup_step=4;
						tick = 0;	
						}
						
				case 4:
					 outputs1->ControlWord = 0x1f;	
					 outputs2->ControlWord = 0x1f;
					 outputs3->ControlWord = 0x1f;
					 outputs4->ControlWord = 0x1f;
					
				if(flag_target_value == 1)   //判断串口指令是否发出
				{
//					flag_target_value = 0;
					flage_TargetPos = 1; 
					tick = 0;
				}
				/*
				备注：下面分别是关节，直线，圆弧，垂直4种插补的判断模式
				每一种插补的使用的方法如下：
				对关节插补: 输入指令为 MO1 X Y eg:MO1 194 287
                                  				表示去到 X 为19.4cm Y为287cm的坐标点
				对直线插补：输入指令为 MO2 X Y eg:MO2 -194 287 
				                                  表示以直线的形式去到-19.4 28.7cm这个坐标点
										                      需要注意，对直线插补需要规避奇异点，即为含义X = 0或者Y = 0的点，比如说原点(400,0)
										                      所以，不能直接从原点开始做直线插补
				对圆弧插补:输入指令为 MO3 central_angle Rad eg:MO3 180 200
																					表示以当前目标点为起点作圆心角为180度，且半径为20cm的圆弧
				对垂直插补：输入指令为 MO4 Z   eg: MO4 20
																					表示末端端点上升2cm
				
				需要特别注意的是:该scara机械臂的总臂长为40cm,所以其能够到达的区域为以40cm为半径画圆所得到的区域，注意坐标位置的选择
				
				*/
				if(tick<=tick_num)                          //分1000个节拍输送目标增量
				{
					  tick++;
					 
					  if(flag_mode == 1)
						{                      //接收关节插补的值
							cmd_pos_raw[0] += delta_pos[0];
							cmd_pos_raw[1] += delta_pos[1];
							cmd_pos_raw[2] += delta_pos[2];
							cmd_pos_raw[3] += delta_pos[3];
						}
						if(flag_mode == 2)
						{
					   linear_interpolation();//进行直线插补							
						}
						if(flag_mode == 3)
						{
						 Circle_interpolation();//进行圆弧插补
						}
						if(flag_mode == 4)
						{
								cmd_pos_raw[3] += delta_pos_Vertical[3]; //进行垂直插补
						}
						 flag_target_value = 0;
				}
				break;
				default :
					startup_step=1;
				  outputs1->ControlWord = 0x03;//0x6040
				  outputs2->ControlWord = 0x03;//0x6040
				  outputs3->ControlWord = 0x03;//0x6040
				  outputs4->ControlWord = 0x03;//0x6040
				break;
			}
		
	 outputs1->TargetMode = 0x8;
	 outputs2->TargetMode = 0x8;
	 outputs3->TargetMode = 0x8;
	 outputs4->TargetMode = 0x8;
	
	if(flage_TargetPos == 1){		  //防止初始化的时候误触发，检测到串口指令时开启
			 outputs1->TargetPos = cmd_pos_raw[0];
		   outputs2->TargetPos = cmd_pos_raw[1];
		   outputs3->TargetPos = cmd_pos_raw[2];
		   outputs4->TargetPos = cmd_pos_raw[3];
  	}		
			
//		if(i>=500) i = 0;
	 ec_send_processdata();	
		ec_pdo_outframe();
	
}

这里用了一个switch结构，前面3个case都是用来建立通讯连接，查设备状态用的，只有case4是实际进行节拍运算，将计算出来的目标量增量传递给

                            cmd_pos_raw[0] += delta_pos[0];
                           cmd_pos_raw[1] += delta_pos[1];
                           cmd_pos_raw[2] += delta_pos[2];
                           cmd_pos_raw[3] += delta_pos[3];

然后在传给结构体：

                        outputs1->TargetPos = cmd_pos_raw[0];
                    outputs2->TargetPos = cmd_pos_raw[1];
                      outputs3->TargetPos = cmd_pos_raw[2];
                    outputs4->TargetPos = cmd_pos_raw[3];

最后再由通讯协议将其输出传递给驱动器。

6.3：运动学正解：

	float Current_PX;
	float Current_PY;

void positive(float Current_Angle_1,float Current_Angle_2)   //正解函数
{
	   Current_Angle_1 = Current_Angle_1*3.14/180;
	   Current_Angle_2 = Current_Angle_2*3.14/180;
	   Current_PX = arm2*cos(Current_Angle_1 + Current_Angle_2)+arm1*cos(Current_Angle_1);  //µÃµ½Êµ¼ÊµÄ×ø±êÖµ
     Current_PY = arm2*sin(Current_Angle_1 + Current_Angle_2)+arm1*sin(Current_Angle_1);
		
}

6.4：运动学逆解：

 float A = 0;
	float phi = 0;
	float theta1 = 0;//基座逆解角度
	float theta2 = 0;//摆臂逆解角度
  float r = 0;
  float arm1 = 0.225;    //大臂的长度
	float arm2 = 0.175;    //小臂的长度
  int flag_quadrant4 = 0; 
	float quadrant4_theta1 = 0;
	float quadrant4_theta2 = 0;


void inverse(float PX, float PY)   //将目标坐标点转换为对应的弧度值 即逆解
{
			PX *=0.001;
			PY *=0.001;
			if(PX<0&&PY<0) //判断是否为第4象限的坐标点
			{
				PY = -1*PY;
				flag_quadrant4 = 1;
			}
			r = sqrt(PX * PX + PY * PY);
			A = (PX*PX + PY*PY + arm1*arm1 - arm2*arm2) / (2.0*arm1*r);
      phi = atan2(PX , PY);
      theta1 = atan2(A,(sqrt(1-A*A)))-phi;
			theta2 = atan2(r*cos(theta1+phi),(r * sin(theta1 + phi)-arm1));
	
			if(flag_quadrant4 == 1)  //镜像处理
			{
				quadrant4_theta1 = -1*theta1;
				quadrant4_theta2 = -1*theta2;
			}
}

6.5:关节插补：

int axis_angle[6];
int target_value_1 = 0;        //目标增量值   1~4分别代表从基座到末端竖杆的目标增量
int target_value_2 = 0;
int target_value_3 = 0;
int target_value_4 = 0;
int flag_target_value = 0;     //串口指令发送标志，用于给机械臂传送目标距离
int32_t delta_pos[4] = {0};    //实际状态下每次的目标增量

extern int32_t act_pos[4];     //当前的增量值
extern int32_t tick;


void Joint_Interpolation(void)  //关节插补
{
		inverse(axis_angle[0],axis_angle[1]); //计算目标所需的弧度
	 
		theta1 = theta1*180/3.14;               //弧度转换为角度
		theta2 = theta2*180/3.14;
		                            
	  if(flag_quadrant4 == 1)                 //判断是否为第四象限的坐标，将镜像后的值转换为角度
		{
			 theta1 = quadrant4_theta1*180/3.14;
			 theta2 = quadrant4_theta2*180/3.14;
			 flag_quadrant4 = 0;
		}
	 
	  target_value_1 =  (int)-1*theta1*1197605;  //将角度转化为目标增量同时符号实际基坐标系
	  target_value_2 =  (int)-1*theta2*1197605;

	  if(axis_angle[0]==0&&axis_angle[1]==0&&axis_angle[2]==0&&axis_angle[3]==0) //回零
		{
			 target_value_1 = 0;
			 target_value_2 = 0;
			 target_value_3 = 0;
			 target_value_4 = 0;
		}	 

//	 target_value_1 =  axis_angle[0]*1197605;  
//	 target_value_2 =  axis_angle[1]*1197605; 
//	 target_value_3 =  axis_angle[2]*470588;  
//	 target_value_4 =  axis_angle[3]*1000000; 
	 
	 delta_pos[0] = ( target_value_1 - act_pos[0])/tick_num ;    //计算单次目标差值，分1000次，每次10ms即为10s
	 delta_pos[1] = ( target_value_2 - act_pos[1])/tick_num ;
	 delta_pos[2] = ( target_value_3 - act_pos[2])/tick_num ;
	 delta_pos[3] = ( target_value_4 - act_pos[3])/tick_num ;
	 
}

6.6:直线插补：

 float startX = 0.0;
	 float startY = 0.0;
	 float endX = 0.0;    //终点坐标
	 float endY = 0.0;
	 float deltaX = 0.0;  //每一步的坐标增量
	 float deltaY = 0.0;
	 float deltas = 0.0;
	 float target_X = 0.0;
	 float target_Y = 0.0;
	 float theta = 0.0;
	 uint8_t flage_linear = 0;
	 int32_t  delta_pos_linear[4] = {0};
   extern int32_t cmd_pos_raw[4];	 
	 
	void linear_interpolation(void)  //直线插补函数
	{
		  if(flag_target_value==1&&tick==1)  
			{
				 positive(act_pos[0]*-1.0/1197605,act_pos[1]*-1.0/1197605); //获取起点坐标
				
				 startX =  Current_PX;       
		     startY =  Current_PY;
				
				 endX =  axis_angle[0]*0.001;   //获取终点坐标
		     endY =  axis_angle[1]*0.001;
		  
				 deltaX = (endX - startX)/tick_num;  
				 deltaY = (endY - startY)/tick_num;
				 
				 deltas = sqrt(deltaX*deltaX + deltaY*deltaY);  //得到每步坐标增量
				
				 theta =atan2((endY - startY),(endX - startX));   //得到直线的倾斜角
								
			}
					
//				 normal_function(act_pos[0]*-1.0/1197605,act_pos[1]*-1.0/1197605);  
					
				 target_X =  (startX+deltas*tick*cos(theta))*1000;   //更新当前目标位置并转换为mm制
				 target_Y =  (startY+deltas*tick*sin(theta))*1000;   
				 
//         printf("target_X")					
         					
				 inverse(target_X,target_Y);  //进行逆解得到下次的目标弧度
					
         theta1 = theta1*180/3.14;               //弧度转换为角度
		     theta2 = theta2*180/3.14;
		                            
					if(flag_quadrant4 == 1)                 //判断是否为四象限坐标，将镜像后的值转换为角度
					{
						 theta1 = quadrant4_theta1*180/3.14;
						 theta2 = quadrant4_theta2*180/3.14;
						 flag_quadrant4 = 0;
					}
	 
					delta_pos_linear[0] =  (int)-1*theta1*1197605;  //将角度转换为目标增量同时传递给实际状态下每次的目标增量值
					delta_pos_linear[1] =  (int)-1*theta2*1197605;

					cmd_pos_raw[0] = delta_pos_linear[0];
					cmd_pos_raw[1] = delta_pos_linear[1];					
					

	}

关于直线插补读者可以结合下面这张图来辅助理解：

6.7：圆弧插补：

float startX_C = 0.0;
	float startY_C = 0.0;
	float endX_C = 0.0;
	float endY_C = 0.0;
	float target_X_C = 0.0;            //实际目标位置
	float target_Y_C = 0.0;
	
	float centre_point_X = 0.1;        //圆心点坐标
	float centre_point_Y = 0.1;
	float Rad = 0.0;                   //半径
	float arc_length = 0.0;            //弧长	
	float central_angle = 0.0;       //120度圆心角对应的弧度	
	float central_angle_singe = 0.0;   //单份圆心角
	
 int32_t  delta_pos_Circle[4] = {0};	
	
 void Circle_interpolation(void)      //圆弧插补
 {
		if(flag_target_value==1&&tick==1)
		{			
			central_angle = axis_angle[0]*3.14/180; //获取圆心角并将其改为弧度
			Rad = axis_angle[1]*0.001; //获取圆弧半径
			
			arc_length = central_angle*Rad/tick_num; //获取单步的弧长
			
			central_angle_singe =  arc_length/Rad;     //获取单步的圆心角的弧度
		}
		
		  target_X_C =  (centre_point_X + Rad*cos(central_angle_singe*tick))*1000;  //更新为实际目标值并转换为mm制
		  target_Y_C =  (centre_point_Y + Rad*sin(central_angle_singe*tick))*1000;
		
			inverse(target_X_C,target_Y_C);  //进行逆解得到下次的目标弧度
					
         theta1 = theta1*180/3.14;               //弧度转换为角度
		     theta2 = theta2*180/3.14;
		                            
					if(flag_quadrant4 == 1)                 //判断是否为四象限坐标，将镜像后的值转换为角度
					{
						 theta1 = quadrant4_theta1*180/3.14;
						 theta2 = quadrant4_theta2*180/3.14;
						 flag_quadrant4 = 0;
					}
	 
					delta_pos_Circle[0] =  (int)-1*theta1*1197605;  //将角度转化为目标增量同时传递给实际状态下每次的目标增量
					delta_pos_Circle[1] =  (int)-1*theta2*1197605;

					cmd_pos_raw[0] = delta_pos_Circle[0];
					cmd_pos_raw[1] = delta_pos_Circle[1];					
		
 }

关于圆弧插补可理解下面这张图：

6.8：垂直插补：

 int32_t delta_pos_Vertical[4] = {0};
  
 void Vertical_interpolation(void)  
{ 
	target_value_4 =  axis_angle[0]*1000000;
	delta_pos_Vertical[3] = (target_value_4 -act_pos[3])/tick_num;
	
}

6.9：坐标获取并转换

下面这个判断串口数据的代码我写在mian里面，方便及时获取坐标数据

  if(USART_RX_STA & 0x8000)
			{
				USART_RX_BUF[USART_RX_STA & 0x3FFF] = '\0';
				if(USART_RX_BUF[2]=='1')
					{
						flag_mode = 1 ;
						printf("flag_mod==1\r\n");
				  }
				else if(USART_RX_BUF[2]=='2')
					{
						flag_mode = 2 ;
						printf("flag_mod==2\r\n");
					}
        else if(USART_RX_BUF[2]=='3')
				{
						flag_mode = 3 ;
				}
				else if(USART_RX_BUF[2]=='4')
				{
					  flag_mode = 4 ;
				}
						processReceivedString(USART_RX_BUF);   //串口指令分解函数	
				USART_RX_STA = 0; 
			}

下面为坐标分解：

void  processReceivedString(u8 *shr)  
{
	 char* p = strtok(shr, " ");
	
	 int index = 0;
	 while(p != NULL)                        //此循环用以获取目标坐标值
	 {
		 p = strtok(NULL, " ");
		 axis_angle[index]  = atoi(p);
		 index++;
		 if(index>=6)
		 {
				break;
		 }
	 }
	 if(flag_mode == 1)
	 {
	   Joint_Interpolation();//¹Ø½Ú²å²¹ 
	 }
	 if(flag_mode == 4)
	 {
		 Vertical_interpolation();
	 }
   flag_target_value = 1;
	  tick = 0;          //¿ªÊ¼Ç°½«½ÚÅÄÊýÇåÁã

	 }

以上便是全部的主体代码了。