我有一个移动的刚体，我需要计算曲面上一个点的位置，作为时间的函数。我得到了以下输入：
- 第一个时间步长的全局坐标系中感兴趣点的坐标
- 对于每个时间步长，通过重心的3个相同的正交矢量，也在全局坐标中给出
我首先确定第一个时间步长的点在局部(身体固定(坐标系中的位置。然后，我使用相应的旋转矩阵(如本问题所示(，在第二步中计算该点在全局坐标中的坐标。我手动测量了那个点应该在哪里，但这并不匹配。
下面的示例代码可能更好地说明了我的方法和问题。方法或实施是否错误？我该如何正确处理这个问题？代码可能有点长，但允许执行。我在Matlab中做这件事。
%% Clean workspace and set path
% Clean up the workspace, command window, close figures
clear all
close all
clc
%% Object inputs
% Coordinates of 3 orthogonal axes through the center of gravity, in the
% global coordinate system. At the first timestep.
object{1}.X.startpoint = [24.7349058855438,-11.5479658533523,1098.57662582473];
object{1}.X.endpoint = [-0.832905885543847,1.10916585335228,1097.37317417527];
object{1}.Y.startpoint = [5.676667643105184,-17.997569574970736,1.096883018501248e+03];
object{1}.Y.endpoint = [18.225332356894818,7.558769574970736,1.099066781498751e+03];
object{1}.Z.startpoint = [12.973532739613240,-4.506163733481916,1.083752143929416e+03];
object{1}.Z.endpoint = [10.4904947081313,-6.23813055135267,1118.28956543937];
% Same information for the object at a second timestep
object{2}.X.startpoint = [24.656923458165010,-8.251284344143922,1.104914286298065e+03];
object{2}.X.endpoint = [0.352676541834988,3.899684344143922,1.091666913701935e+03];
object{2}.Y.startpoint = [13.544852623105038,9.838707132571910,1.107402610954567e+03];
object{2}.Y.endpoint = [11.464747376894960,-14.190307132571911,1.089178589045433e+03];
object{2}.Z.startpoint = [3.577145171754987,-9.045948694099657,1.108368142901039e+03];
object{2}.Z.endpoint = [21.432454828245014,4.694348694099658,1.088213057098961e+03];
% Coordinates of a point fixed on the (rigid) object, in the global
% coordinate system. At the first timestep.
object{1}.POI.dorsal.global = [19.231140624851460,-0.281237811310405,1.099798468105154e+03];
%% Check
% We measured manualy what the position of the same point should be at the
% second timestep. We'll use this to check the calculate. A small error is
% to be expected.
check = [16.17432165785938 3.6337839926906312 1103.4455838115914];
%% Calculate the coordinate of the origin of the coordinate system fixed to the object
for i=1:2
M_start = [object{i}.X.startpoint;object{i}.Y.startpoint;object{i}.Z.startpoint];
M_end = [object{i}.X.endpoint;object{i}.Y.endpoint;object{i}.Z.endpoint];
[P,dist] = lineIntersect3D(M_start,M_end);
object{i}.COG = P;
end
% to execute the code without the lineInterSect3D function, use
% object{1}.COG = [11.951000000000000,-5.219399999999998,1.097974900000000e+03];
% object{2}.COG = [12.504799999999960,-2.175799999999953,1.098290600000000e+03];
%% Rotation matrix
% Create from the known axis in the global coordinate system a normalized
% orthogonal matrix. This is the rotation matrix.
for i=1:2
% x,y,z coordinates in the global frame of the original axis line end points
M_end = [object{i}.X.endpoint;object{i}.Y.endpoint;object{i}.Z.endpoint];
% subtract the COG coodinates for each the origin of the object frame
% with the global coordinate system
M_tmp = M_end-repmat(object{i}.COG,3,1);
% Normalize
V = [M_tmp(1,:)/norm(M_tmp(1,:));M_tmp(2,:)/norm(M_tmp(2,:));M_tmp(3,:)/norm(M_tmp(3,:))];
object{i}.rotMatrix = V;
end
%% POI to local coordinates for the 1st timestep
% First shift the point with the COG vector then rotate with the rotation
% matrix.
tmp = object{1}.POI.dorsal.global - object{1}.COG;
object{1}.POI.dorsal.local = object{1}.rotMatrix*tmp';
%% And back to global in the second timestep
% test if the transformation works backwards for the 1st points:
test = (object{1}.rotMatrix.' * object{1}.POI.dorsal.local)' + object{1}.COG % first rotate, then translate. This should give back the global coord.
object{1}.POI.dorsal.global
dist_1 = norm(object{1}.POI.dorsal.global-test) % that works
% now do this for the second timestep
% the coordinate of the point of interest in the local (fixed to the
% object) coordinate system has not changed
object{2}.POI.dorsal.local = object{1}.POI.dorsal.local;
% rotate with the transpose of the rotation matrix (of the second timestep)
% and translate with the vector to the object fixed coordinate system
object{2}.POI.dorsal.global = (object{2}.rotMatrix' * object{2}.POI.dorsal.local)' + object{2}.COG;
%% Check the results
% The results don't match with what we manualy measured however
object{2}.POI.dorsal.global
check
dist_2 = norm(object{2}.POI.dorsal.global-check) % that doesn't work
% Plot the result
c = object{2};
P_actual = check; % for datapoint 2 lunate dorsal global
vecX_X = [c.X.startpoint(1) c.X.endpoint(1)];vecX_Y = [c.X.startpoint(2) c.X.endpoint(2)];vecX_Z = [c.X.startpoint(3) c.X.endpoint(3)];
vecY_X = [c.Y.startpoint(1) c.Y.endpoint(1)];vecY_Y = [c.Y.startpoint(2) c.Y.endpoint(2)];vecY_Z = [c.Y.startpoint(3) c.Y.endpoint(3)];
vecZ_X = [c.Z.startpoint(1) c.Z.endpoint(1)];vecZ_Y = [c.Z.startpoint(2) c.Z.endpoint(2)];vecZ_Z = [c.Z.startpoint(3) c.Z.endpoint(3)];
POI_X = [c.COG(1) c.POI.dorsal.global(1)];POI_Y = [c.COG(2) c.POI.dorsal.global(2)];POI_Z = [c.COG(3) c.POI.dorsal.global(3)];
figure
title('Global')
hold all
plot3(vecX_X,vecX_Y,vecX_Z,'b-')
plot3(vecY_X,vecY_Y,vecY_Z,'r-')
plot3(vecZ_X,vecZ_Y,vecZ_Z,'k-')
plot3(POI_X,POI_Y,POI_Z,'mo--')
plot3(P_actual(1),P_actual(2),P_actual(3),'gd')
plot3(c.COG(1),c.COG(2),c.COG(3),'kd')
plot3(c.X.endpoint(1),c.X.endpoint(2),c.X.endpoint(3),'bo')
plot3(c.Y.endpoint(1),c.Y.endpoint(2),c.Y.endpoint(3),'ro')
plot3(c.Z.endpoint(1),c.Z.endpoint(2),c.Z.endpoint(3),'ko')
legend('X','Y','Z','POI','checkpoint','COG')
grid on
检查旋转矩阵的行列式，我发现它是-1。事实上，身体固定轴定义了一个左手系统，并转换为右手系统。为了确保这不是问题的原因，我将起点和终点换成了Y矢量(有效地创建了一个右手坐标系。没有用。
- 查找从坐标系原点到曲面上关心的点的矢量(在全局帧中(。称这个向量为V
- 计算V在x、y和z轴上的投影；即取点积：点(V，x(、点(V、y(、点的乘积(V，z(。这些点积是局部帧中的矢量。这也可以通过inv(R(*V来计算。由于R是正交旋转矩阵，inv(R(=R'，所以它只是R'V
不确定为什么旋转矩阵的det=-1，但我会检查一下，你构造向量的方式是右旋的。事实上，既然你知道X和Y，为什么不使用Z=cross(X，Y(呢？如果x和y是正交的，你就可以保证得到一个右手CS。