CN115793709A - 一种基于pomdp模型的apf无人机路径规划方法 - Google Patents

Abstract

本发明公开了一种基于POMDP模型的APF无人机路径规划方法，使用无人机所携带的传感器获取障碍物的信息，建立障碍物部分可观测马尔可夫预测模型，利用该模型预测障碍物下一步可能出现的位置；为了保证无人机在路径规划中的安全性，建立无人机和障碍物的影响模型，避免路径规划的过程中无人机与障碍物发生碰撞的情况；对传统的人工势场法进行改进，确保无人机能够安全到达目标点的同时避免其陷入局部极小值问题。本方法在UAV实时路径规划中具有良好的可行性和有效性，能够使UAV有效地避开障碍物，找到一条最优路径。

Description

一种基于POMDP模型的APF无人机路径规划方法

技术领域

本发明涉及无人机路径规划技术领域，具体涉及一种基于POMDP模型的APF无人机路径规划方法。

背景技术

无人机是一种由动力驱动，无人驾驶、可重复使用的航空器，英文常用UnmannedAerial Vehicle表示，缩写为UAV。无人机是人类拓展认知，探索天空的重要工具，它可以在人类无法到达的地方自主运行，完成人类无法完成的作业，具有非常深远的研究价值。与有人驾驶飞机相比，无人机具有尺寸小、重量轻、制造成本低、操作简单等优点。随着科学技术的突飞猛进，无人机在许多应用领域迅速增长，包括实时监控、遥感、搜索和救援、精准农业等。针对无人机的研究在最近几年得到了广泛的关注，无人机路径规划技术也成为了航空领域最具挑战性和研究价值的技术之一。

无人机路径规划技术是一门综合数学、统计学、运动学及人工智能等多个学科的最新研究成果，其发展对军事、交通、农业和电影等行业具有重大的影响。无人机路径规划是指在无人机飞行环境地图中设置起点和终点，并使用相关的路径规划方法规划一条无碰撞、最优和安全的无人机飞行路径。路径规划是提高无人机自主飞行能力、保障飞行安全的关键技术，合适的路径规划方法是无人机顺利完成飞行任务的前提和基础。常用无人机路径规划方式有很多，比如遗传算法、粒子群算法、A-star算法、快速随机搜索树算法、Dijkstra算法、人工势场法(Artificial Potential Field、APF)等。

人工势场法APF是一种原理比较简单的路径规划方法，由于其具有结构简单、计算量小以及实时性较高等优点被广泛的应用在局部路径规划方法中。此外，人工势场法还具有很强的移植性，通过改变人工势场的来源，还可以解决多智能体系统的避障问题和地形避障问题。但是，现有人工势场法也存在一些问题，Koren和Borrenstein指出了APF方法固有的四个重要问题。其中，局部最小值是亟需解决的问题。

由于空中的无人机以及运动障碍物具有很高的速度，仅仅依靠传感器的检测，无法满足无人机路径规划的实时性。因此，需要根据检测到的几个时刻数据，进一步估算障碍物的下一步运动位置，来保证路径规划的安全性和实时性。由于MDP模型在求解最优策略时需要已知全部的状态空间，而POMDP模型能够很好地用于环境、动作以及观测的不确定情况，目前使用POMDP模型进行无人机路径规划的文章中，多数都以无人机为中心建立模型，这很有可能造成无人机频繁的进行路径切换，降低路径规划的效率。此外，多数算法在无人机路径规划的过程中都将障碍物看作一个粒子，而忽略了障碍物的实际形状所造成的影响；这种情况下很有可能致使无人机和障碍物发生碰撞，导致路径规划失败。

发明内容

本发明在充分利用无人机路径规划特性的基础上，将部分可观察马尔科夫决策POMDP模型和改进的人工势场法APF结合，能够保证无人机在未知环境中路径规划的安全性。

为实现上述目的，本申请提出一种基于POMDP模型的APF无人机路径规划方法，包括：

步骤1：设置无人机的起始点、目标点坐标，无人机的初始状态为奔向目标点的直线飞行；

步骤2：在无人机飞行过程中通过机载传感器检测周围的障碍物，使用云台获取障碍物信息，建立障碍物影响模型；

步骤3：当检测到无人机在障碍物影响模型内时，得到障碍物当前时刻的位置，通过POMDP模型预测动态障碍物轨迹，得到障碍物下一时刻的位置；

步骤4：由障碍物当前时刻和下一时刻的位置，对障碍物斥力建模，找到势场中最低斥力势场集合；

