CN119290000B 一种室内无人机定位导航路径规划方法及系统 （洲际联合超伦科技（北京）有限公司）

司一种室内无人机定位导航路径规划方法及本发明公开了一种室内无人机定位导航路像特征点建立图像特征点的时序对应关系数据人机飞行路径与视觉半直接法对无人机的位姿2对无人机安装双目相机设备与惯性测量单元传感器，对所述无对所述双目相机图像进行特征提取得到图像特征点，基于图像特征利用所述图像特征点的时序对应关系数据库与所述惯性测量单元数据进行估计无人通过所述图像特征点构建3D空间，利用所述图像特征点对所述构建基于所述无人机飞行路径与视觉半直接法对所述无人机的位姿通过所述图像特征点构建3D空间，利用所述图像特征点对所述构建对时序对应关系数据库中的连续图像帧之间的图像特征点进行构建3对每个栅格区域内的点云进行计算点云密集度，通过所述点云并根据3D空间内每个栅格区域内的点云密集度的栅格占据概率进行构建局部环境地对无人机输入起点坐标以及目标位置坐标；通过RRT算法对所述局部环境地图进行采通过所述环境特征与所述栅格占据概率进行计算无人机的自适应步长和确定扩展方通过自适应步长与扩展方向进行计算采样点的最优路径，生成路径迭代得到一个路径节点序列，将所述路径节点序列中的路径节点根据RRT算法进行平滑处通过RRT算法对所述局部环境地图进行采样规划路径，每次采样根据3D空间内的栅格根据所述局部环境地图中的每个栅格区域进行计算采样概率密度，233基于所述采样概率密度，计算局部环境地图中的每个栅格区域g的所述当前栅格区域坐标到目标位置坐标的欧式距离G的计算利用RRT算法根据目标引导项G(x,y,z)再次对序列表E中的栅格区域的候选采样点进根据所述局部环境地图的环境特征与局部环境地图内的每个栅格区域的栅格占据概无人机与采样点目标之间的距离与所述预设安全距离计算所述无人机与采样点目标的步通过所述预设安全距离对无人机进行计算加速通过所述当前无人机与采样点目标之间的距离与预设安全距离计算无人机的姿态调通过所述无人机的加速度因子与所述姿态调整值对步进距离进行调整4根据中心差分法对所述栅格区域的栅格占据概率进行偏导数计算根据所述当前栅格区域坐标的采样点与所述目标位置坐标进行计通过所述环境特征梯度与栅格区域的占据概率梯度以及目标方向进行计算通过所述自适应步长与所述扩展方向对所述当前栅格区域坐标的采样点进行生成新预设占据概率阈值v，对所述新的栅格区域坐标的采样点的栅格占据概率进行占据概若否，则新的栅格区域坐标的采样点判定为处于局部环境通过所述自适应步长和所述扩展方向进行迭代生成有效的采样点，5.根据权利要求4所述一种室内无人机定位导航对所述路径节点序列中的路径节点g1与路径节点g3进行直线连接，所述路径线段上的每一个采样点进行判断栅格区域的栅格占据概率是否小于占据概率阈若否，则该路径节点g1与路径节点g3的路径线段上的采样点若是，则对该路径节点g1与路径节点g3的路径线段进行保留，得到新的路对更新后的保留的路径线段的路径节点进行作为新的起点继续上述步骤的递归操作，6.一种室内无人机定位导航路径规划系统，用于实现权利要求1_5任一项所述的一种数据采集模块用于对无人机安装双目相机设备与惯性测量单5数据库模块用于对所述双目相机图像进行特征提取得到图像特估计位姿模块用于利用所述图像特征点的时序对应关系数据库与所述惯性测量单元飞行路径模块用于通过所述图像特征点构建3D空间，利用所优化位姿模块用于基于所述无人机飞行路径与视觉半直接法对所述无人机的位姿进6[0015]利用所述图像特征点的时序对应关系数据库与所述惯性测量单元数据进行估计7在物体；通过每个栅格区域的占据概率判断出了哪个栅格区域可能存在更高的物体概率，RRT算法对局部环境地图进行采样规划路径，每次采样根据3D空间内的栅格区域的栅格占通过自适应步长与扩展方向对当前栅格区域坐标的采样点进行生成新的栅格区域坐标的8[0041]通过双目相机设备与惯性测量单元传感器进行获取无人机的双目相机图像与惯9[0044]需要说明的是，对双目相机图像可以利用FAST角点检测算法进行提取图像特征从金字塔顶层开始匹配比较左图像金字塔和右图像金字塔中的图像特征点的位置坐标和如在第n帧图像中提取的图像特征点它可以与前后几帧图像中的相同图像特征点进行关[0056]S41：对时序对应关系数据库中的连续图像帧之间的图像特征点进行构建3D空间（即三维空间对3D空间中的每一个图像特征点进行确定像素坐标（即3D空间坐标利用像素坐标生成对应的点云；计算栅格占据概率，通过每个栅格区域的栅格占据概率进行判断该栅格区域是否存在物体[0061]并根据3D空间内每个栅格区域内的点云密集度的栅格占据概率进行构建局部环[0064]通过每个栅格区域的占据概率判断出了哪个栅格区域可的终点位置通过RRT算法对局部环境地图进行采样规划路径，每次采样根据3D空间内的栅格区域的栅格占据概率与目标位置坐标进行选择[0070]S45：通过环境特征与栅格占据概率进行计算无人机的自适应步长和确定扩展方整个迭代得到一个路径节点序列，将路径节点序列中的路径节点根据RRT算法进行平滑处的计算通常会结合环境特征和栅格占据概率，以确保路径尽可能避开障碍物并且是最短[0073]RRT算法通过随机采样来选择树的扩展方向和新的节点位置（即新的采样点位择出最优的路径的路径节点，作为无人机的最终飞行路径并评估哪些采样点是最优的径，每次采样根据3D空间内的栅格区域的栅格占据概率与目标位置坐标进行选择采样点，;;[0094]若欧式距离G越小，表示栅格区域离目标位置坐标越近，采样点的优先级应当更[0100]需要说明的是，整个方案的目标是利用采样概率密度来引导选择出一个合适的点作为路径规划或地图构建栅格区域的栅格占据概率进行计算局部环境地图的搜索空间的自适应无人机与采样点目标之间的距离与预设安全距离计算无人机与采样点[0104]通过当前无人机与采样点目标之间的距离与预设安全距离计算无人机的姿态调征梯度与栅格区域的占据概率梯度以及目标方向进行计算;;;[0128]通过当前无人机与采样点目标之间的距离与预设安全距离计算无人机的姿态调无人机在接近障碍物时的步进距离如果障碍物靠得比较近则步进距离会减小避免碰无人机在接近障碍物时的步进距离如果障碍物靠得比较近则步进距离会减小避免碰G-g=(x,-yc-yzc-)[0140]G-g=(x,-yc-yzc-)[0143]通过环境特征梯度与栅格区域的占据概率梯度以及目标：；VDVD[0148]表示为当前栅格区域坐标的采样点到目标位置坐标的目标[0154]需要说明的是，预设占据概率阈值v判断新的栅格区域坐标的采样点的栅格占据所有最佳路径的路径节点进行串联形成无人机[0164]如果直线线段上某个采样点的栅格占据概率O(q)大于或等于占据阈值v，则直线[0165]如果直线线段上所有采样点的栅格占据概率O(q)都低于占据概率阈值v，则说明与路径节点g3的保留路径线段进行更新，对路径节点g1与路径节点g4的路径线段进行保[0168]S4593：对更新后的保留的路径线段的路径节点进行作为新的起点继续步骤占据概率较高的物体概率从而构建局部环境地图；并进一