一种无人机保证可视性的弹性目标跟踪的轨迹生成方法与流程

1.本发明涉及无人机导航技术领域，具体涉及一种无人机保证可视性的弹性目标跟踪的轨迹生成方法。


背景技术：

2.无人机，包括多旋翼无人机、固定翼无人机和混合翼无人机，在进行跟踪任务导航的时候，需要保障至少以下三点均得到满足a.复杂障碍物环境中的飞行安全；b.机体自身动力上的物理限制；c.与目标保持合适的观察距离，且避免被障碍物遮挡。
3.为保证无人机的飞行安全，周围环境的障碍物地图、禁飞区信息以及其它无人机既有的航线通常以离线或在线更新的方式生成出来，以提供后续无人机轨迹生成的碰撞信息，例如三维占据栅格地图、欧式距离地图或者点云地图。为保证航行轨迹满足无人机的物理飞行限制，机体本身具有的复杂动力学限制也应被轨迹所满足，例如跟踪轨迹所需要的最大滚转速率、电机推力或者最大倾角等实际物理量的限位。最后，持续跟踪的需求通常采用爬升并于目标处在不同高度的方式，然而这种方式在室内等复杂环境中较难实现，尤其是在有障碍的环境中很容易被遮挡。
4.对于无人机对运动目标的自主跟踪，近年比较有影响力的是mbz国际机器人挑战赛(mohamed bin zayed international robotics challenge)，2017年该比赛的题目之一是无人机定位、跟踪并降落在绘制有给定图案的低速移动平台上，如图1所示。这里不涉及到对目标的搜索与识别，有gps辅助且环境开放空旷，重点考察无人机定位、规划和控制性能。该届赛事冠军方案使用rtk(差分gps)融合视觉惯性里程计估计无人机位姿，使用激光雷达进行与运动目标的碰撞检测，在鱼眼镜头下利用四边形检测法定位标记物，并根据已知标记物尺度信息直接得到3d位置，最后使用非线性mpc进行跟踪控制。然而该比赛要求目标有特定的模板，目标运动较为缓慢，且环境较简单；同时该方法还要依赖卫星导航。
5.近年来无人机广泛应用于辅助摄影，无人机的跟踪技术也有了相应有了一些新的成果。nageli等人提出了一种实时滚动优化的规划器，同时对机器人的轨迹和云台的控制进行优化，如图2所示，相似地，penin等人构建了一个非线性模型预测控制(nmpc)并且用序列二次规划(sqp)去解决。然而，这些方法对动态目标为椭球的假设太强，同时也无法解决环境中结构化的静态障碍物。
6.jeon提出了一个基于图搜索的前端路径生成器和一个基于安全飞行走廊的轨迹优化器，如图3所示。前者通过采样搜索生成一系列合适的观察点，接下来的优化器跟随这些观察点并对轨迹做光滑。然而，该方法在采样阶段耗费极大的算力，同时，该方法还假设全局地图已知，同样不适合未知环境的跟踪。


技术实现要素：