步骤5：分别从当前时刻和下一时刻最低斥力势场集合中，选取使目标函数D最小的两个位置；

步骤6：根据这两个位置和无人机当前的位置确定一个圆弧，无人机沿这条圆弧飞行，直到下一个时间段(可以设置为间隔2s)机载传感器检测到周围环境信息；

步骤7：若检测仍有障碍物存在于障碍物影响模型中，转到步骤3；若未检测到周围环境有障碍物或不在障碍物影响模型中，则无人机朝着目标点飞行，直至到达目标点。

进一步的，通过POMDP预测动态障碍物轨迹，具体为：

获取状态空间S，所述状态空间S包括两个子系统状态，分别是无人机的运动状态UAV_t和障碍物的运动状态Obstacle_t；故在t时刻的状态空间定义为：

S_t＝(UAV_t,Obstacle_t) (1)

其中，无人机的运动状态UAV_t表示无人机在时刻t的位置和速度；它由一个五维向量表示

其中

表示无人机在t时刻的位置，

表示无人机在t时刻的速度，

表示无人机在t时刻的运动方向；同理，障碍物的运动状态

其中

表示障碍物在t时刻的位置，

表示障碍物在t时刻的运动速度，

表示障碍物在t时刻的运动方向；

使用t时刻障碍物采取的动作A_t作为动作空间：

其中，A_t指的是t时刻障碍物所采取的动作，

是指在t时刻状态下障碍物相对于t-1时刻在大地坐标系上所偏移的距离，

是指相对于t-1时刻的倾斜角度，

是指t时刻障碍物的加速度；

将观测空间定义为t时刻无人机通过机载传感器观测到障碍物的运动状态Ob_t：

Ob_t＝{ob_t|ob_t∈S_t} (3)

其中，Ob_t为t时刻机载传感器所观测到的障碍物运动状态，包括此时障碍物的位置、速度、运动方向以及相对于无人机的方位角θ；方位角θ由下式获取：

进一步的，在POMDP模型中，将障碍物的观测空间定义成存在噪声情况下的观测概率函数：

O(a_t,s_t+1,o_t+1)＝Pro(o_t+1|s_t+1,a_t)+Err (5)

该公式表示存在噪声时，障碍物在t时刻采取动作a_t后，在t+1时刻到达状态s_t+1时，观察到o_t+1的概率；其中，a_t∈A_t、s_t+1∈S_t、o_t+1∈Ob_t，Err为传感器的观测噪声序列，如下式所示：

Err＝k·Dis(UAV_t,Obstacle_t)+m (6)

观测概率函数依赖无人机相对于障碍物的位置，其中k、m是一个大于0且固定的系数，Dis(UAV_t,Obstacle_t)表示t时刻无人机和障碍物之间的距离。

进一步的，所述无人机的状态转移函数为：

函数

是无人机的状态控制动力学方程的过程，采用的映射关系如下：

通过式(8)得出无人机在t+1时刻的位置；其中，α是无人机的速度方向和y_z平面的夹角，β是无人机的速度方向和x_y平面的夹角，T为两个时刻之间的采样周期；

所述障碍物的状态转移函数定义为状态转移概率：

T(s_t,a_t,s_t+1)＝Pro(s_t+1|s_t,a_t,s_t-1,a_t-1,…s₀,a₀)＝Pro(s_t+1|s_t,a_t) (9)

该公式表示在t时刻障碍物采取动作a_t后，在t+1时刻到达s_t+1状态的概率，具有马尔可夫性。

进一步的，在POMDP模型中引入信念状态，所述信念状态是每个状态的后验概率分布，表示对预测状态的可信度，其更新是贝叶斯定律根据历史观察和动作值获取的：

B_t+1＝Pro(S_t+1|B_t,o_t,a_t) (10)

该公式表示障碍物在t时刻信念状态为B_t的前提下，选择动作a_t后转移到下一个信念状态B_t+1的概率过程。

更进一步的，用无人机在t时刻根据机载传感器获得的障碍物状态采取行动后的安全性以及无人机运动方向改变次数来表示回报函数，具体为：

R(s,a)＝R_count(s,a)+R_safe(s,a) (11)

如果无人机与障碍物发生冲突，则R_safe(s,a)奖励为0，路径规划失败；如果无人机能够顺利到达目标点，则R_safe(s,a)安全性的奖励是100；R_count(s,a)表示无人机检测到障碍物后所改变运动方向的次数，R_count(s,a)每增加1，则表示无人机在此时刻需要调整一次运动方向，奖励为-10。

