一种无人机跟拍路径规划与跟踪方法
【技术领域】
[0001] 本发明设及飞行器导航、制导与控制领域。具体设及一种无人机跟拍路径规划与 跟踪方法。适用于多旋翼无人机跟拍目标时的路径规划与路径跟踪。
【背景技术】
[0002] 多旋翼无人机W其结构简单、成本低廉、机动灵活等特点，使得在航拍摄影、地图 测绘、侦察监视等行业的应用日趋广泛。在航拍应用中，无人机自动跟拍是一种拍摄效果良 好，且对操作人员技术要求较低的一种拍摄模式，目前多旋翼无人机跟拍方式均为预先设 定好无人机与跟拍目标的相对位置，跟拍目标携带定位设备并将实时位置发送至无人机， 而无人机在飞行过程中保持与跟拍目标的相对位置不变，运类方案中，无人机的运行轨迹 不够平滑，航拍效果因此受到影响。
【发明内容】
[0003] 本发明的目的是针对目前存在的上述问题，提供一种无人机跟拍路径规划与跟踪 方法，跟拍轨迹平滑、拍摄角度实时可调的特点，能够实现更好的航拍效果。
[0004] 本发明的上述目的通过W下技术方案实现：
[0005] -种多旋翼无人机跟拍路径规划与跟踪方法，包括W下步骤：
[0006] 步骤1，在无人机启动跟拍飞行时设定无人机与跟拍目标之间的相对位置参数，或 者跟拍飞行过程中在当前制导周期内设定无人机与跟拍目标之间的相对位置参数，
[0007] 相对位置参数包括当前制导周期的期望距离L、当前制导周期的期望视线俯仰角 ζ、当前制导周期的期望视线偏角巧;i
[000引步骤2,获得跟拍目标当前的位置作为跟拍目标当前制导周期的位置0,获得跟拍 目标当前的速度矢量作为跟拍目标当前制导周期的速度矢量记；
[0009] 步骤3,根据当前制导周期的期望距离L、当前制导周期的期望视线俯仰角（、当前 制导周期的期望视线偏角0、跟拍目标当前制导周期的位置0和跟拍目标当前制导周期的 速度矢量頑，获得无人机当前制导周期的期望位置J;
[0010] 步骤4, W无人机当前制导周期的期望位置J为当前制导周期内的目标航路点，并 根据上一个制导周期计算所得目标航路点I与当前制导周期内的目标航路点对当前制导周 期内的目标航路点进行跟踪，将上一个制导周期的目标航路点I与当前制导周期内的目标 航路点连线得到连线IJ，并将无人机当前的位置P向该连线IJ或其延长线投影得到当前投 影点Q，通过凌= <、百7获得点K的坐标，从而获得无人机的期望速度矢量方向瓦，
[00川其中，巧为当前投影点昭晚人机当前制导周期的期望位置J的矢量，点K将矢量 琢分割为灰和面，S为分割的比例系数，
[0012]在当前制导周期中W固定的控制周期At对当前投影点Q和点K的坐标进行更新， 无人机在当前制导周期中的各个控制周期中W对应的无人机的期望速度矢量方向盛进行 里艮踪；
[0013]步骤5,根据无人机的位置P与跟拍目标当前制导周期的位置0设置无人机相机云 台期望角度；
[0014] 步骤6,在下一个制导周期来临时，返回步骤1。
[0015] 如上所述的步骤3中无人机当前制导周期的期望位置J通过W下公式过得：
[0016]
[0017] 其中，Jx，Jy，Jz分别为无人机当前制导周期的期望位置J在导航坐标系x，y，z轴上 的坐标，L为当前制导周期的期望距离、C为当前制导周期的期望视线俯仰角为当前制导 周期的期望视线偏角，Φ为无人机航向角。
[0018] 如上所述的步骤5中设置无人机相机云台期望角度包括W下步骤：
[0019] 步骤5.1、通过W下公式求取跟拍目标当前制导周期的位置0相对于无人机当前位 置P的视线俯仰角γ ;则跟拍目标当前制导周期的位置0相对于无人机携带的相机云台的期 望俯仰角为-丫；
[0020]
[0021] 〇x，〇y，〇z分别为跟拍目标当前制导周期的位置0在导航坐标系x，y，z轴上的坐标， Px，Py，Pz分别为无人机当前位置P在导航坐标系x，y，z轴上的坐标；
[0022] 步骤5.2、通过W下公式求取跟拍目标当前制导周期的位置0相对于无人机携带的 相机云台的期望视线偏角ξ:
[0023]
[0024] 步骤5.3、相机云台根据-丫和ξ进行实时跟拍。
[0025] 本发明与现有技术相比具有W下有益效果：
[0026] 1、本发明W跟拍对象的速度矢量为参考，通过视线俯仰角和视线偏角计算无人机 期望位置，因此能够锁定无人机的拍摄视角，并能够根据需要实时改变视角;而现有方案通 过无人机与跟拍目标在导航坐标系中的相对位置计算无人机期望位置，无人机拍摄视角随 着跟拍目标速度方向的变化而变化，需要手动进行修正。
[0027] 2、本发明在航线跟踪环节采用了航向角的比例控制，相对于现有跟拍方案中的直 接飞向目标航路点，本发明所得的无人机轨迹更为平滑。
【附图说明】
[0028] 图1无人机期望位置与跟拍目标当前位置在导航坐标系中的几何关系；
[0029] 图2跟拍目标当前位置相对于无人机当前位置的视线俯仰角和视线偏角；
[0030] 图3无人机位置投影在两个航路点连线IJ上时的航向计算示意图；
[0031 ]图4无人机位置投影在两个航路点连线IJ延长线时的航向计算示意图。
【具体实施方式】
[0032] W下结合具体实施例对本发明的技术方案做进一步描述。
[0033] -种多旋翼无人机跟拍路径规划与跟踪方法，包括W下几个步骤：
[0034] 步骤1，在无人机启动跟拍飞行时设定无人机与跟拍目标之间的相对位置参数，或 者跟拍飞行过程中在当前制导周期内设定无人机与跟拍目标之间的相对位置参数。如图1 所示，导航坐标系采用原点0在跟拍目标质屯、，x，y，z轴分别指向北、东、地的当地铅键坐标 系，其中，0为跟拍目标当前制导周期的位置，窥为跟拍目标当前制导周期的速度矢量，因 此无人机的航向角即为跟拍目标当前制导周期的速度矢量現在当地水平面上的投影頑·' 与正北方向的夹角则分别为无人机当前制导周期的期望位置W及无人机当前制导 周期的期望位置在导航坐标系的当地水平面上的投影点。
[0035] 需要设置或调整的无人机当前制导周期的期望位置与跟拍目标之间的相对位置 参数包括当前制导周期的期望距离L、当前制导周期的期望视线俯仰角（、当前制导周期的 期望视线偏角巧；
[0036] (1)无人机当前制导周期的期望位置与跟拍目标当前制导周期的位置的当前制导 周期的期望距离L，由于无人机携带相机的视场大小限制W及拍摄的不同需求，无人机需要 与跟拍目标保持一定的直线距离，即图1中0J线段的长度，该距离与跟拍目标的速度、摄像 机视场大小W及具体应用要求有关，需要预先设置和根据需要动态调整。
[0037] (2)无人机当前制导周期的期望位置相对于跟拍目标当前制导周期的速度矢量的 当前制导周期的期望视线俯仰角ζ，该角度为无人机当前制导周期的期望位置与跟拍目标 当前制导周期的位置之间的连线療与过导航坐标系X轴和跟拍目标当前制导周期的速度 矢量的平面之间的夹角，即图1中的角度ζ，该角度决定了无人机相机W多大的角度俯拍跟 拍目标，其大小与具体应用要求有关。
[0038] (3)无人机当前制导周期的期望位置相对于跟拍目标当前制导周期的速度矢量的 当前制导周期的期望视线偏角9 ,该角度为无人机当前制导周期的期望位置与跟拍目标当 前制导周期的位置0之间的连线瓦/与通过无人机速度矢量的导航坐标系的当地铅键面之 间的夹角，即图1中的角度^>，该角度决定了无人机相机在什么方向拍摄跟拍目标，其大小 与具体应用要求有关。
[0039] 步骤2,获得跟拍目标当前的位置作为跟拍目标当前制导周期的位置0,获得跟拍 目标当前的速度矢量作为跟拍目标当前制导周期的速度矢量瓦，为了获得良好的跟拍效 果，需要跟拍目标在每个制导周期测量并发送一次跟拍目标当前的位置和跟拍目标当前的 速度矢量到无人机，跟拍目标当前的位置和跟拍目标当前的速度矢量可W通过跟拍目标上 的GI^设备获得。
[0040] 步骤3，由于通过GI^设备获得的跟拍目标当前的位置和跟拍目标当前的速度矢量 的频率较低，即只是在每个制导周期获得一次跟拍目标当前的位置和跟拍目标当前的速度 矢量作为跟拍目标当前制导周期的位置0和跟拍目标当前制导周期的速度矢量盈，为了使 跟拍轨迹平滑，需要将计算所得的无人机当前制导周期的期望