7.针对现有技术的不足，本发明旨在提供一种无人机保证可视性的弹性目标跟踪的轨迹生成方法。
8.为了实现上述目的，本发明采用如下技术方案：
9.1、一种无人机保证可视性的弹性目标跟踪的轨迹生成方法，其特征在于，包括如下步骤：
10.一、前端的路径搜索与优化预处理
11.根据历史观测到的动态目标状态，对目标未来t
p
时间内的轨迹zk做出预测：
[0012][0013]
其中，为预测的被跟踪目标的未来位置的个数，为与目标未来每个位置相对应的时刻；
[0014]
同时考虑距离保持和遮挡的情况，为每一个目标预测位置zk定义了一个无遮挡观察区域φk；
[0015]
为了获得一个合适的拓扑以帮助后续的轨迹优化，需要找到一条路径依次经过φ1，φ2，...；φk被设置为第k个a*的终点区域，而代价函数和启发式函数分别如下所示：
[0016]fk
(n)＝gk(n) hk(n)
[0017][0018]
其中，a*是一种通用的图搜索方法，其中f表示最小代价估计，g表示代价函数，h表示启发式函数；和分别是拓展的节点n与目标预测位置点zk的距离的水平和竖直方向分量；dd是无人机和目标的期望距离；除了碰到障碍物的节点外，在合适距离但被遮挡的节点也被认为是不可拓展的节点；单步路径搜索的停止条件为n∈φk，且每一个终止点sk被设为下一步搜索的起点，直到最后一步完成；
[0019]
在得到了一条保证安全性和可视性的路径以后，沿着该路径可以生成一个安全飞行走廊：
[0020][0021]
其中，为飞行走廊中包含的凸多面体的个数，表示第i个凸多面体的解析表达，ai、bi是用线性不等式表示凸多面体每个面的一侧空间的交集；
[0022]
对于每一个目标预测位置zk和一个可视点sk作为种子，设计一个扇形可视区域：
[0023][0024]
其中ξk表示扇形的角平分线向量；
[0025]
二、后端的轨迹优化
[0026]
记期望轨迹的总时长t，使时间间隙t≥tp并将目标设置为最小化总时长和最小化能量的权衡，随后，对目标的柔性跟踪可以归结为以下优化问题：
[0027]
[0028][0029][0030][0031][0032][0033]
t≥t
p
[0034]
其中，是优化问题的目标函数，p
[s-1]
表示p的0，1，...，s-1阶导数；p
(1)
表示p的一阶导数；p(t)为无人机未来的轨迹，和别为始末状态，ρ是一个可人为调整的能量与时间优化的权重系数，vm和am分别为无人机的最大速度和最大加速度约束，为安全区域的抽象表示，d
l
，du为无人机与目标期望保持的距离范围，t
p
为期望的轨迹时长；
[0035]
那么梯度和可以求得：
[0036][0037][0038]ci
，ti分别表示向量c，t的第i个元素；β
(s)
(t)表示β(t)的s阶导数。
[0039]
进一步地，在后端的轨迹优化中，使用s＝3的minco轨迹类，并且设置轨迹段数m＝2m
p
以提供足够的自由度。
[0040]
进一步地，在后端的轨迹优化中，优化问题的其它几项约束的计算如下：
[0041]
1、相对时间积分惩罚函数法
[0042]
对于动力学约束和避障碍约束，可以设置阈值惩罚函数如下：
[0043][0044]
这样连续时间的约束，可以转化为积分惩罚，进而再用如下的采样惩罚函数即可构造出软约束的目标函数和梯度：
[0045][0046]
[0047][0048][0049]
★
＝{h，v，a}
[0050]
其中，是根据梯形公式算出的积分系数，κi为每段轨迹离散的积分段数，χ为权重向量；
[0051]
2、绝对时间构造目标函数法
[0052]
由于目标的运动预测是未来的一系列离散的确切时间点的位置，假设tk处于第j段轨迹，即：
[0053][0054]
关于c和t的梯度可以这样计算：
[0055][0056][0057]
即为p(tk)的导数，即p在tk处的导数；
[0058]
然而每一段轨迹的时长是待优化的，每一个时间点属于哪一段会随着时间的优化而变化；这会导致目标梯度的不连续，使得优化失败；幸运的是，尽管梯度对c和t是不连续的，但在经过minco的变换以后，梯度对q和t是连续的；
[0059]
对于距离保持约束，即同目标保持跟踪的同时保持安全距离的约束，由于无人机缺少独立的俯仰角的自由度，对距离的竖直δv和水平δh方向分量设置了不同形式的目标函数，对于前者，设置了一个竖直方向距离的容忍度而对于后者，设置了一个c2的惩罚函数：
[0060]
[0061]
其中d
l
，du分别为期望的跟踪距离的上下限，∈是一个很小的常量；那么这一项目标函数可以写成：
[0062][0063]
其中和分别为p(tk)和zk竖直和水平的距离；
[0064]
对于遮挡的约束，给定一个可调的角度容忍值θ
∈
，设计如下的代价函数：
[0065][0066]
3、时间约束消除
[0067]
时间松弛约束即
[0068][0069]
可以通过微分同胚变换，将其替换为新的变量τ＝(τ1，τ2，...，τm)，其变换如下：
[0070][0071][0072][0073]
这样，时间约束就这样被消除了。
[0074]
本发明的有益效果在于：本发明中同时考虑安全性和遮挡的路径搜索算法，无需构建欧式距离函数(esdf)即可解析地度量遮挡的代价函数。另外，本发明中针对跟踪问题设计了独特的距离保持的代价函数结构。实验证明，本发明方法相比现有技术，在质量和效率上都有较大的提升。
附图说明
[0075]
图1为现有技术mbzirc给定目标模板下的运动物体追踪示意图；
[0076]
图2为现有技术基于滚动优化的规划器实施示意图；
[0077]
图3为现有技术于前端生成观察点和后端航点跟随和光滑的方法实施示意图；
[0078]
图4为现有技术由给定给路径生成安全飞行走廊的方法的实施示意图；
[0079]
图5为本发明实施例1中为每一个目标预测位置zk定义了一个无遮挡观察区域φk的示例图；
[0080]
图6为本发明实施例1中扇形可视区域的示意图；
[0081]
图7为本发明实施例1中c2的惩罚函数的示意图；
[0082]
图8为本发明实施例2中复杂仿真环境中进行验证的实施示意图；
[0083]
图9为本发明实施例2中三种不同方案下，被跟踪目标在无人机fov中的分布对比示意图。
具体实施方式
[0084]
以下将结合附图对本发明作进一步的描述，需要说明的是，本实施例以本技术方案为前提，给出了详细的实施方式和具体的操作过程，但本发明的保护范围并不限于本实施例。
[0085]
实施例1
[0086]
本实施例方法的实施基于如下两种技术：
[0087]
1、安全飞行走廊
[0088]
无人机的轨迹规划需要前端提供一个抽象的地图表征，而安全飞行走廊正是其中一种很有代表性的表征方式。飞行走廊是由一系列首尾相连的凸多面体组成的安全区域，用于为后端轨迹优化提供安全性约束。
[0089]
本实施例采用的是由给定给路径生成安全飞行走廊的方法
[8]
，如图4所示，给定一条由几段线段组成的安全路径(p0→
p1→
p2→
p3...)，该算法利用路径的指向性，生成不包含障碍物的椭球(图4(a))，再通过椭球的不断膨胀，生成最终的安全飞行走廊(图4(b))。
[0090]
2、minco轨迹类
[0091]
机器人的运动轨迹有多种表征方式，本实施例采用的minco轨迹类
[9]
，是一个最小化控制输出量的多项式轨迹类、s阶minco：
[0092][0093]
其中p(t)是一个m维用m段n＝2s-1阶多项式表达的轨迹，第i段轨迹表示为：
[0094][0095]
是第i段多项式的系数矩阵，β(t)＝(1，t，...，tn)
t
是自然基。时间向量t＝(t1，t2，...，tm)
t
，ti是第i段轨迹的持续时间。
[0096]
所有的minco轨迹类中轨迹都只用q和t来做参数化。其中q＝(q1，q2，...，q
m-1
)，qi是轨迹经过的航点。用q和t评估一整条轨迹可以通过如下的线性复杂度的变换：
[0097][0098]
这使得任意一个二阶连续的目标函数及其求得的梯度可以应用在用q和t表达的minco轨迹。更确切地说，目标函数可以这样计算：
[0099][0100]
那么，上述变换提供了一种线性复杂度的方法从和来计算和随后，一个外层的优化器可以高效地对目标函数进行优化。
[0101]
基于上述技术，本实施例提供一种无人机保证可视性的弹性目标跟踪的轨迹生成
方法，主要分为前端和后端两部分：
[0102]
一、前端的路径搜索与优化预处理
[0103]
根据历史观测到的动态目标状态，对目标未来t
p
时间内的轨迹zk做出预测：
[0104][0105]
其中，为预测的被跟踪目标的未来位置的个数，为与目标未来每个位置相对应的时刻。
[0106]
同时考虑距离保持和遮挡的情况，本实施例为每一个目标预测位置zk定义了一个无遮挡观察区域φk，如图5所示。
[0107]
为了获得一个合适的拓扑以帮助后续的轨迹优化，需要找到一条路径依次经过φ1，φ2，...。考虑效率问题，使用贪心的策略，将问题分解为多个更小的多目标路径搜索问题。φk被设置为第k个a*的终点区域，而代价函数和启发式函数分别如下所示：
[0108]fk
(n)＝gk(n) hk(n)
[0109][0110]
其中，a*是一种通用的图搜索方法，其中f表示最小代价估计，g表示代价函数，h表示启发式函数；和分别是拓展的节点n与目标预测位置点zk的距离的水平和竖直方向分量。dd是无人机和目标的期望距离。除了碰到障碍物的节点外，在合适距离但被遮挡的节点也被认为是不可拓展的节点。单步路径搜索的停止条件为n∈φk，且每一个终止点sk被设为下一步搜索的起点，直到最后一步完成。
[0111]
在得到了一条保证安全性和可视性的路径以后，沿着该路径可以生成一个安全飞行走廊：
[0112][0113]
其中，为飞行走廊中包含的凸多面体的个数，表示第i个凸多面体的解析表达，ai、bi是用线性不等式表示凸多面体每个面的一侧空间的交集。
[0114]
对于每一个目标预测位置zk和一个可视点sk作为种子，本实施例设计了一个扇形可视区域：
[0115][0116]
其中ξk表示扇形的角平分线向量，θk为扇形角大小的一半，如图6所示。
[0117]
二、后端的轨迹优化
[0118]
本实施例中期望轨迹的总时长t，应当与t
p
相等，然而，无人机在固定的时间内达到最终状态可能在某些情况下导致动力学约束不能满足，例如，当目标比追逐者移动得更快。因此，使时间间隙t≥tp并将目标设置为最小化总时长和最小化能量的权衡。随后，对目标的柔性跟踪可以归结为以下优化问题：
[0119][0120][0121][0122][0123][0124][0125]
t≥t
p
[0126]
其中，是优化问题的目标函数，p
[s-1]
表示p的0，1，...，s一1阶导数。p
(1)
表示p的一阶导数。p(t)为无人机未来的轨迹，和别为始末状态，p是一个可人为调整的能量与时间优化的权重系数，vm和am分别为无人机的最大速度和最大加速度约束，为安全区域的抽象表示，d
l
，du为无人机与目标期望保持的距离范围，t
p
为期望的轨迹时长。
[0127]
本实施例使用s＝3的minco轨迹类，并且设置轨迹段数m＝2m
p
以提供足够的自由度。那么梯度和可以求得：
[0128][0129][0130]ci
，ti分别表示向量c，t的第i个元素；β
(3)
(t)表示β(t)的3阶导数。
[0131]
上述优化问题的其它几项约束的计算如下：
[0132]
1、相对时间积分惩罚函数法
[0133]
对于动力学约束和避障碍约束，可以设置阈值惩罚函数如下：
[0134][0135]
这样连续时间的约束，可以转化为积分惩罚，进而再用如下的采样惩罚函数即可构造出软约束的目标函数和梯度：
[0136]
[0137][0138][0139][0140]
★
＝{h，v，a}
[0141]
其中，是根据梯形公式算出的积分系数，κi为每段轨迹离散的积分段数，χ为权重向量。
[0142]
2、绝对时间构造目标函数法
[0143]
由于目标的运动预测是未来的一系列离散的确切时间点的位置，假设tk处于第j段轨迹，即：
[0144][0145]
关于c和t的梯度可以这样计算：
[0146][0147][0148]
即为p(tk)的导数，即p在tk处的导数。
[0149]
然而每一段轨迹的时长是待优化的，每一个时间点属于哪一段会随着时间的优化而变化。这会导致目标梯度的不连续，使得优化失败。幸运的是，尽管梯度对c和t是不连续的，但在经过minco的变换以后，梯度对q和t是连续的。
[0150]
对于距离保持约束，即同目标保持跟踪的同时保持安全距离的约束，由于无人机缺少独立的俯仰角的自由度，对距离的竖直(δv)和水平(δh)方向分量设置了不同形式的目标函数。对于前者，本实施例设置了一个竖直方向距离的容忍度而对于后者，设计了一个c2的惩罚函数：
[0151][0152]
其形状如图7所示。其中d
l
，du分别为期望的跟踪距离的上下限，∈是一个很小的常量。
[0153]
那么这一项目标函数可以写成：
[0154][0155]
其中和分别为p(tk)和zk竖直和水平的距离。
[0156]
对于遮挡的约束，给定一个可调的角度容忍值θ
∈
，设计如下的代价函数：
[0157][0158]
3、时间约束消除
[0159]
时间松弛约束即
[0160][0161]
可以通过微分同胚变换，将其替换为新的变量τ＝(τ1，τ2，...，τm)，其变换如下：
[0162][0163][0164][0165]
这样，时间约束就这样被消除了。
[0166]
实施例2
[0167]
本实施例将实施例1所述方法在一个复杂仿真环境中进行验证，并与目前主流方案进行效率和质量的对比。
[0168]
图8(a)所示为在一个复杂仿真环境中被跟踪目标的路径；图8(b)所示为目标在无人机视野图像中的速度投影；图8(c)所示为三种方案(han，wang，实施例1方法)的失败率。
[0169]
三种不同方案下，被跟踪目标在无人机fov中的分布对比分别如图9(a)、(b)、(c)所示。
[0170]
三种不同方案下，各项计算任务消耗的时间如表1所示。
[0171]
表1
[0172][0173]
可见实施例1方法不论在效率还是质量上都超过当前主流技术。其中，t
path
，t
corridor
，t
esdf
，t
optimize
，t
total
分别表示路径搜索、建立飞行走廊、建立欧式距离函数、轨迹优化及总共的时间。
[0174]
对于本领域的技术人员来说，可以根据以上的技术方案和构思，给出各种相应的改变和变形，而所有的这些改变和变形，都应该包括在本发明权利要求的保护范围之内。