导航地图构建方法、装置、飞行设备及存储介质

技术领域

本申请涉及导航技术领域，更具体地，涉及一种导航地图构建方法、装置、飞行设备及存储介质。

背景技术

飞行设备在存在障碍物的环境中自主飞行，需要准确地导航地图进行导航。

通过飞行设备在飞行过程中获取的检测信号进行导航地图的构建。然而，飞行环境复杂，获取检测信号的干扰因素和约束条件较多，影响对障碍物的准确判断。

发明内容

鉴于上述问题，本发明提出了一种偏移控制方法、装置、电磁加热设备及存储介质。

第一方面，本申请实施例提供了一种导航地图构建方法，应用于飞行设备，飞行设备包括单线激光雷达、惯性传感器以及定位模块；该方法包括：获取单线激光雷达检测的当前帧点云数据；基于惯性传感器检测的惯性检测数据和定位模块检测的定位数据，将当前帧点云数据映射于飞行设备所在水平面，确定映射点云数据；基于惯性检测数据对映射点云数据进行运动畸变矫正，确定目标点云数据；基于相邻帧的目标点云数据确定飞行设备的位姿，并根据位姿构建点云地图；将点云地图进行栅格化处理，确定导航地图。

第二方面，本申请实施例还提供了一种导航地图构建装置，应用于飞行设备，飞行设备包括单线激光雷达、惯性传感器以及定位模块；该装置包括：获取模块、映射模块、矫正模块、构建模块以及导航模块；其中，获取模块用于获取单线激光雷达检测的当前帧点云数据；映射模块用于基于惯性传感器检测的惯性检测数据和定位模块检测的定位数据，将当前帧点云数据映射于飞行设备所在水平面，确定映射点云数据；矫正模块用于基于惯性检测数据对映射点云数据进行运动畸变矫正，确定目标点云数据；构建模块用于基于相邻帧的目标点云数据确定飞行设备的位姿，并根据位姿构建点云地图；导航模块用于将点云地图进行栅格化处理，确定导航地图。

第三方面，本申请实施例还提供了一种飞行设备，包括：一个或多个处理器；存储器；一个或多个应用程序；其中一个或多个应用程序被存储在存储器中并被配置为由一个或多个处理器执行，一个或多个应用程序配置用于执行如上述第一方面所述的导航地图构建方法。

第四方面，本申请实施例还提供了一种计算机可读取存储介质，计算机可读取存储介质中存储有程序代码，程序代码可被处理器调用执行如上述第一方面所述的导航地图构建方法。

本发明提供的技术方案，应用于飞行设备，飞行设备包括单线激光雷达、惯性传感器以及定位模块；该导航地图构建方法包括：获取单线激光雷达检测的当前帧点云数据；基于惯性传感器检测的惯性检测数据和定位模块检测的定位数据，将当前帧点云数据映射于飞行设备所在水平面，确定映射点云数据；基于惯性检测数据对映射点云数据进行运动畸变矫正，确定目标点云数据；基于相邻帧的目标点云数据确定飞行设备的位姿，并根据位姿构建点云地图；将点云地图进行栅格化处理，确定导航地图。从而利用单线激光雷达感知的点云数据，结合定位模块和惯性传感器检测的数据，得到飞行设备更准确的位姿信息，从而实现在存在障碍物的环境中精准构建导航地图。

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，而不是全部的实施例。基于本申请实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例及附图，都属于本发明保护的范围。

图1示出了本申请实施例提供的一种导航地图构建方法的流程示意图。

图2示出了本申请实施例提供的一种点云地图划分为网格的结构示意图。

图3示出了本申请实施例提供的一种导航地图构建装置的结构示意图。

图4示出了本申请实施例提供的一种飞行设备的结构示意图。

图5示出了本申请实施例提供的一种计算机可读取存储介质的结构示意图。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述。

飞行设备在存在障碍物的环境中自主飞行，需要准确地导航地图进行导航。

通过飞行设备在飞行过程中获取的检测信号进行导航地图的构建。然而，飞行环境复杂，获取检测信号的干扰因素和约束条件较多，影响对障碍物的准确判断。

