CN116048120B - 一种未知动态环境下小型四旋翼无人机自主导航系统及方法 - Google Patents

Abstract

本发明公开了一种未知动态复杂环境下小型四旋翼无人机自主导航系统及方法。本发明完全利用机载深度相机传感器感知周围环境，实现对环境中的动态障碍物的检测和运动状态估计，通过改进的最优相互避碰集(ORCA)算法实现对环境中的动态和静态障碍物进行避障，利用B样条曲线生成平滑连续的运动轨迹到达目标点。本发明解决了小型四旋翼无人机在未知的、包含动态和静态障碍物的非结构化环境下，无碰撞地到达预定目标点的自主导航问题。

Description

一种未知动态环境下小型四旋翼无人机自主导航系统及方法

技术领域

本发明涉及一种未知动态环境下小型四旋翼无人机自主导航系统及方法，属于无人机自主导航技术领域。

背景技术

小型无人机由于其具备较高的机动能力和较灵活的控制方式，能够在复杂环境下执行多种任务，因而受到国内外学术界和工业界的广泛关注。无人机自主导航系统是无人机提高智能化程度以完成复杂任务的基础，理想中移动机器人的自主导航系统应当具备在非结构化的复杂环境中通过自身携带的传感器完成对环境的感知，规划出合理的运动轨迹到达指定目的地的能力。无人机自主导航技术对于灾害救援、环境探索和物流配送等方面的实际应用具有重要意义。小型四旋翼无人机的实际使用环境中通常具有静态障碍物和动态障碍物，因此需要设计能够使无人机安全无碰撞地在复杂环境中完成既定任务的自主导航系统。由于小型四旋翼无人机的载荷能力有限，无法搭载高精度的激光雷达等传感器，其使用的传感器通常受噪声影响较大，机载处理器往往计算能力有限，对环境中障碍物的运动状态进行合理的估计和对动态障碍物进行避障给动态环境下的小型四旋翼无人机自主导航系统带来了挑战。

针对无人机在动态复杂环境下的自主导航任务，现有以下几种主要解决方案：

方案1：文献(Wang Y,Ji J,Wang Q,et al.Autonomous Flights in DynamicEnvironments with Onboard Vision[J].arXiv preprint arXiv:2103.05870,2021.)使用基于文献(Zhou X,Wang Z,Ye H,et al.EGO-Planner:An ESDF-free Gradient-basedLocal Planner for Quadrotors[J].IEEE Robotics and Automation Letters,2020,6(2):478-485.)改进的动态环境自主导航系统，其处理动态障碍物避碰时仅从动态障碍物的位置设计避碰轨迹，难以处理障碍物运动模式较复杂的情况。

方案2：文献(Berg J,Guy S.Reciprocal n-body Collision Avoidance[J].Robotics Research.2011:3-19.)提出了最优相互避碰集(ORCA)的方法，其基于速度障碍集(VO)，将求解一定时间段内能避免碰撞的速度的问题建模为一个约束较为简单的优化问题进行求解。但其只是提出了避碰速度的选择方案，并且没有生成符合无人机动力学的连续光滑轨迹。

发明内容

有鉴于此，本发明提出一种未知动态环境下小型四旋翼无人机自主导航系统及方法，针对包含动态和静态障碍物的未知复杂环境中的无人机自主导航问题，提出了一种仅依靠自身传感器进行环境感知，使用自身携带的计算处理器进行自主航路规划的小型四旋翼无人机自主导航系统。无人机通过机载传感器获取环境信息对环境建立导航地图，基于改进的最优相互避碰集(ORCA)方法进行规划完成避碰，生成到达目标点的平滑连续的无碰撞安全航路完成自主导航任务。该方法为一种基于改进的速度障碍集的小型四旋翼无人机动态环境下自主导航系统设计方法，使得无人机具备仅依靠机载传感器，在同时包含动态和静态障碍物的未知复杂环境中进行自主导航的能力，使得无人机能够更好地应用于实际复杂场景中执行多样化的任务。

实现本发明的技术方案如下：

一种未知动态环境下小型四旋翼无人机自主导航系统，该导航系统包括：环境感知模块和轨迹规划模块；

