ck3m多轴运动控制器笔记2：正逆运动学的编写

嵌入式全窄小白

已于 2023-01-31 12:16:23 修改

阅读量953

点赞数

分类专栏： Power PMAC（CK3M）文章标签：嵌入式硬件

于 2022-11-24 11:12:38 首次发布

本文链接：https://blog.csdn.net/p975468132/article/details/128012989

版权

Power PMAC（CK3M）专栏收录该内容

2 篇文章 6 订阅

订阅专栏

文章目录

基础介绍
问题解答

基础介绍

关于官方文档中对正逆运动学的讲解可参考博主罗伯特祥：https://blog.csdn.net/weixin_43455581/article/details/99102542

问题解答

下面就我在编写正逆运动学时遇到的一些问题进行总结：

1.为什么要使用正逆运动学？

对于开发机械臂来说，正向运动学可以由输入的各个关节角得出机械臂末端对应的XYZABC姿态，逆向运动学可以由机械臂末端反向计算出各个关节角对应的角度。道理大家都懂，那么在pmac的开发中，其正逆运动学在电机的实际控制过程中都起着什么作用呢。
前期开发，所用的软件为Power PMAC IDE，按照学习流程，应该在成功和电机实现通信和点动控制后，编写简单的运动程序，也就是Motion Programs中的prog.pmc，我这里简单编写的程序为：

&1	// 指定坐标系为坐标系1，即&nbmr中的n
#1->3055X	//把电机1与X轴关联起来，比例系数为3055

open prog 1	//定义prog程序块，指定程序名称为1，即&nbmr中的m
	local data;	//定义局部变量，使用local关键字声明
	abs			//指定是相对模式还是绝对模式
	linear		//指定运动模式
	TA 20		//加减速参数设置
	TS 20
	F 200
//	tm 900
	X 10	//运动到10的位置，此单位跟设置有关，如果用户单位设置为1，则此处运动到100cts的位置，由于程序开头10倍系数的存在
	dwell 1000	//延时1秒
	X 20
	dwell 1000
	X 0

close		//与open相对应的

程序里的比例3055是这么得出的：电机转一圈的编码器对用读书为1100000，那么每一度也就对应着1100000/360=3055个ct，1100000是这么计算得：假如你的电机减速比为100，编码器分辨率为1024，那么减速器输出一圈就为102400cts。
在这里插入图片描述
电机配置拓扑图里，不建议直接更改用户单位为3055，容易造成危险。这里默认为1。
到此为止，通过调整电机与X轴的比例系数，以及程序中的进给速度F，就可以简单的实现通过运动程序对电机进行控制。

2.正向运动学

下面，开始编写简单的正逆运动学（博主初期调试硬件只有一个总线电机）：
正向运动学程序Forward1.kin为：


&1
open forward (1) 

if(KinVelEna) callsub 100;
KinAxisUsed=$40        
N100:
//resolution1=3055
theta1=KinPosMotor1/resolution1*2*pi;
aa1=KinPosMotor1;//通过aa1的值来监测电机的实际编码器的读数
n1=n1+1;//n1用来在监测窗口观察正向运动学的运行次数
px=0.1*theta1;//这里将电机转动角和其对应的X轴的值简单的引入了0.1倍的关系，也就是说如果X=10，那么theta1=100°
KinPosAxisX=px;
aa2=KinPosAxisX;//aa2用来监测X轴的值
return;
close

编写完正向运动学，就可以开始验证正运动学，首先#1j/使能电机后，令j=0（目的是，如果你的电机当前值与运动程序中的第一条位置指令，也就是X10相差很大的话，电机会以很快的速度跑至X10，另外全程电机拓扑图里用户单位均为1，中途更改回引起电机剧烈震动，报警），然后终端输入&1b1r，这时会观察到n1由0->1，说明正向运动学运行一次，然后aa1和aa2也会有相应的值。
在这里插入图片描述
另外一种调用正向运动学的方法为：pmatch命令
比如你输入j=100，然后输入pmatch，这时会计算j=100处的正向运动学结果。
这里正向运动学的作用为：起到反馈作用，计算运动程序开始时，当前电机编码器所对应的末端X的值。

3.逆向运动学

接下来编写逆向运动学：
初学者可能会想，我现在没有逆向运动学，我的运动程序也能按照我的想法进行运动呀，其实是因为，你的运动程序中这一行命令：#1->3055X //把电机1与X轴关联起来，比例系数为3055，这也就意味着你的电机与X轴是一个线性关系，这种情况下没有逆运动也没什么问题，那如果你控制的是一个6轴的串联机械臂呢，这时它末端的XYZABC与各个电机之间就不再是线性关系，而要经过很复杂的运算，你的运动程序中的X10，X20这些命令就需要逆运动学来计算了。
接下来，注释掉#1->3055X //把电机1与X轴关联起来，比例系数为3055这条命令，我们用逆运动学来将X10、X20与电机对应起来，此时的运动程序应如图所示：
在这里插入图片描述
同时，在头文件里写入#1->I，将电机1用于逆运动学计算，我编写的简单逆运动学程序为：

&1
open inverse (1)     

x_i=KinPosAxisX;
theta1_i=x_i*10;//与正向运动学中的0.1求逆
KinPosMotor1=theta1_i*resolution1/2/pi;//将X对应的角度转换为电机编码器值
P1300=KinPosMotor1;//P1300用来在监视窗口观察电机编码器数值
n2=n2+1;//n2用来在watch窗口观察逆运动学计算了多少次

close

到此为止，一个对应着一个电机的简单正逆运动学就都编写完了，此时再次#1j/使能电机，j=0，&1b1r运行运动程序1，会观察到watch窗口中n2会一直在快速上涨，也就是说在运动程序执行过程中，一直在实时计算逆运动学。相信到了这一步，应该对正逆运动学有了初步的了解，单电机跑通后，可以用虚拟轴的方法，来验证6轴串联机械臂的正逆运动学。