一种基于多传感器融合的农业无人车定位里程计方法及系统.pdf

(19)国家知识产权局(12)发明专利申请(10)申请公布号CN115451948A(43)申请公布日2022.12.09(21)申请号202210950380.X(22)申请日2022.08.09(71)申请人中国科学院计算技术研究所地址100080北京市海淀区中关村科学院南路6号(72)发明人赵紫旭陆在旺张玉成刘子辰龙隆刘清华(74)专利代理机构北京律诚同业知识产权代理有限公司11006专利代理师祁建国陈思远(51)Int.Cl.G01C21/16(2006.01)G01C22/00(2006.01)G01S17/93(2020.01)G01S17/86(2020.01)权利要求书5页说明书12页附图2页(54)发明名称一种基于多传感器融合的农业无人车定位里程计方法及系统(57)摘要本发明提出一种基于多传感器融合的农业无人车定位里程计方法和系统，包括：通过视觉传感器检测获取农业无人车所处环境的多个视觉特征点，利用深度传感器采集到的环境点云获取视觉特征点的深度信息，将获取到深度信息的视觉特征点作为稳定特征点，统计稳定特征点数量占视觉特征点总数的比例，作为复杂度；惯性传感器采集相对位姿信息，根据相对位姿信息生成惯性位姿信息；通过卡尔曼滤波器融合惯性位姿信息和视觉特征点，得到第一位姿信息，根据相对位姿信息为环境点云进行去畸形化处理，得到校正点云，以结合惯性位姿信息执行点云配准处理，得到第二位姿信息；根据复杂度，选择第一位姿信息或第二位姿信息，作为农业无人车的最终定位结果。CN115451948ACN115451948A权利要求书1/5页1.一种基于多传感器融合的农业无人车定位里程计方法，其特征在于，包括：步骤1、农业无人车设有视觉传感器、惯性传感器和深度传感器，通过该视觉传感器检测获取该农业无人车所处环境的多个视觉特征点，同时利用该深度传感器采集到的环境点云获取视觉特征点的深度信息，将获取到深度信息的视觉特征点作为稳定特征点，统计稳定特征点数量占视觉特征点总数的比例，作为当前环境的复杂度；步骤2、该惯性传感器采集相对位姿信息，根据该相对位姿信息生成惯性位姿信息；通过卡尔曼滤波器融合该惯性位姿信息和该视觉特征点，得到第一位姿信息，根据该相对位姿信息为该环境点云进行去畸形化处理，得到校正点云，以结合该惯性位姿信息执行点云配准处理，得到第二位姿信息；步骤3、根据该复杂度，选择该第一位姿信息或该第二位姿信息，作为该农业无人车的最终定位结果。2.如权利要求1所述的基于多传感器融合的农业无人车定位里程计方法，其特征在于，该去畸形化处理包括：其中Δφtk代表t时刻接收到的点云中的第k个点的姿态修正信息，Δptk代表t时刻接收到的点云中的第k个点的位置修正信息，t代表该点实际接收到的时刻，tk代表该帧点云接受的起始时间，Δt代表该帧点云接收经历的总时长，Δφk，k+1和Δpk，k+1分别代表该惯性传感器计算出的该帧点云相对于上一帧的姿态变换和位置变换；融合该姿态修正信息和该位置修正信息，构成位姿变换矩阵，通过该姿变换矩阵与该环境点云的3D坐标相乘，得到该校正点云的3D坐标。3.如权利要求1所述的基于多传感器融合的农业无人车定位里程计方法，其特征在于，该惯性位姿信息的生成过程包括：该惯性位姿信息包括和分别代表姿态和位置的变换信息，tk和tk‑1代表接收到两帧连续点云或者图像的时刻，下标k代表点云或者图像的序号，ωt和at代表t时该惯性传感器采集的角速度和加速度，vk‑1代表接收到第k‑1点云或者图像时测量得到的速度；4.如权利要求3所述的基于多传感器融合的农业无人车定位里程计方法，其特征在于，该点云配准处理包括：将该校正点云进行网格化处理，对每一个网格中所包含的激光点云采用主成分分析的方法进行正态分布计算；其中每一帧点云经过PCA后的点云分布表示如下：A＝{ai}，其中代表该帧点云第i个网格中正态分布的中心点坐标，代表其协方差矩阵；2CN115451948A权利要求书2/5页待匹配的两帧点云分别为P＝{pi}和Q＝{qi}，基于正态分布变换的迭代最近点的目标函数为：式中R和τ分别代表两帧点云变化间的旋转信息和平移信息，取该目标函数值最小时对应的R和τ，作为变换关系Tkk‑1，Tkk‑1代表第k帧点云和第k‑1点云之间的旋转信息R和平移信息t；该第二位姿信息Tk为：Tk＝∏kTkTk‑1。5.如权利要求4所述的基于多传感器融合的农业无人车定位里程计方法，其特征在于，该第一位姿信息生成过程包括：根据激光雷达坐标系和相机坐标系间的变换信息TLC，将点云坐标投影从激光雷达坐标系投影到相机坐标系中，该视觉特征点通过搜索即可获取到深度；设激光点云坐标为p(x,y,z)，投影后的坐标为p'(x',y',z')，跟据下列公式进行投影：p′＝TLCp视觉特征点在进