Моделирование движения платформы с использованием объектов траектории

В этом разделе описывается использование трех различных объектов траектории для моделирования траекторий платформы и выбор между ними.

Введение

Sensor Fusion and Tracking Toolbox™ предоставляет три системных объекта, которые можно использовать для моделирования траекторий платформ, включая наземные транспортные средства, корабли, самолеты и космические аппараты. Можно выбрать между этими объектами траектории на основе имеющейся информации о траектории и диапазона расстояния траектории.

waypointTrajectory - определяет траекторию, используя несколько ППМ в декартовых координатах, через которые должна проходить траектория. Траектория предполагает, что опорным кадром является фиксированный кадр «север-восток-вниз» (NED) или «восток-север-вверх» (RUS). Поскольку интерполяция траектории предполагает, что гравитационное ускорение, выраженное в системе отсчёта траектории, является постоянным, waypointTrajectory обычно используется для траектории, определенной в области, которая охватывает только десятки или сотни километров.
geoTrajectory - определяет траекторию, используя несколько ППМ в геодезических координатах (широта, долгота и высота), через которые должна проходить траектория. Поскольку ППМ выражены в геодезических координатах, geoTrajectory обычно используется для траектории от сотен до тысяч километров расстояния.
kinematicTrajectory - определяет траекторию, используя кинематические свойства, такие как ускорение и угловое ускорение, выраженные в раме корпуса платформы. Вы можете использовать kinematicTrajectory генерировать траекторию любого диапазона расстояний до тех пор, пока доступна кинематическая информация траектории. Объект принимает декартову координатную опорную рамку.

Два объекта траектории на основе ППМ (waypointTrajectory и geoTrajectory) может автоматически рассчитать информацию о линейной скорости платформы, но также можно явно указать линейную скорость с помощью Velocity свойства или комбинации Course, GroundSpeed, и ClimbRate свойства.

Траектория платформы состоит из вращательного движения и поступательного движения. По умолчанию два объекта траектории на основе ППМ (waypointTrajectory и geoTrajectory) автоматически генерируют ориентацию платформы в каждой ППМ, выравнивая угол рыскания с траекторией траектории, но можно явно указать ориентацию с помощью Orientation собственность. В качестве альтернативы можно использовать AutoPitch и AutoBank свойства, позволяющие автоматизировать углы наклона и крена соответственно. Для kinematicTrajectory, вам нужно использовать Orientation и входной угловой скорости для задания вращательного движения траектории.

`waypointTrajectory`

waypointTrajectory Системный объект определяет траекторию, которая плавно проходит через ППМ, выраженные в декартовых координатах. Как правило, можно использовать waypointTrajectory для моделирования транспортных средств, движущихся в пределах сотен километров. К ним относятся автомобили, надводные морские суда и коммерческие самолеты (вертолеты, самолеты и квадрокоптеры). Ссылочный кадр можно выбрать в качестве фиксированного кадра RUS или NED с помощью ReferenceFrame собственность. Дополнительные сведения о том, как объект генерирует траекторию, см. в разделе Алгоритмы waypointTrajectory.

`waypointTrajectory` Пример посадки самолета

Открыть сценарий в реальном времени

Определите траекторию посадочного самолета с помощью waypointTrajectory объект.

waypoints = [-421 -384 2000;
    47 -294 1600;
    1368 174 1300;
    995 1037 900;
    -285 293 600;
    -1274 84 350;
    -2328 101 150;
    -3209 83 0];
timeOfArrival =  [0; 16.71; 76.00; 121.8; 204.3; 280.31; 404.33; 624.6];
aircraftTraj = waypointTrajectory(waypoints,timeOfArrival);

Создать theaterPlot объект для визуализации траектории и летательного аппарата.

minCoords = min(waypoints);
maxCoords = max(waypoints);
tp = theaterPlot('XLimits',1.2*[minCoords(1) maxCoords(1)], ...
    'YLimits',1.2*[minCoords(2) maxCoords(2)], ...
    'ZLimits',1.2*[minCoords(3) maxCoords(3)]);

% Create a trajectory plotter and a platform plotter
tPlotter = trajectoryPlotter(tp,'DisplayName','Trajectory');
pPlotter = platformPlotter(tp,'DisplayName','Aircraft');

Получение декартовых ППМ траектории с помощью lookupPose объектная функция.

sampleTimes = timeOfArrival(1):timeOfArrival(end);
numSteps = length(sampleTimes);
[positions,orientations] = lookupPose(aircraftTraj,sampleTimes);
plotTrajectory(tPlotter,{positions})
axis equal

Постройте график движения платформы с использованием объекта-сетки самолета.