所述环境感知模块用于接收载视觉传感器获取的环境信息，并根据接收到的环境信息建立占据栅格地图，然后对建立的占据栅格地图中表示障碍物的点云进行聚类和追踪，获得环境中独立障碍物信息，再通过卡尔曼滤波估计获得的独立障碍物的位置和运动速度，并将获得的独立障碍物的位置和运动速度提供给轨迹规划模块用以生成安全无碰撞地轨迹；

所述的轨迹规划模块用于接收环境感知模块输出的独立障碍物的位置和运动速度，并根据接收到的独立障碍物的位置和运动速度生成最优相互避碰集(ORCA)，再通过生成的最优相互避碰集(ORCA)约束无人机的运动速度，实现对障碍物的避碰，根据无人机的运动速度利用B样条曲线生成平滑连续的运动轨迹。

所述的建立占据栅格地图的方法具体为：首先使用光线投影法查找需要更新的栅格，然后使用贝叶斯估计方法估计栅格占据的概率，以减小传感器噪声的影响；

使用贝叶斯估计方法估计栅格占据的概率的方法为：

对于占据栅格地图中的体素栅格m_i，初始时被占据的概率用P(m_i)表示，空闲的概率为显然P(m_i)与/>之和为1，在未被观测到的情况下，占据和空闲的概率相等均为0.5；

设定每次观测z_t的置信概率P(m_i|z_t)为考虑设备测量精度的定值，定义对数置信度l_t,i、反演观测l_inv,i和先验概率l₀如下：

根据贝叶斯公式将栅格占据情况随观测的更新迭代转化为一次加法运算为l_t,i＝l_t-1,i+l_inv,i-l₀；

所述进行聚类的方法为：在占据栅格地图的基础上，使用DBSCAN聚类方法对环境信息中的从属于独立障碍物的点云进行聚类分割；

所述的追踪的方法为：对提取的独立障碍物构建AABB包围盒并计算几何中心，通过搜索前后帧之间距离最近的几何中心，关联前后帧中表示同一个障碍物的点云簇；

通过卡尔曼滤波估计获得独立障碍物的运动速度的方法为：

使用卡尔曼滤波方法根据各帧间障碍物包围盒几何中心的位置变化，对每个障碍物的运动速度进行估计，具体为：

使用x_k表示k时刻障碍物包围盒中心的空间位置，用表示k时刻障碍物的速度，Δt表示观测时间的间隔，假设障碍物具有恒定的运动速度，其运动状态方程为：

在t_k时刻对障碍物运动状态的估计值/>与测量值z_k如下：

其中，v为属于高斯分布N(0,Q)的过程噪声，w为属于高斯分布N(0,R)的测量噪声，卡尔曼滤波迭代更新如下：

生成最优相互避碰集(ORCA)的方法为：

考虑二维平面上的规划问题，将障碍物建模为设定半径的圆形，定义对于障碍物i相对于无人机的速度障碍集为VO_uav|i，假设障碍物的速度保持不变，则若无人机取VO_uav|i中的速度，则一定时间后二者一定会发生碰撞，记为无人机会与障碍物发生碰撞的速度障碍集的边界，当无人机速度在集合VO_uav|i中，定义无人机速度到/>中最近的距离为/>argmin表示取函数的最小值，||·||表示取模运算，定义无人机与障碍物i的最优相互避碰集如下：

ORCA_uav|i＝{v|(v-(v_uav+u))·n≥0)}

ORCA_uav|i是基于速度变化量u构造的无碰撞速度集合，其几何形状为边界为直线的一个半平面，ORCA_uav|i的计算方法为找到所有与u的内积大于或等于0的速度，使得无人机选择集合内的速度时与障碍物i都是安全无碰撞的，其中，n为垂直于该半平面直线边界的的法向量，其计算方法为取u的单位向量；

对于环境感知模块获取的障碍物信息，将所有障碍物的ORCA_uav|i取交集记为∑ORCA，在x-O-y平面中生成安全速度集合ORCA＝D(0,v_max)∩∑ORCA，其中v_max为水平面内最大容许速度，D(0,v_max)＝{p|||p-0||<v_max}为所有模值小于v_max的水平面内速度集合，其为一圆形区域；

所述约束无人机的运动速度的方法为：

设置参考速度v_pref方向为自当前位置指向最终目标点，大小根据无人机平台的运动状况决定，在ORCA区域中选择到参考速度在速度平面中距离最近的速度记为v_opt，即无人机按最优相互避碰集算法计算出的最优速度。

所述约束无人机的运动速度的优选方法为：