更进一步的，针对障碍物的位置预测使用笛卡尔矩形栅格表示环境，由机载传感器测得的障碍物信息被映射到环境坐标系中，在栅格坐标系中障碍物由当前位置可能向其周围8个飞行航向角飞行或者保持当前位置不变，8个飞行航向角分别代表东、东北、北、西北、西、西南、南、东南。

更进一步的，所述障碍物影响模型具体是利用最小正方体完全包围障碍物，以正方体的对角线长度为直径做障碍物的外接球。

作为更进一步的，在无人机运动过程中为其建立一个虚拟的人造斥力场，单个障碍物形成一个斥力势场，多个障碍物时等效为一个特定区域的斥力势场；假设单个障碍物所形成的斥力势场是以该障碍物为中心的球体，其势场影响范围由障碍物影响模型决定；

障碍物Obs₁在点B的斥力势场为

障碍物Obs₂在点B的斥力势场为

则Obs₁和Obs₂两个障碍物形成的合势场为

多个障碍物在某点生成的合势场表示为：

其中，η为斥力系数，R_o为障碍物模型的球体半径，r_i为障碍物Obs₁的最小安全飞行距离，r为任意一点与障碍物之间的距离；

当无人机进入势力场范围时，所受斥力表示为：

其中，R_u为无人机模型的球体半径。

作为更进一步的，根据无人机在当前时刻检测到的未知环境中障碍物信息，建立当前时刻未知环境的斥力等效势场，通过POMDP模型得到的障碍物预测信息，建立下一时刻未知环境的斥力等效势场；联合当前时刻和下一时刻未知环境的斥力等效势场从而得到势场最低位置：

无人机飞行路径在最低斥力势场位置中进行选择，则其目标函数如式(14)所示；其中，N表示斥力势场中场强最低点的坐标集合，d_1i指的是集合N中的第i个点与参考航线之间的距离，d_2i指的是集合N中的第i个点与无人机所处位置P的距离，d_3i指的是集合N中的第i个点与最近障碍物距离的倒数。

本发明采用的以上技术方案，与现有技术相比，具有的优点是：本发明解决了传统人工势场法在无人机路径规划过程中会陷入局部最小值，从而导致目标不可达的问题。且首次提出以障碍物为中心的POMDP模型，预测障碍物的运动状态，提高无人机路径规划效率。基于障碍物的正方体外接球模型，能够有效地避免无人机与障碍物碰撞的情况发生，提高路径规划的安全性。本方法能够保证无人机在未知环境中路径规划的安全性。

附图说明

图1为本发明提出的二维障碍物模型图；

图2为本发明的障碍物影响模型图；

图3为本发明的受力分析作用图；

图4为本发明的等效势场图；

图5为本发明的分析流程示意图；

图6为多个静态障碍物的无人机轨迹对比图；

图7为多个静态障碍物的无人机轨迹对比图的局部放大图；

图8为单个动态障碍物的无人机轨迹对比图；

图9为单个动态障碍物的无人机轨迹对比图的局部放大图；

图10为多个动态障碍物的无人机轨迹对比图；

图11为多个动态障碍物的无人机轨迹对比图的局部放大图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本申请，并不用于限定本申请，即所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。

因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

在无人机的飞行地图中，由于无人机对环境状态的状态是未知的，周围环境较为复杂，无人机从起点向目标飞行的过程中，可能会遇到许多典型的障碍物，例如建筑物、山脉、火灾威胁以及其它飞行物等。此时，无人机需要利用携带的传感器实时获取静态或者动态障碍物的距离和方位角，并根据这些测量结果实时规划无人机的飞行路径以保证可以顺利避开障碍物，到达目标点。本发明假设无人机的飞行速度固定，无人机开始以恒定的速度和方向朝着目标点飞去。故提供一种基于POMDP模型的APF无人机路径规划方法，技术要点如下：

使用无人机所携带的机载传感器获取障碍物信息，建立部分可观测马尔可夫决策过程POMDP模型，利用该模型预测障碍物下一时刻可能出现的位置；

具体的，本发明关注无人机在未知动态环境中的路径规划问题，其飞行环境中随机分布静态障碍物和动态障碍物，本发明的目的是使无人机在顺利躲避障碍物的前提下，成功到达目标点。

在无人机路径规划问题中，状态空间S包含两个子系统状态分别是无人机的运动状态UAV_t和障碍物的运动状态Obstacle_t；在t时刻的状态空间可以定义为：

S_t＝(UAV_t,Obstacle_t) (1)

