Объединение вращений с использованием makehgtform - PullRequest
Новая
       7

Объединение вращений с использованием makehgtform

0 голосов
/ 02 января 2019

Я написал небольшую программу Matlab для моделирования самолета с гироскопами с ортогональной скоростью. Он основан на примере Matlab. Программа позволяет задавать начальные условия для крена самолета, тангажа и рыскания, а также создавать массивы гироскопических данных скорости. Функция Matlab makehgtform () используется в цикле для обновления ориентации самолета, используя данные гироскопа скорости. Оси трехмерного графика - это глобальные оси, а гироскопы скорости выровнены с локальными осями самолета.

Проблема заключается в следующем: когда я устанавливаю ненулевое начальное условие для крена, то данные гироскопа с ненулевой скоростью для тангажа или рыскания приводят к тому, что самолет поворачивается или поворачивается вокруг своих собственных осей, как и должно быть. Когда начальное условие рыскания не равно нулю, а данные о тангаже или крене применяются через гироскопы скорости, самолет наклоняется или катится по глобальным осям, а не по своим собственным локальным осям. Если начальное значение шага отличное от нуля, то данные скоростного гироскопа работают правильно, но данные крена заставляют самолет вращаться вокруг глобальной, а не локальной оси.

Самый простой способ увидеть проблему - это сделать только одно начальное условие ненулевым и применить гироскопические данные скорости к любой из двух других осей; например, установите шаг = 0,5 и Gyro_yaw = единицы (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
...