当求出的最优速度v_opt为0时，直接将其作为新的起始点进行处理，其次考虑直接从速度障碍集中将速度推出，若当前参考速度处在速度障碍集中，则求出其到速度障碍集两侧边界的垂足，若垂足速度在其他速度障碍集中，则求出其在以相同斜率变化方向的边界上的垂足速度，即例如将首选速度推向了斜率较小一侧的速度障碍集边界得到垂足为v₁，当v₁仍处在其他速度障碍集中时，同样向斜率较小的边界一侧求出垂足速度v₂，以此类推直到速度不再处于速度障碍集中；

求出ORCA中与参考速度v_pref相差最小的速度，与直接从速度障碍集中推出的两个速度一起，选择三者中大于设定阈值且与参考速度偏差最小的速度作为最终的最优避碰速度运行设定时间，不断根据环境感知情况进行更新，以此保证速度变化较小、始终尽可能朝向目标运动且能够避免与环境障碍物发生碰撞；

生成平滑连续的运动轨迹的方法为：使用3次B样条曲线生成平滑连续的轨迹，其基函数如下所示：

最开始将初始四个控制点设置为起始点，对于每次最优速度的更新，在新最优速度的执行时间内，设置两个新的控制点，将每段单独的曲线段的时间从最优速度的更新时间t映射到u/2上，其中一个为按照原来速度运行一半执行时间即t/2到达的位置，应使其处在上一时刻生成的轨迹周围ε范围内，如果超出则取其与范围边界的交点作为一个控制点，以此加上前段整条轨迹中最后三个控制点，生成一段B样条曲线，另一个控制点为按照最优速度运行整个执行时间到达的位置，再加上之前t/2时间的曲线的后三个控制点，再生成一段轨迹。

一种未知动态环境下小型四旋翼无人机自主导航方法，该方法的步骤包括：

第一步，环境感知模块接收载视觉传感器获取的环境信息，并根据接收到的环境信息建立占据栅格地图；

第二步，环境感知模块对建立的占据栅格地图中表示障碍物的点云进行聚类和追踪，获得环境中独立障碍物信息；

第三步，环境感知模块通过卡尔曼滤波估计获得的独立障碍物的位置和运动速度，并将获得的独立障碍物的位置和运动速度提供给轨迹规划模块；

第四步，轨迹规划模块接收到环境感知模块输出的独立障碍物的位置和运动速度，并根据接收到的独立障碍物的位置和运动速度生成最优相互避碰集(ORCA)；

第五步，轨迹规划模块通过生成的最优相互避碰集(ORCA)约束无人机的运动速度，实现对障碍物的避碰；

第六步，轨迹规划模块根据无人机的运动速度利用B样条曲线生成平滑连续的运动轨迹。

有益效果

(1)本发明的方法解决了一个工程中实际存在的问题，即无人机在同时包含动态和静态障碍物的未知复杂环境下仅依靠机载传感器完成自主导航任务。

(2)本发明的方法能够处理包括动态障碍物的复杂未知场景，且实时性较好，较为轻量化；

(3)本发明的方法解决了原有最优相互避碰集(ORCA)算法在特定情况下可行域损失较大的问题，对反应性算法短视的缺点有所改善，提高了系统的鲁棒性。

(4)本发明的方法能够生成光滑连续的运动轨迹，满足无人机动力学约束，提高系统的鲁棒性和安全性。

(5)本发明的方法可以仅依赖机载相机和处理器实现全部功能，可以提高无人机智能化程度并节省硬件成本。

(6)本发明的方法采用实时观测的障碍物信息进行规划，可提高规划的安全性，权衡了任务与安全两项指标，可以使得系统在保证安全的前提下完成既定任务。

附图说明

图1为本发明相对速度障碍集与速度障碍集示意图；

图2为本发明最优相互避碰集(ORCA)示意图；

图3为本发明参考的原始ORCA算法一种典型的零解情况；

图4为本发明对原始ORCA算法改进的示意图。

图5为本发明改进后算法在图3情况下的求解结果；

图6为感知模块对环境建立的占据栅格地图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

一种用于未知动态环境下的小型四旋翼无人机自主导航系统及方法，包括：采用深度相机感知周围环境，根据获得的的深度图对环境构建占据栅格地图，根据占据栅格地图提取独立障碍物并估计其运动状态，根据环境中独立障碍物的位置和速度采用基于最优相互避碰集(ORCA)改进的算法实现无人机与环境中的动态和静态障碍物进行避碰，采用B样条曲线在避碰的基础上生成平滑连续的到达目标点的运动轨迹。