其中，无人机的状态UAV_t表示无人机在时刻t的位置和速度；它由一个五维向量表示

其中

表示无人机在t时刻的位置，

表示无人机在t时刻的速度，

表示无人机在t时刻的运动方向。同理可以表示出t时刻障碍物的运动状态

其中，

表示t时刻障碍物的运动速度。障碍物在路径规划中的部分数据是可以通过机载传感器观察到的，比如位置、速度、航向等。

在路径规划问题中，无人机能够根据传感器获得的障碍物运动状态采取相应的动作，从而控制无人机的状态，改变飞行路径。因此，本发明使用t时刻障碍物采取的动作A_t作为动作空间：

其中，A_t指的是t时刻障碍物所采取的动作，

是指在t时刻状态下障碍物相对于t-1时刻在大地坐标系上所偏移的距离，

是指相对于t-1时刻的倾斜角度，

是指t时刻障碍物的加速度。

将观测空间定义为t时刻无人机通过机载传感器观测到障碍物的运动状态Ob_t；

Ob_t＝{ob_t|ob_t∈S_t} (3)

其中，Ob_t为t时刻传感器所观测到的障碍物运动状态，包括此时障碍物的位置、速度、运动方向以及相对于无人机的方位角θ；方位角θ可以由下式获取：

在POMDP模型中，由于距离以及周围环境因素，无人机携带的传感器不能够准确的测量障碍物在t时刻的状态。在某一时刻，只能得到包含噪声的观测结果。因此，将障碍物的观测方程定义成存在噪声情况下的观测概率函数：

O(a_t,s_t+1,o_t+1)＝Pro(o_t+1|s_t+1,a_t)+Err (5)

该公式表示存在噪声时，障碍物在t时刻采取动作a_t后，在t+1时刻到达状态s_t+1时，观察到o_t+1的概率。其中，a_t∈A_t、s_t+1∈S_t、o_t+1∈Ob_t，Err为传感器的观测噪声序列。

在获取障碍物的状态过程中，Err与无人机和障碍物之间的位置有关。当无人机与障碍物距离较近时，观测噪声小，观测精度高。当距离较远时，噪声较大，观测误差也相应变大。噪声Err可以表示为：

Err＝k·Dis(UAV_t,Obstacle_t)+m (6)

观测函数依赖无人机相对于障碍物的位置，其中k、m是一个大于0且固定的系数，Dis(UAV_t,Obstacle_t)表示t时刻无人机和障碍物之间的距离。设置m的主要原因是避免距离为0时，无人机与障碍物发生碰撞的状况。

由于周围环境较为复杂，无人机通过传感器获取障碍物信息时，获取到的目标信息也存在测量偏差值，因此本发明在无人机通过传感器初次获得障碍物相关信息时的安全距离设置较大，避免因为传感器观测的误差而出现无人机和障碍物发生碰撞的情况。在距离越近时，噪声越小，噪声按照式(6)动态调节，进而无人机自动调整与障碍物之间的飞行距离。

状态转移函数表示在当前时刻采取行动后，下一时刻状态的概率分布。对于本发明定义的状态空间中无人机和障碍物两个子系统，它们相应的状态转移分别定义如下：

无人机的状态转移函数为：

函数

是设计无人机状态控制的动力学方程的过程；这里，本发明采用的映射关系如下：

通过式(8)得出无人机在t+1时刻的位置。其中，α是无人机的速度方向和y_z平面的夹角，β是无人机的速度方向和x_y平面的夹角，T为两个时刻之间的采样周期。

障碍物的状态转移函数可以定义为状态转移概率，表示在t时刻障碍物采取动作a_t后，在t+1时刻到达s_t+1状态的概率，具有马尔可夫性。根据全概率公式，状态转移概率可以分解为：

T(s_t,a_t,s_t+1)＝Pro(s_t+1|s_t,a_t,s_t-1,a_t-1,…s₀,a₀)＝Pro(s_t+1|s_t,a_t) (9)

由于无人机的飞行环境状态不能完全可观察，因此在POMDP模型中引入了信念状态。信念状态是每个状态的后验概率分布，表示对预测状态的可信度，具有马尔可夫性。它的更新是通过贝叶斯定律根据历史观察和动作值获取的：

B_t+1＝Pro(S_t+1|B_t,o_t,a_t) (10)

该公式表示障碍物在t时刻信念状态为B_t的前提下，选择动作a_t后转移到下一个信念状态B_t+1的概率过程。

