一种考虑视场角限制以及运动不确定性的结合记忆的旋翼飞行器高速飞行避障方法与流程

本发明涉及飞行机器人运动避障领域，尤其涉及一种考虑视场角限制以及运动不确定性的结合记忆的旋翼飞行器高速飞行避障方法。

背景技术：

旋翼飞行器快速运动下如何躲避障碍物是一个困难的问题，常见的方法是根据历史位姿用最大似然估计构建地图，现有四旋翼飞行器高速运动下姿态估计不准确，地图建立缓慢且精度不高，耗费计算量和存储量大。因此，现有技术中的一个值得注意的趋势是开发仅使用最新的传感器测量的状态空间规划方法，其有效地消除了姿态估计不确定性的问题。

技术实现要素：

本发明的目的在于针对现有四旋翼飞行器高速运动下姿态估计不准确，地图建立缓慢且精度不高，提出一种考虑视场角限制以及运动不确定性的结合记忆的旋翼飞行器高速飞行避障方法。该方法使用能够输出深度图的视觉传感器，如双目摄像机，深度摄像机等，且考虑了摄像机的视场角限制以及运动估计的不确定性，仅需短暂记忆飞行器位姿图和深度图，无需建立全局地图，计算量和存储量都小，可实现高速飞行下避开障碍物。

为了实现以上目的，本发明所采用的技术方案如下：一种考虑视场角限制以及运动不确定性的结合记忆的旋翼飞行器高速飞行避障方法，该方法包括如下步骤：

步骤(1)：旋翼飞行器在飞行过程中记录最新一段时间的n个位姿图及其对应时刻的深度图，记作({x,σ},d)，其中x∈{x0,x-1,…,x-n}是前n个时刻的飞行器位姿，σ是各个位姿之间带有噪声的变换矩阵，d＝{d0,d-1,…d-n}是前n个时刻位姿对应的观测到的深度图；

步骤(2)：在某一时刻，当飞行器需要决定接下来的运动轨迹时，将控制输入离散化；

步骤(3)：选择一定数量的控制输入量，每一个选择的控制输入量即决定一条预计的运动轨迹，再根据飞行器的动力学模型，预计一段时间后每一条轨迹上飞行器所将要到达的多个位置的概率分布；

步骤(4)：判断每一条轨迹上的每一个预计的位置概率分布是否超出了当前深度图的观测范围，即视场角所限制，若没有超过，则将此条轨迹上每一个位置概率分布与当前时刻的深度图结合计算此条轨迹的碰撞概率；若超过，则将该位置概率分布按所记录的最新一段时间的位姿图反向推理，即重新估计该条轨迹上的位置概率分布在之前时刻位姿图中的位置概率分布，直到找到没有超过观测范围的位置概率分布估计，或者超出查找范围，即所记录的位姿图中没有符合要求的；

步骤(5)：再将找到的位置概率分布分别与各个位姿图所对应时刻的深度图结合计算此条轨迹的碰撞概率；选取碰撞概率最小的轨迹所对应的控制输入量作为飞行器的控制输入；

步骤(6)：重复步骤(1)-步骤(5)即完成避障飞行。

进一步的，所述步骤(2)具体如下：

步骤(2.1)：确定飞行器的控制输入，控制输入为加速度，记为a；

步骤(2.2)：根据飞行器所能产生的最大升力限制，确定最大垂直上升加速度，最大垂直下架加速度和最大水平移动加速度，则所能产生的各个方向的最大加速度形成一个类椭球包络面，选择nm个离散的加速度作为备选的控制输入量，记为ai，所对应的轨迹记为mi，1≤i≤nm,i∈z。

进一步的，所述步骤(3)具体如下：

步骤(3.1)：飞行器当前时刻的速度记为v0，当前时刻的加速度记为a0，当前所观测到的深度图为d0，假设其具有高斯噪声，即

步骤(3.2)：当前时刻至tjf时刻，假设飞行器做恒加加速运动，某加速度ai对应的恒加加速度为则恒加加速度阶段，飞行器的轨迹描述为其中pi(t)定义为按第i个选择的控制输入量在t时刻飞行器所对应的位置概率分布；

步骤(3.3)：tjf时刻至tf时刻，假设飞行器做恒加速运动，恒加速度即为ai，则恒加速度阶段，飞行器的轨迹为将t取具体值时即可获得多个位置的概率分布。

进一步的，所述步骤(4)具体如下：

步骤(4.1)：对于每一条轨迹mi，采样nt个时间点，对于每一个采样时间点ij,1≤j≤nt,j∈z所对应的飞行器位置概率分布pi(tj)，判断其中心位置是否在当前时刻所观测到的深度图d0的视野范围内；

步骤(4.2)：若不在视野范围内，则将此位置概率分布根据所记录的位姿图映射到前一个位姿坐标中，即再判断新得到的位置概率分布的中心位置是否在该时刻所观测到的深度图d-1的视野范围内，如此重复，直到找到符合的位置概率分布或超出查找范围。

进一步的，所述步骤(5)具体如下：

步骤(5.1)：计算所有位置概率分布在深度图中的碰撞概率，假设无人机体积很小，则碰撞概率为p(collision,pi(tj))＝v·pdf(pi,μ(tj)-d)，其中v为无人机的体积，pi,μ(tj)为tj时刻位置概率分布pi(tj)的均值，d为对应深度图中离该均值最近的障碍物的位置，pdf()即为高斯分布的概率密度函数；

步骤(5.2)：假设一条轨迹中每一个采样时刻都独立，则可计算一条轨迹mi的碰撞概率为选取最小的碰撞概率对应的轨迹所对应的控制量作为当前控制输入。