本发明在三维空间中使用多个坐标系来描述无人机位姿及环境信息，分别为世界坐标系W，机体坐标系B和相机坐标系C。

世界坐标系使用右手坐标系，固定于飞行场景中定义的水平地面，作为系统的全局坐标系。其中轴x_W平行于水平地面指向任意方向，轴z_W垂直水平地面向上，轴y_W与轴x_W和轴z_W垂直，正方向遵从右手定则。

机体坐标系的原点固定于四旋翼无人机的质心，轴x_B指向机头方向，轴z_B指向机顶方向，轴y_B与轴x_B和轴z_B垂直，正方向遵从右手定则。使用坐标变换增广矩阵表示从机体坐标系到世界坐标系的旋转平移变换。

相机坐标系需要根据相机安装的位置和图像数据的坐标系统设定，其到机体机体坐标系的坐标变换增广矩阵用表示，其到世界坐标系的坐标变换增广矩阵为

本发明的系统主要包括环境感知模块和轨迹规划模块，环境感知模块通过输入双目相机获得的深度图信息和无人机的位姿信息，对环境建立占据栅格地图，对占据栅格点云进行聚类提取出独立障碍物，对前后帧间的独立障碍物进行关联追踪，对每个独立障碍物的几何中心位置变化建立卡尔曼滤波器，获得其运动状态的估计。轨迹规划模块采用基于最优相互避碰集(ORCA)改进的算法实现对动态和静态障碍物的避障，通过加入直接将速度从速度障碍集中推出的操作，克服原算法在构造凸优化问题时可行域损失过多的问题，并使用B样条曲线在避障的基础上生成平滑连续的运动轨迹。

环境感知模块：

通过深度相机获取环境的深度信息，实际世界中相机坐标系下点P(X_C,Y_C,Z_C)与成像平面中像素p(x,y)的对应关系如下式所示，等式左侧为相机成像平面齐次坐标，矩阵K称为内参数矩阵，f_x和f_y表示像素相对于成像平面尺度的缩放，c_x和c_y表示像素坐标系原点相对于光轴与像平面交点的平移。

空间中一点p在左目和右目像平面成像分别为P_L和P_R，两相机的光心的距离称为基线(Baseline)，记作B。根据三角形相似原理，p点实际深度Z满足其中d为左右目像点的横坐标之差。根据定位系统的位姿估计，将深度相机获取的环境中深度点的坐标P_C从相机坐标系C下转换到世界坐标系W下。

利用光线追踪法建立环境的占据栅格地图，为了方便数据的存储，本发明中使用一个一维数组存储各个栅格的占据概率。栅格的编号为其X坐标+(Y坐标×X方向的栅格数)+(Z坐标×X方向的栅格数×Y方向的栅格数)。对占据栅格采用贝叶斯估计方法，估计其占据的概率，以减小传感器噪声的影响，具体方法为对于占据栅格地图中的体素栅格m_i，其初始时被占据的概率用P(m_i)表示，空闲的概率为P(m_i)与/>之和为1。在未被观测到的情况下，占据和空闲的概率相等均为0.5。

设定每次观测的置信概率P(m_i|z_t)为考虑设备测量精度的定值，定义对数置信度l_t,i、反演观测l_inv,i和先验概率l₀如下：

根据贝叶斯公式将栅格占据情况随观测的更新迭代转化为一次加法运算为l_t,i＝l_t-1,i+l_inv,i-l₀。当对数置信度超过一定值，则将栅格设置为占据，否则设置为空闲。每隔一定时间将所有栅格的历史信息清楚重新初始化，以适应动态环境变化减小延迟。

将环境栅格的占据情况存储为点云格式。对获取的表示栅格占据信息的点云使用基于密度的DBSCAN方法进行聚类。该聚类方法的核心思想是基于环境深度点云在障碍物表面与无障碍物的空间中分布密度的差异，把空间分布中属于同一障碍物的点云分割为一个簇。具体方法为遍历环境点云中的每一个点作为搜索的中心点，搜索与其空间距离小于设定值R的点，若其搜索范围内点云的点数大于设定的阈值，则说明该点在表示一个障碍物的簇的内部，反之则说明其在障碍物点云的边缘。再将该点和其搜索范围内的周围点放入一个队列中，逐个以队列中的点为中心点进行搜索判断。在搜索前调先对环境点云构建kd树以加速邻域搜索，降低算法的时间复杂度以保证算法的实时性。聚类后对每个点云簇构建AABB包围盒并计算其几何中心，以便对障碍物进行追踪和速度求解。

将当前帧中各个点云簇包围盒的几何中心使用全局最近邻(GNN)算法与前一帧中表示同一个障碍物点云簇包围盒的几何中心进行关联。如果后帧中的簇的包围盒中心点与前帧中的所有中心点中全局最近邻居的距离大于设定的阈值，或者其最近邻点已经被更近的后帧中的中心点关联，则认为后帧中该簇与前帧关联失败。以此追踪前后帧间表示同一个障碍物的点云簇。

完成关联后对于点云簇使用卡尔曼滤波方法估计其运动速度。由于感知模块对环境信息的更新速率较高，可认为在环境信息更新的很短的时间段内障碍物运动速度保持不变，故而通过保守运动模型对障碍物的运动状态进行估计。障碍物在t_k时刻的位置x_k和速度的状态方程如下所示：

其中Δt＝t_k-t_k-1。在t_k时刻对障碍物运动状态的估计/>与测量值z_k如下所示：

其中v为属于高斯分布Ν(0,Q)的过程噪声，w为属于高斯分布Ν(0,R)的测量噪声。构造卡尔曼滤波器按照如下方式迭代：

K_k＝P_p,kH^T(HP_p,kH^T+R)^-1

每一个簇都需要一个上述的卡尔曼滤波器进行状态估计，如果后一帧中的簇与前一帧中的簇关联成功，则后一帧的簇继承前一帧对应簇的卡尔曼滤波器进行迭代。若关联失败则对后一帧簇建立新的卡尔曼滤波器。

轨迹规划模块：

对于小型旋翼无人机的轨迹规划要求主要包括两个方面，首先是避免与障碍物发生碰撞从而安全到达目标点，其次是轨迹尽可能平滑连续以减少能量消耗和保证动力学约束。本发明中轨迹规划模块利用基于最优相互避碰集(ORCA)的改进方法实现与环境中的动静态障碍物避碰，在避碰的基础上生成平滑的B样条曲线轨迹，以完成在动态复杂环境下安全到达指定目标点的自主飞行任务。考虑二维平面上的规划问题，将障碍物建模为一定半径的圆形。对于障碍物i，其与无人机的相对速度障碍集为图1中虚线内阴影部分，即假设障碍物的速度保持不变，当无人机与障碍物i的相对速度在该区域内时，在经过足够的时间后两者必会发生碰撞。将相对速度障碍集中的速度加上障碍物的速度，即得到速度障碍集如图1中灰色部分所示。当无人机的速度处在这个区域中时，一定时间后两者必会发生碰撞。

定义对于障碍物i相对于无人机的速度障碍集为VO_uav|i，记为无人机会与障碍物发生碰撞的速度障碍集的边界，当无人机速度在集合VO_uav|i中，需要将其从速度障碍集中推出。定义其到/>中最近的距离为/> argmin表示取函数的最小值，||·||表示取模运算。定义无人机与障碍物i的最优相互避碰集如下：

ORCA_uav|i＝{v|(v-(v_uav+u))·n≥0)}

ORCA_uav|i是基于速度变化量u构造的无碰撞速度集合，其几何形状为边界为直线的一个半平面。ORCA_uav|i的计算方法为找到所有与u的内积大于或等于0的速度，使得无人机选择集合内的速度时与障碍物i都是安全无碰撞的。其中，n为垂直于该半平面直线边界的的法向量，其计算方法为取u的单位向量。最终生成的ORCA_uav|i为图2所示的阴影区域。

对于环境感知模块获取的障碍物信息，将所有障碍物的取交集记为∑ORCA，在x-O-y平面中生成安全速度集合ORCA＝D(0,v_max)∩∑ORCA，其中v_max为水平面内最大容许速度，D(0,v_max)＝{p|||p-0||<v_max}为所有模值小于v_max的水平面内速度集合，其为一圆形区域，表示水平面中无人机所有可供选择的速度。

设置参考速度v_pref方向为自当前位置指向最终目标点，大小根据无人机平台的运动状况决定，在ORCA区域中选择到参考速度在速度平面中距离最近的速度，以此保证速度尽可能朝向目标点。

