一种无人机毫米波雷达自适应地面拟合避障测高方法及系统
本发明涉及无人机环境感知与自主飞行控制,尤其涉及一种无人机毫米波雷达自适应地面拟合避障测高方法及系统。
背景技术:
1、无人机在农业植保、电力巡检、地形测绘及应急救援等低空飞行场景中,通常需要同时获取前方障碍物信息和距地高度信息,以实现自主避障、地形跟随及飞行高度控制。毫米波雷达具有受光照、雨雾和粉尘影响较小的特点,适于作为无人机环境感知装置。
2、现有无人机通常分别配置前向避障雷达和下视测高雷达。前向避障雷达用于检测飞行方向上的障碍物,下视测高雷达用于测量无人机与地面之间的距离。该方案需要设置两套雷达,增加了设备成本、机载重量、功耗及安装调试难度,不利于小型无人机的集成和续航。
3、为减少雷达数量,现有技术也尝试利用单个毫米波雷达兼顾避障和测高。然而,雷达点云中通常同时包含地面散射点、障碍物点和噪声点。若直接将地面散射点作为杂波滤除,会丢失测高和地形识别所需的信息;若仅选取最强点或最近点作为地面点,又容易受到地表反射差异、局部凸起和孤立噪声的影响,难以获得稳定的地面基准。
4、此外,现有地面拟合方法多采用固定阈值或固定阶数模型,难以同时适应平坦地面、坡地及起伏地形;等权拟合未充分考虑散射点的距离、邻域密度及其与地面变化趋势的一致性,异常点容易引起拟合偏差,进而造成地面点误判、测高波动及障碍物漏检。
5、因此,本发明提供一种无人机毫米波雷达自适应地面拟合避障测高方法及系统,利用同一毫米波雷达点云自适应拟合地面,实现无人机距地测高、障碍物识别、避障控制及地形跟随。
技术实现思路
1、针对上述现有技术缺陷,本发明的目的是提供一种无人机毫米波雷达自适应地面拟合避障测高方法及系统,旨在解决单雷达在复杂地形下难以准确分离地面点与障碍物点,导致测高波动和避障误判的技术问题。
2、为实现上述目的,本发明提供一种无人机毫米波雷达自适应地面拟合避障测高方法,包括:
3、通过安装于无人机上的同一毫米波雷达采集点云数据,所述毫米波雷达的探测波束同时覆盖无人机前方探测区域和前下方地面区域,所述点云数据至少包括各散射点的距离坐标、高度坐标和反射强度,对所述点云数据进行过滤并保留用于表征地面起伏的散射点;
4、根据过滤后点云的高度离散程度和沿飞行前向的相邻高度变化程度确定地形复杂度,依据所述地形复杂度确定地面曲线的拟合阶数及地面判定阈值的调节量;
5、沿飞行前向将点云划分为多个距离区段,从各距离区段筛选低高度点作为候选地面点,根据各候选地面点到毫米波雷达的距离、邻域点云密度及其高度与地面趋势预测高度的一致程度确定综合权重;
6、按照所述拟合阶数和综合权重对候选地面点进行加权多项式拟合,根据候选地面点相对拟合地面曲线的高度残差及所述地形复杂度确定地面判定阈值,更新地面点集合、重新确定综合权重并重新拟合,直至满足收敛条件,得到目标地面曲线和非地面点云;
7、根据所述目标地面曲线确定无人机距地高度,并以所述目标地面曲线为基准从非地面点云中识别障碍物,根据所述距地高度及障碍物的位置和高度向飞行控制装置输出高度调节指令和避障指令。
8、作为上述方案进一步的改进,所述毫米波雷达发射线性调频连续波;采集所述点云数据时,将恒虚警检测采用的检测信杂噪比阈值设置为低于仅进行障碍物检测时采用的检测信杂噪比阈值,以保留目标散射点以及用于地面拟合的地面散射点;
9、对所述点云数据进行过滤包括:根据预设探测距离范围进行距离过滤,保留处于所述预设探测距离范围内的点;根据预设反射强度阈值进行强度过滤,剔除雷达散射截面积低于所述预设反射强度阈值的点;以及采用统计滤波,根据预设邻域点数和预设标准差倍数剔除离群点。
10、优选的,预设探测距离范围为0.5~30m,预设反射强度阈值为-10dbsm,统计滤波采用的邻域点数为20、标准差倍数为2.0。上述参数可根据毫米波雷达的探测范围、点云密度及作业环境进行设置。
11、作为上述方案进一步的改进,设过滤后的第个点的高度坐标为、前向距离坐标为,所述地形复杂度按照下式确定,具体的,
12、先将点云投影到xy平面,按照下式计算高度标准差:
13、;
14、再计算高度变化率(反映地形陡峭程度),对点云按y坐标排序,计算相邻点的最大相邻高度差:
15、;
16、然后按照下式计算地形复杂度:
17、;
18、式中,为点云数量,为高度坐标平均值,和分别为对应项的权重系数,通过实验优化获得,优选的,,;
19、各点按照前向距离坐标由小到大排列;
20、当时采用一阶线性拟合(平坦地形);
21、当时采用二阶多项式拟合(起伏地形);
22、当时采用三阶多项式拟合(复杂地形)。
23、作为上述方案进一步的改进,沿飞行前向每间隔划分一个距离区段,在每个距离区段内选取高度最低的k个点()作为候选地面点,并剔除相对于相邻地面点的高度差大于预设高度差的悬空点,优选的,所述预设高度差为1m,;第个候选地面点的综合权重按照下式确定:
24、;
25、其中距离权重按照下式计算获得:
26、;
27、密度权重按照下式计算获得:
28、;
29、一致性权重按照下式计算获得:
30、;
31、式中,为候选地面点到毫米波雷达的距离,表示距离权重衰减系数,用于控制候选地面点的距离权重随其与毫米波雷达之间距离增加而衰减的速度;为以该候选地面点为中心的0.5m半径邻域内的点云数量,为各候选地面点对应的最大邻域点云数量,为相邻已拟合地面段在该候选地面点处的预测高度。
32、作为上述方案进一步的改进,按照所述拟合阶数n建立地面曲线:
33、;
34、通过最小化下式确定拟合系数:
35、;
36、按照下式确定第i个候选地面点相对于所述拟合地面曲线的高度残差:
37、;
38、根据各候选地面点的高度残差确定残差中位数μ和稳健离散程度,并根据所述残差中位数、稳健离散程度及地形复杂度确定地面判定阈值:
39、;
40、;
41、;
42、;
43、;
44、将满足<的候选地面点保留于地面点集合,根据更新后的地面点集合重新确定综合权重和拟合系数并进行迭代更新,直至满足下式所示的收敛条件或者达到预设最大迭代次数:
45、;
46、式中,为拟合地面曲线,y为飞行前向坐标,n为拟合阶数,为第j阶拟合系数,n为参与当前次拟合的候选地面点数量,和分别为第i个候选地面点的高度坐标和飞行前向坐标,为第i个候选地面点的综合权重,为第i个候选地面点相对于拟合地面曲线的高度残差,为地面判定阈值,μ为高度残差中位数,为高度残差的稳健离散程度,为稳健尺度换算系数,k为地面判定阈值的自适应调节系数,为基础调节系数,γ为地形复杂度调节系数,tc为地形复杂度,和分别为当前次迭代和前一次迭代得到的拟合系数向量,表示二范数,ε为预设收敛阈值。
47、优选的,当以m计时,可通过预先标定确定和,例如=2.5,γ=0.5m-1;=;=
48、作为上述方案进一步的改进,根据所述目标地面曲线确定无人机距地高度,包括:在第帧,将所述目标地面曲线近似为使拟合误差最小的地面直线:
49、;
50、根据所述地面直线的斜率和截距,按照下式确定拟合地面倾角、初始距地距离以及经标定校正的距地高度:
51、;
52、;
53、;
54、其中,表示点云帧序号,为第帧地面直线的斜率,为第帧地面直线在高度坐标轴上的截距,为所述地面直线相对于雷达坐标系前向轴的倾斜角,为雷达参考点至拟合地面的初始法向距离估计值,为雷达参考点至无人机高度基准点在拟合地面法向上的等效偏移量,为高度尺度校正系数,为高度偏置校正量,为第帧经尺度和偏置校正后的距地高度估计值;
55、对所述距地高度估计值进行异常检测,当:时,将当前帧高度估计值确定为异常值,并采用卡尔曼滤波预测高度进行替换,得到修正后的高度;对连续多帧修正高度进行加权平滑:
56、;
57、其中,为预设高度跳变阈值,为第帧最终输出的距地高度,优选为2m。
58、作为上述方案进一步的改进,从所述非地面点云中识别障碍物,包括:对与所述目标地面曲线之间的高度残差小于预设近地判定阈值的非地面点进行二次判别,根据所述非地面点的雷达散射截面积与地面点雷达散射截面积的统计分布确定其是否为障碍物候选点,优选的,所述预设近地判定阈值为0.3m;
59、沿飞行前向对所述障碍物候选点进行分段,根据各段内障碍物候选点的高度坐标平均值和高度坐标标准差剔除高度异常点,并对剩余障碍物候选点进行基于密度的有噪空间聚类,得到障碍物点云簇;
60、根据所述障碍物点云簇的高度坐标标准差和预先标定的比例系数确定障碍物估计高度:
61、;
62、式中,为障碍物估计高度,为聚类后障碍物点云簇的高度坐标标准差,c为根据实测标定确定的比例系数;
63、将所述目标地面曲线向上偏移第一高度形成底线,将所述底线向上偏移第二高度形成中线,将所述中线向上偏移第三高度形成顶线,分别确定障碍物点云簇在底线与中线之间、中线与顶线之间以及顶线以上的点数占比;当底线与中线之间的点数占比大于第一占比阈值,且中线与顶线之间的点数占比小于第二占比阈值时,将对应障碍物判定为坡状障碍物;当顶线以上的点数占比大于第三占比阈值时,将对应障碍物判定为高大障碍物;优选的,所述第一占比阈值为60%,所述第二占比阈值为30%,所述第三占比阈值为20%;
64、其中,所述第一高度、第二高度和第三高度根据无人机距地高度和飞行任务参数确定。
65、第二方面,本发明还提供一种无人机毫米波雷达自适应地面拟合避障测高系统,包括无人机本体、毫米波雷达、数据处理装置和飞行控制装置;
66、所述毫米波雷达设于无人机本体,探测波束同时覆盖前方和前下方地面区域,用于采集包括散射点距离坐标、高度坐标和反射强度的点云数据;
67、所述数据处理装置包括点云预处理模块、地形评估模块、地面拟合模块和避障测高模块;
68、所述点云预处理模块用于过滤点云并保留表征地面起伏的散射点;
69、所述地形评估模块用于根据过滤后点云的高度离散程度和沿飞行前向的相邻高度变化程度确定地形复杂度,并据此确定拟合阶数及地面判定阈值调节量;
70、所述地面拟合模块用于沿飞行前向分段筛选低高度候选地面点,根据其距离、邻域点云密度及与地面趋势预测高度的一致程度确定综合权重,按所述拟合阶数进行加权多项式拟合,并根据高度残差和地形复杂度确定地面判定阈值,迭代更新地面点、综合权重和拟合结果直至收敛,得到目标地面曲线和非地面点云;
71、所述避障测高模块用于根据目标地面曲线确定距地高度,并以其为基准识别非地面点云中的障碍物,确定障碍物的位置和高度,生成高度调节指令和避障指令;
72、所述飞行控制装置用于根据所述指令控制无人机执行高度调节和避障动作。
73、作为上述方案进一步的改进,所述毫米波雷达为具有俯仰维探测能力的三维毫米波雷达,包括沿无人机竖直方向排列的多个水平极化天线,所述毫米波雷达发射线性调频连续波,并相对于无人机水平基准面以预设安装俯仰角设置;所述预设安装俯仰角根据所述毫米波雷达的俯仰波束宽度、所述前方探测区域和所述前下方地面区域确定,以使所述毫米波雷达的俯仰探测波束同时覆盖所述前方探测区域和所述前下方地面区域;
74、优选的,毫米波雷达相对于无人机水平基准面的安装俯仰角可根据雷达俯仰波束宽度及所需探测区域进行设置,例如设置在-30°至12°范围内。安装俯仰角的正负方向按照预设雷达坐标系确定,其具体取值以雷达俯仰探测波束能够同时覆盖无人机前方避障区域和前下方地面测高区域为准。
75、作为上述方案进一步的改进,所述避障测高模块包括高度解算单元、姿态校验单元、高度优化单元和障碍物分析单元;
76、所述高度解算单元用于将目标地面曲线近似为地面直线,根据地面直线与雷达坐标系原点之间的位置关系、毫米波雷达的安装偏移以及预先标定的尺度和偏置参数确定无人机距地高度;
77、所述姿态校验单元用于根据地面直线斜率和毫米波雷达安装倾角确定雷达估计俯仰角,并在其与所述飞行控制装置提供的俯仰角之间的差值超过预设阈值时舍弃当前帧数据;
78、所述高度优化单元用于对距地高度进行异常检测,在检测到高度跳变值时采用卡尔曼滤波预测值进行修正,并对修正后的距地高度进行多帧平滑;
79、所述障碍物分析单元用于对非地面点云进行近地反射强度判别、异常点剔除和密度聚类,确定障碍物的位置,根据障碍物点云簇的高度分布确定障碍物高度,并根据所述障碍物点云簇相对于底线、中线和顶线的点数占比确定障碍物类别。
80、由于本发明采用了以上技术方案,使本发明具备的有益效果在于:
81、本发明提供一种无人机毫米波雷达自适应地面拟合避障测高方法,利用同一毫米波雷达获取同时覆盖无人机前方区域和前下方地面区域的点云,并通过自适应地面拟合建立统一的地面参考基准,在同源点云基础上实现无人机距地测高、障碍物识别、避障控制及地形跟随,具有以下有益效果:
82、1、实现同一毫米波雷达兼顾避障和测高。本发明使同一毫米波雷达的探测波束同时覆盖前方避障区域和前下方地面区域,使采集的同一组点云中既包含用于表征地面起伏的地面散射点,又包含用于障碍物识别的非地面散射点;通过对所述同源点云进行统一处理,即可分别获得距地高度和障碍物信息,无需分别设置前向避障雷达和下视测高雷达,从而减少雷达硬件数量以及由多雷达配置带来的机载重量、功耗、安装空间和数据配准需求。该效果与原始交底书中针对两套独立雷达存在成本、功耗、重量及安装复杂度问题的分析相对应。
83、2、提高复杂地形下地面拟合的适应性,为距地测高提供稳定的地面基准。本发明没有将地面散射点直接作为杂波滤除,而是保留能够表征地面起伏的散射点,并根据点云高度离散程度和沿飞行前向的相邻高度变化程度确定地形复杂度,再据此调节地面曲线的拟合阶数和地面判定阈值。因此,当无人机由平坦区域进入坡地或起伏区域时,地面拟合策略能够随地形变化进行调整,降低采用固定拟合模型时局部起伏地面被误判为障碍物或者地面基准发生偏移的可能,为后续距地高度计算提供较为稳定的参考。
84、3、降低异常散射点对地面模型的影响,提高地面点与障碍物点的分离可靠性。本发明沿飞行前向分段筛选候选地面点,并综合候选点与毫米波雷达之间的距离、邻域点云密度以及其高度与地面趋势预测高度的一致程度确定综合权重,使可信程度较高的候选地面点在拟合过程中具有较大作用;同时,根据高度残差和地形复杂度更新地面点集合,并重新确定综合权重和拟合曲线直至满足收敛条件。由此可逐步降低孤立点、远距离误差点以及偏离地面变化趋势的散射点对拟合结果的影响,减小地面点误判造成的测高波动和障碍物识别偏差。
85、4、利用同一目标地面曲线同时服务于距地测高和障碍物识别。收敛得到的目标地面曲线一方面用于确定无人机相对于当前地面的距地高度,另一方面作为非地面点云中障碍物位置和高度判断的统一参考。由于测高和障碍物识别来源于同一毫米波雷达点云并采用同一地面模型,可减少分别采用不同传感器或不同地面基准时产生的空间基准不一致,使测高结果与障碍物识别结果在空间关系上保持对应。
86、5、基于测高结果和障碍物信息协同实现避障控制和地形跟随。本发明根据目标地面曲线获得当前距地高度,同时根据非地面点云获得障碍物的位置和高度,由此向飞行控制装置分别提供高度调节依据和避障依据。当目标地面发生坡度或起伏变化时,可依据距地高度进行飞行高度调整;当前方存在突出于目标地面的障碍物时,可依据障碍物位置和高度执行避让,从而使无人机的高度调节与前向避障建立在同一地面参考基准下,有利于在坡地、起伏地面等复杂地形中兼顾地形跟随和避障控制。
版权声明:本站专利文献数据主要来源于国家知识产权局等公开渠道,由 技术新讯 进行整理汇编。页面设计及数据汇编的著作权归本站所有。全部内容仅供个人学习、科研评估与技术交流参考,严禁用于任何商业用途(包括但不限于直接生产制造、技术转让、倒卖数据等)。
免责条款:专利技术具有时效性,本站不对资料的绝对准确性、完整性和可实施性提供任何明示或暗示的担保。因参考本站内容造成的直接或间接损失,本站不承担相关法律责任。若您认为本站内容有侵权问题,请及时提供权属证明与我们联系,本站将在核实后依法处理。反馈意见:yyfuon@163.com