本发明涉及无人机控制技术领域，尤其涉及一种无人机目标跟踪中的分层避障控制方法。
背景技术：
无人机(uav)已广泛用于军事以及民用领域，无人机在目标跟踪中，目标的可观察性是通过操纵安装在无人机上的传感器来实现的，正是由于无人机的感知能力受到无人机动态性能和机载传感器性能的限制，无人机的动作决策成为目前主要的研究热点之一。同时在复杂的环境下，无人机对目标进行跟踪时可能出现突发的威胁或障碍，即飞行空间出现禁飞区(统称为障碍区)，即在目标跟踪过程中还存在障碍规避问题。
现有技术中无人机目标跟踪过程中通常都是采用惩罚函数法进行碰撞检测及碰撞规避，会存在以下问题：
1、使用惩罚函数法进行碰撞检测及规避，所需的计算时间长，使得避障控制效率较低，在检测到障碍时无法及时响应；
2、使用惩罚函数法进行碰撞检测及规避时，仍需考虑目标跟踪策略，为同时保证目标与跟踪精度，无人机的避障效果通常较差，使得无人机位于危险区域的路径较长。
技术实现要素：
本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种实现方法简单、所需计算时间少、跟踪及避障的效率及精度高且避障效果好的无人机目标跟踪中的分层避障控制方法。
为解决上述技术问题，本发明提出的技术方案为：
一种无人机目标跟踪中的分层避障控制方法，步骤包括：
s1.基于无人机、目标的状态构建无人机目标跟踪模型；
s2.基于所述无人机目标跟踪模型控制无人机进行目标跟踪，使用基于目标信息累积量的跟踪决策函数控制无人机动作决策，以寻找使得所述目标信息累积量最大化的最佳动作序列；
s3.当预测到障碍且判定需要进行障碍规避时，启动对无人机进行避碰控制，使用预先基于无人机位置与障碍位置之间关系构建的避碰决策函数控制无人机动作决策，以使得无人机远离预测到的障碍。
作为本发明的进一步改进：所述步骤s1中具体基于pomdp(部分客观马尔可夫，partiallyobservablemarkovdecisionprocess)构建无人机目标跟踪模型，构建的模型中包括基于目标信息累积量定义的回报函数。
作为本发明的进一步改进：基于pomdp构建无人机目标跟踪模型具体为：基于系统状态s、动作u、观测率ω、观测值o、回报函数r、状态转移律t以及置信状态b0构建无人机目标跟踪模型，定义一个无限区间的pomdp为一个数组m＝<s,u,t,ω,o,r,b0>，其中所述系统状态s包括无人机的状态
所述状态转移律t为：
所述观测率ω包括传感器的观测律：
作为本发明的进一步改进：所述步骤s2中具体使用基于fisher信息矩阵计算无人机轨迹的信息累积量，构建基于fisher信息矩阵的跟踪决策函数。
作为本发明的进一步改进，构建所述基于fisher信息矩阵的跟踪决策函数具体步骤为：
定义k时刻的fisher信息矩阵为：
其中，p(z1:k|xk)表示测量的似然概率，z1:k表示在笛卡尔坐标中从t1到当前时刻对目标位置的观测序列，
以递归的形式计算fisher信息矩阵：
其中，
使用在h个采样点处fisher信息矩阵的行列式之和近似无人机轨迹的累积信息，即：
其中，ftracking为信息累积量，gi为i时刻fisher信息矩阵；
构建得到基于fisher信息矩阵的跟踪决策函数，以使得无人机的动作决策过程找到所述信息累积量ftracking最大化的最佳动作序列。
作为本发明的进一步改进，所述步骤s2中控制无人机进行目标跟踪步骤包括：
s21.初始化置信状态；
s22.获得观测值后更新目标的置信状态；
s23.估计未来指定时间段内的信息累积量；
s24.求解所述跟踪决策函数的最优策略，以寻找到使得所述信息累积量最大化的最佳动作序列。
作为本发明的进一步改进：所述步骤s22中具体使用卡尔曼滤波方法更新目标的置信状态。
作为本发明的进一步改进：所述步骤s3中具体通过由无人机到危险圈的切线构建锥体，基于构建的锥体使用碰撞锥法预测障碍，当无人机的速度矢量位于碰撞锥内时，判定无人机将进入危险区域或禁飞区域，启动对无人机进行避碰控制。
作为本发明的进一步改进：所述避碰决策函数具体为基于无人机位置与所述碰撞锥位置之间的距离构建得到，所述步骤s3无人机决策过程中基于所述避碰决策函数控制使无人机尽快远离所述碰撞锥。
作为本发明的进一步改进，所述避碰决策函数具体为：
其中，dis(.)表示两点之间的距离，(xo,yo)为所述碰撞锥的切点，
所述步骤s3无人机决策过程中使用使fh＝1最小策略，使无人机尽可能的接近所述碰撞锥的切点，以尽快远离碰撞锥区域。
与现有技术相比，本发明的优点在于：
1)本发明无人机目标跟踪中的分层避障控制方法，通过将无人机动作决策分为目标跟踪和避障两部分，实现无人机在目标跟踪过程中的分层障碍规避，进行目标跟踪时使用基于信息累积量的跟踪决策函数，无人机由目标信息累积量确定最佳动作序列，可以降低计算成本和位置不确定性，有效提高目标跟踪的精度及效率，同时当预测到碰撞时启动避碰控制，调整使用基于无人机位置与障碍位置之间关系的避碰决策函数以规避障碍，由于避碰控制时只考虑避障，不考虑跟踪策略，能够减少计算时间并提高避障精度，在无人机检测到障碍时可以快速响应而避免进一步接近障碍，从而在整个无人机动作决策过程中可以同时确保跟踪和避障的精度和效率。
2)本发明无人机目标跟踪中的分层避障控制方法，进一步基于pomdp构建无人机目标跟踪模型，可以在pomdp决策框架下充分利用信息累积量进行无人机快速避障控制。
3)本发明无人机目标跟踪中的分层避障控制方法，进一步使用fisher信息距离来解决pomdp的优化问题，基于fisher信息量来构建跟踪决策函数，可以从信息几何的角度对测量信息进行建模，从而能够定量的描述测量的本质特征，进一步提高跟踪精度。
4)本发明无人机目标跟踪中的分层避障控制方法，使用碰撞锥进行障碍检测判断，同时在检测到障碍后的下一个运动周期中只考虑避碰策略，基于无人机位置与碰撞锥位置之间的距离构建的避碰决策函数控制无人机进行避障，从而通过牺牲一定的跟踪精度来尽可能的快速响应而避免障碍，使得无人机可以快速响应后尽快的远离碰撞锥，相比于传统的惩罚函数法相比，所需的计算时间更小，并且无人机避障效果更好。
附图说明
图1是本实施例无人机目标跟踪中的分层避障控制方法的实现流程示意图。
图2是本实施例构建的碰撞锥的结构原理示意图。
图3是采用本发明方法与传统惩罚函数法的无人机运动轨迹对比结果示意图。
图4是采用本发明方法与传统惩罚函数法的跟踪误差对比结果示意图。
图5是采用本发明方法与传统惩罚函数法的计算时间对比结果示意图。
具体实施方式
以下结合说明书附图和具体优选的实施例对本发明作进一步描述，但并不因此而限制本发明的保护范围。
如图1所示，本实施例无人机目标跟踪中的分层避障控制方法，步骤包括：
s1.基于无人机、目标的状态构建无人机目标跟踪模型；
s2.基于无人机目标跟踪模型控制无人机进行目标跟踪，使用基于目标信息累积量的跟踪决策函数控制无人机动作决策，以寻找使得目标信息累积量最大化的最佳动作序列；
s3.当预测到障碍且判定需要进行障碍规避时，启动对无人机进行避碰控制，使用预先基于无人机位置与障碍位置之间关系构建的避碰决策函数控制无人机动作决策，以使得无人机快速远离预测到的障碍。
本实施例通过上述控制方法，将无人机动作决策分为目标跟踪和避障两部分，实现无人机在目标跟踪过程中的分层障碍规避，进行目标跟踪时使用基于信息累积量的跟踪决策函数，无人机由目标信息累积量确定最佳动作序列，可以降低计算成本和位置不确定性，有效提高目标跟踪的精度及效率，同时当预测到碰撞时启动避碰控制，调整使用基于无人机位置与障碍位置之间关系的避碰决策函数以规避障碍，由于避碰控制时只考虑避障，不考虑跟踪策略，能够减少计算时间并提高避障性能，在无人机检测到障碍时可以快速响应而避免进一步接近障碍，从而在整个无人机动作决策过程中可以同时确保跟踪和避障的精度和效率。
本实施例中，步骤s1中具体基于pomdp(部分客观马尔可夫)构建无人机目标跟踪模型，构建的模型中包括基于目标信息累积量定义的回报函数，以在pomdp决策框架下充分利用信息累积量进行无人机避障控制，可以进一步提高避障效率，实现无人机快速、高效避障。
本实施例中，基于pomdp构建无人机目标跟踪模型具体为：基于系统状态s、动作u、观测率ω、观测值o、回报函数r、状态转移律t以及置信状态b0构建无人机目标跟踪模型，定义一个无限区间的pomdp为一个数组m＝<s,u,t,ω,o,r,b0＞，其中系统状态s包括无人机的状态
状态转移律t为：
观测率ω包括传感器的观测律：
其中，rk、
本实施例中，步骤s2中具体使用基于fisher信息矩阵计算无人机轨迹的信息累积量，构建基于fisher信息矩阵(fim)的跟踪决策函数。本实施例将传感器在特定时间内获得的目标信息量用信息距离来表征，又在离散测量采样的想定中，信息距离由fim的行列式之和给出，本实施例使用统计中fisher信息距离来解决pomdp的优化问题，基于fisher信息量来构建跟踪决策函数，可以从信息几何的角度对测量信息进行建模，从而能够定量的描述测量的本质特征，进一步降低计算成本和位置不确定性，提高跟踪精度。
本实施例中根据信息几何的相关知识，通过雷达检测模型推导出fisher信息矩阵(fim)，构建基于fisher信息矩阵的跟踪决策函数具体步骤为：
定义k时刻的fisher信息矩阵为：
其中，p(z1:k|xk)表示测量的似然概率，z1:k表示在笛卡尔坐标中从t1到当前时刻对目标位置的观测序列，
以递归的形式计算fisher信息矩阵：
其中，
使用在h个采样点处fisher信息矩阵的行列式之和近似无人机轨迹的累积信息，即：
其中，ftracking为信息累积量，gi为i时刻fisher信息矩阵；
构建得到基于fisher信息矩阵的跟踪决策函数，以使得无人机的动作决策过程找到信息累积量ftracking最大化的最佳动作序列，即无人机在动作决策过程中，基于pomdp选择合适的动作序列，使得累计回报ftracking最大，即为找到使信息累积量最大化的最佳动作序列，在这个过程中，具体可以利用近似滚动时域方法获得整个过程中的最优动作序列。
本实施例中，步骤s2中控制无人机进行目标跟踪步骤包括：
s21.初始化置信状态；
s22.获得观测值后更新目标的置信状态；
s23.使用估计未来指定时间段内的信息累积量；
s24.求解跟踪决策函数的最优策略，以寻找到使得信息累积量最大化的最佳动作序列。
本实施例中，步骤s22中具体使用卡尔曼滤波方法更新目标的置信状态。
本实施例具体基于上述构建的pomdp模型，先初始化置信状态，获得观测值后再使用卡尔曼滤波方法更新目标的置信状态，利用上述式(5)的迭代计算估计未来指定时间段内的信息累积量ftracking，最后求解使ftracking最大化的最优策略，得到控制无人机目标跟踪的最佳动作序列。
本实施例中，步骤s3中具体通过由无人机到危险圈的切线构建锥体，基于构建的锥体使用碰撞锥法预测障碍，当无人机的速度矢量位于碰撞锥内时，判定无人机将进入危险区域或禁飞区域，启动对无人机进行避碰控制。
本实施例中，避碰决策函数具体为基于无人机位置与碰撞锥位置之间的距离构建得到，步骤s3无人机决策过程中基于避碰决策函数控制使无人机尽快远离碰撞锥。在障碍环境中，无人机需要决定在何时对检测到的障碍进行规避，本实施例使用碰撞锥法，可以方便有效的预测出障碍。本实施例构建的碰撞锥结构如图3所示，其中d为无人机和障碍之间的距离，v为无人机的速度，rdanger为危险区域的半径，obstacle为障碍中心，rno-fly为禁飞区域的半径，由无人机到危险圈的切线构建锥体，如果无人机的速度矢量位于碰撞锥内，则说明无人机将进入危险区域甚至禁飞区域。
假设无人机检测到障碍时，即t0的状态为
其中，
考虑到决策周期足够小，本实施例在检测到障碍后的下一个运动周期中只考虑避碰策略，通过牺牲一定的跟踪精度来尽可能的快速响应而避免障碍，基于无人机位置与碰撞锥位置之间的距离构建的避碰决策函数控制无人机进行避障，使得无人机可以快速响应后尽快的远离碰撞锥，一旦无人机飞离危险区域，返回执行s1以恢复目标跟踪，即使用基于目标信息累积量的跟踪决策函数控制无人机进行目标跟踪。
本实施例中，避碰决策函数具体为：
其中，dis(.)表示两点之间的距离，(xo,yo)为碰撞锥的切点。
步骤s3无人机决策过程中即使用fh＝1使无人机尽可能的接近碰撞锥的切点，即当预测到可能碰撞时，使用如式(8)所示的目标函数最小的策略获得下一周期的动作策略，以使无人机尽可能的接近碰撞锥的切点(xo,yo)而尽快远离碰撞锥区域。
当然在其他实施例中，根据实际的避障精度和求解时间需求，避碰决策函数也可以其他函数形式，如采用与预测位置相关的指数型函数，以使得无人机位置能够尽可能的远离碰撞锥。
在具体应用实施例中，利用上述控制方法实现无人机目标跟踪中的分层避障的详细流程包括：
(a)目标跟踪决策过程
步骤1.构建pomdp模型，并初始化置信状态；
步骤2.获得观测值后，利用卡尔曼滤波方法更新目标的置信状态；
步骤3.根据式(5)的迭代计算方法，估计未来有限时间内的信息累积量ftracking；
步骤4.利用matlab最优化求解库函数fmincon函数求解最优策略，以寻找ftracking最大化的最佳动作序列。
(b)障碍规避过程
步骤1.检测到障碍后，使用上述碰撞锥方法判断是否需要障碍规避；
步骤2.如果需要障碍规避，使用式(8)的避障决策函数获得下一周期的动作策略；
步骤3.继续进行障碍检测，如果不需要避障，则利用(a)中的目标跟踪决策方法获得无人机的动作策略。
为验证本发明的有效性，分别使用本发明上述控制方法以及传统的惩罚函数法对无人机目标跟踪进行分层避障控制实验，实验中目标的移动速度为15m/s，障碍位于500m处，障碍的半径为50m，实验中详细计算了危险区域的平均跟踪误差，平均计算时间和航点数，使用本发明上述控制方法与传统的惩罚函数法得到的实验结果对比如图3、4、5所示，其中图3为无人机的运动轨迹对比结果，图4为跟踪误差对比结果，图5为计算时间对比结果。从实验结果可以看出，采用本发明控制方法，能够实现无人机在目标跟踪过程中的障碍规避，与传统的惩罚函数法相比，算法的平均计算时间更小，并且无人机避障效果更好，即无人机位于危险区域的路径更短。
上述只是本发明的较佳实施例，并非对本发明作任何形式上的限制。虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明。因此，凡是未脱离本发明技术方案的内容，依据本发明技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均应落在本发明技术方案保护的范围内。