本发明的有益效果如下：本发明使用能够输出深度图的视觉传感器，如双目摄像机，深度摄像机等，且考虑了摄像机的视场角限制以及运动估计的不确定性，仅需短暂记忆飞行器位姿图和深度图，无需建立全局地图，有效减小了计算量和存储量，克服现有旋翼飞行器为了实现高速运动下的避障而存在的姿态估计不准确，地图建立缓慢且精度不高的缺点。

附图说明

图1为深度图分割示意图；

图2为本发明避障方法的流程图；

图3为位姿图和对应深度图组合示意图；图4为飞行器加速度离散化示意图；

图5为飞行器轨迹描述图；

图6为飞行器位置概率分布描述图；

图中：1-因视野限制而不可见的部分，2-因被障碍物遮挡为不可见的部分，3-障碍物，4-可见部分。

具体实施方式

下面根据附图详细说明本发明。

一种考虑视场角限制以及运动不确定性的结合记忆的四旋翼飞行器高速飞行避障方法，在四旋翼飞行器上搭载深度传感器来获得深度图，深度图的表示如图1所示，图分为因视野限制而不可见的部分1、因被障碍物遮挡为不可见的部分2、障碍物3和可见部分4四个部分，实际使用时，因被障碍物遮挡为不可见的部分也视为障碍物。

如图2所示，本发明提供的一种考虑视场角限制以及运动不确定性的结合记忆的四旋翼飞行器高速飞行避障方法，具体包括如下步骤：

步骤(1)：旋翼飞行器在飞行过程中记录最新一段时间的n个位姿图及其对应时刻的深度图，记作({x,σ},d)，其中x∈{x0,x-1,…,x-n}是前n个时刻的飞行器位姿，σ是各个位姿之间带有噪声的变换矩阵，d＝{d0,d-1,…d-n}是前n个时刻位姿对应的观测到的深度图，如图3所示；

步骤(2)：在某一时刻，当飞行器需要决定接下来的运动轨迹时，将控制输入离散化；具体如下：

步骤(2.1)：确定飞行器的控制输入，例如控制输入为加速度、速度、方位角等，本实施例以控制输入为加速度为例，记为a；

步骤(2.2)：根据飞行器所能产生的最大升力限制，确定最大垂直上升加速度，最大垂直下架加速度和最大水平移动加速度，则所能产生的各个方向的最大加速度形成一个类椭球包络面，选择nm个离散的加速度作为备选的控制输入量，记为ai，所对应的轨迹记为mi，1≤i≤nm,i∈z，如图4所示，取26个离散的加速度量，则nm＝26。

步骤(3)：选择一定数量的控制输入量，每一个选择的控制输入量即决定一条预计的运动轨迹，再根据飞行器的动力学模型，并且考虑当前飞行器状态估计的不确定性，预计一段时间后每一条轨迹上飞行器所将要到达的多个位置的概率分布；具体如下：

步骤(3.1)：飞行器当前时刻的速度记为v0，当前时刻的加速度记为a0，当前所观测到的深度图为d0，假设其具有高斯噪声，即

步骤(3.2)：当前时刻至tjf时刻，假设飞行器做恒加加速运动，某加速度ai对应的恒加加速度为则恒加加速度阶段，飞行器的轨迹描述为其中pi(t)定义为按第i个选择的控制输入量在t时刻飞行器所对应的位置概率分布；

步骤(3.3)：tjf时刻至tf时刻，假设飞行器做恒加速运动，恒加速度即为ai，则恒加速度阶段，飞行器的轨迹为如图5所示，将t取具体值时即可获得多个位置的概率分布。

步骤(4)：判断每一条轨迹上的每一个预计的位置概率分布是否超出了当前深度图的观测范围，即视场角所限制，若没有超过，则将此条轨迹上每一个位置概率分布与当前时刻的深度图结合计算此条轨迹的碰撞概率；若超过，则将该位置概率分布按所记录的最新一段时间的位姿图反向推理，即重新估计该条轨迹上的位置概率分布在之前时刻位姿图中的位置概率分布，直到找到没有超过观测范围的位置概率分布估计，或者超出查找范围，即所记录的位姿图中没有符合要求的；具体如下：

步骤(4.1)：对于每一条轨迹mi，采样nt个时间点，对于每一个采样时间点ij,1≤j≤nt,j∈z所对应的飞行器位置概率分布pi(tj)，判断其中心位置是否在当前时刻所观测到的深度图d0的视野范围内，如图6所示；

步骤(4.2)：若不在视野范围内，则将此位置概率分布根据所记录的位姿图映射到前一个位姿坐标中，即再判断新得到的位置概率分布的中心位置是否在该时刻所观测到的深度图d-1的视野范围内，如此重复，直到找到符合的位置概率分布或超出查找范围。

步骤(5)：再将找到的位置概率分布分别与各个位姿图所对应时刻的深度图结合计算此条轨迹的碰撞概率；选取碰撞概率最小的轨迹所对应的控制输入量作为飞行器的控制输入；具体如下：

进一步的，所述步骤(5)具体如下：

步骤(5.1)：计算所有位置概率分布在深度图中的碰撞概率，假设无人机体积很小，则碰撞概率为p(collision,pi(tj))＝v·pdf(pi,μ(tj)-d)，其中v为无人机的体积，pi,μ(tj)为tj时刻位置概率分布pi(tj)的均值，d为对应深度图中离该均值最近的障碍物的位置，pdf()即为高斯分布的概率密度函数；

步骤(5.2)：假设一条轨迹中每一个采样时刻都独立，则可计算一条轨迹mi的碰撞概率为选取最小的碰撞概率对应的轨迹所对应的控制量作为当前控制输入。

步骤(6)：重复步骤(1)-步骤(5)即完成避障飞行。