回报函数表示在某种状态下采取行动后获得的回报值，是预测障碍物下一时刻位置的重要判断依据，通常需要根据多个标准来定义。鉴于路径规划过程中，无人机导航的目标是最小化到达目标点的路径长度，同时避开障碍物。因此首要考虑的因素是安全性，其次是最短路径因素。

本发明的回报函数用无人机在t时刻根据传感器获得的障碍物的状态采取行动后的安全性以及无人机运动方向改变次数来表示。回报函数为：

R(s,a)＝R_count(s,a)+R_safe(s,a) (11)

如果无人机与障碍物发生冲突，则R_safe(s,a)奖励为0，路径规划失败；如果无人机能够顺利到达目标点，则R_safe(s,a)安全性的奖励是100。R_count(s,a)表示无人机检测到障碍物后所改变运动方向的次数，R_count(s,a)每增加1，则表示无人机在此时刻需要调整一次运动方向，奖励为-10。

针对障碍物的位置预测使用笛卡尔矩形栅格表示环境，由传感器测得的障碍物信息可以被映射到环境坐标系中。在该栅格坐标系中障碍物由当前位置可能向其周围8个飞行航向角飞行或者保持当前位置不变，8个飞行航向角分别代表东、东北、北、西北、西、西南、南、东南。

通过上述组件的配置以及栅格地图法，使用本发明提出的POMDP模型能够很好地预测出障碍物的下一步位置Ret，完成障碍物的轨迹生成，为无人机路径规划做好基础。

为了保证无人机在路径规划中的安全性，建立无人机和障碍物影响模型，避免路径规划的过程中无人机与障碍物发生碰撞的情况；

无人机在飞行过程中不仅要考虑自身约束还要考虑各种障碍物的威胁。多数路径规划算法中，将无人机和各种障碍物都看作一个粒子，但实际情况中，遇到的障碍物多为不规则的三维物体，比如小鸟、山丘等，如果只是将其简单看作粒子，会加大路径规划的风险。由于空中静态或者动态的障碍物多数都近似于球体，比如热气球、蝙蝠、小鸟等，因此本发明主要处理球体或者类似球体的障碍物，只要障碍物的长度、宽度之比在5:2之间都可以近似看作球体。

为了保证无人机飞行路径的生成以及遇到障碍物时的安全性，本发明提出嵌套最小正方体的外接球障碍物模型。具体改进方法是通过无人机所携带的云台检测障碍物的图像信息，利用最小正方体完全包围障碍物，以正方体的对角线长度为直径做障碍物的外接球，二维障碍物模型如附图1所示。其中，最内侧的实线表示障碍物的二维平面图，中间的虚线为正方体的二维模型，外侧的实线为外接球的二维模型。

无人机在路径规划过程中，将无人机和障碍物都假设为一个球体，障碍物和无人机位置是各自的球心，通过障碍物影响模型设计一个无人机飞行路径安全范围，为避障提供进一步保障。障碍物影响模型如附图2所示。其中，R_u代表无人机的球体半径，R_o代表障碍物的球体半径。只要满足无人机球心和障碍物球心之间的距离大于半径之和，即dis＞Dis(R_u,R_o)，就认为这是一条安全有效的路径，无人机通过这条路径能够安全的到达终点。其中，dis表示无人机球心和障碍物球心之间的距离。

对传统的人工势场法进行改进，确保无人机能够安全到达目标点的同时避免其陷入局部极小值问题。

无人机前往目标点的过程中，会存在某个点引力和斥力相同，从而无人机陷入僵持，无法移动，这被称为局部极小值。此时无人机所受斥力合力与引力大小相等，方向相反。

在无人机运动过程中为其建立一个虚拟的人造斥力场，单个障碍物形成一个斥力势场，多个障碍物时等效为一个特定区域的斥力势场，受力分析如附图3所示。假设单个障碍物所形成的斥力势场是以该障碍物为中心的球体，其势场影响范围由障碍物影响模型决定。

障碍物Obs₁在点B的斥力势场为

障碍物Obs₂在点B的斥力势场为

则Obs₁和Obs₂两个障碍物形成的合势场为

根据分析可知，多个障碍物在某点生成的合势场可表示为：

其中，η为斥力系数，R_o为障碍物模型的球体半径，r_i为障碍物Obs₁的最小安全飞行距离，r为任意一点与障碍物之间的距离。

当无人机进入势力场范围时，所受斥力表示为：

其中，R_u为无人机模型的球体半径。

