CN116954265B - 局部运动轨迹的重规划方法、装置和电子设备 - Google Patents
局部运动轨迹的重规划方法、装置和电子设备 Download PDFInfo
- Publication number
- CN116954265B CN116954265B CN202311213020.2A CN202311213020A CN116954265B CN 116954265 B CN116954265 B CN 116954265B CN 202311213020 A CN202311213020 A CN 202311213020A CN 116954265 B CN116954265 B CN 116954265B
- Authority
- CN
- China
- Prior art keywords
- obstacle
- point cloud
- collision detection
- local
- map
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 55
- 238000001514 detection method Methods 0.000 claims description 92
- 238000010276 construction Methods 0.000 claims description 25
- 238000004422 calculation algorithm Methods 0.000 claims description 19
- 230000015654 memory Effects 0.000 claims description 17
- 238000013507 mapping Methods 0.000 claims description 9
- 238000004590 computer program Methods 0.000 claims description 6
- 238000010586 diagram Methods 0.000 description 10
- 230000008569 process Effects 0.000 description 7
- 230000006870 function Effects 0.000 description 6
- 230000008447 perception Effects 0.000 description 6
- 238000012545 processing Methods 0.000 description 6
- 238000004891 communication Methods 0.000 description 4
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 2
- 241000251468 Actinopterygii Species 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
本发明提供了一种局部运动轨迹的重规划方法、装置和电子设备，属于无人机的技术领域，本发明的局部运动轨迹的重规划方法中，在局部运动轨迹重规划的同时，又增加了碰撞检测，如此，能够根据实时的碰撞检测结果指导预先规划的运动轨迹的执行，可以有效实现无人机的局部运动轨迹重规划，进而实现无人机的有效自主避障。
Description
技术领域
本发明涉及无人机的技术领域，尤其是涉及一种局部运动轨迹的重规划方法、装置和电子设备。
背景技术
无人机以其重量轻、体积小、飞行灵活、价格便宜等优点，被广泛应用于电力巡检、资源勘探、城市安全、航空拍摄领域。在无人机工业级应用中，现有技术作业前往往需要进行三维重建及航线规划，耗时费力；如果执行随机地图任务，则需要专业操作人员全程控制无人机到达指定地点，受操作者视程及专业素质的影响大，这些都极大的限制了无人机的实用性和灵活性。同时，随着无人机的普及，一些安全问题也随之而来，如，无人机作业时，经常会遇到突发的威胁或障碍；又如无人机和操控人员距离太远时，仅仅通过操控人员操控无人机进行避障是不理想的；甚至在操作人员的可控制范围内，也可能会因为失误而导致无人机发生碰撞。因此无人机需要有较强的自主避障能力。完善实用的自主避障系统能够避免因操作失误而造成的财产损失和人身安全危险，是无人机技术的重要研究方向之一。
目前，无人机局部轨迹重规划方法，综合考虑了障碍物、飞行动力学、光滑性对无人机飞行轨迹的影响，上述局部轨迹重规划方法通过比较碰撞轨迹与无碰撞引导路径，得到惩罚函数中的碰撞项，无需事先构建地图，有效节约了机载电脑的时空资源，提升了路径规划与避障效率。
无人机局部轨迹重规划方法在局部路径规划上具有良好的性能，但其对不准确的感知和复杂环境的适应性还需增强。当深度感知不准确时，其会基于不准确的感知信息规划路径，进而规划得到的路径会导致碰撞；或者，无人机到障碍物前感知才发现周围有障碍物，此时局部轨迹重规划认为无人机在障碍物中，无人机会悬停进而无法到达指定航点。
综上，如何有效实现无人机的局部运动轨迹重规划，进而实现无人机的有效自主避障成为目前亟需解决的技术问题。
发明内容
有鉴于此，本发明的目的在于提供一种局部运动轨迹的重规划方法、装置和电子设备，以缓解现有技术无法有效实现无人机的局部运动轨迹重规划，进而无法实现无人机的有效自主避障的技术问题。
第一方面，本发明实施例提供了一种局部运动轨迹的重规划方法，包括：
实时获取无人机周围的障碍物点云和所述无人机的位姿；
根据所述障碍物点云和所述位姿进行局部地图构建，得到局部格点地图；
接收到遥控器通道信息或航点信息后，根据所述局部格点地图和所述位姿进行碰撞检测和局部运动轨迹重规划，得到碰撞检测结果和运动轨迹；
若根据所述碰撞检测结果确定所述无人机与障碍物距离小于预设距离阈值，则控制所述无人机悬停，不执行所述运动轨迹对应的轨迹控制指令，直至碰撞检测得到障碍物清除，再执行所述运动轨迹对应的轨迹控制指令，直至到达对应的航点。
进一步的，根据所述障碍物点云和所述位姿进行局部地图构建，包括：
去除所述障碍物点云中的地面点云，得到去除地面点云后的障碍物点云；
对所述去除地面点云后的障碍物点云进行下采样，得到下采样后的障碍物点云；
根据所述位姿将所述下采样后的障碍物点云映射至空白格点地图，并对映射至空白格点地图的障碍物点云进行垂直方向的障碍物膨胀，得到所述局部格点地图。
进一步的，去除所述障碍物点云中的地面点云，包括：
采用预设的去除地面点云算法去除所述障碍物点云中的地面点云，得到所述去除地面点云后的障碍物点云。
进一步的，采用预设的去除地面点云算法去除所述障碍物点云中的地面点云，包括：
根据在所述障碍物点云中随机选择的预设数量个点云拟合平面模型；
计算各所述障碍物点云到所述平面模型的距离，并基于所述距离在所述障碍物点云中确定所述平面模型的内点；
若所述内点的数量超过预设阈值，则记录所述平面模型的参数；
重复执行上述步骤预设次数，得到多个所述平面模型的参数；
根据各所述平面模型对应的内点的数量，在多个所述平面模型中确定目标平面模型；
计算各所述障碍物点云到所述目标平面模型的距离，并基于所述距离在所述障碍物点中确定非地面点云，进而得到所述去除地面点云后的障碍物点云。
进一步的，根据所述局部格点地图和所述位姿进行碰撞检测，包括：
基于所述位姿采用预设的碰撞检测算法在所述局部格点地图上进行碰撞检测，得到所述碰撞检测结果，其中，所述碰撞检测结果包括：障碍物距离和方位。
进一步的，基于所述位姿采用预设的碰撞检测算法在所述局部格点地图上进行碰撞检测，包括：
将所述位姿所属的局部格点地图中的体素作为当前体素，并确定所述当前体素的坐标；
初始化步长变量，其中，所述步长变量表示遍历相邻体素的方向；
从所述当前体素的坐标开始沿着所述步长变量的方向计算射线穿过当前体素的各方向的t值和射线移动一个体素单元各方向需要增加的t值；
循环比较所述射线穿过当前体素的各方向的t值，并从中确定最小的t值；
根据所述最小的t值对应的方向移动一个体素，得到新的当前体素；
将对应的方向的t值增加对应方向需要增加的t值；
对新的所述当前体素和所述局部格点地图进行交点检测；
根据交点检测结果确定所述碰撞检测结果。
第二方面，本发明实施例还提供了一种局部运动轨迹的重规划装置，包括：
获取单元，用于实时获取无人机周围的障碍物点云和所述无人机的位姿；
局部地图构建单元，用于根据所述障碍物点云和所述位姿进行局部地图构建，得到局部格点地图；
碰撞检测和轨迹重规划单元，用于接收到遥控器通道信息或航点信息后，根据所述局部格点地图和所述位姿进行碰撞检测和局部运动轨迹重规划，得到碰撞检测结果和运动轨迹；
控制单元，用于若根据所述碰撞检测结果确定所述无人机与障碍物距离小于预设距离阈值，则控制所述无人机悬停，不执行所述运动轨迹对应的轨迹控制指令，直至碰撞检测得到障碍物清除，再执行所述运动轨迹对应的轨迹控制指令，直至到达对应的航点。
进一步的，所述局部地图构建单元还用于：
去除所述障碍物点云中的地面点云，得到去除地面点云后的障碍物点云；
对所述去除地面点云后的障碍物点云进行下采样，得到下采样后的障碍物点云；
根据所述位姿将所述下采样后的障碍物点云映射至空白格点地图，并对映射至空白格点地图的障碍物点云进行垂直方向的障碍物膨胀，得到所述局部格点地图。
第三方面，本发明实施例还提供了一种电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述第一方面任一项所述的方法的步骤。
第四方面，本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有机器可运行指令，所述机器可运行指令在被处理器调用和运行时，所述机器可运行指令促使所述处理器运行上述第一方面任一项所述的方法。
在本发明实施例中，提供了一种局部运动轨迹的重规划方法，包括：实时获取无人机周围的障碍物点云和无人机的位姿；根据障碍物点云和位姿进行局部地图构建，得到局部格点地图；接收到遥控器通道信息或航点信息后，根据局部格点地图和位姿进行碰撞检测和局部运动轨迹重规划，得到碰撞检测结果和运动轨迹；若根据碰撞检测结果确定无人机与障碍物距离小于预设距离阈值，则控制无人机悬停，不执行运动轨迹对应的轨迹控制指令，直至碰撞检测得到障碍物清除，再执行运动轨迹对应的轨迹控制指令，直至到达对应的航点。通过上述描述可知，本发明的局部运动轨迹的重规划方法中，在局部运动轨迹重规划的同时，又增加了碰撞检测，如此，能够根据实时的碰撞检测结果指导预先规划的运动轨迹的执行，可以有效实现无人机的局部运动轨迹重规划，进而实现无人机的有效自主避障，缓解了现有技术无法有效实现无人机的局部运动轨迹重规划，进而无法实现无人机的有效自主避障的技术问题。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种局部运动轨迹的重规划方法的流程图；
图2为本发明实施例提供的系统结构示意图；
图3为本发明实施例提供的飞行状态控制流程示意图；
图4为本发明实施例提供的一种局部运动轨迹的重规划装置的示意图；
图5为本发明实施例提供的一种电子设备的示意图。
具体实施方式
下面将结合实施例对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
现有技术无法有效实现无人机的局部运动轨迹重规划，进而无法实现无人机的有效自主避障。
基于此，本发明的局部运动轨迹的重规划方法中，在局部运动轨迹重规划的同时，又增加了碰撞检测，如此，能够根据实时的碰撞检测结果指导预先规划的运动轨迹的执行，可以有效实现无人机的局部运动轨迹重规划，进而实现无人机的有效自主避障。
为便于对本实施例进行理解，首先对本发明实施例所公开的一种局部运动轨迹的重规划方法进行详细介绍。
实施例一：
根据本发明实施例，提供了一种局部运动轨迹的重规划方法的实施例，需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
图1是根据本发明实施例的一种局部运动轨迹的重规划方法的流程图，如图1所示，该方法包括如下步骤：
步骤S102，实时获取无人机周围的障碍物点云和无人机的位姿；
在本发明实施例中，无人机上设置有机载感应器和机载电脑，对应的系统结构图如图2所示。机载感应器主要包括：四目鱼眼（其中包括全向感知算法）和IMU，机载电脑中包括：局部建图模块、定位模块、运动规划模块和控制模块，各模块之间通信基于ROS架构。
在上述系统运行时，系统（具体为局部建图模块）实时获取无人机周围的障碍物点云和无人机的位姿，上述障碍物点云为基于四目鱼眼的全向感知算法对无人机周围的障碍物进行感知得到的，上述位姿具体为定位模块融合多源数据计算得到的无人机的当前位姿。
步骤S104，根据障碍物点云和位姿进行局部地图构建，得到局部格点地图；
具体的，系统中的局部建图模块根据障碍物点云和位姿进行局部地图构建，进而得到以体素为单位的局部格点地图。下文中再对该过程进行详细描述，在此不再赘述。
步骤S106，接收到遥控器通道信息或航点信息后，根据局部格点地图和位姿进行碰撞检测和局部运动轨迹重规划，得到碰撞检测结果和运动轨迹；
具体的，系统中的运动规划模块根据局部格点地图和位姿进行碰撞检测和局部运动轨迹重规划，需要说明的是，碰撞检测实际上是实时的，而局部运动轨迹重规划可以是定时的，也就是说得到的运动轨迹实际上是预先规划的，而碰撞检测结果是实时得到的，能够及时检测得到突然出现的障碍物。
步骤S108，若根据碰撞检测结果确定无人机与障碍物距离小于预设距离阈值，则控制无人机悬停，不执行运动轨迹对应的轨迹控制指令，直至碰撞检测得到障碍物清除，再执行运动轨迹对应的轨迹控制指令，直至到达对应的航点。
具体的，控制无人机悬停后，继续进行上述步骤S102至步骤S106的过程，直至到达对应的航点。
若根据碰撞检测结果确定无人机与障碍物距离不小于预设距离阈值，则运动规划模块根据运动轨迹生成轨迹控制指令，进而控制模块接收运动规划模块生成的轨迹控制指令并发送至飞控执行。
需要说明的是，只要无人机与障碍物距离小于预设距离阈值，运动规划模块便生成控制无人机悬停指令，进而控制模块接收无人机悬停指令并发送至飞控执行，也就是说该障碍物即便是不在无人机的运动轨迹上，也要控制无人机悬停，以应对突然出现的障碍物，实现可靠的自主避障。
在本发明实施例中，提供了一种局部运动轨迹的重规划方法，包括：实时获取无人机周围的障碍物点云和无人机的位姿；根据障碍物点云和位姿进行局部地图构建，得到局部格点地图；接收到遥控器通道信息或航点信息后，根据局部格点地图和位姿进行碰撞检测和局部运动轨迹重规划，得到碰撞检测结果和运动轨迹；若根据碰撞检测结果确定无人机与障碍物距离小于预设距离阈值，则控制无人机悬停，不执行运动轨迹对应的轨迹控制指令，直至碰撞检测得到障碍物清除，再执行运动轨迹对应的轨迹控制指令，直至到达对应的航点。通过上述描述可知，本发明的局部运动轨迹的重规划方法中，在局部运动轨迹重规划的同时，又增加了碰撞检测，如此，能够根据实时的碰撞检测结果指导预先规划的运动轨迹的执行，可以有效实现无人机的局部运动轨迹重规划，进而实现无人机的有效自主避障，缓解了现有技术无法有效实现无人机的局部运动轨迹重规划，进而无法实现无人机的有效自主避障的技术问题。
上述内容对本发明的局部运动轨迹的重规划方法进行了简要介绍，下面对其中涉及到的具体内容进行详细描述。
在本发明的一个可选实施例中，上述步骤S104，根据障碍物点云和位姿进行局部地图构建，具体包括如下步骤：
（1）去除障碍物点云中的地面点云，得到去除地面点云后的障碍物点云；
发明人考虑到感知得到的地面不是很准确时，后续垂直方向上进行膨胀的策略会进一步加强不准确的结果，导致避障失败，所以需要进行地面点云的去除，提高建图准确性的同时减低建图时间。
具体的，采用预设的去除地面点云算法去除障碍物点云中的地面点云，得到去除地面点云后的障碍物点云，具体包括如下步骤：
（11）根据在障碍物点云中随机选择的预设数量个点云拟合平面模型；
（12）计算各障碍物点云到平面模型的距离，并基于距离在障碍物点云中确定平面模型的内点；
（13）若内点的数量超过预设阈值，则记录平面模型的参数；
（14）重复执行上述步骤预设次数，得到多个平面模型的参数；
（15）根据各平面模型对应的内点的数量，在多个平面模型中确定目标平面模型；
（16）计算各障碍物点云到目标平面模型的距离，并基于距离在障碍物点中确定非地面点云，进而得到去除地面点云后的障碍物点云。
下面再对预设的去除地面点云算法的基本思路进行介绍：
1.从所有障碍物点云中随机选择几个点云，拟合一个平面模型；
2.计算所有其他点云到该平面模型的距离，小于第一距离阈值的点认为是内点；
3.如果内点数目超过预设阈值，则认为找到了一个好的平面模型；
4.记下此时的平面模型参数；
5.重复1-4步骤多次，找出内点最多的平面模型；
6.使用最后得到的平面模型参数，计算所有点云到该平面模型的距离；
7.距离大于第二距离阈值的点云即为非地面点云；
8.返回非地面点云作为去除地面点云后的障碍物点云。
（2）对去除地面点云后的障碍物点云进行下采样，得到下采样后的障碍物点云；
发明人考虑到感知算法感知得到的障碍物点云过多，局部地图构建处理时间较长，这样会导致运动规划得到的信息滞后，基于此，设计了下采样的方法以减少障碍物点云。
具体的，局部建图模块的局部格点地图分辨率在10-20cm，但收到的去除地面点云后的障碍物点云分辨率远高于此，所以在收到去除地面点云后的障碍物点云的时候，根据局部格点地图分辨率对去除地面点云后的障碍物点云进行下采样，以降低建图时间。
（3）根据位姿将下采样后的障碍物点云映射至空白格点地图，并对映射至空白格点地图的障碍物点云进行垂直方向的障碍物膨胀，得到局部格点地图。
发明人考虑到在复杂环境，比如树林，障碍物低矮且密集的场景下，常常会感知不准，为了抑制感知不准所带来的避障失败，在将下采样后的障碍物点云映射至空白格点地图时，对障碍物点云进行垂直方向的障碍物膨胀，通过在垂直方向上进行障碍物膨胀，即为局部格点地图中(x,y,zi), i=1,2,3..处体素均为障碍物，可以降低绕障的复杂度。
另外，由于感知的不准确会导致建图的不准确，所以需要定时清空局部格点地图，只在相对较新的感知信息（即实时获取的障碍物点云）上进行绕障规划。
在本发明的一个可选实施例中，上述步骤S106，根据局部格点地图和位姿进行碰撞检测，具体包括如下步骤：
基于位姿采用预设的碰撞检测算法在局部格点地图上进行碰撞检测，得到碰撞检测结果，其中，碰撞检测结果包括：障碍物距离和方位。
通过预设的碰撞检测算法可以高效访问射线所经过的所有体素，以此判断该体素位置是否有碰撞。
预设的碰撞检测算法进行碰撞检测的具体过程包括：
（1）将位姿所属的局部格点地图中的体素作为当前体素，并确定当前体素的坐标；
（2）初始化步长变量，其中，步长变量表示遍历相邻体素的方向；
（3）从当前体素的坐标开始沿着步长变量的方向计算射线穿过当前体素的各方向的t值和射线移动一个体素单元各方向需要增加的t值；
（4）循环比较射线穿过当前体素的各方向的t值，并从中确定最小的t值；
（5）根据最小的t值对应的方向移动一个体素，得到新的当前体素；
（6）将对应的方向的t值增加对应方向需要增加的t值；
（7）对新的当前体素和局部格点地图（具体为其中的障碍物）进行交点检测；
（8）根据交点检测结果确定碰撞检测结果。
下面对上述碰撞检测的过程进行描述：
1.初始化阶段：
a)确定射线起点（即无人机的当前位姿）所在的体素，设为当前体素；
b)初始化变量X、Y、Z为当前体素的坐标；
c)初始化步长变量stepX、stepY、stepZ，表示遍历相邻体素的方向；
d)计算tMaxX、tMaxY、tMaxZ，表示射线穿过当前体素边界的t值；
e)计算tDeltaX、tDeltaY、tDeltaZ，表示射线移动一个体素单元需要增加的t值；
2.递增遍历阶段：
a)循环比较tMaxX、tMaxY、tMaxZ，选择最小者；
b)根据最小t值对应的方向移动一个体素(更新X/Y/Z)；
c)更新对应tMax变量增加一个tDelta；
d)检查列表ObjectList[X][Y][Z]（存储的是射线射入的下一个体素的位置）是否为空；
e)如果列表不为空，进行交点检测；如果交点在当前体素内，返回障碍物结果；
f)如果列表为空，继续循环遍历下一个体素。
3.遍历结束条件：找到非空列表或遍历完所有体素。
为了便于对无人机的整个飞行状态控制进行更好的理解，下面参考图3对飞行状态控制流程进行说明：
系统中的运动规划算法共有六个状态：初始化、等待、生成新轨迹、执行规划、局部轨迹重规划、紧急停止。运动规划算法启动时为初始化状态，读取到无人机状态信息后进入等待状态。此时若没有航点，可通过遥控器控制无人机；若有航点传入，则进入生成新轨迹状态（全局轨迹规划），轨迹规划成功后，进入执行轨迹状态，发出无人机轨迹控制指令。执行轨迹状态下，系统会以100Hz的频率进入局部轨迹重规划状态，根据局部地图以及无人机位置重新规划未来运动轨迹。当碰撞检测模块发现危险障碍物时，飞机悬停，等待周围的障碍物清除，也会进入轨迹重规划状态。重规划成功则再次进入执行轨迹状态。若执行轨迹过程中没有危险障碍物或新的航点，到达目标航点后进入等待状态。
本发明的局部运动轨迹的重规划方法包括自主作业模式下的路径规划，遥控器模式和自主作业模式下的自主避障。系统通过提高规划频率、减低地图复杂度以及提高建图效率的策略，提高避障、绕障的成功率。与现有技术相比，本发明实现了可以进行高效局部规划、自主避障，提高了无人机综合作业能力。
实施例二：
本发明实施例还提供了一种局部运动轨迹的重规划装置，该局部运动轨迹的重规划装置主要用于执行本发明实施例一中所提供的局部运动轨迹的重规划方法，以下对本发明实施例提供的局部运动轨迹的重规划装置做具体介绍。
图4是根据本发明实施例的一种局部运动轨迹的重规划装置的示意图，如图4所示，该装置主要包括：获取单元10、局部地图构建单元20、碰撞检测和轨迹重规划单元30、控制单元40，其中：
获取单元，用于实时获取无人机周围的障碍物点云和无人机的位姿；
局部地图构建单元，用于根据障碍物点云和位姿进行局部地图构建，得到局部格点地图；
碰撞检测和轨迹重规划单元，用于接收到遥控器通道信息或航点信息后，根据局部格点地图和位姿进行碰撞检测和局部运动轨迹重规划，得到碰撞检测结果和运动轨迹；
控制单元，用于若根据碰撞检测结果确定无人机与障碍物距离小于预设距离阈值，则控制无人机悬停，不执行运动轨迹对应的轨迹控制指令，直至碰撞检测得到障碍物清除，再执行运动轨迹对应的轨迹控制指令，直至到达对应的航点。
在本发明实施例中，提供了一种局部运动轨迹的重规划装置，包括：实时获取无人机周围的障碍物点云和无人机的位姿；根据障碍物点云和位姿进行局部地图构建，得到局部格点地图；接收到遥控器通道信息或航点信息后，根据局部格点地图和位姿进行碰撞检测和局部运动轨迹重规划，得到碰撞检测结果和运动轨迹；若根据碰撞检测结果确定无人机与障碍物距离小于预设距离阈值，则控制无人机悬停，不执行运动轨迹对应的轨迹控制指令，直至碰撞检测得到障碍物清除，再执行运动轨迹对应的轨迹控制指令，直至到达对应的航点。通过上述描述可知，本发明的局部运动轨迹的重规划装置中，在局部运动轨迹重规划的同时，又增加了碰撞检测，如此，能够根据实时的碰撞检测结果指导预先规划的运动轨迹的执行，可以有效实现无人机的局部运动轨迹重规划，进而实现无人机的有效自主避障，缓解了现有技术无法有效实现无人机的局部运动轨迹重规划，进而无法实现无人机的有效自主避障的技术问题。
可选地，局部地图构建单元还用于：去除障碍物点云中的地面点云，得到去除地面点云后的障碍物点云；对去除地面点云后的障碍物点云进行下采样，得到下采样后的障碍物点云；根据位姿将下采样后的障碍物点云映射至空白格点地图，并对映射至空白格点地图的障碍物点云进行垂直方向的障碍物膨胀，得到局部格点地图。
可选地，局部地图构建单元还用于：采用预设的去除地面点云算法去除障碍物点云中的地面点云，得到去除地面点云后的障碍物点云。
可选地，局部地图构建单元还用于：根据在障碍物点云中随机选择的预设数量个点云拟合平面模型；计算各障碍物点云到平面模型的距离，并基于距离在障碍物点云中确定平面模型的内点；若内点的数量超过预设阈值，则记录平面模型的参数；重复执行上述步骤预设次数，得到多个平面模型的参数；根据各平面模型对应的内点的数量，在多个平面模型中确定目标平面模型；计算各障碍物点云到目标平面模型的距离，并基于距离在障碍物点中确定非地面点云，进而得到去除地面点云后的障碍物点云。
可选地，碰撞检测和轨迹重规划单元还用于：基于位姿采用预设的碰撞检测算法在局部格点地图上进行碰撞检测，得到碰撞检测结果，其中，碰撞检测结果包括：障碍物距离和方位。
可选地，碰撞检测和轨迹重规划单元还用于：将位姿所属的局部格点地图中的体素作为当前体素，并确定当前体素的坐标；初始化步长变量，其中，步长变量表示遍历相邻体素的方向；从当前体素的坐标开始沿着步长变量的方向计算射线穿过当前体素的各方向的t值和射线移动一个体素单元各方向需要增加的t值；循环比较射线穿过当前体素的各方向的t值，并从中确定最小的t值；根据最小的t值对应的方向移动一个体素，得到新的当前体素；将对应的方向的t值增加对应方向需要增加的t值；对新的当前体素和局部格点地图进行交点检测；根据交点检测结果确定碰撞检测结果。
本发明实施例所提供的装置，其实现原理及产生的技术效果和前述方法实施例相同，为简要描述，装置实施例部分未提及之处，可参考前述方法实施例中相应内容。
如图5所示，本申请实施例提供的一种电子设备600，包括：处理器601、存储器602和总线，所述存储器602存储有所述处理器601可执行的机器可读指令，当电子设备运行时，所述处理器601与所述存储器602之间通过总线通信，所述处理器601执行所述机器可读指令，以执行如上述局部运动轨迹的重规划方法的步骤。
具体地，上述存储器602和处理器601能够为通用的存储器和处理器，这里不做具体限定，当处理器601运行存储器602存储的计算机程序时，能够执行上述局部运动轨迹的重规划方法。
处理器601可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器601中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器601可以是通用处理器，包括中央处理器(Central Processing Unit，简称CPU)、网络处理器(Network Processor，简称NP)等；还可以是数字信号处理器(DigitalSignal Processing，简称DSP)、专用集成电路(Application Specific IntegratedCircuit，简称ASIC)、现场可编程门阵列(Field-Programmable Gate Array，简称FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本申请实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器602，处理器601读取存储器602中的信息，结合其硬件完成上述方法的步骤。
对应于上述局部运动轨迹的重规划方法，本申请实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有机器可运行指令，所述机器可运行指令在被处理器调用和运行时，所述机器可运行指令促使所述处理器运行上述局部运动轨迹的重规划方法的步骤。
本申请实施例所提供的局部运动轨迹的重规划装置可以为设备上的特定硬件或者安装于设备上的软件或固件等。本申请实施例所提供的装置，其实现原理及产生的技术效果和前述方法实施例相同，为简要描述，装置实施例部分未提及之处，可参考前述方法实施例中相应内容。所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，前述描述的系统、装置和单元的具体工作过程，均可以参考上述方法实施例中的对应过程，在此不再赘述。
在本申请所提供的实施例中，应该理解到，所揭露装置和方法，可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
再例如，附图中的流程图和框图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请提供的实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台电子设备（可以是个人计算机，服务器，或者网络设备等）执行本申请各个实施例所述局部运动轨迹的重规划方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（Read-OnlyMemory，简称ROM）、随机存取存储器（Random Access Memory，简称RAM）、磁碟或者光盘等各种可以存储程序代码的介质。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释，此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
最后应说明的是：以上所述实施例，仅为本申请的具体实施方式，用以说明本申请的技术方案，而非对其限制，本申请的保护范围并不局限于此，尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本申请实施例技术方案的范围。都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以权利要求的保护范围为准。
Claims (9)
1.一种局部运动轨迹的重规划方法，其特征在于，包括：
实时获取无人机周围的障碍物点云和所述无人机的位姿；
根据所述障碍物点云和所述位姿进行局部地图构建，得到局部格点地图；
接收到遥控器通道信息或航点信息后，根据所述局部格点地图和所述位姿进行碰撞检测和局部运动轨迹重规划，得到碰撞检测结果和运动轨迹；
若根据所述碰撞检测结果确定所述无人机与障碍物距离小于预设距离阈值，则控制所述无人机悬停，不执行所述运动轨迹对应的轨迹控制指令，直至碰撞检测得到障碍物清除，再执行所述运动轨迹对应的轨迹控制指令，直至到达对应的航点；
其中，根据所述局部格点地图和所述位姿进行碰撞检测，包括：
将所述位姿所属的局部格点地图中的体素作为当前体素，并确定所述当前体素的坐标；
初始化步长变量，其中，所述步长变量表示遍历相邻体素的方向；从所述当前体素的坐标开始沿着所述步长变量的方向计算射线穿过当前体素的各方向的t值和射线移动一个体素单元各方向需要增加的t值；
循环比较所述射线穿过当前体素的各方向的t值，并从中确定最小的t值；根据所述最小的t值对应的方向移动一个体素，得到新的当前体素；将对应的方向的t值增加对应方向需要增加的t值；对新的所述当前体素和所述局部格点地图进行交点检测；根据交点检测结果确定所述碰撞检测结果。
2.根据权利要求1所述的方法，其特征在于，根据所述障碍物点云和所述位姿进行局部地图构建，包括：
去除所述障碍物点云中的地面点云，得到去除地面点云后的障碍物点云；
对所述去除地面点云后的障碍物点云进行下采样，得到下采样后的障碍物点云；
根据所述位姿将所述下采样后的障碍物点云映射至空白格点地图，并对映射至空白格点地图的障碍物点云进行垂直方向的障碍物膨胀，得到所述局部格点地图。
3.根据权利要求2所述的方法，其特征在于，去除所述障碍物点云中的地面点云，包括：
采用预设的去除地面点云算法去除所述障碍物点云中的地面点云，得到所述去除地面点云后的障碍物点云。
4.根据权利要求3所述的方法，其特征在于，采用预设的去除地面点云算法去除所述障碍物点云中的地面点云，包括：
根据在所述障碍物点云中随机选择的预设数量个点云拟合平面模型；
计算各所述障碍物点云到所述平面模型的距离，并基于所述距离在所述障碍物点云中确定所述平面模型的内点；
若所述内点的数量超过预设阈值，则记录所述平面模型的参数；
重复执行上述步骤预设次数，得到多个所述平面模型的参数；
根据各所述平面模型对应的内点的数量，在多个所述平面模型中确定目标平面模型；
计算各所述障碍物点云到所述目标平面模型的距离，并基于所述距离在所述障碍物点中确定非地面点云，进而得到所述去除地面点云后的障碍物点云。
5.根据权利要求1所述的方法，其特征在于，根据所述局部格点地图和所述位姿进行碰撞检测，包括：
基于所述位姿采用预设的碰撞检测算法在所述局部格点地图上进行碰撞检测，得到所述碰撞检测结果，其中，所述碰撞检测结果包括：障碍物距离和方位。
6.一种局部运动轨迹的重规划装置，其特征在于，包括：
获取单元，用于实时获取无人机周围的障碍物点云和所述无人机的位姿；
局部地图构建单元，用于根据所述障碍物点云和所述位姿进行局部地图构建，得到局部格点地图；
碰撞检测和轨迹重规划单元，用于接收到遥控器通道信息或航点信息后，根据所述局部格点地图和所述位姿进行碰撞检测和局部运动轨迹重规划，得到碰撞检测结果和运动轨迹；
控制单元，用于若根据所述碰撞检测结果确定所述无人机与障碍物距离小于预设距离阈值，则控制所述无人机悬停，不执行所述运动轨迹对应的轨迹控制指令，直至碰撞检测得到障碍物清除，再执行所述运动轨迹对应的轨迹控制指令，直至到达对应的航点；
所述碰撞检测和轨迹重规划单元还用于：将所述位姿所属的局部格点地图中的体素作为当前体素，并确定所述当前体素的坐标；初始化步长变量，其中，所述步长变量表示遍历相邻体素的方向；从所述当前体素的坐标开始沿着所述步长变量的方向计算射线穿过当前体素的各方向的t值和射线移动一个体素单元各方向需要增加的t值；循环比较所述射线穿过当前体素的各方向的t值，并从中确定最小的t值；根据所述最小的t值对应的方向移动一个体素，得到新的当前体素；将对应的方向的t值增加对应方向需要增加的t值；对新的所述当前体素和所述局部格点地图进行交点检测；根据交点检测结果确定所述碰撞检测结果。
7.根据权利要求6所述的装置，其特征在于，所述局部地图构建单元还用于：
去除所述障碍物点云中的地面点云，得到去除地面点云后的障碍物点云；
对所述去除地面点云后的障碍物点云进行下采样，得到下采样后的障碍物点云；
根据所述位姿将所述下采样后的障碍物点云映射至空白格点地图，并对映射至空白格点地图的障碍物点云进行垂直方向的障碍物膨胀，得到所述局部格点地图。
8.一种电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现上述权利要求1至5中任一项所述的方法的步骤。
9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有机器可运行指令，所述机器可运行指令在被处理器调用和运行时，所述机器可运行指令促使所述处理器运行上述权利要求1至5中任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311213020.2A CN116954265B (zh) | 2023-09-20 | 2023-09-20 | 局部运动轨迹的重规划方法、装置和电子设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311213020.2A CN116954265B (zh) | 2023-09-20 | 2023-09-20 | 局部运动轨迹的重规划方法、装置和电子设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116954265A CN116954265A (zh) | 2023-10-27 |
| CN116954265B true CN116954265B (zh) | 2023-12-05 |
Family
ID=88454970
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311213020.2A Active CN116954265B (zh) | 2023-09-20 | 2023-09-20 | 局部运动轨迹的重规划方法、装置和电子设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116954265B (zh) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106067193A (zh) * | 2016-06-06 | 2016-11-02 | 中国人民解放军信息工程大学 | 一种点云数据的体素化方法和体素的遮挡裁剪方法 |
| CN109144097A (zh) * | 2018-08-15 | 2019-01-04 | 广州极飞科技有限公司 | 障碍物或地面识别及飞行控制方法、装置、设备及介质 |
| CN109783842A (zh) * | 2018-12-05 | 2019-05-21 | 苏州蜗牛数字科技股份有限公司 | 一种基于体素数据的射线检测方法及系统 |
| CN110889903A (zh) * | 2019-11-26 | 2020-03-17 | 南京理工大学 | 一种基于切层策略的三维模型快速体素化方法 |
| CN111984021A (zh) * | 2020-07-21 | 2020-11-24 | 武汉智会创新科技有限公司 | 无人机的控制方法及系统、无人机设备、远程控制设备 |
| CN113138397A (zh) * | 2021-06-01 | 2021-07-20 | 中国计量大学 | 一种无人机避障装置及无人机 |
| CN113985429A (zh) * | 2021-09-23 | 2022-01-28 | 天津大学 | 基于三维激光雷达的无人机环境扫描与重构方法 |
| CN114474065A (zh) * | 2022-03-04 | 2022-05-13 | 美智纵横科技有限责任公司 | 机器人控制方法及其装置、机器人和存储介质 |
| CN115205717A (zh) * | 2022-09-14 | 2022-10-18 | 广东汇天航空航天科技有限公司 | 障碍物点云数据处理方法以及飞行设备 |
| CN115542947A (zh) * | 2022-10-31 | 2022-12-30 | 华东理工大学 | 一种四旋翼无人机的自主导航方法及无人机装置 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102017004705A1 (de) * | 2017-05-16 | 2018-11-22 | Ziehm Imaging Gmbh | Verfahren zum Erzeugen eines in der Zentralschicht vollständigen 3D-Datensatzes zur Volumenrekonstruktion und Röntgensystem mit einem Kegelstrahl-C-Bogen-Röntgengerät zur Durchführung des Verfahrens |
-
2023
- 2023-09-20 CN CN202311213020.2A patent/CN116954265B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106067193A (zh) * | 2016-06-06 | 2016-11-02 | 中国人民解放军信息工程大学 | 一种点云数据的体素化方法和体素的遮挡裁剪方法 |
| CN109144097A (zh) * | 2018-08-15 | 2019-01-04 | 广州极飞科技有限公司 | 障碍物或地面识别及飞行控制方法、装置、设备及介质 |
| CN109783842A (zh) * | 2018-12-05 | 2019-05-21 | 苏州蜗牛数字科技股份有限公司 | 一种基于体素数据的射线检测方法及系统 |
| CN110889903A (zh) * | 2019-11-26 | 2020-03-17 | 南京理工大学 | 一种基于切层策略的三维模型快速体素化方法 |
| CN111984021A (zh) * | 2020-07-21 | 2020-11-24 | 武汉智会创新科技有限公司 | 无人机的控制方法及系统、无人机设备、远程控制设备 |
| CN113138397A (zh) * | 2021-06-01 | 2021-07-20 | 中国计量大学 | 一种无人机避障装置及无人机 |
| CN113985429A (zh) * | 2021-09-23 | 2022-01-28 | 天津大学 | 基于三维激光雷达的无人机环境扫描与重构方法 |
| CN114474065A (zh) * | 2022-03-04 | 2022-05-13 | 美智纵横科技有限责任公司 | 机器人控制方法及其装置、机器人和存储介质 |
| CN115205717A (zh) * | 2022-09-14 | 2022-10-18 | 广东汇天航空航天科技有限公司 | 障碍物点云数据处理方法以及飞行设备 |
| CN115542947A (zh) * | 2022-10-31 | 2022-12-30 | 华东理工大学 | 一种四旋翼无人机的自主导航方法及无人机装置 |
Non-Patent Citations (2)
| Title |
|---|
| Fangping Chen,et al..Real-Time active detection of targets and path planning using UAVs.2021 IEEE International Conference on Robotics and Automation.2021,391-397. * |
| 基于深度学习的无监督三维点云配准算法研究;谭培;中国优秀硕士学位论文全文数据库 信息科技辑(第1期);第26-27页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116954265A (zh) | 2023-10-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108776492B (zh) | 一种基于双目相机的四轴飞行器自主避障与导航方法 | |
| US10352711B2 (en) | Computer-implemented method and a system for guiding a vehicle within a scenario with obstacles | |
| CN106873630B (zh) | 一种飞行控制方法及装置，执行设备 | |
| CN109116867B (zh) | 一种无人机飞行避障方法、装置、电子设备及存储介质 | |
| CN110967031A (zh) | 生成无碰撞行进路径的方法和计算系统 | |
| US9129523B2 (en) | Method and system for obstacle detection for vehicles using planar sensor data | |
| JP2015006874A (ja) | ３次元証拠グリッドを使用する自律着陸のためのシステムおよび方法 | |
| EP2101277B1 (en) | Method and device for three-dimensional path planning to avoid obstacles using multiple planes | |
| Bucki et al. | Rectangular pyramid partitioning using integrated depth sensors (rappids): A fast planner for multicopter navigation | |
| EP3213158B1 (en) | Space partitioning for motion planning | |
| US10134135B1 (en) | System and method for finding open space efficiently in three dimensions for mobile robot exploration | |
| CN112034479A (zh) | 一种应用于煤矿井下智能巡检无人机的定位方法及系统 | |
| Marzat et al. | Reactive MPC for autonomous MAV navigation in indoor cluttered environments: Flight experiments | |
| CN112256052A (zh) | 无人机的速度控制方法、装置、无人机和存储介质 | |
| CN112762928A (zh) | 含有激光slam的odom与dm地标组合移动机器人及导航方法 | |
| CN112313594B (zh) | 无人机的控制方法、装置、设备及存储介质 | |
| US11383704B2 (en) | Enhanced vehicle operation | |
| CN114136318A (zh) | 一种机器智能导航方法和装置 | |
| CN117406771B (zh) | 一种基于四旋翼无人机的高效自主探索方法、系统及设备 | |
| CN113448340B (zh) | 一种无人机的路径规划方法、装置、无人机及存储介质 | |
| CN116954265B (zh) | 局部运动轨迹的重规划方法、装置和电子设备 | |
| CN112753000A (zh) | 避障方法、可移动平台、控制设备和存储介质 | |
| CN115016546B (zh) | 无人机三维路径规划方法、装置、电子设备和存储介质 | |
| CN114489050A (zh) | 沿直线行驶的避障路线控制方法、装置、设备及存储介质 | |
| CN111684457B (zh) | 一种状态检测方法、装置及可移动平台 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |