一种基于点线面特征融合的无人机位姿估计方法技术

本发明专利技术提供一种基于点线面特征融合的无人机位姿估计方法，包括如下步骤：S1利用RGB

【技术实现步骤摘要】
一种基于点线面特征融合的无人机位姿估计方法


[0001]本专利技术涉及智能交互
，特别是一种基于点线面特征融合的无人机位姿估计方法。

技术介绍

[0002]无人机具有使用灵活、适应性强和性价比高等优点，在实际需求与技术发展的双重推动下，其应用范围不断拓展。
[0003]无人机位姿估计是指无人机利用传感器等信息构建周围环境地图、获取自身定位、并以估计自身姿态等步骤的过程，是实现无人机自主驾驶的一个重要步骤，实现精准的、高鲁棒的位姿估计对于无人机的自主驾驶有着重要意义。
[0004]现有的无人机运动规划主要实现方式有两种：一种是通过激光SLAM或者基于特征点的视觉SLAM构建三维图，并在其上进行位姿估计；另一种是利用GPS、UWB、运动捕捉器等外部传感器实现对自身位姿的估计。
[0005]现有技术的缺点主要有：定位精度低，抗干扰能力弱，对弱纹理环境易丢失定位，设备价格昂贵等。无人机实现运动规划的重点实现精准位姿估计。对于已有无人机位姿估计算法方法来说：利用GPS、UWB等外部传感器进行无人机位姿估计的方法，对信号环境要求较高，在无GPS的环境下或GPS受干扰严重的环境下极容易出错；利用激光SLAM进行无人机位姿估计的方法需要用到激光雷达，而激光雷达价格昂贵且对于安装条件有一定要求，而无人机这种空间珍贵、负载能力低，因此激光SLAM对于无人机位姿估计而言也不是一个良策。
[0006]而目前常用的视觉SLAM位姿估计算法，其主要利用特征点信息进行位姿估计，对于弱纹理环境而言，极易因提取不到合适的特征导致定位失败，同时，利用特征点进行位姿估计的方案还容易受到外界旋转变换、尺度变换及亮度变换等条件干扰，导致位姿估计精度下降。

技术实现思路