根据以上分析可知，多个障碍物在未知环境下形成一个等效势场，等效势场如附图4所示。势场是运动障碍物周围环境里虚拟的特殊存在，主要体现在对进入其中的无人机有斥力作用，势场大小由障碍物模型半径以及无人机与障碍物距离共同决定，势场的方向都是由障碍物中心指向无人机。在引入新的斥力后，就避免了无人机飞行至局部最小值点，从根本上避免局部极小值的出现。

将无人机起始位置到目标位置作为参考航线l，在飞行的未知环境中构建人工势场，其中障碍物的位置坐标由传感器测量值以及POMDP模型对障碍物的轨迹建模可得。

根据无人机在当前时刻检测到的未知环境中障碍物信息，建立当前时刻未知环境的斥力等效势场，通过POMDP模型得到的障碍物预测信息，建立下一时刻未知环境的斥力等效势场；联合当前时刻和下一时刻未知环境的斥力等效势场从而得到势场最低位置：

为了减少路径规划过程中的能量消耗，本发明选用最低斥力势场集合中的位置点作为规划路径。无人机飞行路径在最低斥力势场位置中进行选择，位置点的选择与其到参考航线、障碍物以及无人机的距离有关，其中，N表示斥力势场中场强最低点的坐标集合，d_1i指的是集合N中的第i个点与参考航线之间的距离，d_2i指的是集合N中的第i个点与无人机所处位置P的距离，d_3i指的是集合N中的第i个点与最近障碍物距离的倒数，则其目标函数如公式(14)所示。

图6和图7为多个静态障碍物环境下无人机的飞行轨迹图。由图6能够看出，在无人机开始运动时，四种算法所规划的路径都是正常飞行。当机载传感器检测到障碍物obs₂时，TAPF算法规划的路径选择从obs₂的上方绕过去，其他三种算法选择从其从下方飞行过去。在无人机从起点(0,0,0)处到(31,28,30)点处，Improve APF-fuzzy算法、CPFIBA算法以及本发明的POMDP-APF策略几乎相差无几，效果均优于TAPF算法。但是当传感器检测到obs₃和obs₄存在的时候，TAPF算法和Improve APF-fuzzy算法均出现了较大的转向角，并且TAPF算法在遇到obs₅的时候陷入局部最小值，导致本次路径规划失败。Improve APF-fuzzy算法虽然能够完成路径规划，但是从仿真结果来看，远远没有本发明提出的POMDP-APF策略效率高。由图7局部放大图不难看出，相较于Improve APF-fuzzy算法，CPFIBA算法和POMDP-APF策略相对来说整个避障路径轨迹完整，没有振荡和较大转向情况的出现。并且POMDP-APF策略相较于其他三种算法能够产生更短的路径，提供安全的避障。

图8和图9为单个动态障碍物环境下的无人机飞行轨迹图。从图8可以看到，在obs₁位置，三种算法都能够有效的绕过obs₁障碍物，相较于TAPF算法以及Improve APF-fuzzy算法，POMDP-APF策略在绕过obs₁时的转角以及路径长度更小。无人机绕过obs₁后，此时障碍物由静止状态开始运动，图8中的虚线表示使用POMDP模型所预测的障碍物运动轨迹，一段时间后，障碍物由obs₁运动到obs₁'的位置。此时TAPF算法以及Improve APF-fuzzy算法都未能够及时的改变无人机运动方向，致使TAPF算法在(75,77,90)位置处与障碍物发生碰撞，Improve APF-fuzzy算法在(83,78,90)处发生碰撞，路径规划失败。而本发明提出的POMDP-APF策略在初次检测到障碍物时一直实时预测障碍物的下一个运动位置，如果判断无人机不在障碍物影响范围之内，则选择不避障直奔目标点。

图10和图11为多个动态障碍物环境下的无人机的飞行轨迹图。在多个动态障碍物的情况下，进行POMDP-APF策略和TAPF算法以及Improve APF-fuzzy算法的仿真对比，参数设置如表1所示。其中，obs₂和obs₃为静态障碍物，obs₁和obs₄初始时为静止状态，在无人机通过传感器检测到其存在并绕过时，将obs₁和obs₄设置成运动状态。图10显示了在多个动态障碍物情况下，无人机避障的仿真结果。与TAPF方法和Improve APF-fuzzy算法相比，本发明提出的POMDP-APF策略能够有效地避开所有障碍物。TAPF算法在obs₁处于静止状态时，能够完成避障任务，但是当obs₁处于运动状态时，由于其不能做到动态避障，在(18,54,40)处与obs₁发生碰撞，致使路径规划失败。Improve APF-fuzzy算法的仿真效果优于TAPF算法，在检测到静止的obs₁时能够选择一条正确的路径规避，但是在obs₄开始运动时，在(72,86,62)处发生了碰撞行为，图11为障碍物obs₄部分的局部放大图。POMDP-APF策略在整个避障过程中，不管是检测到了静态障碍物，还是动态障碍物，无人机都可以获得一条相对平滑的运动路径。而且在实时避障过程中，POMDP-APF策略规划的无人机运动路径是完整，没有振荡。

