CN105628045A - 一种无人机跟拍路径规划与跟踪方法 - Google Patents

Abstract

本发明公开了一种多旋翼无人机跟拍路径规划与跟踪方法，设定无人机与跟拍目标之间的相对位置参数，获得跟拍目标当前制导周期的位置，获得跟拍目标当前制导周期的速度矢量；根据无人机与跟拍目标之间的相对位置参数、跟拍目标当前制导周期的位置与速度矢量，获得无人机当前制导周期的期望位置；根据上一个制导周期计算所得目标航路点与当前制导周期内的目标航路点对当前制导周期内的目标航路点进行跟踪；相机云台根据相机云台的期望俯仰角和相机云台的期望视线偏角进行实时跟拍。本发明能够锁定无人机的拍摄视角，并能够根据需要实时改变视角；航线跟踪环节采用了航向角的比例控制，所得的无人机轨迹更为平滑。

Description

一种无人机跟拍路径规划与跟踪方法

技术领域

本发明涉及飞行器导航、制导与控制领域。具体涉及一种无人机跟拍路径规划与跟踪方法。适用于多旋翼无人机跟拍目标时的路径规划与路径跟踪。

背景技术

多旋翼无人机以其结构简单、成本低廉、机动灵活等特点，使得在航拍摄影、地图测绘、侦察监视等行业的应用日趋广泛。在航拍应用中，无人机自动跟拍是一种拍摄效果良好，且对操作人员技术要求较低的一种拍摄模式，目前多旋翼无人机跟拍方式均为预先设定好无人机与跟拍目标的相对位置，跟拍目标携带定位设备并将实时位置发送至无人机，而无人机在飞行过程中保持与跟拍目标的相对位置不变，这类方案中，无人机的运行轨迹不够平滑，航拍效果因此受到影响。

发明内容

本发明的目的是针对目前存在的上述问题，提供一种无人机跟拍路径规划与跟踪方法，跟拍轨迹平滑、拍摄角度实时可调的特点，能够实现更好的航拍效果。

本发明的上述目的通过以下技术方案实现：

一种多旋翼无人机跟拍路径规划与跟踪方法，包括以下步骤：

步骤1，在无人机启动跟拍飞行时设定无人机与跟拍目标之间的相对位置参数，或者跟拍飞行过程中在当前制导周期内设定无人机与跟拍目标之间的相对位置参数，

相对位置参数包括当前制导周期的期望距离L、当前制导周期的期望视线俯仰角ζ、当前制导周期的期望视线偏角

步骤2，获得跟拍目标当前的位置作为跟拍目标当前制导周期的位置O，获得跟拍目标当前的速度矢量作为跟拍目标当前制导周期的速度矢量

步骤3，根据当前制导周期的期望距离L、当前制导周期的期望视线俯仰角ζ、当前制导周期的期望视线偏角跟拍目标当前制导周期的位置O和跟拍目标当前制导周期的速度矢量获得无人机当前制导周期的期望位置J；

步骤4，以无人机当前制导周期的期望位置J为当前制导周期内的目标航路点，并根据上一个制导周期计算所得目标航路点I与当前制导周期内的目标航路点对当前制导周期内的目标航路点进行跟踪，将上一个制导周期的目标航路点I与当前制导周期内的目标航路点连线得到连线IJ，并将无人机当前的位置P向该连线IJ或其延长线投影得到当前投影点Q，通过获得点K的坐标，从而获得无人机的期望速度矢量方向

其中，为当前投影点Q到无人机当前制导周期的期望位置J的矢量，点K将矢量分割为和δ为分割的比例系数，

在当前制导周期中以固定的控制周期Δt对当前投影点Q和点K的坐标进行更新，无人机在当前制导周期中的各个控制周期中以对应的无人机的期望速度矢量方向进行跟踪；

步骤5，根据无人机的位置P与跟拍目标当前制导周期的位置O设置无人机相机云台期望角度；

步骤6，在下一个制导周期来临时，返回步骤1。

