关于3D坐标变换的数学原理 3D坐标变换被广泛应用,在机器人导航和器械臂控制上我们常常需要变换坐标系,参杂平移和旋转,本文对相关的数学原理做一个梳理。
(1)为了便于理解,首先从1维开始探讨。 )))人在车上走,车在地面上行驶。人相对于地面的速度 = 人相对于车的速度+车相对于地面的速度 :
图1:一维坐标变换图示
(2)二维坐标变换的简单情况——只有平移: )))向量(A), 在S1坐标系中有坐标(xa_s1, ya_s1), S1坐标系原点相对于S2坐标系的坐标是(xs1_s2, ys1_s2)。 我们要得到(A)在S2中的坐标(xa_s2,ya_s2),有:
)))))))) xa_s2 = xa_s1 + xs1_s2;
)))))))) ya_s2 = ya_s1 + ys1_s2.
这里可以看成是在 (xa_s1, ya_s1) 引入一个平移变换(xs1_s2, ys1_s2), 这个平移就相当于一维例子中“车相对于地面的速度”。
(配图2:二维坐标平移变换图示)
(3)二维坐标变换的一般情况——平移和旋转: )))向量(A), 在S1坐标系中有坐标(xa_s1, ya_s1), S1坐标系原点相对于S2坐标系的坐标是(xs1_s2, ys1_s2),S1坐标系相对于S2坐标系的旋转角是θ, (逆时针为正方向) 。 我们要得到(A)在S2中的坐标(xa_s2,ya_s2),分两步完成:
图3:二维坐标一般变换,平移和旋转图示
第一步: 旋转,使用欧拉旋转矩阵R(θ) = [ cos(θ) -sin(θ) ; sin(θ) cos(θ)]:
[ x_1 ; y_1 ] = [ cos(θ) -sin(θ) ; sin(θ) cos(θ)] * [xa_s1, ya_s1] .
此处矩阵乘法。用MATLAB中的协议表示一个矩阵,分号代表提行。
第二步: 平移, 如上述:
[ xa_s2 ; ya_s2 ] = [ x_1 ; y_1 ] + [xs1_s2; ys1_s2].
…