但ORCA在多个速度障碍集叠加的情况下提供的速度选择范围较小，容易导致解的质量较差，例如会选择绕行较远的距离和陷入零解。如图3所示，图中箭头v_pref和v_now分别表示首选速度和当前速度。在这种情况下ORCA边界会选择下方的两直线，直接导致距离参考速度最近的速度接近0速度，进而导致一直维持0解而停止不动。

针对类似情况本设计对ORCA算法进行了改进，首先当速度为0时，直接将其作为新的轨迹起始点进行处理。其次考虑直接从速度障碍集中将速度推出，若当前参考速度处在速度障碍集a中，则求出其到速度障碍集两侧边界的垂足，若垂足速度在其他速度障碍集中，则求出其在以相同斜率变化方向的边界上的垂足速度。如图4所示，从v_uav到v₁是推向了斜率较小一侧的速度障碍集边界，因此当v₁仍处在其他速度障碍集中时，同样向斜率较小的边界一侧求出垂足速度v₂。以此将速度推出速度障碍集，同时保证了速度大小尽量不因斜率变化而产生大的变化。

求出ORCA中与参考速度v_pref相差最小的速度，与直接从速度障碍集中推出的两个速度一起，选择三者中大于一定阈值且与参考速度偏差最小的速度作为最终的最优避碰速度运行一定时间，不断根据环境感知情况进行更新，以此保证速度变化较小、始终尽可能朝向目标运动且能够避免与环境障碍物发生碰撞。如图5所示的情况中，如果采用传统的ORCA方法，会产生和图3相同的零解问题，而本设计采用的方法可以将首选速度直接推出速度障碍集，选择如图5中粗线所示的速度作为最优速度运行形成轨迹，能够有限避免陷入0解或产生不必要绕行的情况，解决了单纯使用ORCA算法导致的短视问题。

无人机运动过程中，应保证轨迹的连续性与平滑性。B样条曲线是贝塞尔曲线的一种一般化推广，对于k阶均匀B样条曲线，其定义在均匀时间间隔的节点向量K＝[s₀,s₁,...,s_M+k]上，表达式如下式所示，其中c_i为称为控制点，分段非零基函数满足下式。

在本发明中，对于避碰模块获得的最优速度，由于对障碍物进行了合理的膨胀处理，因此可认为在最优速度周围一定范围内的速度均能达到避碰的要求。将轨迹限制在按照最优速度运动相应时间的参考轨迹周围ε范围内，同时保证位置、速度、加速度的连续性即可。

本发明采用三次B样条曲线分段生成轨迹。其基函数如下式所示，其中m为辅助参数变量。

为保证轨迹通过初始位置，将初始四个控制点为起始点。对于每次最优速度的更新，在新最优速度的执行时间内，设置两个新的控制点，将每段单独的曲线段的时间从最优速度的更新时间t映射到m/2上，其中一个为按照原来速度运行一半执行时间即t/2到达的位置，应使其处在参考轨迹周围设定的ε范围内，如果超出则取其与范围边界的交点作为一个控制点，以此加上前段整条轨迹中最后三个控制点，生成一段B样条曲线。另一个控制点为按照最优速度运行整个执行时间到达的位置，再加上之前m/2时间的曲线的后三个控制点，再生成一段轨迹。根据B样条曲线的凸包性，即可保证每段轨迹均处于参考轨迹周围ε范围内，同时可以保证生成的轨迹速度变化不会过大，轨迹平滑连续且能避免碰撞。

对提出的自主导航系统进行了仿真和实物验证。经验证，算法在具有多个动态和静态障碍物的环境中能够有效实现障碍物的避碰，能够规划出平滑连续的轨迹。

在仿真实验中，在障碍物前表面和上表面按照10cm的间隔生成点云，模拟深度相机获取的深度点云，将其提供给动态环境感知模块，动态环境感知模块按照前述的方法基于占据栅格地图生成速度估计。仿真场景下共有三个静态障碍物和动态障碍物，动态障碍物的运动速度为1.5m/s，在该场景中，本发明的动态环境感知算法和动态规划算法能够可靠地进行障碍物检测、追踪和避碰，最终生成安全无碰撞的到达指定地点的轨迹。

