Соедините положения Используя путь к связи UAV Dubins

Создайте uavDubinsConnection объект.

connectionObj = uavDubinsConnection;

Задайте запускаются, и цель изображает из себя [xYZ, headingAngleВекторы.

startPose = [0 0 0 0]; % [meters, meters, meters, radians]
goalPose = [0 0 20 pi];

Вычислите допустимый сегмент пути и соедините положения.

[pathSegObj,pathCost] = connect(connectionObj,startPose,goalPose);

Покажите сгенерированный путь.

show(pathSegObj{1})

Интерполируйте положения

Задайте интервал, чтобы интерполировать вдоль пути.

stepSize = pathSegObj{1}.Length/10;
lengths = 0:stepSize:pathSegObj{1}.Length;

Интерполируйте положения вдоль сегмента пути в заданных длинах пути.

poses = interpolate(pathSegObj{1},lengths); % [x, y, z, headingAngle, flightPathAngle, rollAngle]

Визуализируйте положения перехода

Вычислите матрицу перевода и вращения положений перехода, исключая запуск и целевые положения. posesTranslation матрица состоит из первых трех столбцов poses матрица, задающая положение xY, и z.

posesTranslation = poses(2:end-1,1:3); % [x, y, z]

Постепенно увеличьте элементы четвертого столбца poses матрица, представляющая headingAngle pi и присвойте его как первый столбец матрицы вращения posesEulRot в представлении Угла Эйлера ZYX. Столбец pi и столбец нулей формирует второе и третьи столбцы posesEulRot матрица, соответственно. Преобразуйте posesEulRot матрица от Углов Эйлера до кватерниона и присвоения к posesRotation.

N = size(poses,1)-2;
posesEulRot = [poses(2:end-1,4)+pi,ones(N,1)*pi,zeros(N,1)]; % [headingAngle + pi, pi, 0]
posesRotation = quaternion(eul2quat(posesEulRot,'ZYX'));

Постройте преобразовывают систему координат положений перехода путем определения их переводов и вращений с помощью plotTransforms.

hold on
plotTransforms(posesTranslation,posesRotation,'MeshFilePath','fixedwing.stl','MeshColor','cyan')