一种高线数三维激光雷达点云处理方法及系统与流程
本发明涉及智能感知,尤其涉及一种高线数三维激光雷达点云处理方法及系统。
背景技术:
1、随着机器人技术的快速发展,机器人在工业制造、家庭服务、特种作业等领域的应用日益广泛。机器人作业场景日益复杂,对环境感知的精度和实时性提出了更高要求。三维激光雷达作为机器人环境感知的核心传感器,在实际工程实现中仍面临诸多技术挑战。
2、首先,激光雷达线数不足导致感知精度有限。现有机器人用激光雷达通常为16线或32线,垂直角分辨率仅为1°至2°,无法实现高密度三维空间扫描。行业测试数据显示,32线激光雷达在10m距离处的垂直点间距约为17cm,无法识别小型障碍物(如直径10cm的管道)。96线激光雷达可将垂直点间距缩小至5.7cm,但实现96通道vcsel激光器的同步驱动和信号处理是技术难点。
3、其次,复杂工况下点云数据质量严重退化。机器人作业环境存在粉尘、强光、振动等复杂工况,会导致点云数据出现噪声点、异常点、缺失点等问题。工业现场测试表明,在粉尘环境下,点云噪声比例可达30%以上;在强光环境下,反射强度方差增大2-3倍;在振动环境下,距离方差增大5-10倍。传统固定参数滤波算法无法有效应对这些复杂工况。
4、第三,动态障碍物跟踪仅实现简单识别,无法预判轨迹。现有技术通常只能识别障碍物的当前位置,无法预测未来时刻的位置。机器人高速作业场景(速度>1m/s)要求提前500ms预判碰撞风险,但传统方案只能在障碍物进入防护区域后才触发报警,无法满足提前预警需求。
5、第四,高密度点云数据的实时处理效率低。96线激光雷达每秒产生约200万个点云数据点,在30ms内完成滤波、重建、跟踪是技术难点。传统arm处理器方案的处理延迟通常>50ms,无法满足实时性要求。
技术实现思路
1、有鉴于此,本发明提供了一种高线数三维激光雷达点云处理方法及系统,用以提升感知精度,增强复杂工况的适应性和动态目标跟踪能力,实现高效率实时处理要求。
2、第一方面,本发明提供了一种高线数三维激光雷达点云处理方法,所述方法包括:
3、步骤1、构建96通道垂直腔面发射激光器vcsel收发架构,采集原始点云数据;
4、步骤2、基于原始点云数据,提取噪声特征判断工况类型,并自适应调整滤波参数;
5、步骤3、根据工况类型和滤波参数,采用自适应高斯滤波算法对点云数据进行降噪,获得滤波后点云数据;
6、步骤4、基于滤波后点云数据,采用卡尔曼滤波算法对动态障碍物进行跟踪与轨迹预判,得到障碍物轨迹信息和碰撞风险等级;
7、步骤5、根据障碍物轨迹信息和碰撞风险等级,采用现场可编程门阵列fpga与arm并行处理流水线对各任务进行并行分配与实时处理,输出处理结果。
8、可选地,所述步骤1包括:
9、采用96通道vcsel激光器阵列发射激光束,所有通道共享同一触发信号源并通过时钟分配芯片分发,通道间同步偏差小于100ps;通过无刷直流电机驱动旋转扫描机构实现360度扫描;反射激光由雪崩光电二极管接收并经时间数字转换器转换为时间差。
10、可选地,包括:
11、96个vcsel激光器采用并联驱动方式,每个激光器由独立的驱动芯片控制;驱动芯片接收来自现场可编程门阵列fpga的触发信号,触发信号为5ns脉宽的ttl电平信号;驱动芯片内部采用恒流源设计,峰值电流为15a,上升时间小于2ns,单通道峰值输出功率为75w;为保证96通道同步发射,所有通道共享同一个触发信号源,触发信号通过时钟分配芯片分发到96个驱动芯片,通道间同步偏差小于100ps;
12、每个vcsel激光器发出的光束发散角约为20°×30°,通过光学系统准直;采用非球面透镜组进行准直,第一片透镜为非球面镜,将发散角压缩到0.3mrad×0.5mrad;第二片透镜为柱面镜,用于调整光束形状,光学效率大于85%;
13、vcsel激光器阵列和光学系统安装在一个旋转平台上,由无刷直流电机bldc驱动旋转;无刷直流电机转速为1200rpm,对应20hz扫描频率;旋转平台采用交叉滚子轴承支撑;角度编码器采用光电编码器,分辨率18位,精度为±5角秒,实时输出旋转角度信息;
14、反射回来的激光由雪崩光电二极管apd接收,apd响应波长为905nm,响应度为0.9a/w,暗电流小于1na,带宽为1ghz;apd输出的微弱电流信号通过跨阻放大器tia转换为电压信号,tia增益10kω,带宽为500mhz,输入噪声小于5pa/hz;电压信号通过时间数字转换器tdc转换为时间差,tdc分辨率为50ps,精度为±100ps,通道数96通道;每个通道独立测量激光飞行时间,计算距离的表达式为:d=c×t/2,其中d为测量距离,c为光速,t为激光飞行时间。
15、可选地,所述步骤2包括:
16、基于原始点云数据,将三维空间划分为体素网格统计平均点云密度,计算每个点与k近邻点的距离标准差,统计异常点比例,计算强度标准差,得到四个噪声特征;根据阈值判断工况类型并确定对应的滤波参数调整策略,输出工况类型和滤波参数;
17、步骤21、点云密度计算,将三维空间划分为体素网格,统计每个体素内的点云数量,计算平均点云密度;正常环境下,点云密度的范围为0.5-2.0点/m3;粉尘环境下,点云密度小于0.5点/m3;
18、步骤22、距离方差计算,对于每个点云数据点,计算其与k近邻点的距离标准差;正常环境下,距离标准差的范围为0.01-0.05m;振动环境下,距离标准差大于0.1m;
19、步骤23、异常点比例计算,正常环境下,异常点比例小于5%;强光环境下,异常点比例大于20%;
20、步骤24、强度方差计算,计算所有点云数据点的强度标准差,正常环境下,强度标准差小于50;强光环境下,强度标准差大于100;
21、步骤25、工况类型包括粉尘环境、强光环境、振动环境和正常环境,其工况判断的阈值包括密度阈值、异常点比例阈值、强度标准差阈值和距离标准差阈值;粉尘环境的密度阈值小于0.5点/m3,异常点比例阈值大于20%;强光环境的强度标准差阈值大于100;振动环境的距离标准差阈值大于0.1m;正常环境的密度阈值大于或等于0.5点/m3,异常点比例阈值小于或等于20%,强度标准差阈值小于或等于100,距离标准差阈值小于或等于0.1m2;
22、步骤26、滤波参数自适应调整,根据工况类型调整滤波参数:粉尘环境增大滤波半径σ0×1.5,降低权重阈值×0.8;强光环境增大强度滤波阈值×1.3;振动环境增大距离滤波阈值×1.2;正常环境使用基准参数。
23、可选地,所述步骤3包括:
24、基于工况类型和滤波参数,进行高速点云滤波;计算当前帧噪声点数量与总点数的噪声比例,根据噪声比例计算自适应滤波标准差,其中基准标准差为0.1m,调整系数为1.0,噪声比例系数为2.0;对每个点查找k近邻点,按高斯权重公式计算权重并进行加权平均滤波,输出滤波后点云数据;
25、步骤31、噪声点识别与噪声比例计算,对于每个点云数据点,计算其与k近邻点的平均距离;若平均距离超过阈值,则判定当前点为噪声点;统计整帧点云数据中的噪声点数量n_noise,计算噪声比例r_noise=n_noise/n_p,其中n_p为当前帧总点数;
26、步骤32、自适应滤波参数调整,其表达式为:σ'=α•σ0• (1 + β • n_noise / n_p);其中σ'为自适应滤波标准差,α为调整系数,σ0为基准标准差,β为噪声比例系数;当噪声比例为0时,σ'=σ0;当噪声比例为30%时,σ'=1.6σ0;
27、步骤33、k近邻高斯滤波,对于每个点云数据点,查找其k近邻点,计算高斯权重并进行加权平均滤波,高斯滤波权重的表达式为:wij= exp(-dij2/ (2σ'2));其中,wij为高斯滤波权重,dij为点i与点j的欧几里得距离。
28、可选地,所述步骤4包括:
29、基于滤波后点云数据,采用欧几里得距离聚类算法对点云进行聚类分割以检测障碍物,采用匈牙利算法对连续两帧障碍物进行关联与id分配;对每个跟踪的障碍物建立卡尔曼滤波器,以位置、速度、加速度九维状态向量进行状态估计;根据估计状态预测未来轨迹,计算机器人与预判轨迹的最小距离并输出碰撞风险等级;
30、步骤41、障碍物检测与分割,对滤波后的点云数据进行聚类分割,将空间上相邻的点云聚集为一个障碍物;聚类算法采用欧几里得距离聚类,距离阈值为0.1m;对于每个聚类,计算其边界框,得到障碍物的位置和尺寸;
31、步骤42、障碍物关联与id分配,对于连续两帧点云数据,将当前帧的障碍物与上一帧的障碍物进行关联;关联方法采用匈牙利算法,代价函数为两个障碍物边界框中心点的欧几里得距离;若关联成功,则继承上一帧的障碍物id;若关联失败即出现新的障碍物,则分配新的id;
32、步骤43、卡尔曼滤波状态估计,对于每个跟踪的障碍物,建立卡尔曼滤波器进行状态估计;状态向量:x_t=[x,y,z,vx,vy,vz,ax,ay,az],其中x,y,z为t时刻障碍物位置分量,vx,vy,vz为t时刻速度分量,ax,ay,az为t时刻加速度分量;
33、状态转移方程:x_(t+1)=f·x_t+w_t,其中f为9×9状态转移矩阵,w_t为过程噪声,q为协方差矩阵;
34、观测方程:z_t=h·x_t+v_t,其z_t=[x,y,z]t为观测向量,h为3×9观测矩阵,v_t为测量噪声,r为协方差矩阵;
35、步骤44、轨迹预判,根据卡尔曼滤波器估计的状态,预测障碍物未来时刻的位置,轨迹预判表达式为:p(t+δt)=p(t)+v(t)·δt+0.5·a(t)·δt2,其中p(t)为当前时刻障碍物位置向量,v(t)为速度向量,a(t)为加速度向量,δt为预判时间;
36、步骤45、碰撞风险评估,计算机器人与预判轨迹的最小距离;碰撞风险分为三个等级:当最小距离在0.5-1.0m之间时,为预警等级;当最小距离在0.3-0.5m之间时,为危险等级;当最小距离小于0.3m时,为紧急等级。
37、可选地,所述步骤5包括:
38、基于障碍物轨迹信息和碰撞风险等级,采用fpga执行时间戳标注和坐标转换实时性要求高的任务,采用arm执行工况判断和滤波处理算法复杂度高的任务,fpga与arm之间通过直接内存访问通道以双缓冲机制传输数据,并行完成点云处理;输出处理结果用于机器人运动控制和安全防护;
39、步骤51、任务分配策略,将点云处理任务分为两类:①实时性要求高的任务:原始数据预处理、噪声特征提取、时间戳标注、坐标转换;②算法复杂度高的任务:自适应滤波、三维重建、障碍物跟踪、轨迹预判;
40、步骤52、fpga预处理流程,fpga接收来自时间数字转换器tdc的原始时间数据,执行以下处理:(1)时间戳标注:为每个数据点添加精确的时间戳,时间戳来自fpga内部的高精度时钟;(2)空间坐标转换:将极坐标转换为笛卡尔坐标,转换表达式为:x=d•cosθ•cosφ,y=d•cosθ•sinφ,z=d•sinθ,其中d为激光测距距离,θ为俯仰角,φ为方位角;(3)初步噪声过滤:过滤掉距离为0或超过最大测量范围100m的无效数据点;(4)噪声特征提取:计算点云密度、距离方差、异常点比例、强度方差,为步骤2工况类型判断提供依据;
41、步骤53、高速数据传输,fpga预处理完成后,通过dma通道将数据传输到arm处理器;dma传输采用双缓冲机制,即fpga写入缓冲区a的同时,arm读取缓冲区b,反之亦然,dma传输延迟小于10μs;
42、步骤54、arm高级处理流程,arm处理器接收fpga预处理后的数据,执行以下处理:(1)工况类型判断:根据fpga提取的噪声特征,判断工况类型;(2)自适应滤波参数计算:根据工况类型,计算自适应滤波参数σ';(3)自适应高斯滤波:使用计算得到的滤波参数,对点云数据进行k近邻高斯滤波;(4)三维重建:基于滤波后的点云数据,构建三维空间模型;(5)障碍物跟踪:对动态障碍物进行检测、关联、跟踪,估计其运动状态;(6)轨迹预判:根据障碍物的运动状态,预测其未来500ms的轨迹,评估碰撞风险;
43、步骤55、性能优化,为确保总处理延迟小于30ms,在fpga与arm并行处理流水线下采取以下优化措施:fpga采用流水线设计,每个处理阶段并行执行;arm采用多线程设计,滤波、重建、跟踪并行执行;内存采用ddr4,带宽>10gb/s;算法采用simd指令优化。
44、第二方面,本发明提供了一种高线数三维激光雷达点云处理系统,所述系统用于执行第一方面或第一方面任一可能的实现方式中的高线数三维激光雷达点云处理方法;所述系统包括激光雷达硬件层、点云处理算法层和应用输出层;
45、激光雷达硬件层包括多通道vcsel激光发射模块、高精度旋转扫描机构、光电接收与信号处理模块,三个模块依次连接,多通道vcsel激光发射模块输出激光束经高精度旋转扫描机构扫描后由光电接收与信号处理模块接收;
46、点云处理算法层包括点云数据采集模块、高速点云滤波模块、三维重建模块、动态障碍物跟踪模块,四个模块依次串联,数据从点云数据采集模块流向高速点云滤波模块再流向三维重建模块和动态障碍物跟踪模块;
47、应用输出层包括环境三维模型、障碍物轨迹预判、人机距离监测、碰撞风险预警,接收点云处理算法层的处理结果并输出安全控制信号。
48、本发明提供的技术方案中,该方法包括构建96通道垂直腔面发射激光器vcsel收发架构,采集原始点云数据;基于原始点云数据,提取噪声特征判断工况类型,并自适应调整滤波参数;根据工况类型和滤波参数,采用自适应高斯滤波算法对点云数据进行降噪,获得滤波后点云数据;基于滤波后点云数据,采用卡尔曼滤波算法对动态障碍物进行跟踪与轨迹预判,得到障碍物轨迹信息和碰撞风险等级;根据障碍物轨迹信息和碰撞风险等级,采用现场可编程门阵列fpga与arm并行处理流水线对各任务进行并行分配与实时处理,输出处理结果,该方法提升了感知精度,增强了复杂工况的适应性和动态目标跟踪能力,实现了高效率实时处理要求。
版权声明:本站专利文献数据主要来源于国家知识产权局等公开渠道,由 技术新讯 进行整理汇编。页面设计及数据汇编的著作权归本站所有。全部内容仅供个人学习、科研评估与技术交流参考,严禁用于任何商业用途(包括但不限于直接生产制造、技术转让、倒卖数据等)。
免责条款:专利技术具有时效性,本站不对资料的绝对准确性、完整性和可实施性提供任何明示或暗示的担保。因参考本站内容造成的直接或间接损失,本站不承担相关法律责任。若您认为本站内容有侵权问题,请及时提供权属证明与我们联系,本站将在核实后依法处理。反馈意见:yyfuon@163.com