运动学算法

2. 运动学算法设置
(1)两种轴定义方式 1.线性定义方式：#n->1000x 例如：#1->1000x 电机位置与指尖位置仅是线性关系,他们虽然也包含正逆运动算法,但算法仅仅是: 逆运动学等式：#1（电机位置）=1000X（指尖横坐标） 正运动学等式(通常不写)：X（指尖横坐标）=（1/1000）#1（电机位置） 如果电机位置与指尖位置之间的关系并非简单的线性关系,就不能使用这样的定义方式. 2.逆解定义方式：#n->I 非线性的轴定义需加入正逆解子程序来指定电机位置与指尖位置的关系. 使用正逆运动学算法之后,只需把轴按逆解方式定义，PMAC 就会自动识别并计算电机位置. 在运行运动程序之前，应先执行正运动学程序，所以需要确定指尖的初始位置，这可以通过 使用 PMATCH 命令强制执行. 如果指尖起始位置设置完成，PMAC 将使用逆解计算电机的位置。
轴标识 X Y Z
(1)配置示例 //坐标系 1，正逆解算法配置示例 &1 ；指定坐标系 1 Open Forward Clear ；打开并清理正解缓冲区 ;Q1=f1(P1,P2) ；轴 A( 指尖坐标)与 1 号 2 号电机的函数关系 ;Q2=f2(P1,P2) ；轴 B（指尖坐标）与 1 号 2 号电机的函数关系 Close ；关闭正解缓冲区 Open Inverse Clear ；打开并清理逆解缓冲区 ;P1=g1(Q1,Q2) ；电机 1 位置与轴 A 和 B（指尖坐标）的函数关系 ;P2=g2(Q1,Q2) ；电机 2 位置与轴 A 和 B（指尖坐标）的函数关系 Close ；关闭逆解缓冲区 I5150=1 ；允许坐标系 1 运动学计算 I5113=10 ；设置粗插补周期为 10ms #1->I ；定义#1 使用逆解结果 #2->I ；定义#2 使用逆解结果 (2)配置步骤 第一步：指定使用子程序的坐标系 第二步：定义正逆解子程序 使用 Open Forward/Inverse 代码编写算法程序，结束后使用 Close，f1,f2,g1,g2 仅是方程 符号，您必须将他们用你自己的函数代替也，就是说,你需要自己编写运动学方程. 第三步：激活运动学子程序和粗插补 Isx50=1 允许该坐标系进行正逆解运算，Isx13，指定该坐标系粗插补周期（默认为 0 无法 启动正逆解算法，单位：ms） 使用#n->I 定义各个电机
（3）运动程序(矩形运动)
End Gat Del Gat Close &1 ;指定坐标系 1 I5150=1 ;Isx50 允许坐标系 x 运动学计算 I5113=10 ;Isx13 坐标系 x 粗插补周期 #1->I ;使用逆解计算电机#1 位置 #2->I ;使用逆解计算电机#2 位置 Open Prog 1 Clear Linear ;线性插补模式 Abs ;绝对运动模式 TA 100 ;加速时间 100ms TS 0 ;S 曲线时间 0 TM 1000 ;运动时间 100ms X 1.5 Y 0 ;运动至开始位置 X=1.5 Y=0 CMD "EndGat" ;停止当前数据采集 CMD "DelGat" ;删除采集缓冲区数据(释放缓冲区) CMD "DefGat" ;定义采集缓冲区 CMD "Gat" ;开始数据采集 X 2 Y 0 ;运动至 X=2 Y=0 Dwell 0 ;等待 0ms 禁止混合运动 X 4 Y 0 ;运动至 X=4 Y=0 Dwell 0 ;等待 0ms 禁止混合运动 X 4 Y 3 ;运动至 X=4 Y=3 Dwell 0 ;等待 0ms 禁止混合运动 X 2 Y 3;运动至 X=2 Y=3 Dwell 0 ;等待 0ms 禁止混合运动 X 2 Y 0;运动至 X=2 Y=0 Dwell 1000 ;等待 1000ms CMD "EndG";停止数据采集 Close ;关闭缓冲区
4.运动学程序示例--2 轴肩肘机械手（SCARA）
Y [X,Y] B L2 L1 A X
（1） 公式推导
指尖以 X/Y 为横/纵坐标 A 和 B 为关节角度,即实际电机位置 L1=4 英寸，L2=3 英寸 正解算法 用 A,B 表示 X，Y
X = L1cos(A) + L 2 cos(A + B) Y = L1sin(A) + L 2sin(A + B)
#define Mtr2Pos P2 ；电机#2 位置 //变量定义/替换 #define Length1 Q91 ；L1 长度（英寸） #define Length2 Q92 ；L2 长度（英寸） #define Mtr1SF Q93 ；电机#1 编码器值转换为角度的系数 cts/deg #define Mtr2SF Q94 ；电机#2 编码器值转换为角度的系数 cts/deg #define TempA Q95 ；储存 A（角度）计算值的临时存储器 #define TempB Q96 ；储存 B（角度）计算值的临时存储器 2 2 #define Xsq_p_Ysq Q97 ；X +Y 2 2 #define L1sq_p_L2sq Q98 ; L1 +L2 2 2 #define L1sq_s_L2sq Q99 ; L1 -L2 #define TempCosB Q100 ;cos(B) Length1=4 ;L1 长度为 4 英寸-用户输入 Length2=3 ;L2 长度为 3 英寸-用户输入 Mtr1SF=2000/360 ;电机 1 系数 cts/deg Mtr2SF=2000/360 ;电机 2 系数 cts/deg L1sq_p_L2sq=Length1*Length1+Length2*Length2 L1sq_s_L2sq=Length1*Length1-Length2*Length2 //状态标志 #define Mtr1HmComp M145 ;电机#1 回零完毕状态(建议 M 变量) #define Mtr2HmComp M245 ;电机#2 回零完毕状态(建议 M 变量) #define CS1_RunTimeError M5182 ;坐标系 1 错误状态(建议 M 变量) Mtr1HmComp->Y:$0000C0,10,1 ;#1 回零完毕标志地址定义 Mtr2HmComp->Y:$000140,10,1 ;#2 回零完毕标志地址定义 CS1_RunTimeError->Y:$00203F,22,1 ;坐标系 1 错误标志地址定义 Undefine all &1 ;指定坐标系 1 // ==============正运动学 ===============// Open Forward Clear If(Mtr1HmComp=1 and Mtr2HmComp=1) ;回零完毕? TempA=Mtr1Pos/Mtr1SF ;电机#1 位置(角度) TempB=Mtr2Pos/Mtr2SF ;电机#2 位置(角度) KinPosX=Length1*cos(TempA)+Length2*cos(TempA+TempB) KinPosY=Length1*sin(TempA)+Length2*sin(TempA+TempB) Else CS1_RunTimeError=1 ;若回零未完成退出并报错 Endif Close //============== 逆运动学 =============== // Open Inverse Clear Xsq_p_Ysq=KinPosX*KinPosX+KinPosY*KinPosY TempCosB=(Xsq_p_Ysq-L1sq_p_L2sq)/(2*Length1*Length2) If (ABS(TempCosB)<0.9998) ;可能引起问题的特殊角度处理 Q0=KinPosX ; Q0 是 Atan2 函数中的分母,KinPosY 是分子
TempA=atan2(KinPosY)-acos((Xsq_p_Ysq+L1sq_s_L2sq)/(2*Length1*sqrt(Xsq_p_Ysq ))) TempB=acos(TempCosB) Mtr1Pos=Mtr1SF*TempA Mtr2Pos=Mtr2SF*TempB Else CS1_RunTimeError=1 ;报错并退出 Endif Close // =======Fra Baidu bibliotek============================ //
(2)用于位置计算的预留变量
如果使用正逆解算法，下列 P 变量和 Q 变量就是为正逆解预留的变量: P1～P32 对应电机 1～32 的位置 Q1～Q9 对应如下：
预留的 Q 变量 Q1 Q2 Q3 3.正逆解子程序
轴标识 A B C
预留的 Q 变量 Q4 Q5 Q6
轴标识 U V W
预留的 Q 变量 Q7 Q8 Q9
逆解算法 用 X,Y 表示 A,B
2 X 2 + Y 2 − L2 1 − L2 B = +cos −1 2L1L 2 X 2 + Y 2 + L2 − L2 1 2 A = atan2(Y, X) − cos −1 2L X 2 + Y 2 1
注意：Atan2 为 Turbo PMAC 特殊的反正切算符,详情请参看 Turbo PMAC 软件手册 -1 COS 的自变量只能为正角度
（2）正/逆解算法程序
End Gat Del Gat Close //运动学参数定义 #define KinPosX Q7 ；指尖横坐标（X）单位：mm #define KinPosY Q8 ；指尖总坐标（Y）单位：mm #define Mtr1Pos P1 ；电机#1 位置
运动学算法
1. 什么是运动学算法
正运动学算法 运动程序 位置指令
电机位置
指尖位置
逆运动学算法
（1）运动学算法是实际电机位置与指尖位置的关系式 正运动学算法（正解）以电机位置作为输入计算出指尖位置（用户单位） 逆运动学算法（逆解）以指尖位置作为输入计算出各电机位置（用户单位） （2）运动程序将直接按指尖坐标运动 用户仅需要直接给出期望的指尖运动轨迹,PMAC会在运动过程中通过逆运动学算法自动计算 实际电机运动轨迹
(4)位置报告 PLC
使用 PLC 做位置报告避免了潜在的寄存器重复使用,还可以提供位置的实时报告 (使用 Watch Window)与绘图.不要在 PLC 程序中使用 P1-P32 和 Q1-Q9 #define CS_number P1000 #define Actual_X P1001 #define Actual_Y P1002 #define Actual_A P1003 #define Actual_B P1004 #define Actual_Mtr1Pos M162 #define Actual_Mtr2Pos M262 #define CMD_X P2001 #define CMD_Y P2002 #define CMD_A P2003 #define CMD_B P2004 #define CS_Number P2005 #define CMD_Mtr1Pos M161 #define CMD_Mtr2Pos M261 #define Length1 Q91 ;L1 长度(英寸) #define Length2 Q92 ;L2 长度(英寸) #define Mtr1SF Q93 ;电机#1 系数 cts/deg #define Mtr2SF Q94 ;电机#2 系数 cts/deg Actual_Mtr1Pos->D:$8B Actual_Mtr2Pos->D:$10B CMD_Mtr1Pos->D:$88 CMD_Mtr2Pos->D:$108 Open PLC 10 Clear CS_Number=1 Address&CS_Number ;选择坐标系 1 Actual_A=Actual_Mtr1Pos/(I108*32*Mtr1SF) Actual_B=Actual_Mtr2Pos/(I208*32*Mtr2SF) Actual_X=Length1*cos(Actual_A)+Length2*cos(Actual_A+Actual_B) Actual_Y=Length1*sin(Actual_A)+Length2*sin(Actual_A+Actual_B) CMD_A=CMD_Mtr1Pos/(I108*32*Mtr1SF) CMD_B=CMD_Mtr2Pos/(I208*32*Mtr2SF) CMD_X=Length1*cos(CMD_A)+Length2*cos(CMD_A+CMD_B) CMD_Y=Length1*sin(CMD_A)+Length2*sin(CMD_A+CMD_B) Close