如上所述的步骤3中无人机当前制导周期的期望位置J通过以下公式过得：

其中，J_x,J_y,J_z分别为无人机当前制导周期的期望位置J在导航坐标系x,y,z轴上的坐标，L为当前制导周期的期望距离、ζ为当前制导周期的期望视线俯仰角、为当前制导周期的期望视线偏角，ψ为无人机航向角。

如上所述的步骤5中设置无人机相机云台期望角度包括以下步骤：

步骤5.1、通过以下公式求取跟拍目标当前制导周期的位置O相对于无人机当前位置P的视线俯仰角γ；则跟拍目标当前制导周期的位置O相对于无人机携带的相机云台的期望俯仰角为-γ；

$\mathrm{\γ}=\arctan \left(\frac{O_z-P_z}{\sqrt{{(O_x-P_x)}^2+{(O_y-P_y)}^2}}\right)$

O_x,O_y,O_z分别为跟拍目标当前制导周期的位置O在导航坐标系x,y,z轴上的坐标，P_x,P_y,P_z分别为无人机当前位置P在导航坐标系x,y,z轴上的坐标；

步骤5.2、通过以下公式求取跟拍目标当前制导周期的位置O相对于无人机携带的相机云台的期望视线偏角ξ：

$\mathrm{\ξ}=\arctan \left(\frac{O_y-P_y}{O_x-P_x}\right)$

步骤5.3、相机云台根据-γ和ξ进行实时跟拍。

本发明与现有技术相比具有以下有益效果：

1、本发明以跟拍对象的速度矢量为参考，通过视线俯仰角和视线偏角计算无人机期望位置，因此能够锁定无人机的拍摄视角，并能够根据需要实时改变视角；而现有方案通过无人机与跟拍目标在导航坐标系中的相对位置计算无人机期望位置，无人机拍摄视角随着跟拍目标速度方向的变化而变化，需要手动进行修正。

2、本发明在航线跟踪环节采用了航向角的比例控制，相对于现有跟拍方案中的直接飞向目标航路点，本发明所得的无人机轨迹更为平滑。

附图说明

图1无人机期望位置与跟拍目标当前位置在导航坐标系中的几何关系；

图2跟拍目标当前位置相对于无人机当前位置的视线俯仰角和视线偏角；

图3无人机位置投影在两个航路点连线IJ上时的航向计算示意图；

图4无人机位置投影在两个航路点连线IJ延长线时的航向计算示意图。

具体实施方式

以下结合具体实施例对本发明的技术方案做进一步描述。

一种多旋翼无人机跟拍路径规划与跟踪方法，包括以下几个步骤：

步骤1，在无人机启动跟拍飞行时设定无人机与跟拍目标之间的相对位置参数，或者跟拍飞行过程中在当前制导周期内设定无人机与跟拍目标之间的相对位置参数。如图1所示，导航坐标系采用原点O在跟拍目标质心，x,y,z轴分别指向北、东、地的当地铅锤坐标系，其中，O为跟拍目标当前制导周期的位置，为跟拍目标当前制导周期的速度矢量，因此无人机的航向角即为跟拍目标当前制导周期的速度矢量在当地水平面上的投影与正北方向的夹角ψ，J,J'则分别为无人机当前制导周期的期望位置以及无人机当前制导周期的期望位置在导航坐标系的当地水平面上的投影点。

需要设置或调整的无人机当前制导周期的期望位置与跟拍目标之间的相对位置参数包括当前制导周期的期望距离L、当前制导周期的期望视线俯仰角ζ、当前制导周期的期望视线偏角

(1)无人机当前制导周期的期望位置与跟拍目标当前制导周期的位置的当前制导周期的期望距离L，由于无人机携带相机的视场大小限制以及拍摄的不同需求，无人机需要与跟拍目标保持一定的直线距离，即图1中OJ线段的长度，该距离与跟拍目标的速度、摄像机视场大小以及具体应用要求有关，需要预先设置和根据需要动态调整。

