位置J作为当前制导周期的 目标航路点，然后再应用航路点跟踪算法进行跟踪。
[0041] 由于当前制导周期的期望距离L、当前制导周期的期望视线俯仰角（、当前制导周 期的期望视线偏角0构成了球面坐标的Ξ个参数，因此跟拍目标当前制导周期的位置0和 跟拍目标当前制导周期的速度矢量涵确定后，无人机当前制导周期的期望位置J就能够唯 一确定，如图2所示，W固定时间间隔ΔΤ为制导周期，根据步骤1设定的相对位置参数，计算 无人机当前制导周期的期望位置J:
[0042]
[0043] 设定/为导航坐标系中跟拍目标当前制导周期的位置0到无人机当前制导周期的 期望位置J的向量，Jx，Jy，Jz分别为无人机当前制导周期的期望位置J在导航坐标系X，y，z轴 上的坐标，L为当前制导周期的期望距离、C为当前制导周期的期望视线俯仰角、r为当前制 导周期的期望视线偏角，Φ为无人机航向角。
[0044] 步骤4, W无人机当前制导周期的期望位置J为当前制导周期内的目标航路点，并 根据上一个制导周期计算所得目标航路点I与当前制导周期内的目标航路点对当前制导周 期内的目标航路点进行跟踪，将上一个制导周期的目标航路点I与当前制导周期内的目标 航路点连线得到连线IJ，并将无人机当前时刻的位置P向该连线IJ或其延长线投影得到当 前投影点Q，如图3和图4所示，当前时刻无人机的期望速度矢量方向为涼，且满足：
[004引其中，査7为当前投影点Q到当前制导周期内的目标航路点（即无人机当前制导周 期的期望位置J)的矢量，点K将矢量获分割为奔和致，δ为分割的比例系数，Kx，Ky，Kz分别 为点K在导航坐标系x，y，z轴上的坐标，Qx，Qy，(iz分别为当前投影点Q在导航坐标系x，y，z轴 上的坐标。
[0049]从中可W解出K点在导航坐标系中的分量：
[(K)加]
[0051] 在当前制导周期内，W固定的时间间隔At为控制周期，在每个控制周期内对当前 投影点Q进行更新，通过当前投影点Q和无人机当前制导周期的期望位置J对点K的位置进行 更新，从而无人机只需跟踪期望速度矢量雨即可完成对航线的跟踪，进而实现对跟拍目标 位置的实时跟踪。
[0052] 而由于当前投影点Q为无人机当前的位置P在线段IJ或其延长线上的投影，假设Z PJI为α，则
[0059] W固定的控制周期At对当前投影点Q和点Κ的坐标进行更新，则可W实现对上一 个制导周期计算所得目标航路点I与当前制导周期的目标航路点（即无人机当前制导周期 的期望位置J)之间航线的平滑跟踪，图3为点Q在线段IJ上时的航线跟踪示意图，其中Κι、Κ2 为后两个控制周期A t对应的点Κ，化、Q2为后两个控制周期Δ t对应的投影点，图4为当前投 影点Q在线段IJ延长线上时的航线跟踪示意图，即当前时刻，无人机当前的位置P，目标航向 即无人机的期望速度矢量为深，在下一个控制周期来临时，无人机已飞至点Pi，此时更新 化，Κι点的坐标，目标航向即无人机的期望速度矢量则变为坏，W此类推。
[0060] 步骤5,根据无人机的位置Ρ与跟拍目标当前制导周期的位置0设置无人机相机云 台期望角度。如图2所示，跟拍目标当前制导周期的位置0相对于无人机当前时刻位置Ρ的视 线俯仰角为：
[0061]
[0062] 因此在忽略无人机当前位置与无人机所载相机云台当前位置误差的情况下，跟拍 目标当前制导周期的位置0相对于无人机携带的相机云台的期望俯仰角为-丫。
[0063] 而跟拍目标当前制导周期的位置0相对于无人机携带的相机云台的期望视线偏角 ξ则为：
[0064]
[0065] 其中，Οχ,0y，0z分别为导航坐标系中跟拍目标当前制导周期的位置的x，y，z分量， Px，Py，Pz分别为导航坐标系中无人机当前位置的x，y，z分量，相机云台W运两个角度为指令 偏转即可实现对目标的实时跟拍。
[0066] 步骤6,在下一个制导周期来临时，返回步骤1，即对相对位置参数、无人机当前制 导周期的目标航路点J、上一个制导周期的目标航路点I进行更新，然后进行下一个制导周 期的跟拍动作。
[0067] 本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领 域的技术人员可W对所描述的具体实施例做各种各样的修改或补充或优化或采用类似的 方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。
【主权项】
1. 一种多旋翼无人机跟拍路径规划与跟踪方法，其特征在于，包括以下步骤： 步骤1，在无人机启动跟拍飞行时设定无人机与跟拍目标之间的相对位置参数，或者跟 拍飞行过程中在当前制导周期内设定无人机与跟拍目标之间的相对位置参数， 相对位置参数包括当前制导周期的期望距离L、当前制导周期的期望视线俯仰角ζ、当 前制导周期的期望视线偏角^ 步骤2,获得跟拍目标当前的位置作为跟拍目标当前制导周期的位置0,获得跟拍目标 当前的速度矢量作为跟拍目标当前制导周期的速度矢量01; 步骤3,根据当前制导周期的期望距离L、当前制导周期的期望视线俯仰角ζ、当前制导 周期的期望视线偏角〇、跟拍目标当前制导周期的位置〇和跟拍目标当前制导周期的速度 矢量页，获得无人机当前制导周期的期望位置J; 步骤4,以无人机当前制导周期的期望位置J为当前制导周期内的目标航路点，并根据 上一个制导周期计算所得目标航路点I与当前制导周期内的目标航路点对当前制导周期内 的目标航路点进行跟踪，将上一个制导周期的目标航路点I与当前制导周期内的目标航路 点连线得到连线IJ，并将无人机当前的位置Ρ向该连线IJ或其延长线投影得到当前投影点 Q，通过尿=<>_灰获得点1(的坐标，从而获得无人机的期望速度矢量方向琢， 其中，反为当前投影点Q到无人机当前制导周期的期望位置J的矢量，点Κ将矢量设分 割为函和'反，S为分割的比例系数， 在当前制导周期中以固定的控制周期At对当前投影点Q和点Κ的坐标进行更新，无人 机在当前制导周期中的各个控制周期中以对应的无人机的期望速度矢量方向兩：进行跟 踪； 步骤5,根据无人机的位置P与跟拍目标当前制导周期的位置0设置无人机相机云台期 望角度； 步骤6,在下一个制导周期来临时，返回步骤1。2. 根据权利要求1所述的一种多旋翼无人机跟拍路径规划与跟踪方法，其特征在于，所 述的步骤3中无人机当前制导周期的期望位置J通过以下公式过得：其中，Jx，Jy，Jz分别为无人机当前制导周期的期望位置J在导航坐标系x，y，z轴上的坐 标，L为当前制导周期的期望距离、ζ为当前制导周期的期望视线俯仰角、P为当前制导周期 的期望视线偏角，Φ为无人机航向角。3. 根据权利要求1所述的一种多旋翼无人机跟拍路径规划与跟踪方法，其特征在于，所 述的步骤5中设置无人机相机云台期望角度包括以下步骤： 步骤5.1、通过以下公式求取跟拍目标当前制导周期的位置0相对于无人机当前位置Ρ 的视线俯仰角γ ;则跟拍目标当前制导周期的位置〇相对于无人机携带的相机云台的期望 俯仰角为-γ ;Ox，Oy，Oz分别为跟拍目标当前制导周期的位置0在导航坐标系x，y，z轴上的坐标，P x，Py， Pz分别为无人机当前位置P在导航坐标系x，y，z轴上的坐标； 步骤5.2、通过以下公式求取跟拍目标当前制导周期的位置0相对于无人机携带的相机 云台的期望视线偏角ξ:步骤5.3、相机云台根据-γ和ξ进行实时跟拍。
【专利摘要】本发明公开了一种多旋翼无人机跟拍路径规划与跟踪方法，设定无人机与跟拍目标之间的相对位置参数，获得跟拍目标当前制导周期的位置，获得跟拍目标当前制导周期的速度矢量；根据无人机与跟拍目标之间的相对位置参数、跟拍目标当前制导周期的位置与速度矢量，获得无人机当前制导周期的期望位置；根据上一个制导周期计算所得目标航路点与当前制导周期内的目标航路点对当前制导周期内的目标航路点进行跟踪；相机云台根据相机云台的期望俯仰角和相机云台的期望视线偏角进行实时跟拍。本发明能够锁定无人机的拍摄视角，并能够根据需要实时改变视角；航线跟踪环节采用了航向角的比例控制，所得的无人机轨迹更为平滑。
【IPC分类】G01C21/34
【公开号】CN105628045
【申请号】CN201511026140
【发明人】王谦, 金卫民, 胡镇, 胡阳, 张耀, 邓海, 李顶根
【申请人】武汉顶翔智控科技有限公司, 江西顶翔智控科技有限公司
【公开日】2016年6月1日
【申请日】2015年12月31日