在实际实验中，无人机的目标点为无人机当前位置正前方5米处，而在起点和终点的连线上布置有静态障碍物，动态障碍物运动平均速度为1.25m/s。上位机中系统运行效果图如图6所示，感知模块对环境建立了正确的占据栅格地图，同时提取出了独立的障碍物，并对其进行了关联和运动估计。无人机在绕过正前方静态障碍物后，对于从右向左运动的动态障碍物产生了向左绕行的规避动作，最终安全到达了目标点，无人机轨迹规划的参考运动速度为1.5m/s。在此场景中，本发明能够保证无人机避免与环境中的动态和静态目标发生碰撞，实现动态环境下自主导航的任务，系统的感知和规划频率频率达到20Hz。

仿真和实际实验结果说明，该自主导航系统能够稳定运行，能够实现预期的功能。

以上所述的仅为本发明的较佳实施例而已，本发明不仅仅局限于上述实施例，凡在本发明的精神和原则之内所做的局部改动、等同替换、改进等均应包含在本发明的保护范围之内。

Claims (7)

1.一种未知动态环境下小型四旋翼无人机自主导航系统，其特征在于该导航系统包括环境感知模块和轨迹规划模块；

所述环境感知模块用于接收载视觉传感器获取的环境信息，并根据接收到的环境信息建立占据栅格地图，然后对建立的占据栅格地图中表示障碍物的点云进行聚类和追踪，获得环境中独立障碍物信息，再通过卡尔曼滤波估计获得的独立障碍物的位置和运动速度，并将获得的独立障碍物的位置和运动速度提供给轨迹规划模块用以生成安全无碰撞地轨迹；

所述的轨迹规划模块用于接收环境感知模块输出的独立障碍物的位置和运动速度，并根据接收到的独立障碍物的位置和运动速度生成最优相互避碰集，再通过生成的最优相互避碰集约束无人机的运动速度，实现对障碍物的避碰，根据无人机的运动速度利用B样条曲线生成平滑连续的运动轨迹；

生成最优相互避碰集的方法为：

将障碍物建模为设定半径的圆形，定义对于障碍物i相对于无人机的速度障碍集为VO_uav|i，假设障碍物的速度保持不变，则若无人机取VO_uav|i中的速度，则一定时间后二者一定会发生碰撞，记为无人机会与障碍物发生碰撞的速度障碍集的边界，当无人机速度在集合VO_uav|i中，定义无人机速度到/>中最近的距离为argmin表示取函数的最小值，||·||表示取模运算，定义无人机与障碍物i的最优相互避碰集如下：

ORCA_uav|i＝{v|(v-(v_uav+u))·n≥0)}

ORCA_uav|i是基于u构造的无碰撞速度集合，其几何形状为边界为直线的一个半平面，ORCA_uav|i的计算方法为找到所有与u的内积大于或等于0的速度，使得无人机选择集合内的速度时与障碍物i都是安全无碰撞的，其中，n为垂直于该半平面直线边界的的法向量，其计算方法为取u的单位向量；

对于环境感知模块获取的障碍物信息，将所有障碍物的ORCA_uav|i取交集记为∑ORCA，在x-O-y平面中生成安全速度集合ORCA＝D(0,v_max)∩∑ORCA，其中v_max为水平面内最大容许速度，D(0,v_max)＝{p|||p-0||<v_max}为所有模值小于v_max的水平面内速度集合，其为一圆形区域。

2.根据权利要求1所述的一种未知动态环境下小型四旋翼无人机自主导航系统，其特征在于：

所述的建立占据栅格地图的方法具体为：使用光线投影法查找需要更新的栅格，然后使用贝叶斯估计方法估计栅格占据的概率。

3.根据权利要求2所述的一种未知动态环境下小型四旋翼无人机自主导航系统，其特征在于：

使用贝叶斯估计方法估计栅格占据的概率的方法为：

对于占据栅格地图中的体素栅格m_i，初始时被占据的概率用P(m_i)表示，空闲的概率为P(m_i)与/>之和为1，在未被观测到的情况下，占据和空闲的概率相等均为0.5；

设定每次观测z_t的置信概率P(m_i|z_t)为考虑设备测量精度的定值，定义对数置信度l_t,i、反演观测l_inv,i和先验概率l₀如下：

根据贝叶斯公式将栅格占据情况随观测的更新迭代转化为一次加法运算为l_t,i＝l_t-1,i+l_inv,i-l₀。

4.根据权利要求1-3任一所述的一种未知动态环境下小型四旋翼无人机自主导航系统，其特征在于：

