机器人学中的坐标变换计算很麻烦,并且不好想变换过程很容易计算错误,因此我封装了函数可以计算和可视化坐标的变换,将麻烦的事情简单化。
本程序可以快速计算旋转、平移的变换,以及可视化坐标轴确保变换的正确性。
- 代码已上传至GitHub,持续更新中:代码传送门-GitHub
- 如何判断旋转的正负方向:机器人学-旋转坐标轴正方向的判断方法
- 关于运算顺序:程序是不包括运算顺序的,所以如果是当前的,那么从左到右算就是正常的写法(测试用例是书上的2-10针对的都是当前);如果是相对全局的坐标系,那么计算的时候就加上括号来更改运算顺序完成左乘就可以了。
使用说明:
1.旋转
语法:rot(‘轴’,度数);
例子:即绕y轴旋转90度,支持x,y,z轴的参数输入
rot(‘y’,90)
2.平移
语法:trans(x,y,z);
例子:向x、y、z方向分别移动1、2、3个单位
trans(x,y,z);
3.可视化
语法:ptran(4*4矩阵)
例子:ptran(rot(‘y’,90)*trans(1,2,3));
画出先绕y转90度再以当前坐标系为参考沿着(1,2,3)平移
rot.m – 旋转的齐次变换
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 |
% 标题:机器人学位置分析 % 作者:熊冰 % 时间:2018.1.20 % 功能:旋转齐次变换 function [m] = rot(axis,degree) g = degree*pi/180; switch axis case 'x' m =[1 0 0 0; 0 cos(g) -sin(g) 0; 0 sin(g) cos(g) 0; 0 0 0 1]; case 'y' m =[cos(g) 0 sin(g) 0; 0 1 0 0; -sin(g) 0 cos(g) 0; 0 0 0 1]; case 'z' m =[cos(g) -sin(g) 0 0; sin(g) cos(g) 0 0; 0 0 1 0; 0 0 0 1]; end end |
trans.m – 平移的齐次变换
1 2 3 4 5 6 7 8 9 10 11 |
% 标题:机器人学位置分析 % 作者:熊冰 % 时间:2018.1.20 % 功能:平移的齐次变换 function [m] = trans(a,b,c) m = [1 0 0 a;0 1 0 b;0 0 1 c;0 0 0 1]; end |
ptran.m – 绘制新的坐标
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 |
% 标题:机器人学位置分析 % 作者:熊冰 % 时间:2018.1.20 % 功能:绘制变换后的坐标轴 function [] = ptran(m) point = [m(1,4),m(2,4),m(3,4)]; plot3(m(1,4),m(2,4),m(3,4)); line([point(1,1),point(1,1)+m(1,1)],[point(1,2),point(1,2)+m(2,1)],[point(1,3),point(1,3)+m(3,1)],'color','r','linewidth',3); line([point(1,1),point(1,1)+m(1,2)],[point(1,2),point(1,2)+m(2,2)],[point(1,3),point(1,3)+m(3,2)],'color','g','linewidth',3); line([point(1,1),point(1,1)+m(1,3)],[point(1,2),point(1,2)+m(2,3)],[point(1,3),point(1,3)+m(3,3)],'color','b','linewidth',3); hold on quiver3(0,0,0,1,0,0,5); quiver3(0,0,0,0,1,0,5); quiver3(0,0,0,0,0,1,5); grid on view(-5,5) legend('point','x','y','z') xlabel( 'x-r'); ylabel( 'y-g'); zlabel( 'z-b'); hold on axis equal fprintf('Input is:\n'); disp(m); end |
测试用例1:计算最终位置并可视化
1 |
ptran(rot('z',90)*trans(4,-3,7)*rot('y',90)) |
测试用例2:计算变换过程并可视化
说明:先到了第一个位置然后沿着y(当前的坐标系的y)转了90
测试用例三:计算变换过程并可视化在多张图上
1 2 3 4 5 6 7 |
%控制台 ptran(rot('x',90)) figure ptran(rot('x',90)*rot('y',-90)) figure ptran(rot('x',90)*rot('y',-90)*trans(-1,0,0)) |