mesh = scale(rotate(tracking.scenario.airplaneMesh,[0 0 180]),15); % Exaggerated scale for better visualization
view(20.545,-20.6978)
for i = 1:numSteps
    plotPlatform(pPlotter,positions(i,:),mesh,orientations(i))
    % Uncomment the next line to slow the aircraft motion animation
    % pause(1e-7)
end

В анимации угол рыскания самолета по умолчанию выравнивается с траекторией.

Создайте вторую траекторию самолета с теми же ППМ, что и первая траектория самолета, но установите ее AutoPitch и AutoBank свойства для true. Это генерирует траекторию, более репрезентативную для возможных маневров самолета.

aircraftTraj2 = waypointTrajectory(waypoints,timeOfArrival, ...
    'AutoPitch',true, ...
    'AutoBank',true);

Постройте график второй траектории и понаблюдайте за изменением ориентации самолета.

[positions2,orientations2] = lookupPose(aircraftTraj2,sampleTimes);
for i = 1:numSteps
    plotPlatform(pPlotter,positions2(i,:),mesh,orientations2(i));
    % Uncomment the next line to slow the aircraft motion animation
    % pause(1e-7)
end

Figure contains an axes. The axes contains 2 objects of type line. These objects represent Trajectory, Aircraft.

Визуализируйте различия в ориентации двух траекторий в углах.

distance = dist(orientations2,orientations);
figure
plot(sampleTimes,distance*180/pi)
xlabel('Time (sec)')
ylabel('Angular Ditance (dge)')
title('Orientation Difference Between Two Waypoint Trajectories')

Figure contains an axes. The axes with title Orientation Difference Between Two Waypoint Trajectories contains an object of type line.

`geoTrajectory`

geoTrajectory Системный объект генерирует траекторию с использованием ППМ таким же образом, как и waypointTrajectory объект, но он имеет два основных отличия в том, как указать ППМ и входы скорости.

При указании ППМ для geoTrajectory, выразите каждый waypoint в геодезических координатах широты, долготы и высоты выше эллиптической модели WG84. Используя геодезические координаты, можно удобно задать дальние траектории, например траекторию полета самолета на реалистичной модели Земли.
При задании информации об ориентации и скорости для каждой ППМ опорным кадром для ориентации и скорости является локальный кадр NED или RUS, определенный в текущей ППМ траектории. Например, кадр N1-E1-D1, показанный на чертеже, является локальным опорным кадром NED.

На рисунке

Ex, Ey и Ez - это три оси земно-центрированного земно-фиксированного (ECEF) каркаса, который закреплен на Земле.
(_λ1, _ϕ1, _h1), и (_λ2, _ϕ2, _h2) геодезические координаты самолета в двух waypoints.
(N1, E1, D1) и (N2, E2, D2) являются двумя локальными кадрами NED, соответствующими двум путевым точкам траектории.
Bx, By и Bz _{- три оси}рамы корпуса платформы, которая закреплена на платформе.

гео-`Trajectory` Пример для траектории полета

Открыть сценарий в реальном времени

Загрузите полетные данные траектории полета из Лос-Анджелеса в Бостон. Данные содержат полетную информацию, включая время полета, геодезические координаты для каждого ППМ, курса и наземной скорости.

load flightData.mat

Создать geoTrajectory объект на основе полетных данных.

planeTraj = geoTrajectory([latitudes longitudes heights],timeOfArrival, ...
    'Course',courses,'GroundSpeed',speeds);

Найдите декартовы координаты ППМ в кадре ECEF.

sampleTimes = 0:1000:3600*10;
positionsCart = lookupPose(planeTraj,sampleTimes,'ECEF');

Показать траекторию с помощью helperGlobeView класс, который аппроксимирует сферу Земли.

viewer = helperGlobeView;
plot3(positionsCart(:,1),positionsCart(:,2),positionsCart(:,3),'r')

Далее можно исследовать траекторию, запросив другие выходные данные траектории.

`kinematicTrajectory`

В отличие от двух объектов траектории ППМ, kinematicTrajectory Объект System использует кинематические атрибуты для задания траектории. Рассмотрим траекторию как численное интегрирование позы (положения и ориентации) и линейной скорости платформы на основе информации о линейном ускорении и угловом ускорении. Поза и линейная скорость задаются с учетом выбранной, фиксированной рамы сценария, а линейное ускорение и угловая скорость задаются с учетом рамы корпуса платформы.

`kinematicTrajectory` Пример для пути БПЛА

Открыть сценарий в реальном времени

Создать kinematicTrajectory объект для моделирования траектории БПЛА. Укажите временной интервал траектории как 120 секунд.

traj = kinematicTrajectory('SampleRate',1, ...
    'AngularVelocitySource','Property');

tStart = 0;
tFinal = 120;
tspan = tStart:tFinal;

numSteps = length(tspan);
positions = NaN(3,numSteps);
velocities = NaN(3,numSteps);
vel = NaN(1,numSteps);

Для формирования квадратного пути, охватывающего небольшую область, разделите траекторию БПЛА на шесть сегментов:

Взлет и подъем в направлении z
Перемещение в положительном направлении x
Движение в положительном направлении по оси Y
Перемещение в отрицательном направлении x
Движение в отрицательном направлении y
Спуск по z-направлению и посадка

В каждом сегменте БПЛА ускоряется в одном направлении, а затем замедляется в этом направлении с одинаковой величиной ускорения. В результате в конце каждого сегмента скорость БПЛА равна нулю.

segSteps = floor(numSteps/12);
accelerations = zeros(3,numSteps);
acc = 1;
% Acceleration for taking off and ascending in the z-direction
accelerations(3,1:segSteps) = acc;
accelerations(3,segSteps+1:2*segSteps) = -acc;

% Acceleration for moving in the positive x-direction
accelerations(1,2*segSteps+1:3*segSteps) = acc;
accelerations(1,3*segSteps+1:4*segSteps) = -acc;

% Acceleration for moving in the positive y-direction
accelerations(2,4*segSteps+1:5*segSteps) = acc;
accelerations(2,5*segSteps+1:6*segSteps) = -acc;

% Acceleration for moving in the negative x-direction
accelerations(1,6*segSteps+1:7*segSteps) = -acc;
accelerations(1,7*segSteps+1:8*segSteps) = acc;

% Acceleration for moving in the negative y-direction 
accelerations(2,8*segSteps+1:9*segSteps) = -acc;
accelerations(2,9*segSteps+1:10*segSteps) = acc;

% Descending in the z-direction and landing
accelerations(3,10*segSteps+1:11*segSteps) =-acc;
accelerations(3,11*segSteps+1:end) = acc;

Смоделировать траекторию, вызвав kinematicTrajectory объект с указанным ускорением.

for i = 1:numSteps
    [positions(:,i),~,velocities(:,i)] = traj(accelerations(:,i)');  
     vel(i) = norm(velocities(:,i));
end

Визуализация траектории с помощью theaterPlot.

% Create a theaterPlot object and create plotters for the trajectory and the
% UAV Platform.
figure
tp = theaterPlot('XLimits',[-30 130],'YLimits',[-30 130],'ZLimits',[-30 130]);
tPlotter = trajectoryPlotter(tp,'DisplayName','UAV trajectory');
pPlotter = platformPlotter(tp,'DisplayName','UAV','MarkerFaceColor','g');

% Plot the trajectory.
plotTrajectory(tPlotter,{positions'})
view(-43.18,56.49)

% Use a cube to represent the UAV platform.
dims = struct('Length',10, ...
    'Width',5, ...
    'Height',3, ...
    'OriginOffset',[0 0 0]);

% Animate the UAV platform position.
for i = 1:numSteps
    plotPlatform(pPlotter,positions(:,i)',dims,eye(3))
    pause(0.01)
end

Figure contains an axes. The axes contains 2 objects of type line. These objects represent UAV trajectory, UAV.

Показать величину скорости платформы БПЛА.

figure
plot(tspan,vel)
xlabel('Time (s)')
ylabel('Velocity (m/s)')
title('Magnitude of the UAV Velocity')

Figure contains an axes. The axes with title Magnitude of the UAV Velocity contains an object of type line.

`kinematicTrajectory` Пример траектории космического аппарата

Открыть сценарий в реальном времени

Использовать kinematicTrajetory задание круговой траектории космического аппарата. Орбита имеет следующие элементы:

Радиус орбиты ( $r$ ) - 7000 км
Наклон ( $i$ ) - 60 градусов
Аргумент восходящего узла ( $Λ$ ) - 90 градусов. Направление восходящего узла выравнивается с направлением Y.
Истинная аномалия ( $start$ ) - -90 градусов

На рисунке X-Y-Z представляет собой инерциальную рамку с центром Земли (ECI), которая имеет фиксированное положение и ориентацию в пространстве. x-y-z - каркас корпуса космического аппарата, закрепленный на космическом аппарате. $\overset{r\to}{}$ и $\overset{v\to}{}$ - исходное положение и скорость космического аппарата.

Задание круговой орбиты с помощью kinematicTrajectory, необходимо указать исходное положение, начальную скорость и начальную ориентацию космического аппарата относительно кадра ECI. Для выбранной истинной аномалии ( $v$ = - $^{90\circ}$ ) скорость космического аппарата выравнивается по направлению Y.

inclination = 60; % degrees
mu = 3.986e14; % standard earth gravitational parameter
radius = 7000e3;% meters
v = sqrt(mu/radius); % speed
initialPosition = [radius*cosd(inclination),0,-radius*sind(inclination)]';
initialVelocity = [0 v 0]';

Предположим, что x-направление рамы корпуса космического аппарата является радиальным направлением, z-направление - нормальным направлением орбитальной плоскости, а y-направление завершает правило правой руки. Используйте допущения для задания ориентации рамы кузова в исходном положении.

orientation = quaternion([0 inclination 0],'eulerd','zyx','frame');

Выражайте угловую скорость и угловое ускорение траектории в раме корпуса платформы.

omega = v/radius;
angularVelocity = [0 0 omega]';
a = v^2/radius;
acceleration = [-a 0 0]';

Установка времени моделирования одного орбитального периода. Укажите шаг моделирования как 2 секунды.

tFinal = 2*pi/omega;
dt = 2;
sampleRate = 1/dt;
tspan = 0:dt:tFinal;
numSteps = length(tspan);

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

traj = kinematicTrajectory('SampleRate',sampleRate, ...
    'Position',initialPosition, ...
    'Velocity',initialVelocity, ...
    'Orientation',orientation, ...
    'AngularVelocity',angularVelocity, ...
    'Acceleration',acceleration, ...
    'AccelerationSource','Property', ...
    'AngularVelocitySource','Property');

% Generate position and orientation outputs.
positions = NaN(3,numSteps);
orientations = zeros(numSteps,1,'quaternion');
for i = 1:numSteps    
   [positions(:,i),orientations(i)] = traj();     
end

Используйте helperGlobeView класс и theaterPlot для отображения траектории.

viewer = helperGlobeView(0,[60 0]);
tp = theaterPlot('Parent',gca,...
    'XLimits',1.2*[-radius radius],...
    'YLimits',1.2*[-radius radius],...
    'ZLimits',1.2*[-radius radius]);
tPlotter = trajectoryPlotter(tp,'LineWidth',2);
pPlotter = platformPlotter(tp,'MarkerFaceColor','m');
legend(gca,'off')

plotTrajectory(tPlotter,{positions'})
% Use a cube with exaggerated dimensions to represent the spacecraft.
dims = struct('Length',8e5,'Width',4e5,'Height',2e5,'OriginOffset',[0 0 0]);

for i = 1:numSteps    
   plotPlatform(pPlotter,positions(:,i)',dims,orientations(i))
   % Since the reference frame is the ECI frame, earth rotates with respect to it.
   rotate(viewer,dt)
end

Резюме

В этом разделе рассматривается использование трех объектов траектории для настройки собственных траекторий на основе имеющейся информации. Кроме того, вы узнали фундаментальные различия в их применении. В этой таблице представлены основные атрибуты этих объектов траектории.

Объект-траектория	Позиционные входы	Входные данные линейной скорости	Ориентация	Входы ускорения и угловой скорости	Рекомендуемый диапазон расстояний
`waypointTrajectory`	Декартовы ППМ, выраженные в фиксированном кадре (NED или ENU)	Один из следующих вариантов: Автоматически создавать скорость для гладкой траектории по умолчанию Укажите скорость в фиксированном кадре в каждой точке ППМ Укажите курс, скорость движения на земле и скорость набора высоты в фиксированном кадре в каждом ППМ	Один из следующих вариантов: Автоматический рыскание по умолчанию, автоматический шаг по выбору и автоматический банк по выбору Указание ориентации в фиксированном кадре	Невозможно указать	От десятков до сотен километров
`geoTrajectory`	Геодезические ППМ в рамке ECEF	Один из следующих вариантов: Автоматически создавать скорость для гладкой траектории по умолчанию Укажите скорость в локальном кадре (NED или RUS) для каждого ППМ Укажите курс, скорость движения на земле и скорость набора высоты в локальном кадре (NED или RUS) для каждого ППМ	Один из следующих вариантов: Автоматический рыскание по умолчанию, автоматический шаг по выбору и автоматический банк по выбору Указание ориентации в локальной рамке	Невозможно указать	От сотен до тысяч километров
`kinematicTrajectory`	Исходное положение, выраженное в выбранном фиксированном кадре	Только начальная скорость в фиксированном кадре	Только начальная ориентация в неподвижной рамке	Определение ускорения и угловой скорости в корпусе платформы	Неограниченное расстояние

Документация