表1实例参数设置

前述对本发明的具体示例性实施方案的描述是为了说明和例证的目的。这些描述并非想将本发明限定为所公开的精确形式，并且很显然，根据上述教导，可以进行很多改变和变化。对示例性实施例进行选择和描述的目的在于解释本发明的特定原理及其实际应用，从而使得本领域的技术人员能够实现并利用本发明的各种不同的示例性实施方案以及各种不同的选择和改变。本发明的范围意在由权利要求书及其等同形式所限定。

Claims (10)

1.一种基于POMDP模型的APF无人机路径规划方法，其特征在于，包括：

步骤1：设置无人机的起始点、目标点坐标，无人机的初始状态为奔向目标点的直线飞行；

步骤2：在无人机飞行过程中通过机载传感器检测周围的障碍物，使用云台获取障碍物信息，建立障碍物影响模型；

步骤3：当检测到无人机在障碍物影响模型内时，得到障碍物当前时刻的位置，通过POMDP模型预测动态障碍物轨迹，得到障碍物下一时刻的位置；

步骤4：由障碍物当前时刻和下一时刻的位置，对障碍物斥力建模，找到势场中最低斥力势场集合；

步骤5：分别从当前时刻和下一时刻最低斥力势场集合中，选取使目标函数D最小的两个位置；

步骤6：根据这两个位置和无人机当前的位置确定一个圆弧，无人机沿这条圆弧飞行，直到下一个时间段机载传感器检测到周围环境信息；

步骤7：若检测仍有障碍物存在于障碍物影响模型中，转到步骤3；若未检测到周围环境有障碍物或不在障碍物影响模型中，则无人机朝着目标点飞行，直至到达目标点。

2.根据权利要求1所述一种基于POMDP模型的APF无人机路径规划方法，其特征在于，通过POMDP预测动态障碍物轨迹，具体为：

获取状态空间S，所述状态空间S包括两个子系统状态，分别是无人机的运动状态UAV_t和障碍物的运动状态Obstacle_t；故在t时刻的状态空间定义为：

S_t＝(UAV_t,Obstacle_t) (1)

其中，无人机的运动状态UAV_t表示无人机在时刻t的位置和速度；它由一个五维向量表示

其中

表示无人机在t时刻的位置，

表示无人机在t时刻的速度，

表示无人机在t时刻的运动方向；同理，障碍物的运动状态

其中

表示障碍物在t时刻的位置，

表示障碍物在t时刻的运动速度，

表示障碍物在t时刻的运动方向；

使用t时刻障碍物采取的动作A_t作为动作空间：

其中，A_t指的是t时刻障碍物所采取的动作，

是指在t时刻状态下障碍物相对于t-1时刻在大地坐标系上所偏移的距离，

是指相对于t-1时刻的倾斜角度，

是指t时刻障碍物的加速度；

将观测空间定义为t时刻无人机通过机载传感器观测到障碍物的运动状态Ob_t：

Ob_t＝{ob_t|ob_t∈S_t} (3)

其中，Ob_t为t时刻机载传感器所观测到的障碍物运动状态，包括此时障碍物的位置、速度、运动方向以及相对于无人机的方位角θ；方位角θ由下式获取：

3.根据权利要求2所述一种基于POMDP模型的APF无人机路径规划方法，其特征在于，在POMDP模型中，将障碍物的观测空间定义成存在噪声情况下的观测概率函数：

O(a_t,s_t+1,o_t+1)＝Pro(o_t+1|s_t+1,a_t)+Err (5)

该公式表示存在噪声时，障碍物在t时刻采取动作a_t后，在t+1时刻到达状态s_t+1时，观察到o_t+1的概率；其中，a_t∈A_t、s_t+1∈S_t、o_t+1∈Ob_t，Err为传感器的观测噪声序列，如下式所示：

Err＝k·Dis(UAV_t,Obstacle_t)+m (6)

