Инерциальная навигационная система и имитационная модель ГНСС/GPS
insSensor Система object™ моделирует устройство, которое плавит измерения от инерциальной навигационной системы (INS) и глобальной навигационной спутниковой системы (GNSS), такой как GPS, и выводит плавкие измерения.
Для вывода результатов измерений плавкой INS и GNSS:
Создать insSensor и задайте его свойства.
Вызовите объект с аргументами, как если бы это была функция.
Дополнительные сведения о работе системных объектов см. в разделе Что такое системные объекты?.
INS = insSensorINS, который моделирует устройство, которое выводит измерения из INS и GNSS.
INS = insSensor(Name,Value)
measurement = INS(gTruth)gTruth.
measurement = INS(gTruth,simTime)simTime. Чтобы включить этот синтаксис, установите TimeInput свойство для true.
Чтобы использовать функцию объекта, укажите объект System в качестве первого входного аргумента. Например, для освобождения системных ресурсов объекта System с именем obj, используйте следующий синтаксис:
release(obj)
Создайте структуру движения, определяющую стационарное положение в локальной точке отсчета на северо-восток вниз (NED). Поскольку платформа является стационарной, необходимо определить только один образец. Предположим, что движение «земля-истина» дискретизируется в течение 10 секунд с частотой дискретизации 100 Гц. Создание значения по умолчанию insSensor object™ системы. Предварительно распределить переменные для хранения выходных данных insSensor объект.
Fs = 100; duration = 10; numSamples = Fs*duration; motion = struct( ... 'Position',zeros(1,3), ... 'Velocity',zeros(1,3), ... 'Orientation',ones(1,1,'quaternion')); INS = insSensor; positionMeasurements = zeros(numSamples,3); velocityMeasurements = zeros(numSamples,3); orientationMeasurements = zeros(numSamples,1,'quaternion');
В цикле, вызов INS со стационарной структурой движения для возврата измерений положения, скорости и ориентации в локальной системе координат NED. Запишите измерения положения, скорости и ориентации.
for i = 1:numSamples measurements = INS(motion); positionMeasurements(i,:) = measurements.Position; velocityMeasurements(i,:) = measurements.Velocity; orientationMeasurements(i) = measurements.Orientation; end
Преобразование ориентации из кватернионов в углы Эйлера для целей визуализации. Постройте график измерений положения, скорости и ориентации во времени.
orientationMeasurements = eulerd(orientationMeasurements,'ZYX','frame'); t = (0:(numSamples-1))/Fs; subplot(3,1,1) plot(t,positionMeasurements) title('Position') xlabel('Time (s)') ylabel('Position (m)') legend('North','East','Down') subplot(3,1,2) plot(t,velocityMeasurements) title('Velocity') xlabel('Time (s)') ylabel('Velocity (m/s)') legend('North','East','Down') subplot(3,1,3) plot(t,orientationMeasurements) title('Orientation') xlabel('Time (s)') ylabel('Rotation (degrees)') legend('Roll', 'Pitch', 'Yaw')
Создание измерений с помощью датчика INS, установленного на транспортном средстве в сценарии вождения. Постройте график измерений INS в зависимости от истинного состояния транспортного средства на земле и визуализируйте профиль скорости и ускорения транспортного средства.
Создание сценария управления
Загрузите географические данные для маршрута вождения в кампусе MathWorks Apple Hill в Натике, штат Массачусетс.
data = load('ahroute.mat');
latIn = data.latitude;
lonIn = data.longitude;
Преобразование координат широты и долготы трассы в декартовы координаты. Задайте начало координат для первой координаты в маршруте движения. Для простоты предположим, что высота маршрута равна 0.
alt = 0; origin = [latIn(1),lonIn(1),alt]; [xEast,yNorth,zUp] = latlon2local(latIn,lonIn,alt,origin);
Создайте сценарий управления. Задайте начало преобразованной трассы в качестве географической опорной точки.
scenario = drivingScenario('GeoReference',origin);
Создайте дорогу на основе декартовых координат трассы.
roadCenters = [xEast,yNorth,zUp]; road(scenario,roadCenters);
Создайте транспортное средство, которое следует по центральной линии дороги. Транспортное средство движется от 4 до 5 метров в секунду (от 9 до 11 миль в час), замедляясь на кривых в дороге. Чтобы создать траекторию, используйте smoothTrajectory функция. Вычисленная траектория минимизирует рывок и позволяет избежать разрывов в ускорении, что является требованием для моделирования датчиков INS.
egoVehicle = vehicle(scenario,'ClassID',1);
egoPath = roadCenters;
egoSpeed = [5 5 5 4 4 4 5 4 4 4 4 5 5 5 5 5];
smoothTrajectory(egoVehicle,egoPath,egoSpeed);
Постройте график сценария и отобразите 3-D вид сзади эго-транспортного средства.
plot(scenario) chasePlot(egoVehicle)
Создание датчика INS
Создайте датчик INS, принимающий ввод времени моделирования. Введите шум в измерения датчика, установив стандартное отклонение измерений скорости и точности 0,1 и 0,05 соответственно.
INS = insSensor('TimeInput',true, ... 'VelocityAccuracy',0.1, ... 'AccelerationAccuracy',0.05);
Визуализация измерений INS
Инициализируйте географический проигрыватель для отображения измерений INS и истинности земли актера. Настройте проигрыватель для отображения последних 10 позиций и установите уровень масштабирования равным 17.
zoomLevel = 17; player = geoplayer(latIn(1),lonIn(1),zoomLevel, ... 'HistoryDepth',10,'HistoryStyle','line');
Предварительно распределите пространство для измерений времени моделирования, скорости и ускорения, которые регистрируются во время моделирования.
numWaypoints = length(latIn); times = zeros(numWaypoints,1); gTruthVelocities = zeros(numWaypoints,1); gTruthAccelerations = zeros(numWaypoints,1); sensorVelocities = zeros(numWaypoints,1); sensorAccelerations = zeros(numWaypoints,1);
Смоделировать сценарий. Во время цикла моделирования получите состояние истинности земли эго-транспортного средства и измерение INS этого состояния. Преобразуйте эти показания в географические координаты и в каждом ППМ визуализируйте истинные данные земли и показания INS на географическом проигрывателе. Сбор данных о скорости и ускорении для построения графиков скорости и ускорения.
nextWaypoint = 2; while advance(scenario) % Obtain ground truth state of ego vehicle. gTruth = state(egoVehicle); % Obtain INS sensor measurement. measurement = INS(gTruth,scenario.SimulationTime); % Convert readings to geographic coordinates. [latOut,lonOut] = local2latlon(measurement.Position(1), ... measurement.Position(2), ... measurement.Position(3),origin); % Plot differences between ground truth locations and locations reported by sensor. reachedWaypoint = sum(abs(roadCenters(nextWaypoint,:) - gTruth.Position)) < 1; if reachedWaypoint plotPosition(player,latIn(nextWaypoint),lonIn(nextWaypoint),'TrackID',1) plotPosition(player,latOut,lonOut,'TrackID',2,'Label','INS') % Capture simulation times, velocities, and accelerations. times(nextWaypoint,1) = scenario.SimulationTime; gTruthVelocities(nextWaypoint,1) = gTruth.Velocity(2); gTruthAccelerations(nextWaypoint,1) = gTruth.Acceleration(2); sensorVelocities(nextWaypoint,1) = measurement.Velocity(2); sensorAccelerations(nextWaypoint,1) = measurement.Acceleration(2); nextWaypoint = nextWaypoint + 1; end if nextWaypoint > numWaypoints break end end
График профиля скорости
Сравните истинную продольную скорость транспортного средства во времени с измерениями скорости, измеренными датчиком INS.
Удалите нули из векторов времени и скорости.
times(times == 0) = []; gTruthVelocities(gTruthVelocities == 0) = []; sensorVelocities(sensorVelocities == 0) = []; figure hold on plot(times,gTruthVelocities) plot(times,sensorVelocities) title('Longitudinal Velocity Profile') xlabel('Time (s)') ylabel('Velocity (m/s)') legend('Ground truth','INS') hold off
Профиль ускорения графика
Сравните истинное продольное ускорение транспортного средства во времени с измерениями ускорения, измеренными датчиком INS.
gTruthAccelerations(gTruthAccelerations == 0) = []; sensorAccelerations(sensorAccelerations == 0) = []; figure hold on plot(times,gTruthAccelerations) plot(times,sensorAccelerations) title('Longitudinal Acceleration Profile') xlabel('Time (s)') ylabel('Acceleration (m/s^2)') legend('Ground truth','INS') hold off
Чтобы получить состояние «земля-истина» актеров в сценарии вождения, используйте state функция.
Датчик сообщает об измерениях в локальной декартовой системе координат. Чтобы преобразовать эти измерения в географические положения для визуализации на карте, используйте local2latlon функция. Чтобы преобразовать эти данные обратно в локальные координаты, используйте latlon2local функция.