Question

我写了一个小的Matlab程序来模拟具有正交速率陀螺仪的飞机。它基于Matlab示例。该程序允许为飞机的侧倾，俯仰和偏航设置初始条件，并创建速率陀螺仪数据数组。 Matlab函数makehgtform（）在循环中使用比率陀螺仪数据更新飞机的方向。 3d图的轴是全局轴，速率陀螺仪与飞机的局部轴对齐。

问题如下：当我为滚动设置非零初始条件时，俯仰或偏航的非零速率陀螺仪数据会导致飞机绕其自身的轴线俯仰或偏航。当偏航的初始条件不为零且通过速率陀螺仪应用俯仰或侧倾数据时，飞机将绕全局轴而不是其自身的局部轴进行俯仰或侧倾。当俯仰的初始值不为零时，速率陀螺偏航数据可以正常工作，但是侧倾数据会导致飞机绕全局轴而不是局部轴滚动。

解决问题的最简单方法是仅使一个初始条件为非零，并将速率陀螺仪数据应用于其他两个轴中的任何一个；例如设置音调= 0.5和Gyro_yaw = 1（nSamples，1）* 0.2; Gyro_roll和Gyro_pitch归零。

我以前尝试过为每个轴使用单独的makehgtform（）调用，然后将结果连接起来，但是这带来了矩阵乘法不可交换的问题，因此变换的乘法没有“正确的顺序”。如我的代码所示，使用合并版本的makehgtform（）的行为恰好是，如果分别执行转换并按x_rotate * y_rotate * z_rotate的顺序进行乘法运算，那当然是不正确的。

clf

% set the limits and select a view
ax = axes('Xlim', [-2 2], 'YLim', [-2 2], 'ZLim', [-1.5 1.5]);
view(3);
grid on;
axis equal

% Set up vehicle model
[xc yc zc] = cylinder([0.1 0.0]); % cone
[x y z] = cylinder([0.2 0.2]);

h(1) = surface(x, z,  - 0.5 * y,  'FaceColor', 'blue');   % left wing
h(2) = surface(zc, yc, xc, 'FaceColor', 'red');    % nose
h(3) = surface(-1.5 * z, y,  0.5 * x, 'FaceColor', 'red');    % fuselage
h(4) = surface(x, -z, 0.5 * y, 'FaceColor', 'yellow'); % right wing
h(5) = surface((1.5 * xc) - 1.3, yc, z, 'FaceColor', 'red');    % tail

% Create group object and parent surfaces
t = hgtransform('Parent', ax);
set(h, 'Parent', t);

% Set the renderer to OpenGL and update the display
set(gcf, 'Renderer', 'opengl');
drawnow

% Initial conditions in radians
roll = 0;
pitch = 0.5;
yaw = 0;

nSamples = 40;

% Set up rate gyro data. Activate individual rate gyro by using ones() 
% and deactivate by using zeros(), in which case, the scaling is 
% irrelevant
Gyro_roll = zeros(nSamples, 1) * 0.2;  % roll
Gyro_pitch = zeros(nSamples, 1) * 0.2; % pitch
Gyro_yaw = ones(nSamples, 1) * 0.2;    % yaw

for i = 1 : numel(Gyro_roll)

    rotation = makehgtform('xrotate', roll, 'yrotate', pitch, 'zrotate', yaw);
    set(t, 'Matrix', rotation);

    % Update orientation
    roll = roll + Gyro_roll(i);
    pitch = pitch + Gyro_pitch(i);
    yaw = yaw + Gyro_yaw(i);

    pause

end

使用makehgtform组合旋转

0 个答案: