Соединить позы с помощью пути соединения БПЛА Дубинс

Создать 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]

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

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

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

Приращение элементов четвертого столбца poses матрица, представляющая headingAngle около pi и назначить его в качестве первого столбца матрицы ротации posesEulRot в представлении угла Euler 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')