Соедините положения с помощью БПЛА Dubins Путь

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

connectionObj = uavDubinsConnection;

Задайте начальное и целевое положения как [x, y, z, 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]

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

Вычислите матрицу перемещений и вращений переходных положений, исключая начальное и целевое положения. The posesTranslation матрица состоит из первых трех столбцов poses матрица, задающая положение x, y, и 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')