(2)无人机当前制导周期的期望位置相对于跟拍目标当前制导周期的速度矢量的当前制导周期的期望视线俯仰角ζ，该角度为无人机当前制导周期的期望位置与跟拍目标当前制导周期的位置之间的连线与过导航坐标系x轴和跟拍目标当前制导周期的速度矢量的平面之间的夹角，即图1中的角度ζ，该角度决定了无人机相机以多大的角度俯拍跟拍目标，其大小与具体应用要求有关。

(3)无人机当前制导周期的期望位置相对于跟拍目标当前制导周期的速度矢量的当前制导周期的期望视线偏角该角度为无人机当前制导周期的期望位置与跟拍目标当前制导周期的位置O之间的连线与通过无人机速度矢量的导航坐标系的当地铅锤面之间的夹角，即图1中的角度该角度决定了无人机相机在什么方向拍摄跟拍目标，其大小与具体应用要求有关。

步骤2，获得跟拍目标当前的位置作为跟拍目标当前制导周期的位置O，获得跟拍目标当前的速度矢量作为跟拍目标当前制导周期的速度矢量为了获得良好的跟拍效果，需要跟拍目标在每个制导周期测量并发送一次跟拍目标当前的位置和跟拍目标当前的速度矢量到无人机，跟拍目标当前的位置和跟拍目标当前的速度矢量可以通过跟拍目标上的GPS设备获得。

步骤3，由于通过GPS设备获得的跟拍目标当前的位置和跟拍目标当前的速度矢量的频率较低，即只是在每个制导周期获得一次跟拍目标当前的位置和跟拍目标当前的速度矢量作为跟拍目标当前制导周期的位置O和跟拍目标当前制导周期的速度矢量为了使跟拍轨迹平滑，需要将计算所得的无人机当前制导周期的期望位置J作为当前制导周期的目标航路点，然后再应用航路点跟踪算法进行跟踪。

由于当前制导周期的期望距离L、当前制导周期的期望视线俯仰角ζ、当前制导周期的期望视线偏角构成了球面坐标的三个参数，因此跟拍目标当前制导周期的位置O和跟拍目标当前制导周期的速度矢量确定后，无人机当前制导周期的期望位置J就能够唯一确定，如图2所示，以固定时间间隔ΔT为制导周期，根据步骤1设定的相对位置参数，计算无人机当前制导周期的期望位置J:

设定为导航坐标系中跟拍目标当前制导周期的位置O到无人机当前制导周期的期望位置J的向量，J_x,J_y,J_z分别为无人机当前制导周期的期望位置J在导航坐标系x,y,z轴上的坐标，L为当前制导周期的期望距离、ζ为当前制导周期的期望视线俯仰角、为当前制导周期的期望视线偏角，ψ为无人机航向角。

步骤4，以无人机当前制导周期的期望位置J为当前制导周期内的目标航路点，并根据上一个制导周期计算所得目标航路点I与当前制导周期内的目标航路点对当前制导周期内的目标航路点进行跟踪，将上一个制导周期的目标航路点I与当前制导周期内的目标航路点连线得到连线IJ，并将无人机当前时刻的位置P向该连线IJ或其延长线投影得到当前投影点Q，如图3和图4所示，当前时刻无人机的期望速度矢量方向为且满足：

$\stackrel{\→}{QK}=\mathrm{\δ}\stackrel{\→}{QJ}$

即

$\left[\begin{array}{c}{K}_x-Q_x\\ K_y-Q_y\\ K_z-Q_z\end{array}\right]=\mathrm{\δ}\left[\begin{array}{c}{J}_x-K_x\\ J_y-K_y\\ J_z-K_z\end{array}\right]$

其中，为当前投影点Q到当前制导周期内的目标航路点(即无人机当前制导周期的期望位置J)的矢量，点K将矢量分割为和δ为分割的比例系数，K_x,K_y,K_z分别为点K在导航坐标系x,y,z轴上的坐标，Q_x,Q_y,Q_z分别为当前投影点Q在导航坐标系x,y,z轴上的坐标。

