Power PMAC中正逆运动学的实现
1. 为什么要使用正逆运动学 ？
2. 怎样创建正逆运动学子程序 ？
3. 如何编写运动学程序？
3.0 预备工作
3.1 逆运动学
3.2 正运动学
3.3 运动学在C语言中的实现
4. 官方例程讲解
5. 机械臂运动学讲解
声明
1. 为什么要使用正逆运动学 ？
正运动学：通过各关节角度求坐标系下的位姿
逆运动学：给定坐标系下的位姿，反解各关节角度

  • 在这里插入图片描述

    

通常情况下，逆运动学是必须的，正运动学是为了反馈数据而使用，而逆运动学是为了把给定数据解析成控制电机的数据值。

    因此，当各轴与坐标系之间的数据关系是非线性时，我们要使用逆运动学解算数据。

2. 怎样创建正逆运动学子程序 ？
    在PMAC中，运动学子程序均存放PMAC Script Language中Kinematic Routines文件夹下。

添加步骤如下：
    首先添加逆运动学程序。右击Kinematic Routines文件夹，选择添加选项下的新建项，选中逆运动学（Inverse Kinematic），点击添加，如下图所示：
在这里插入图片描述

添加完成后，自动弹出如下界面：

在这里插入图片描述

从上图我们可以看到，系统自动生成了一个带注释的逆运动学子函数结构，在第2节中我们进行详细的讲解。
    
    然后，添加正运动学程序，添加步骤如上，不再赘述。

在这里插入图片描述3. 如何编写运动学程序？

3.0 预备工作

首先需要指定逆运动学轴，在全局变量声明文件中写入：

#1->I
#2->I
#3->I
#4->I
....

3.1 逆运动学

在这里插入图片描述

&1				 //使用坐标系1
csglobal InvKinErr;//求解失败的标志位

open inverse (1) // 为坐标系1开启逆运动学缓存区
if(是否在工作空间内)
{
	//{逆运动学方程}
	InvKinErr = 0;
}
else
{
	InvKinErr = 1;
	Coord[1].ErrorStatus = 255; //停止执行
}
close // 关闭逆运动学缓存区

3.2 正运动学

在这里插入图片描述

open forward (1) // 为坐标系1开启正向运动学缓冲区
if(KinVelEna > 0) callsub 100; 
KinAxisUsed = {axis mask} //由上图查看对应的值，十六进制
n100:
// {content} // 逆运动学方程
return;
close // 关闭正向运动学缓冲区

在这里插入图片描述

3.3 运动学在C语言中的实现

    
参考博文《Power PMAC运动控制器 —— 脚本调用C函数(即CfromScript函数的使用)》

不同的是，只需要把运动学算法公式写到CfromScript中即可。

4. 官方例程讲解

   
在这里插入图片描述

正运动学全局变量声明：

// Forward Kinematics Setup
Coord[1].SegMoveTime = 2; //2毫秒分割周期
&1
csglobal Len1, Len2;  // 杆长变量
csglobal CtsPerDeg, DegPerCt;  // 角度转计数值、计数值转角度用的变量
csglobal FwdKinErr;  //错误标志位
Len1 = 400  // Upper-arm link of 400mm
Len2 = 300  // Lower-arm link of 300mm
CtsPerDeg = 1000  // 计数值转角度
DegPerCt = 1/1000  // 角度转计数值

正运动学：

&1
open forward
if (KinVelEna) callsub 100;
KinAxisUsed = $C0  // X and Y axis
N100: 
if (Coord[1].HomeComplete)  // 判断是否已经完成回零
{  
	// 正运动学公式
	 KinPosAxisX=Len1*cosd(KinPosMotor1*DegPerCt)+Len2 * cosd((KinPosMotor1 + KinPosMotor2) * 
	DegPerCt + 90); 
	 KinPosAxisY=Len1*sind(KinPosMotor1*DegPerCt)+Len2 * sind((KinPosMotor1 + KinPosMotor2) * 
	DegPerCt + 90);
}
else //无效，停止操作
{
	 if (Ldata.Status & 40) //motion program调用
	 {
	 Coord[1].ErrorStatus = 255;  //用户设置中止错误
	 }
 else //axis query调用
 {
 KinPosAxisX = sqrt(-1);  // 返回非法值
 KinPosAxisY = sqrt(-1);  // 返回非法值
 }
}
close