所述进行聚类的方法为：在占据栅格地图的基础上，使用DBSCAN聚类方法对环境信息中的从属于独立障碍物的点云进行聚类分割；

所述的追踪的方法为：对提取的独立障碍物构建AABB包围盒并计算几何中心，通过搜索前后帧之间距离最近的几何中心，关联前后帧中表示同一个障碍物的点云簇。

5.根据权利要求1所述的一种未知动态环境下小型四旋翼无人机自主导航系统，其特征在于：

所述约束无人机的运动速度的方法为：

设置参考速度v_pref方向为自当前位置指向最终目标点，大小根据无人机平台的运动状况决定，在最优相互避碰集区域中选择到参考速度在速度平面中距离最近的速度记为v_opt，即无人机按最优相互避碰集算法计算出的最优速度。

6.根据权利要求5所述的一种未知动态环境下小型四旋翼无人机自主导航系统，其特征在于：

所述约束无人机的运动速度的优选方法为：

当求出的最优速度v_opt为0时，直接将其作为新的起始点进行处理，其次考虑直接从速度障碍集中将速度推出，若当前参考速度处在速度障碍集中，则求出其到速度障碍集两侧边界的垂足，若垂足速度在其他速度障碍集中，则求出其在以相同斜率变化方向的边界上的垂足速度；

将首选速度推向了斜率较小一侧的速度障碍集边界得到垂足为v₁，当v₁仍处在其他速度障碍集中时，同样向斜率较小的边界一侧求出垂足速度v₂，以此类推直到速度不再处于速度障碍集中；

求出最优相互避碰集中与参考速度v_pref相差最小的速度，与直接从速度障碍集中推出的两个速度一起，选择三者中大于设定阈值且与参考速度偏差最小的速度作为最终的最优避碰速度运行设定时间，不断根据环境感知情况进行更新，以此保证速度变化较小、始终尽可能朝向目标运动且能够避免与环境障碍物发生碰撞。

7.一种未知动态环境下小型四旋翼无人机自主导航方法，其特征在于该方法的步骤包括：

第一步，环境感知模块接收载视觉传感器获取的环境信息，并根据接收到的环境信息建立占据栅格地图；

第二步，环境感知模块对建立的占据栅格地图中表示障碍物的点云进行聚类和追踪，获得环境中独立障碍物信息；

第三步，环境感知模块通过卡尔曼滤波估计获得的独立障碍物的位置和运动速度，并将获得的独立障碍物的位置和运动速度提供给轨迹规划模块；

第四步，轨迹规划模块接收到环境感知模块输出的独立障碍物的位置和运动速度，并根据接收到的独立障碍物的位置和运动速度生成最优相互避碰集；

生成最优相互避碰集的方法为：

将障碍物建模为设定半径的圆形，定义对于障碍物i相对于无人机的速度障碍集为VO_uav|i，假设障碍物的速度保持不变，则若无人机取VO_uav|i中的速度，则一定时间后二者一定会发生碰撞，记为无人机会与障碍物发生碰撞的速度障碍集的边界，当无人机速度在集合VO_uav|i中，定义无人机速度到/>中最近的距离为argmin表示取函数的最小值，||·||表示取模运算，定义无人机与障碍物i的最优相互避碰集如下：

ORCA_uav|i＝{v|(v-(v_uav+u))·n≥0)}

ORCA_uav|i是基于u构造的无碰撞速度集合，其几何形状为边界为直线的一个半平面，ORCA_uav|i的计算方法为找到所有与u的内积大于或等于0的速度，使得无人机选择集合内的速度时与障碍物i都是安全无碰撞的，其中，n为垂直于该半平面直线边界的的法向量，其计算方法为取u的单位向量；

对于环境感知模块获取的障碍物信息，将所有障碍物的ORCA_uav|i取交集记为∑ORCA，在x-O-y平面中生成安全速度集合ORCA＝D(0,v_max)∩∑ORCA，其中v_max为水平面内最大容许速度，D(0,v_max)＝{p|||p-0||<v_max}为所有模值小于v_max的水平面内速度集合，其为一圆形区域；

第五步，轨迹规划模块通过生成的最优相互避碰集约束无人机的运动速度，实现对障碍物的避碰；

第六步，轨迹规划模块根据无人机的运动速度利用B样条曲线生成平滑连续的运动轨迹。