[0007]鉴于现有技术实现位姿估计及运动规划对环境或硬件设备的要求较高的情况，本专利技术旨在提供一种高精度，高鲁棒性的多旋翼无人机位姿估计方法。
[0008]本专利技术的目的采用以下技术方案来实现：
[0009]本专利技术提出一种基于点线面特征融合的无人机位姿估计方法，包括：
[0010]S1利用RGB
‑
D摄像头提取RGB图像及深度图；
[0011]S2根据深度图构建平面参数空间PPS，将深度图从笛卡尔空间映射到平面参数空间PPS；
[0012]S3将反八叉树结构构建，包括：平面参数空间PPS根据八叉树规则，逆向构造网格节点，对于每个网格节点，采用高斯分布进行拟合；
[0013]S4平面特征提取，包括：基于反八叉树的特征提取算法度进行平面特征提取；
[0014]S5直线特征提取，包括：使用LSD算法检对RGB图像进行直线特征提取；
[0015]S6结合RGB图进行点特征提取；
[0016]S7构造点
‑
线
‑
面关联图；
[0017]S8基于相邻两帧的点
‑
线
‑
面关联图进行匹配；
[0018]S9无人机位姿估计，包括：根据匹配的平面特征进行位姿估计，当平面特征数目不足导致无法求解出位姿时，再结合直线特征或采用特征点特征进行位姿估计。
[0019]一种实施方式中，步骤S2包括：对于笛卡尔空间下深度图的任意一点p，存在平面π＝[n
T
,d]T
，满足n
T
p+d＝0，其中n
T
＝[n
x
,n
y
,n
z
]T
为该平面的法向量，d为坐标系原点到平面的距离；平面参数空间PPS和笛卡尔空间之间的映射关系如下：
[0020][0021]其中，其中为PPS的三个轴[n
x
,n
y
,n
z
,d]为上述所示的笛卡尔空间下平面法向量及平面到坐标系原点距离；则点p在PPS下的坐标可表示为
[0022]一种实施方式中，步骤S2还包括：为避免映射时映射到奇点n
T
＝[0,0,
±
1]T
上，在进行映射前，先利用主成分分析计算所有平面共同旋转矩阵R，然后在笛卡尔空间映射到PPS时，先将平面法向量n左乘矩阵R，同样的，在PPS映射到笛卡尔空间时，将PPS中的点p左乘R
T
再进行反映射，这样可以保证映射到PPS上的点尽可能远离奇点。
[0023]一种实施方式中，步骤S3包括：其中每个网格包含了数据点在PPS下的位置均值协方差以及其包含的数据点数目N
cell
，记为
[0024]一种实施方式中，步骤S4包括：
[0025]对平面参数空间PPS划分为64*64个小正方体，通过遍历这一系列正方体找到正方体区间内包含点数超过设定阈值th
pps
的正方体集合Q
pps
，再遍历Q
pps
中正方体，将其中任一正方体C分别作为反八叉树八个子节点，根据八叉树查看对应的七个子节点是否也都在集合Q
pps
中，是则将八个节点整合为一个大节点并利用这八个节点的参数更新大节点的参数，若正方体C存在多组满足整合条件的节点，根据满足的组数判断，满足组数＝8组，则将这8个大节点再组合成一个更大的节点C
up2
，否则，仅取其中数据点数N
cell
最多的大节点其他节点视为没整合，将整合后的节点插入集合Q
pps
尾部，并从集合Q
pps
中移除节点C；遍历完成后，集合Q
pps
剩余的元素均视为对应笛卡尔空间具有相应平面，每个平面的平面特征，用P
π
＝(π，m
pps
，S
pps
，N
pps
)表示，其中π＝[n
T
，d]T
为位置中心m
pps
反映射获得。
[0026]一种实施方式中，步骤S5包括：使用LSD算法检测RGB图像中的直线，并结合深度图信息将其投影到三维相机坐标系下,得到的三维空间直线参数表示为L＝[u
T
,v
T
]T
，其中v表示直线的单位法向量，u表示直线和相机原点构成的平面的法向量，u的数值大小表示直线到相机原点的距离。
[0027]一种实施方式中，步骤S6包括：遍历步骤S4提取的平面，将每个平面对应的RGB图中的部位划分为10*10的网格，并在每个网格内提取10个ORB特征点，仅取其中响应值最大的特征点保留，舍弃其余特征点。
[0028]一种实施方式中，步骤S7包括：将提取到的特征点、平面及直线特征作为关联图节点，设置特征节点为非激活节点，平面及直线节点为激活节点；
[0029]构造特征点及平面之间的边
[0030][0031]index＝r*10+c,该特征点在该平面的第r行第c列网格提取到的；
[本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于点线面特征融合的无人机位姿估计方法，其特征在于，包括：S1利用RGB
‑
D摄像头提取RGB图像及深度图；S2根据深度图构建平面参数空间PPS，将深度图从笛卡尔空间映射到平面参数空间PPS；S3将反八叉树结构构建，包括：平面参数空间PPS根据八叉树规则，逆向构造网格节点，对于每个网格节点，采用高斯分布进行拟合；S4平面特征提取，包括：基于反八叉树的特征提取算法度进行平面特征提取；S5直线特征提取，包括：使用LSD算法检对RGB图像进行直线特征提取；S6结合RGB图进行点特征提取；S7构造点
‑
线
‑
面关联图；S8基于相邻两帧的点
‑
线
‑
面关联图进行匹配；S9无人机位姿估计，包括：根据匹配的平面特征进行位姿估计，当平面特征数目不足导致无法求解出位姿时，再结合直线特征或采用特征点特征进行位姿估计。2.根据权利要求1所述的一种基于点线面特征融合的无人机位姿估计方法，其特征在于，步骤S2包括：对于笛卡尔空间下深度图的任意一点p，存在平面π＝[n
T
,d]
T
，满足n
T
p+d＝0，其中n
T
＝[n
x
,n
y
,n
z
]
T
为该平面的法向量，d为坐标系原点到平面的距离；平面参数空间PPS和笛卡尔空间之间的映射关系如下：其中，其中为PPS的三个轴[n
x
,n
y
,n
z
,d]为上述所示的笛卡尔空间下平面法向量及平面到坐标系原点距离；则点p在PPS下的坐标可表示为3.根据权利要求2所述的一种基于点线面特征融合的无人机位姿估计方法，其特征在于，步骤S2还包括：为避免映射时映射到奇点n
T
＝[0,0,
±
1]
T
上，在进行映射前，先利用主成分分析计算所有平面共同旋转矩阵R，然后在笛卡尔空间映射到PPS时，先将平面法向量n左乘矩阵R，同样的，在PPS映射到笛卡尔空间时，将PPS中的点p左乘R
T
再进行反映射，这样可以保证映射到PPS上的点尽可能远离奇点。4.根据权利要求3所述的一种基于点线面特征融合的无人机位姿估计方法，其特征在于，步骤S3包括：其中每个网格包含了数据点在PPS下的位置均值协方差以及其包含的数据点数目N
cell
，记为5.根据权利要求4所述的一种基于点线面特征融合的无人机位姿估计方法，其特征在于，步骤S4包括：对平面参数空间PPS划分为64*64个小正方体，通过遍历这一系列正方体找到正方体区间内包含点数超过设定阈值th
pps
的正方体集合Q
pps
，再遍历Q
pps
中正方体，将其中任一正方体C分别作为反八叉树八个子节点，根据八叉树查看对应的七个子节点是否也都在集合Q
pps
中，是则将八个节点整合为一个大节点并利用这八个节点的参数
更新大节点的参数，若正方体C存在多组满足整合条件的节点，根据满足的组数判断，满足组数＝8组，则将这8个大节点再组合成一个更大的节点C
up2
，否则，仅取其中数据点数N
cel
最多的大节点其他节点视为没整合，将整合后的节点插入集合Q
pps
尾部，并从集合Q
pps
中移除节点C；遍历完成后，集合Q
pps
剩余的元素均视为对应笛卡尔空间具有相应平面，每个平面的平面特征，用P
π
＝(π，m
pps
，S
pps
，N
pps
)表示，其中π＝[n
T
，d]
T
为位置中心m
pps
反映射获得。6.根据权利要求5所述的一种基于点线面特征融合的无人机位姿估计方法，其特征在于，步骤S5包括：使用LSD算法检测RGB图像中的直线，并结合深度图信息将其投影到三维相机坐标系下,得到的三维空间直线参数表示为L＝[u
T
,v
T
]
T
，其中v表示直线的单位法向量，u表示直线和相机原点构成的平面的法向量，u的数值大小表示直线到相机原点的距离。7.根据权利要求6所述的一种基于点线面特征融合的无人机位姿估计方法，其特征在于，步骤S6包括：遍历步骤S4提取的平面，将每个平面对应的RGB图中的部位划分为10*10的网格，并在每个网格内提取10个ORB特征点，仅取其中响应值最大的特征点保留，舍弃其余特征点。8.根据权利要求7所...

【专利技术属性】
技术研发人员：鲁仁全，翁剑鸿，陶杰，彭慧，徐雍，
申请(专利权)人：广东工业大学，
类型：发明
国别省市：