逆运动学全局变量声明：

 // Inverse Kinematics Setup
 &1  // 选择坐标系1
 #1->I // 指定电机1到逆运动学轴I
 #2->I // 指定电机2到逆运动学轴I
 // Auto-assigned variable declarations
 //#define KinPosMotor1 L1
 //#define KinPosMotor2 L2
 //#define KinPosAxisX C6
 //#define KinPosAxisY C7
 //#define KinAxisUsed D0
 
 //中间变量
 csglobal SumLenSqrd;  // Len1^2 + Len2^2
 csglobal InvProdOfLens;  // 1/(2*Len1*Len2)
 csglobal DifLenSqrd;  // Len1^2 –Len2^2
 csglobal InvKinErr;  // Error flag for routine
 csglobal TwoLen1;  // 2*Len1
 // 预先计算额外的系统常数
 &1
 SumLenSqrd = Len1 * Len1 + Len2 * Len2
 InvProdOfLens = 1.0/(2.0 * Len1 * Len2)
 DifLenSqrd = Len1 * Len1 - Len2 * Len2
 TwoLen1 = 2.0 * Len1;

逆运动学：

// Inverse-kinematic algorithm to be executed repeatedly
&1
open inverse // Open buffer, clear contents
// 局部变量定义
local X2Y2;  // X^2 + Y^2
local Bcos;  // cos(Elbow)
local Bangle;  // Elbow angle (deg)
local AplusC; // Sum of A and C angles (deg)
local Cangle; // Angle C in triangle (deg)
local Aangle;  // Shoulder angle (deg)

X2Y2 = KinPosAxisX * KinPosAxisX + KinPosAxisY * KinPosAxisY;
Bcos = (X2Y2 - SumLenSqrd) * InvProdOfLens;

if (abs(Bcos) < 0.9998)  // Valid solution w/ 1 deg margin?
{
	 Bangle = acosd(Bcos);
	 AplusC = atan2d(KinPosAxisY, KinPosAxisX);
	 Cangle = acosd((X2Y2 + DifLenSqrd) / (TwoLen1 * sqrt(X2Y2)));
	 Aangle = AplusC - Cangle;
	 KinPosMotor1 = Aangle * CtsPerDeg;
	 KinPosMotor2 = (Bangle - 90) * CtsPerDeg;
	 InvKinErr = 0;
}
else //无效，停止操作
{
	 InvKinErr = 1;  // Set flag for external use
	 Coord[1].ErrorStatus = 255;  // Stop execution
}
close

位置报告PLC：

 global ReportActPosX = 0, ReportActPosY = 0;
 global ReportDesPosX = 0, ReportDesPosY = 0;
 
 open plc PositionReportingPLC
 Ldata.coord = 1 // 选择坐标系1
 PREAD
 ReportActPosX = D6 // Actual X
 ReportActPosY = D7 // Actual Y
 DREAD
 ReportDesPosX = D6 // Desired X
 ReportDesPosY = D7 // Desired Y
 close

运动程序：

 open prog KinProgExample
 enable plc PositionReportingPLC; dwell 100;
 Linear Abs F100 TA 0 TS 100
 Gather.Enable = 2; dwell 0
 X 200 Y 200 dwell 0
 inc
 Y 100 dwell 0
 X 100 dwell 0
 Y -100 dwell 0
 X -100 dwell 0
 Gather.Enable = 0; dwell 0
 disable plc PositionReportingPLC; dwell 100;
 close

运行结果：
在这里插入图片描述

对于一般需求来说，仅逆运动学已经足够我们使用。

5. 机械臂运动学讲解

在后续博文《Power PMAC运动控制器 —— 学习笔记7》讲解

声明

  • 所有用到的用户手册中的图片以及其他PMAC相关的资料版权归欧姆龙所有，本人在此声明引用。