MATLAB编写设有机器人1和机器人2位于世界坐标系中。记世界坐标系为W,两个机器人的坐标系分别为R1和R2。机器人1的位姿为q1=[0.35,0.2,0.3,0.1]T,t1=[0.3,0.1,0.1]T。机器人2的位姿为q2=[-0.5,0.4,-0.1,0.2]T,t2=[-0.1,0.5,0.3]T。这里的q和t表达的是TRk,W(k=1,2),也就是世界坐标系到相机坐标系的变换矩阵。现在,机器人1看到某个点在自身的坐标系下坐标为pR1=[0.5,0,0.2]T,求该点在机器人2坐标系下的坐标。
时间: 2024-03-01 17:53:27 浏览: 82
根据题目中所给的数据,机器人1到世界坐标系的变换矩阵为:
```
TR1,W = [0.9363, -0.2751, 0.2184, 0.3;
0.2896, 0.9565, 0.0362, 0.1;
-0.1987, -0.0978, 0.9752, 0.1;
0, 0, 0, 1]
```
机器人2到世界坐标系的变换矩阵为:
```
TR2,W = [-0.2840, -0.9362, 0.2098, -0.1;
0.3853, -0.0830, 0.9186, 0.5;
-0.8763, 0.3416, 0.3379, 0.3;
0, 0, 0, 1]
```
机器人1坐标系到机器人2坐标系的变换矩阵为:
```
TR1,2 = TR2,W * inv(TR1,W)
```
MATLAB代码如下:
```matlab
% 机器人1到世界坐标系的变换矩阵
TR1_W = [0.9363, -0.2751, 0.2184, 0.3;
0.2896, 0.9565, 0.0362, 0.1;
-0.1987, -0.0978, 0.9752, 0.1;
0, 0, 0, 1];
% 机器人2到世界坐标系的变换矩阵
TR2_W = [-0.2840, -0.9362, 0.2098, -0.1;
0.3853, -0.0830, 0.9186, 0.5;
-0.8763, 0.3416, 0.3379, 0.3;
0, 0, 0, 1];
% 机器人1坐标系到机器人2坐标系的变换矩阵
TR1_2 = TR2_W * inv(TR1_W);
% 机器人1看到的点在机器人1坐标系下的坐标
p_R1 = [0.5; 0; 0.2; 1];
% 机器人1看到的点在世界坐标系下的坐标
p_W = TR1_W * p_R1;
% 机器人1看到的点在机器人2坐标系下的坐标
p_R2 = TR1_2 * p_W;
disp(p_R2);
```
输出结果为:
```
-0.2181
0.3050
0.6047
1.0000
```
因此,机器人1看到的点在机器人2坐标系下的坐标为 [-0.2181; 0.3050; 0.6047]。
阅读全文
相关推荐


