相关技术中，采用单目摄像头的检测数据，通过构建深度神经网络结构进行室内避障，然而，单目摄像头在室内理想条件可以有效检测，可对于强光、无光环境，单目摄像头检测的图片检测效果差，且单目摄像头视场角、视距有限，单个摄像头只能检测单方向的数据，需要安装多个摄像头才可以检测全方向的数据，而多个摄像头对于成本、飞行设备的载重、功耗都会造成影响。

相关技术中，还有采用RTK(Real-time kinematic，载波相位差分技术)的方式或者单点GNSS(Global Navigation Satellite System，全球导航卫星系统)结合IMU(Inertial Measurement Unit，惯性测量单元)的方式进行信号检测，然而，采用RTK的方式依赖于地面基站，采用单点GNSS结合IMU的方式在出现高障碍物遮挡时，精度会大大下载。

相关技术中，也有采用单线激光雷达进行信号检测，然而单线激光雷达多用于地面二维运动的运载体，只考虑航向角信息对于导航的影响。而飞行运载体的不只是二维运动，飞行运载体飞行角度的不同会对单线激光雷达感知环境带来影响，直接将采用地面运载体的检测方式的单线激光雷达应用于飞行运载体，当飞行运载体倾斜时，单线激光雷达因此而倾斜，扫描出的数据并不是飞行运载体所处二维平面的障碍物数据，导致障碍物距离判断有误。

为了改善上述问题，发明人提出了本申请提供的导航地图构建方法、装置、飞行设备及存储介质，其中，导航地图构建方法应用于飞行设备，飞行设备包括单线激光雷达、惯性传感器以及定位模块；该导航地图构建方法包括：获取单线激光雷达检测的当前帧点云数据；基于惯性传感器检测的惯性检测数据和定位模块检测的定位数据，将当前帧点云数据映射于飞行设备所在水平面，确定映射点云数据；基于惯性检测数据对映射点云数据进行运动畸变矫正，确定目标点云数据；基于相邻帧的目标点云数据确定飞行设备的位姿，并根据位姿构建点云地图；将点云地图进行栅格化处理，确定导航地图。从而利用单线激光雷达感知的点云数据，结合定位模块和惯性传感器检测的数据，得到飞行设备更准确的位姿信息，从而实现在存在障碍物的环境中精准构建导航地图。

请参阅图1，本申请实施例提供了一种导航地图构建方法，可以应用于飞行设备，飞行设备包括单线激光雷达、惯性传感器以及定位模块。如图1所示，该方法包括：步骤110至步骤步骤150。

步骤110、获取单线激光雷达检测的当前帧点云数据。

其中，单线激光雷达是指激光源发出的线束是单线的雷达，其扫描速度快、分辨率强、可靠性高，在角频率及灵敏度上反应更快捷，在障碍物的测距距离和精度上也更加精准，本申请实施例的导航地图构建方法应用于飞行设备，飞行设备通过单线激光雷达对飞行环境进行扫描，以获取飞行环境的点云数据。

单线激光雷达可以按照预设的扫描周期进行扫描，单线激光雷达每完成一次扫描，可以得到一帧点云数据，当前帧点云数据为单线激光雷达最新扫描得到的点云数据，以用于构建当前最新的飞行环境对应的导航地图。

在一些实施方式中，每一帧点云数据由多个激光点组合，从而由多个激光点的信息表示飞行设备所处的飞行环境。

在本申请的实施例中，以飞行设备采用右前上坐标系为例进行说明，即X轴的正向为右，Y轴的正向为前，Z轴的正向为上，以定位模块的中心作为坐标系的原点。