从中可以解出K点在导航坐标系中的分量：

$\left[\begin{array}{c}{K}_x\\ K_y\\ K_z\end{array}\right]=\frac{1}{2}\{\mathrm{\δ}\left[\begin{array}{c}{J}_x\\ J_y\\ J_z\end{array}\right]+\left[\begin{array}{c}{Q}_x\\ Q_y\\ Q_z\end{array}\right]\}$

在当前制导周期内，以固定的时间间隔Δt为控制周期，在每个控制周期内对当前投影点Q进行更新，通过当前投影点Q和无人机当前制导周期的期望位置J对点K的位置进行更新，从而无人机只需跟踪期望速度矢量即可完成对航线的跟踪，进而实现对跟拍目标位置的实时跟踪。

而由于当前投影点Q为无人机当前的位置P在线段IJ或其延长线上的投影，假设∠PJI为α，则

$cos\mathrm{\α}=\frac{{\left|\stackrel{\→}{PJ}\right|}^2+{\left|\stackrel{\→}{IJ}\right|}^2-{\left|\stackrel{\→}{PI}\right|}^2}{2\×\stackrel{\→}{PJ}\×\stackrel{\→}{IJ}}$

因此

$\left|\stackrel{\→}{JQ}\right|=\frac{{\left|\stackrel{\→}{PJ}\right|}^2+{\left|\stackrel{\→}{IJ}\right|}^2-{\left|\stackrel{\→}{PI}\right|}^2}{2\×\stackrel{\→}{PJ}\×\stackrel{\→}{IJ}}\×L$

令 $\mathrm{\τ}=\frac{\stackrel{\→}{\left|JQ\right|}}{\stackrel{\→}{\left|IJ\right|}}$

则点Q的坐标则为：

$\left[\begin{array}{c}{Q}_x\\ Q_y\\ Q_z\end{array}\right]-\frac{1}{2}\{\mathrm{\τ}\left[\begin{array}{c}{J}_x\\ J_y\\ J_z\end{array}\right]+\left[\begin{array}{c}{I}_x\\ I_y\\ I_z\end{array}\right]\}$

以固定的控制周期Δt对当前投影点Q和点K的坐标进行更新，则可以实现对上一个制导周期计算所得目标航路点I与当前制导周期的目标航路点(即无人机当前制导周期的期望位置J)之间航线的平滑跟踪，图3为点Q在线段IJ上时的航线跟踪示意图，其中K₁、K₂为后两个控制周期Δt对应的点K，Q₁、Q₂为后两个控制周期Δt对应的投影点，图4为当前投影点Q在线段IJ延长线上时的航线跟踪示意图，即当前时刻，无人机当前的位置P，目标航向即无人机的期望速度矢量为在下一个控制周期来临时，无人机已飞至点P₁，此时更新Q₁,K₁点的坐标，目标航向即无人机的期望速度矢量则变为以此类推。

步骤5，根据无人机的位置P与跟拍目标当前制导周期的位置O设置无人机相机云台期望角度。如图2所示，跟拍目标当前制导周期的位置O相对于无人机当前时刻位置P的视线俯仰角为：

$\mathrm{\γ}=\arctan \left(\frac{O_z-P_z}{\sqrt{{(O_x-P_x)}^2+{(O_y-P_y)}^2}}\right)$

因此在忽略无人机当前位置与无人机所载相机云台当前位置误差的情况下，跟拍目标当前制导周期的位置O相对于无人机携带的相机云台的期望俯仰角为-γ。

而跟拍目标当前制导周期的位置O相对于无人机携带的相机云台的期望视线偏角ξ则为：

$\mathrm{\ξ}=\arctan \left(\frac{O_y-P_y}{O_x-P_x}\right)$

其中，O_x,O_y,O_z分别为导航坐标系中跟拍目标当前制导周期的位置的x,y,z分量，P_x,P_y,P_z分别为导航坐标系中无人机当前位置的x,y,z分量，相机云台以这两个角度为指令偏转即可实现对目标的实时跟拍。