观测概率函数依赖无人机相对于障碍物的位置，其中k、m是一个大于0且固定的系数，Dis(UAV_t,Obstacle_t)表示t时刻无人机和障碍物之间的距离。

4.根据权利要求3所述一种基于POMDP模型的APF无人机路径规划方法，其特征在于，所述无人机的状态转移函数为：

函数

是无人机的状态控制动力学方程的过程，采用的映射关系如下：

通过式(8)得出无人机在t+1时刻的位置；其中，α是无人机的速度方向和y_z平面的夹角，β是无人机的速度方向和x_y平面的夹角，T为两个时刻之间的采样周期；

所述障碍物的状态转移函数定义为状态转移概率：

T(s_t,a_t,s_t+1)＝Pro(s_t+1|s_t,a_t,s_t-1,a_t-1,…s₀,a₀)＝Pro(s_t+1|s_t,a_t) (9)

该公式表示在t时刻障碍物采取动作a_t后，在t+1时刻到达s_t+1状态的概率，具有马尔可夫性。

5.根据权利要求1所述一种基于POMDP模型的APF无人机路径规划方法，其特征在于，在POMDP模型中引入信念状态，所述信念状态是每个状态的后验概率分布，表示对预测状态的可信度，其更新是贝叶斯定律根据历史观察和动作值获取的：

B_t+1＝Pro(S_t+1|B_t,o_t,a_t) (10)

该公式表示障碍物在t时刻信念状态为B_t的前提下，选择动作a_t后转移到下一个信念状态B_t+1的概率过程。

6.根据权利要求1所述一种基于POMDP模型的APF无人机路径规划方法，其特征在于，用无人机在t时刻根据机载传感器获得的障碍物状态采取行动后的安全性以及无人机运动方向改变次数来表示回报函数，具体为：

R(s,a)＝R_count(s,a)+R_safe(s,a) (11)

如果无人机与障碍物发生冲突，则R_safe(s,a)奖励为0，路径规划失败；如果无人机能够顺利到达目标点，则R_safe(s,a)奖励是100；R_count(s,a)表示无人机检测到障碍物后所改变运动方向的次数，R_count(s,a)每增加1，则表示无人机在此时刻需要调整一次运动方向，奖励为-10。

7.根据权利要求1所述一种基于POMDP模型的APF无人机路径规划方法，其特征在于，针对障碍物的位置预测使用笛卡尔矩形栅格表示环境，由机载传感器测得的障碍物信息被映射到环境坐标系中，在栅格坐标系中障碍物由当前位置可能向其周围8个飞行航向角飞行或者保持当前位置不变，8个飞行航向角分别代表东、东北、北、西北、西、西南、南、东南。

8.根据权利要求1所述一种基于POMDP模型的APF无人机路径规划方法，其特征在于，所述障碍物影响模型具体是利用最小正方体完全包围障碍物，以正方体的对角线长度为直径做障碍物的外接球。

9.根据权利要求1所述一种基于POMDP模型的APF无人机路径规划方法，其特征在于，在无人机运动过程中为其建立一个虚拟的人造斥力场，单个障碍物形成一个斥力势场，多个障碍物时等效为一个特定区域的斥力势场；假设单个障碍物所形成的斥力势场是以该障碍物为中心的球体，其势场影响范围由障碍物影响模型决定；

障碍物Obs₁在点B的斥力势场为

障碍物Obs₂在点B的斥力势场为

则Obs₁和Obs₂两个障碍物形成的合势场为

多个障碍物在某点生成的合势场表示为：

其中，η为斥力系数，R_o为障碍物模型的球体半径，r_i为障碍物Obs₁的最小安全飞行距离，r为任意一点与障碍物之间的距离；

当无人机进入势力场范围时，所受斥力表示为：

其中，R_u为无人机模型的球体半径。

10.根据权利要求1所述一种基于POMDP模型的APF无人机路径规划方法，其特征在于，根据无人机在当前时刻检测到的未知环境中障碍物信息，建立当前时刻未知环境的斥力等效势场，通过POMDP模型得到的障碍物预测信息，建立下一时刻未知环境的斥力等效势场；联合当前时刻和下一时刻未知环境的斥力等效势场从而得到势场最低位置：

无人机飞行路径在最低斥力势场位置中进行选择，则其目标函数如式(14)所示；其中，N表示斥力势场中场强最低点的坐标集合，d_1i指的是集合N中的第i个点与参考航线之间的距离，d_2i指的是集合N中的第i个点与无人机所处位置P的距离，d_3i指的是集合N中的第i个点与最近障碍物距离的倒数。

Images