将单线激光雷达中心相对定位模块中心的位置坐标设为(x

将激光点的坐标转成直角坐标系，则第i个激光点在直角坐标系的坐标为(ρ(i)*cosθ(i)-x

在单线激光雷达的中心距离定位模块的中心较近时，也可以近视为单线激光雷达的中心与定位模块的中心重合，此时，第i个激光点在直角坐标系的坐标为(ρ(i)*cosθ(i)，ρ(i)*sinθ(i))

步骤120、基于惯性传感器检测的惯性检测数据和定位模块检测的定位数据，将当前帧点云数据映射于飞行设备所在水平面，确定映射点云数据。

由前述记载可以知道，单线激光雷达用于获取二维平面信息，在应用于飞行设备时，由于飞行设备的飞行过程会发生姿态倾斜，导致点云数据反映出的障碍物轮廓并不一定是当前飞行设备所处的水平面的障碍物轮廓。因此，需要将单线激光雷达扫描出来的点云数据映射到飞行设备所在水平面。

在一些实施方式中，步骤120可以包括下述步骤。

(1)基于惯性检测数据、定位数据确定俯仰角和横滚角。

(2)根据俯仰角和横滚角以及每个激光点的点云数据确定每个激光点与单线激光雷达的连线关于水平面的水平夹角。

(3)根据每个激光点的点云数据以及水平夹角，确定每个激光点的映射点云数据。

在本申请的实施例中，定位模块可以采用双天线GNSS。

在一些实施方式中，可以根据惯性检测数据、定位数据确定俯仰角和横滚角。具体地，可以通过GNSS和IMU组合算法进行计算得到，例如可以采用GNSS/INS松组合导航算法。

其中，通过惯性检测数据可以得到俯仰角和横滚角，定位数据可以对俯仰角和横滚角进行修正。

进一步地，步骤基于惯性检测数据、定位数据确定俯仰角和横滚角，可以包括下述步骤。

(a)确定定位模块中心与惯性传感器中心的相对位置。

(b)基于惯性检测数据、定位数据以及相对位置确定俯仰角和横滚角。

以定位模块为双天线GNSS为例，可以标定出双天线GNSS的主天线中心到惯性传感器中心的相对位置的坐标(x

进一步地，假设当前飞行设备通过惯性检测数据和定位数据获取的获取的俯仰角为a，横滚角为b，此时设当前帧点云第i个点与单线激光雷达中心连线关于飞行设备所在水平面的夹角为γ，则有

根据上式(一)求得第i个点与单线激光雷达中心连线关于水平面的夹角γ。

由此，映射到水平面上的激光点距离单线激光雷达中心(即映射点云数据)ρ_level(i)满足

ρ_level(i)＝ρ(i)cosγ

将每帧的所有激光点进行映射，得到当前帧点云数据在飞行设备所在水平面上的映射点云数据。

步骤130、基于惯性检测数据对映射点云数据进行运动畸变矫正，确定目标点云数据。

映射点云数据为与飞行设备在同一水平面的点云数据，但是飞行设备的飞行速度较高，还会对点云数据产生运动畸变，除了每帧点云数据起始时刻的激光点，单线激光雷达在单帧内每一个激光点到单线激光雷达的距离并不是每帧点云起始时刻该激光点到单线激光雷达的距离。为此，需要进行运动畸变矫正，以减少飞行设备高速飞行所带来的误差。

在一些实施方式中，步骤130包括下述步骤。

(1)确定每个激光点的采样时间。

(2)根据采样时间确定多个检测时间。

(3)获取多个检测时间对应的惯性检测数据。

(4)基于采样时间、多个检测时间确定每个激光点的矫正系数。

(5)根据多个检测时间对应的惯性检测数据以及矫正系数确定每个激光点的姿态数据。

(6)根据每个激光点的姿态数据和映射点云数据确定每个激光点的目标点云数据。

在本申请的实施例中，在接收单线激光雷达发送的点云数据的同时，以较高的频率接收惯性传感器发送的惯性检测数据(姿态角数据)，并将其保存在惯性检测数据的循环队列中。

在一些实施方式中，获取到激光点的点云数据的时间不同，单线激光雷达可以以预设采样间隔进行采样，采样得到的激光点的点云数据可以设置时间戳，以用于确定激光点的点云数据的采样时间。

进一步地，可以根据激光点的采样时间确定多个检测时间，多个检测时间可以是与采样时间相邻的时间。从而在惯性检测数据的循环队列中查找与采样时间在时间上相邻的惯性检测数据。

在一些实施方中，假设飞行设备在相邻惯性检测数据间为匀速运动，可以通过球面线性插值的方法估计每个激光点在采样时刻的姿态数据，最终依次将同一帧点云数据的所有激光点变换至同一时刻的飞行设备的汽车姿态下。

其中，球面线性插值(Spherical linear interpolation)主要用于在两个表示旋转的四元数q

假设当前对当前帧点云数据的第n个激光点进行运动畸变校正。当前帧帧开始时刻为t

假设该激光点的采样时间相邻的检测时间为t

则该激光点的矫正系统可以通过下式(二)计算得到。

进一步第，根据球面线性插值，可以通过下式(三)得到该激光点的姿态数据。

q

其中，q

已知该激光点相对于单线激光雷达的坐标系的空间坐标(可以理解为映射点云数据对应的坐标)为p

其中，p

上述实施方式假设了相邻惯性检测数据间飞行设备为匀速运动，实际惯性检测数据的样频率远高于飞行设备的速度变化频率，因此假设合理。通过上述实施方式估计每个激光点在采样时刻的姿态数据，最终依次将同一帧的所有激光点变换至同一时刻的飞行设备的姿态下，从而确定目标点云数据。

步骤140、基于相邻帧的目标点云数据确定飞行设备的位姿，并根据位姿构建点云地图。

在本申请的实施例中，得到运动畸变矫正后的目标点云数据后，可以根据相邻帧的目标点云数据确定飞行设备的位姿，并根据飞行设备的位姿构建点云地图。

在一些实施方式中，对相邻帧的目标点云数据迭代最近点得到里程计信息，里程计信息作为先验信息，雷达信息作为观测，通过求解后验概率，估计飞行设备的位姿。

在一些实施方式中，步骤140包括下述步骤。

(1)根据相邻帧的目标点云数据确定里程计信息。

(2)根据里程计信息和目标点云数据确定飞行设备的位姿。

(3)基于位姿构建点云地图。

在一些实施方式中，步骤根据相邻帧的目标点云数据确定里程计信息包括下述步骤。

(a)确定相邻帧的目标点云数据确定相邻帧的旋转参数和平移参数。

(b)根据旋转参数和平移参数确定里程计信息。

在得到运动畸变矫正的单线激光雷达的目标点云数据后，迭代最近点主要目的就是找到旋转参数(Δpitch,Δroll,Δyaw)和平移参数Δr，将两个不同坐标系下的目标点云数据，以其中一个目标点云数据的点云坐标系为全局坐标系，另一个目标点云数据经过旋转和平移后两组目标点云数据重合部分完全重叠，具体可以参照下述步骤。

(a)将其中一个目标点云数据作为第一目标点云数据，将相邻的另一个目标点云数据作为第二目标点云数据；对于第一目标点云数据中的每个激光点，匹配第二目标点云数据(或选定集合)中的最近激光点。

(b)求得使上述对应激光点对计算均方根最小的刚体变换，求得平移参数和旋转参数。

(c)使用获得的转换矩阵来转换第一目标点云数据。

(d)迭代(重新关联点)，直到满足终止迭代的条件(迭代次数或误差小于阈值)。

这里的误差最小，可以是相邻两次均方根差的绝对值小于某一限差。

利用每后一帧的目标点云数据相对于前一帧的目标点云数据的(Δpitch,Δroll,Δyaw)与Δr，可以得到任意时刻的目标点云数据相对于初始时刻的目标点云数据的位置姿态增量(Δpitch,Δroll,Δyaw,Δr)。

进一步的，利用RBPF(Rao-Blackwellized Particle Filters)以飞行设备的里程计作为先验信息，目标点云数据作为观测，通过求解后验概率，估计飞行设备位姿以及地图更新。t时刻飞行设备位姿使用粒子

粒子表示了t时刻飞行设备的位姿估计(x,y,θ)

p(x

其中，x为飞行设备的运动轨迹，m为环境地图中粒子总个数，z代表系统中的观测，u为系统中的控制量。其中p(x

在一些实施方式中，RBPF算法可以包括如下步骤。

(a)参数初始化，粒子群初始化。选取初始粒子，并进行权重归一化。

(b)进行粒子采样。根据提议分布π，由前一代的粒子状态

(c)计算重要性权重。按照重要性采样原则，对每个粒子进行单独的权重分配。每个粒子的权重如下式(六)所示。

粒子权重用于表征提议分布与目标分布之间的一致性，相似度越高，则对应的权重越大，目标分布为由读取到的里程计估计的位姿，提议分布为最近一帧目标点云数据经匹配后得到的位姿。

(d)自适应重采样。根据粒子权重大小的分布情况，计算有效粒子数Neff，当有效粒子数小于设定的最低阈值时，执行重采样。有效粒子数计算公式如下式(七)所示。

式中，W(i)为第i个粒子归一化后的权重，N为粒子总个数。

(f)更新准确的点云地图。基于飞行设备的位姿轨迹

步骤150、将点云地图进行栅格化处理，确定导航地图。

得到准确的点云地图后，来自单线激光雷达的目标点云数据构建的原地图数据量巨大，为此，可以将点云地图进一步栅格化处理，以用于后续进行导航。

在一些实施方式中，步骤150可以包括下述步骤。

(1)将点云地图划分为多个网格。

(2)根据每个网格对应的数据确定每个网格的状态。

(3)基于所有网络的状态确定导航地图。

具体地，如图2所示，可以将点云地图离散化为一定分辨率的多个网格，网格中保存一定的信息表示该位置是否被障碍物占据，对单个网格中的多个障碍物进行聚类。

每个网格可能有两种状态：被障碍物占据，未被障碍物占据，对点云地图中所有网格二值状态的估计就是导航地图的创建过程。

在一些实施方式中，在对点云地图进行栅格化处理时时需考虑一下几个方面。

(a)栅格分辨率的选取。

栅格的大小是关键因素，栅格选的小，环境分辨率较大，环境信息存储量大，决策速度慢；栅格选的大，环境分辨率较小，环境信息存储量小，决策速度快。但在密集障碍物环境中发现路径的能力较弱，因此，需要根据实际应用情况选择栅格分辨率的大小。

(2)网格状态的确定。

根据障碍物相对飞行设备的位置找到对应导航地图中的序号值，并对相应的网格的状态值进行修改。

其中，自由网格为不包含障碍物的网格，其赋值为0。障碍物网格为包含障碍物的网格，其赋值为1。设网格分辨率为d，网格矩阵设为M[n][n]，其中n是网格矩阵的维度，nd*nd即为网格矩阵设置的单线激光雷达感知到的周围圆形环境内接正方形区域大小，对于栅格矩阵第M[n0][n1]个元素，若：

满足0<＝dn0

(3)导航地图的建立

上述构建的单线激光雷达的0-1矩阵可以用来进行路径规划，但为了直观反映出障碍物栅格与飞行设备的相对方位距离，这里进一步在有障碍的M[n0][n1]处，将0-1转换为坐标值，设飞行设备为原点(0,0)，载体坐标系采用右前上坐标系，若M[n0][n1]处的坐标为(xgrid,ygrid)，则有

xgrid＝0.5*nd-(n

xgrid＝(n

将当前帧目标点云数据中的所有(xgrid,ygrid)集合就组成了以飞行汽车本体为中心，分辨率为d，区域范围nd*nd的导航地图。

请参阅图3，本申请实施例提供了一种导航地图构建装置200，应用于飞行设备，飞行设备包括单线激光雷达、惯性传感器以及定位模块；该装置200包括：获取模块210、映射模块220、矫正模块230、构建模块240以及导航模块250。

其中，获取模块210用于获取单线激光雷达检测的当前帧点云数据；

映射模块220用于基于惯性传感器检测的惯性检测数据和定位模块检测的定位数据，将当前帧点云数据映射于飞行设备所在水平面，确定映射点云数据；

矫正模块230用于基于惯性检测数据对映射点云数据进行运动畸变矫正，确定目标点云数据；

构建模块240用于基于相邻帧的目标点云数据确定飞行设备的位姿，并根据位姿构建点云地图；

导航模块250用于将点云地图进行栅格化处理，确定导航地图。

需要说明的是，对于装置类实施例而言，由于其与方法实施例基本相似，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。对于方法实施例中的所描述的任意的处理方式，在装置实施例中均可以通过相应的处理模块实现，装置实施例中不再一一赘述。

另外，在本申请各个实施例中的各功能模块可以集成在一个处理模块中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。

请参阅图4，基于上述的导航地图构建方法，本申请实施例还提供了一种包括可以执行前述导航地图构建方法的飞行设备300。

在本申请的实施例中，飞行设备300包括一个或多个处理器310、存储器320以及一个或多个应用程序。其中，一个或多个应用程序被存储在所述存储器320中，其中，该存储器320中存储有可以执行前述实施例中内容的程序，而处理器310可以执行该存储器中存储的程序。

其中，处理器310可以包括一个或者多个用于处理数据的核以及消息矩阵单元。处理器310利用各种接口和线路连接整个电子设备内的各个部分，通过运行或执行存储在存储器内的指令、程序、代码集或指令集，以及调用存储在存储器内的数据，执行飞行设备的各种功能和处理数据。可选地，处理器310可以采用数字信号处理(Digital SignalProcessing，DSP)、现场可编程门阵列(Field－Programmable Gate Array，FPGA)、可编程逻辑阵列(Programmable Logic Array，PLA)中的至少一种硬件形式来实现。处理器310可集成中央处理器310(Central Processing Unit，CPU)、图像处理器(Graphics ProcessingUnit，GPU)和调制解调器等中的一种或几种的组合。其中，CPU主要处理操作系统、用户界面和应用程序等；GPU用于负责显示内容的渲染和绘制；调制解调器用于处理无线通信。可以理解的是，上述调制解调器也可以不集成到处理器310中，单独通过一块通信芯片进行实现。

存储器320可以包括随机存储器(Random Access Memory，RAM)，也可以包括只读存储器(Read-Only Memory)。存储器320可用于存储指令、程序、代码、代码集或指令集。存储器可包括存储程序区和存储数据区，其中，存储程序区可存储用于实现操作系统的指令、用于实现至少一个功能的指令、用于实现下述各个方法实施例的指令等。存储数据区还可以存储终端在使用中所创建的数据等。

请参考图5，其示出了本申请实施例提供的一种计算机可读取存储介质400的结构框图。该计算机可读取存储介质400中存储有程序代码410，所述程序代码410可被处理器调用执行上述方法实施例中所描述的导航地图构建方法。

计算机可读取存储介质400可以是诸如闪存、EEPROM(电可擦除可编程只读存储器)、EPROM、硬盘或者ROM之类的电子存储器。可选地，计算机可读取存储介质包括非易失性计算机可读介质(non-transitory computer-readable storage medium)。计算机可读取存储介质600具有执行上述导航地图构建方法中的任何方法步骤的程序代码的存储空间。这些程序代码410可以从一个或者多个计算机程序产品中读出或者写入到这一个或者多个计算机程序产品中。程序代码可以例如以适当形式进行压缩。

综上，本申请实施例提供的一种导航地图构建方法、装置、飞行设备及存储介质，其中，导航地图构建方法应用于飞行设备，飞行设备包括单线激光雷达、惯性传感器以及定位模块；该导航地图构建方法包括：获取单线激光雷达检测的当前帧点云数据；基于惯性传感器检测的惯性检测数据和定位模块检测的定位数据，将当前帧点云数据映射于飞行设备所在水平面，确定映射点云数据；基于惯性检测数据对映射点云数据进行运动畸变矫正，确定目标点云数据；基于相邻帧的目标点云数据确定飞行设备的位姿，并根据位姿构建点云地图；将点云地图进行栅格化处理，确定导航地图。从而利用单线激光雷达感知的点云数据，结合定位模块和惯性传感器检测的数据，得到飞行设备更准确的位姿信息，从而实现在存在障碍物的环境中精准构建导航地图。

最后应说明的是：以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不驱使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。