waypointTrajectory
描述
的waypointTrajectory系统对象™生成基于指定的路径点的轨迹。当您创建的系统对象,您可以选择指定的到达时间,速度,或地面速度在每一个路标。您可以指定其他属性,如定位在每一个路标。看到算法为更多的细节。
从锚点来生成一个轨迹:
创建
waypointTrajectory对象并设置其属性。就好像它是一个函数调用对象。
了解更多关于系统对象是如何工作的,看到的系统对象是什么?。
创建
语法
描述
轨迹= waypointTrajectory轨迹,生成一个基于默认的固定锚点的轨迹。
轨迹= waypointTrajectory (路点,TimeOfArrival)TimeOfArrival属性的更多细节。
提示
当你指定TimeOfArrival论点,你不能指定这些属性:
JerkLimitInitialTimeWaitTime
轨迹= waypointTrajectory (路点,水平速度=水平)水平速度属性的更多细节。
轨迹= waypointTrajectory (路点速度,速度=)速度属性的更多细节。
轨迹= waypointTrajectory (___,名称=值)
例子:轨迹= waypointTrajectory([10 10 0; 20日20日0;20日20日10],[0,0.5,10])创建一个路径轨迹系统对象,轨迹,开始在路标[10 10 0],然后通过[0]20日20日后0.5秒,(20、20、10)后10秒。
属性
使用
输出参数
对象的功能
使用一个目标函数,指定系统对象作为第一个输入参数。例如,释放系统资源的系统对象命名obj使用这个语法:
发行版(obj)
例子
创建默认的waypointTrajectory
轨迹= waypointTrajectory
轨迹= waypointTrajectory属性:SampleRate: 100 SamplesPerFrame: 1路点:[2 x3双]TimeOfArrival: [2 x1双]速度:[2 x3双]:[2 x1双]水平:[2 x1双]ClimbRate: [2 x1双]取向:[2 x1四元数]AutoPitch: 0 AutoBank: 0 ReferenceFrame:“内德”
检查默认路径点,到达的时间通过调用waypointInfo。默认情况下,锚点指示一秒钟一个固定位置。
waypointInfo(轨迹)
ans =2×2表TimeOfArrival路标点_________________ ___________ 0 0 0 0 1 0 0 0
创建广场轨迹
创建一个广场轨迹并检查路径约束之间的关系,采样率,生成的轨迹。
创建一个广场轨迹通过定义的顶点广场。定义每个路标的方向指向运动的方向。指定一个1 Hz采样率和使用默认SamplesPerFrame为1。
路点= [0,0,0;…%初始位置0 1 0;…1 1 0;…1 0 0;…0,0,0);%最终位置toa = 0:4;%到达时间取向=四元数([0,0,0;…45 0 0;…135年,0,0;…225年,0,0;…0,0,0),…“eulerd”,“ZYX股票”,“帧”);轨迹= waypointTrajectory(锚点,…TimeOfArrival = toa,…取向=取向,…SampleRate = 1);
创建一个人物和情节的初始位置平台。
图(1)情节(锚点(1,- 1),锚点(1、2),“b *”)标题(“位置”)轴([1、2、1、2])轴广场包含(“X”)ylabel (“Y”网格)在持有在
在一个循环中,一步通过轨迹输出当前位置和电流方向。当前位置和日志取向的阴谋。使用暂停模拟的实时处理。
orientationLog = 0 (toa(结束)* trajectory.SampleRate 1“四元数”);数= 1;而~结束(轨迹)[,orientationLog(计数)]=轨迹();currentPosition的情节(currentPosition (1), (2),“波”)暂停(trajectory.SamplesPerFrame / trajectory.SampleRate)数=计数+ 1;结束持有从
取向四元数转换为欧拉角,便于解释,然后情节取向。
图(2)eulerAngles = eulerd(((1)方向;orientationLog),“ZYX股票”,“帧”);情节(toa, eulerAngles (: 1),“柯”,…toa, eulerAngles (:, 2),“bd”,…toa, eulerAngles (:, 3),“r”。);标题(“随着时间的推移,取向”)传说(“绕z轴旋转”,“绕轴旋转”,“绕轴旋转”)包含(“时间(秒)”)ylabel (“旋转(度)”网格)在
到目前为止,轨迹对象只有输出路径点指定的施工。锚点之间插入,提高采样率的速度快于时间到达的锚点。设置轨迹采样率为100 Hz和电话重置。
轨迹。SampleRate= 100; reset(trajectory)
创建一个人物和情节的初始位置平台。在一个循环中,一步通过轨迹输出当前位置和电流方向。当前位置和日志取向的阴谋。使用暂停模拟的实时处理。
图(1)情节(锚点(1,- 1),锚点(1、2),“b *”)标题(“位置”)轴([1、2、1、2])轴广场包含(“X”)ylabel (“Y”网格)在持有在orientationLog = 0 (toa(结束)* trajectory.SampleRate 1“四元数”);数= 1;而~结束(轨迹)[,orientationLog(计数)]=轨迹();currentPosition的情节(currentPosition (1), (2),“波”)暂停(trajectory.SamplesPerFrame / trajectory.SampleRate)数=计数+ 1;结束持有从
现在的轨迹输出循环出现。这是因为waypointTrajectory系统对象™最小化插值时的加速度和角速度,从而导致更平稳、更现实的运动在大多数场景。
取向四元数转换为欧拉角,便于解释,然后情节取向。方向插值。
图(2)eulerAngles = eulerd(((1)方向;orientationLog),“ZYX股票”,“帧”);t = 0:1 / trajectory.SampleRate: 4;情节(t, eulerAngles (: 1),“柯”,…t, eulerAngles (:, 2),“bd”,…t, eulerAngles (:, 3),“r”。);标题(“随着时间的推移,取向”)传说(“绕z轴旋转”,“绕轴旋转”,“绕轴旋转”)包含(“时间(秒)”)ylabel (“旋转(度)”网格)在
的waypointTrajectory算法篡改路径点创建一个平滑的轨迹。回到广场轨迹,提供更多的路径点,尤其是在急剧变化。跟踪相应的时间、路线点和方向,指定所有的轨迹信息在一个矩阵。
%的时间,路标,取向trajectoryInfo = [0, 0, 0, 0, 0, 0, 0;…%初始位置0.1,0,0.1,0,0,0,0;…0.9,0,0.9,0,0,0,0;…1,0,1,0,45岁,0,0;…1.1,0.1,1 0 90,0,0;…1.9,0.9,1 0 90,0,0;…135年2 1 1 0,0,0;…2.1、1、0.9,0,180,0,0;…2.9、1、0.1,0,180,0,0;…3、1 0 0 225 0,0;…270年3.1,0.9,0,0,0,0;…270年3.9,0.1,0,0,0,0;…4,0,0,0,270,0,0);%最终位置轨迹= waypointTrajectory (trajectoryInfo (: 2:4)…TimeOfArrival = trajectoryInfo (: 1),…取向=四元数(trajectoryInfo(:, 5:结束),“eulerd”,“ZYX股票”,“帧”),…SampleRate = 100);
创建一个人物和情节的初始位置平台。在一个循环中,一步通过轨迹输出当前位置和电流方向。当前位置和日志取向的阴谋。使用暂停模拟的实时处理。
图(1)情节(锚点(1,- 1),锚点(1、2),“b *”)标题(“位置”)轴([1、2、1、2])轴广场包含(“X”)ylabel (“Y”网格)在持有在orientationLog = 0 (toa(结束)* trajectory.SampleRate 1“四元数”);数= 1;而~结束(轨迹)[,orientationLog(计数)]=轨迹();currentPosition的情节(currentPosition (1), (2),“波”)暂停(trajectory.SamplesPerFrame / trajectory.SampleRate)数=计数+ 1;结束持有从
轨迹输出现在似乎更方,尤其是在顶点锚点。
取向四元数转换为欧拉角,便于解释,然后情节取向。
图(2)eulerAngles = eulerd(((1)方向;orientationLog),“ZYX股票”,“帧”);t = 0:1 / trajectory.SampleRate: 4;eulerAngles =情节(t, eulerAngles (: 1),“柯”,…t, eulerAngles (:, 2),“bd”,…t, eulerAngles (:, 3),“r”。);标题(“随着时间的推移,取向”)传说(“绕z轴旋转”,…“绕轴旋转”,…“绕轴旋转”,…“位置”,“西南”)包含(“时间(秒)”)ylabel (“旋转(度)”网格)在
创建圆弧轨迹
这个例子展示了如何创建一个使用的弧线轨迹waypointTrajectory系统对象™。waypointTrajectory创建一个指定路径锚点,最大限度地减少加速度和角速度。创建一个弧线轨迹之后,你限制在预定范围内的轨迹。
创建一个弧线轨迹
定义一个约束矩阵组成的锚点,到达的时间,为弧形的轨迹和方向。生成的轨迹经过路径点在指定的时间和指定的方向。的waypointTrajectory系统对象需要使用四元数或指定方向旋转矩阵。欧拉角保存在约束矩阵转换为四元数时指定取向财产。
%的到来,中转地点,方向约束=[0 0,20日20日,90年,0,0;3、50、20、0 90 0,0;4,58岁,15.5,0 162 0,0;180年5.5,59.5,0,0,0,0);轨迹= waypointTrajectory(约束(:2:4)…TimeOfArrival =约束(:1),…取向=四元数(约束(:,前书5章7节),“eulerd”,“ZYX股票”,“帧”));
调用waypointInfo在轨迹返回一个指定表的约束。创建属性路点,TimeOfArrival,取向表的是变量。表索引,方便绘图。
tInfo = waypointInfo(轨迹)
tInfo = 4 x3表TimeOfArrival中转地点取向_________________ ____________________ ___________ 0 20 20 0 {1 x1四元数}3 50 20 0 {1 x1四元数}4 58 15.5 0 {1 x1四元数}5.5 - 59.5 0 0 {1 x1四元数}
轨迹对象输出当前位置,速度,加速度和角速度在每个调用。调用轨迹在一个循环中随着时间的推移和情节位置。其他输出缓存。
图(1)情节(tInfo.Waypoints (1, 1), tInfo.Waypoints (1、2),“b *”)标题(“位置”)轴([65 0 25])包含(“北”)ylabel (“东”网格)在daspect ((1 1 1))在东方= 0 (tInfo.TimeOfArrival(结束)* trajectory.SampleRate, 1,“四元数”);韦尔= 0 (tInfo.TimeOfArrival * trajectory.SampleRate(结束),3);acc =韦尔;angVel =韦尔;数= 1;而~结束(轨迹)[pos,东方(计数),或者(计数,:),acc(计数,:),angVel(计数,:)]=轨迹();情节(pos pos (1), (2),“波”)暂停(trajectory.SamplesPerFrame / trajectory.SampleRate)数=计数+ 1;结束
检查方向,速度,加速度和角速度。的waypointTrajectory系统对象™创建一个路径指定的约束最小加速度和角速度。
图(2)timeVector = 0: (1 / trajectory.SampleRate): tInfo.TimeOfArrival(结束);eulerAngles = eulerd ([tInfo.Orientation{1};东方),“ZYX股票”,“帧”);情节(timeVector eulerAngles (: 1),…timeVector eulerAngles (:, 2),…timeVector eulerAngles (:, 3));标题(“随着时间的推移,取向”)传说(“绕z轴旋转”,…“绕轴旋转”,…“绕轴旋转”,…“位置”,“西南”)包含(“时间(秒)”)ylabel (“旋转(度)”网格)在图(3)图(timeVector(2:结束),或者(:1),…timeVector(2:结束),或者(:,2),…timeVector(2:结束),或者(:,3));标题(“随着时间的推移,速度”)传说(“北”,“东”,“向下”)包含(“时间(秒)”)ylabel (“速度(米/秒)”网格)在图(4)情节(timeVector(2:结束)、acc (: 1),…timeVector(2:结束)、acc (:, 2),…timeVector(2:结束)、acc (:, 3));标题(“随着时间的推移,加速”)传说(“北”,“东”,“向下”,“位置”,“西南”)包含(“时间(秒)”)ylabel (“加速度(m / s ^ 2)”网格)在图(5)图(timeVector(2:结束),angVel (: 1),…timeVector(2:结束),angVel (:, 2),…timeVector(2:结束),angVel (:, 3));标题(“角速度随时间”)传说(“北”,“东”,“向下”)包含(“时间(秒)”)ylabel (“角速度(rad / s)”网格)在
限制电弧轨迹在预设范围内
您可以指定额外的路点创建轨迹在给定的范围内。创建上界和下界的弧线轨迹。
图(1)xUpperBound =[(20:50)”, 50 + 10 *罪(0:0.1:π/ 2);60 * 1(11日1)];yUpperBound =[20.5 . *的(31日1);10.5 + 10 * cos(0:0.1:π/ 2);(10:1:0)');xLowerBound =[(20:49)”; 50 + 9 *罪(0:0.1:π/ 2);59 * 1(11日1)];yLowerBound =[19.5 . *的(30、1);10.5 + 9 * cos(0:0.1:π/ 2);(10:1:0)');情节(xUpperBound yUpperBound,“r”,“线宽”2);情节(xLowerBound yLowerBound,“r”,“线宽”,2)
创建一个轨迹范围内,添加额外的锚点。创建一个新的waypointTrajectory™系统对象,然后把它在一个循环中绘制生成的轨迹。缓存方向,速度,加速度和角速度输出轨迹对象。
%的时间,路标,取向约束=[0 0,20日20日,90年,0,0;1.5,35岁,20日0 90 0,0;2.5 45 20 0 90 0,0;3、50、20、0 90 0,0;现年53岁的3.3,19.5,0,108,0,0;126年3.6,55.5,18.25,0,0,0;3.9,57.5,16日0 144 0,0;4.2,59岁,14日0 162 0,0;4.5,59.5,10,180 0,0,0;5,59.5,5,180 0,0,0; 5.5, 59.5,0,0 180,0,0]; trajectory = waypointTrajectory(constraints(:,2:4),…TimeOfArrival =约束(:1),…取向=四元数(约束(:,前书5章7节),“eulerd”,“ZYX股票”,“帧”));tInfo = waypointInfo(轨迹);图(1)情节(tInfo.Waypoints (1, 1), tInfo.Waypoints (1、2),“b *”)数= 1;而~结束(轨迹)[pos,东方(计数),或者(计数,:),acc(计数,:),angVel(计数,:)]=轨迹();情节(pos pos (1), (2),“gd”)暂停(trajectory.SamplesPerFrame / trajectory.SampleRate)数=计数+ 1;结束
现在生成的轨迹符合在指定的范围内。可视化方向、速度、加速度和角速度生成的轨迹。
图(2)timeVector = 0: (1 / trajectory.SampleRate): tInfo.TimeOfArrival(结束);eulerAngles = eulerd(东方,“ZYX股票”,“帧”);情节(timeVector(2:结束),eulerAngles (: 1),…timeVector(2:结束),eulerAngles (:, 2),…timeVector(2:结束),eulerAngles (:, 3));标题(“随着时间的推移,取向”)传说(“绕z轴旋转”,…“绕轴旋转”,…“绕轴旋转”,…“位置”,“西南”)包含(“时间(秒)”)ylabel (“旋转(度)”网格)在图(3)图(timeVector(2:结束),或者(:1),…timeVector(2:结束),或者(:,2),…timeVector(2:结束),或者(:,3));标题(“随着时间的推移,速度”)传说(“北”,“东”,“向下”)包含(“时间(秒)”)ylabel (“速度(米/秒)”网格)在图(4)情节(timeVector(2:结束)、acc (: 1),…timeVector(2:结束)、acc (:, 2),…timeVector(2:结束)、acc (:, 3));标题(“随着时间的推移,加速”)传说(“北”,“东”,“向下”)包含(“时间(秒)”)ylabel (“加速度(m / s ^ 2)”网格)在图(5)图(timeVector(2:结束),angVel (: 1),…timeVector(2:结束),angVel (:, 2),…timeVector(2:结束),angVel (:, 3));标题(“角速度随时间”)传说(“北”,“东”,“向下”)包含(“时间(秒)”)ylabel (“角速度(rad / s)”网格)在
注意,虽然现在生成的轨迹适合空间边界,加速度和角速度的轨迹有些飘忽不定。这是由于过于具体化锚点。
生成赛道轨迹使用waypointTrajectory
考虑一个赛马场轨迹如下。
四个角点的轨迹(0,0,0)、(20,0,0)、(20 5 0)和(0 5 0)米,分别。因此,指定一个循环的路径点为:
wps = [0 0 0;20 0 0;20 5 0;0 5 0;0 0 0];
假设轨道有一个常数2米/秒的速度,因此五路点的速度是:
韦尔= [2 0 0;2 0 0;2 0 0;2 0 0;2 0 0];
五路点的到达时间是:
t = cumsum([0计5 * 20/2π/ 2/2计5 * 20/2π/ 2/2)');
在五路点轨迹的方向是:
eulerAngs = [0 0 0;0 0 0;180 0 0;180 0 0;0 0 0];%角的度。%将欧拉角转换为四元数。皮疹=四元数(eulerAngs,“eulerd”,“ZYX股票”,“帧”);
指定采样率作为平滑轨迹线100。
fs = 100;
构建waypointTrajectory。
traj = waypointTrajectory (wps SampleRate = fs,…速度=韦尔,…TimeOfArrival = t)…取向=皮疹);
样品和情节的轨迹。
[pos,东方,或者,acc angvel] = traj ();i = 1;防晒系数= traj.SamplesPerFrame;而~结束(traj) idx = (i + 1): (i +防晒系数);[pos (idx:),东方(idx:)…韦尔(idx:)、acc (idx:), angvel (idx:)] = traj ();我=我+防晒系数;结束
情节轨迹和指定的锚点。
情节(pos (: 1), pos (:, 2), wps (: 1), wps (:, 2),”——o”)包含(“X”(m))ylabel (“Y (m)”)zlabel (“Z”(m))({传奇“轨迹”,“锚点”})轴平等的
使用路标点和地面速度创建轨迹
创建一个waypointTrajectory对象,连接两个锚点。在两个锚点轨迹的速度是0
和10
,分别。0.5限制混蛋限制
使梯形加速度剖面。
路点= [0 0 0;10 50 10];速度= (10 0);jerkLimit = 0.5;轨迹= waypointTrajectory(锚点,水平速度=速度,JerkLimit = JerkLimit);
获得的初始时间和最后一次轨迹通过查询TimeOfArrival财产。创建时间戳来样本轨迹。
t0 = trajectory.TimeOfArrival (1);tf = trajectory.TimeOfArrival(结束);sampleTimes = linspace (t0,特遣部队100);
获得位置、速度和加速度信息使用这些采样时间邮票lookupPose对象的功能。
(位置,~,速度,加速度,~]= lookupPose(轨迹,sampleTimes);
绘制轨迹。
图()plot3(位置(:1),位置(:,2),位置(:,3))包含(“x”(m))ylabel (“y (m)”)zlabel (“z”(m))标题(“轨迹”)
绘制速度剖面。
图()次要情节(3、1、1)的阴谋(sampleTimes、速度(:1));ylabel (“v_x (m / s)”)标题(“速度剖面”次要情节(3、1、2)情节(sampleTimes、速度(:,2));ylabel (“v_y (m / s)”次要情节(3,1,3)情节(sampleTimes、速度(:,3));ylabel (“v_z (m / s)”)包含(“时间(sec)”)
情节加速度剖面。从结果,加速度的平面运动是梯形。
图()次要情节(3、1、1)的阴谋(sampleTimes、加速度(:1));轴垫ylabel (“a_x (m / s ^ 2)”)标题(“加速概要”次要情节(3、1、2)情节(sampleTimes、加速度(:,2));ylabel (“得+ (m / s ^ 2)”)轴垫次要情节(3、1,3)情节(sampleTimes、加速度(:,3));ylabel (“a_z (m / s ^ 2)”)包含(“时间(sec)”)
算法
的waypointTrajectory系统对象定义了一个轨迹,顺利通过锚点。通过插值轨迹连接路径点假定中表达的重力方向轨迹参考系是恒定的。一般来说,您可以使用waypointTrajectory模型平台或车辆轨迹,在数百公里的距离内。
的平面路径轨迹(x- - - - - -y平面投影)由分段、回旋曲线曲线。连续两个锚点之间的曲线的曲率变化的线性曲线长度。的切线方向的路径选择在每一个路标最小化曲率不连续,除非显式地指定的课程是通过课程财产或隐式地通过速度财产。一旦建立了路径,对象使用三次埃尔米特插值来计算车辆在整个路径的位置作为时间的函数,平面的距离。如果JerkLimit指定属性,物体产生一个水平梯形加速度剖面为两个锚点之间的任何部分。梯形加速度概要文件包含三个子分段:
一个constant-magnitude混蛋亚节
constant-magnitude加速亚节
一个constant-magnitude混蛋亚节
正常的组件(z分)的轨迹是随后选择满足一种保形分段样条(PCHIP)除非爬率是通过显式指定ClimbRate财产或第三列速度财产。选择基于所选择的爬升速度的符号ReferenceFrame:
选择当一个ENU表示的参考系,指定一个积极的爬升率结果的增值z。
选择当一个NED的参考系,指定一个积极的爬升率导致减少的价值z。
您可以定义车辆通过道路的方向在两个主要方面:
如果
取向对象属性被指定,那么使用piecewise-cubic,四元数样条计算方向沿着路径作为时间的函数。如果
取向属性没有指定,那么汽车的偏航总是与路径。然后由横滚和俯仰AutoBank和AutoPitch分别为属性值。AutoBankAutoPitch描述 假假车辆总是水平(零俯仰和滚)。这是通常用于大型海洋船只。 假真正的车距与路径,其辊总是零。这是通常用于地面车辆。 真正的假选择车辆俯仰和滚,这样其本地z设在与净加速度是一致的(包括重力)。这是通常用于旋转翼飞行器。 真正的真正的车辆选择辊,使其局部截面与净加速度(包括重力)。车距与路径。这是通常用于两轮车辆和固定翼飞机。