步骤6，在下一个制导周期来临时，返回步骤1，即对相对位置参数、无人机当前制导周期的目标航路点J、上一个制导周期的目标航路点I进行更新，然后进行下一个制导周期的跟拍动作。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或优化或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (3)

1.一种多旋翼无人机跟拍路径规划与跟踪方法，其特征在于，包括以下步骤：

步骤1，在无人机启动跟拍飞行时设定无人机与跟拍目标之间的相对位置参数，或者跟拍飞行过程中在当前制导周期内设定无人机与跟拍目标之间的相对位置参数，

相对位置参数包括当前制导周期的期望距离L、当前制导周期的期望视线俯仰角ζ、当前制导周期的期望视线偏角

步骤2，获得跟拍目标当前的位置作为跟拍目标当前制导周期的位置O，获得跟拍目标当前的速度矢量作为跟拍目标当前制导周期的速度矢量

步骤3，根据当前制导周期的期望距离L、当前制导周期的期望视线俯仰角ζ、当前制导周期的期望视线偏角跟拍目标当前制导周期的位置O和跟拍目标当前制导周期的速度矢量获得无人机当前制导周期的期望位置J；

步骤4，以无人机当前制导周期的期望位置J为当前制导周期内的目标航路点，并根据上一个制导周期计算所得目标航路点I与当前制导周期内的目标航路点对当前制导周期内的目标航路点进行跟踪，将上一个制导周期的目标航路点I与当前制导周期内的目标航路点连线得到连线IJ，并将无人机当前的位置P向该连线IJ或其延长线投影得到当前投影点Q，通过获得点K的坐标，从而获得无人机的期望速度矢量方向

其中，为当前投影点Q到无人机当前制导周期的期望位置J的矢量，点K将矢量分割为和δ为分割的比例系数，

在当前制导周期中以固定的控制周期Δt对当前投影点Q和点K的坐标进行更新，无人机在当前制导周期中的各个控制周期中以对应的无人机的期望速度矢量方向进行跟踪；

步骤5，根据无人机的位置P与跟拍目标当前制导周期的位置O设置无人机相机云台期望角度；

步骤6，在下一个制导周期来临时，返回步骤1。

2.根据权利要求1所述的一种多旋翼无人机跟拍路径规划与跟踪方法，其特征在于，所述的步骤3中无人机当前制导周期的期望位置J通过以下公式过得：

其中，J_x,J_y,J_z分别为无人机当前制导周期的期望位置J在导航坐标系x,y,z轴上的坐标，L为当前制导周期的期望距离、ζ为当前制导周期的期望视线俯仰角、为当前制导周期的期望视线偏角，ψ为无人机航向角。

3.根据权利要求1所述的一种多旋翼无人机跟拍路径规划与跟踪方法，其特征在于，所述的步骤5中设置无人机相机云台期望角度包括以下步骤：

步骤5.1、通过以下公式求取跟拍目标当前制导周期的位置O相对于无人机当前位置P的视线俯仰角γ；则跟拍目标当前制导周期的位置O相对于无人机携带的相机云台的期望俯仰角为-γ；

$\mathrm{\γ}=arctan\left(\frac{O_z-P_z}{\sqrt{{(O_x-P_x)}^2+{(O_y-P_y)}^2}}\right)$

O_x,O_y,O_z分别为跟拍目标当前制导周期的位置O在导航坐标系x,y,z轴上的坐标，P_x,P_y,P_z分别为无人机当前位置P在导航坐标系x,y,z轴上的坐标；

步骤5.2、通过以下公式求取跟拍目标当前制导周期的位置O相对于无人机携带的相机云台的期望视线偏角ξ：

$\mathrm{\ξ}=arctan\left(\frac{O_y-P_y}{O_x-P_x}\right)$

步骤5.3、相机云台根据-γ和ξ进行实时跟拍。