CN115792947B 一种基于ekf的管道巡检机器人激光雷达辅助实时定位方法 （南京理工大学）

(12)发明专利李胜李福马寅瑞姜烨YuDing.PipeenvironmenCloud.《2022ICCSI》.2022,202-207.人激光雷达辅助实时定位方法，建立动力学模21.一种基于EKF的管道巡检机器人激光雷达辅助实时定位方法，其特征在于，包括以下步骤1、建立通风管道巡检机器人系统动力学模型；步骤2、将机器人放置在通风管道中采集点云数据并人工标注，训练激光雷达点云关于通风管道管壁的反射畸变模型人工神经网络；步骤3、在开始工作点初始化机器人状态及协方差；步骤4、通过系统方程预测状态矢量和协方差；步骤5、通过安装于机器人侧面的激光雷达采集点云数据，并基于反射畸变校正人工神经网络对于在通风管道环境下的畸变点云进行去畸变处理；步骤6、基于激光雷达点云强度与密度提取直线特征进行逆解算计算机器人朝向角与横向偏移；步骤7、由测量值更新卡尔曼增益、机器人系统状态及协方差。2.根据权利要求1所述的一种基于EKF的管道巡检机器人激光雷达辅助实时定位方法，其特征在于，步骤1中的建立通风管道巡检机器人系统动力学模型，具体步骤如下：步骤1-1、建立管道机器人陀螺仪与编码器的传感器模型：分别为机器人在导航坐标系下沿x轴与v轴速度.v与θ分别为机器人在导航坐标系下编码器输出速度与航向角；步骤1-2、利用机器人陀螺仪与编码器的传感器模型建立机器人系统动力学模型为：别表示机器人在导航坐标系下k时刻的沿x轴方向坐标，沿y轴方向坐标，沿x轴方向速度，沿y轴速度及朝向角。3.根据权利要求1所述的一种基于EKF的管道巡检机器人激光雷达辅助实时定位方法，其特征在于，步骤2中的将机器人放置在通风管道中采集点云数据并人工标注，训练激光雷达点云关于通风管道管壁的反射畸变模型人工神经网络，具体步骤如下：步骤2-1、将机器人放置在工作环境中，对于采集的点云数据的距离属性根据人工测量真值进行人工标注修改后与点云强度与角度重新组合构建训练集t;步骤2-2、构建多层感知器结构的全连接网络M,具体为：3其中，向量x、y分别为网络的输入层和输出层，x向量由点云的角度和强度组成，y向量活函数分别为tansig函数和purelin函数；步骤2-3、根据损失函数J计算网络M输出结果与真实值的误差，迭代训练网络参数，具体为：4.根据权利要求1所述的一种基于EKF的管道巡检机器人激光雷达辅助实时定位方法，其特征在于，步骤3中的在开始工作点初始化机器人状态及协方差，具体步骤如下：定义机器人系统动力学状态与真实状态的误差状态矢量为：δx、δy分别为机器人在导航坐标系下位置坐标误差；δvxδv,为机器人在导航坐标系下移误差；系统协方差由机器人装配的编码器及陀螺仪性能确定，T为矩阵转置符号。5.根据权利要求1所述的一种基于EKF的管道巡检机器人激光雷达辅助实时定位方法，其特征在于，步骤4中的通过系统方程预测误差状态矢量和协方差，具体为：4o=[0√2Yoa02aδt√2β₂o28t]T因子的自相关时间常数，o2a为对应的方差；β₂为陀螺仪随机漂移的自相关时间常数，o²为对应的方差。6.根据权利要求1所述的一种基于EKF的管道巡检机器人激光雷达辅助实时定位方法，其特征在于，步骤5中的激光雷达点云距离范围为[0.03m,0.3m,角度范围为,采集频率为30Hz。7.根据权利要求1所述的一种基于EKF的管道巡检机器人激光雷达辅助实时定位方法，其特征在于，步骤6中基于激光雷达点云强度与密度提取直线特征进行解算计算机器人朝步骤6-1、对于步骤3中经去畸变处理的点云，选取具有最大反射强度的点p;步骤6-3、将此属性与距离阈值∈相比较，如果在p点周围至少存在minpts个点，则将这些点聚类为一类，具体的，参数∈,minpts分别设置为0.02m和10;步骤6-4、在剩余点中重复步骤4-1至4-3直到所有的点都被判别过；步骤6-7、计算在该类中其余点关于该直线的距离的误差函数，具体为：步骤6-8、重新随机选取两个点，重复步骤4-5至4-7直到选取完成所有点；步骤6-9、选取具有最小误差函数值的一组点，计算直线模型参数作为所提取的直线特征mbest,best;步骤6-10、对线特征进行解算计算机器人朝向角与横向偏移，具体为：8.根据权利要求1所述的一种基于EKF的管道巡检机器人激光雷达辅助实时定位方法，其特征在于，步骤7中的由测量值更新卡尔曼增益、机器人系统状态及协方差，计算机器人位姿具体为：5其中，δZ为由步骤6中激光雷达解算出的位姿与机器人动力学模型估计的位姿之差，yLidar和yRISS分别为激光雷达解算的在导航坐标系下机器人的y轴坐标与机器人系统动力学模型解算的在导航坐标系下机器人的y轴坐标，θLi导航坐标系下机器人的朝向角与机器人系统动力学模型解算的在导航坐标系下机器人的为设置为高斯白噪声的测量误差向量；步骤7-2、由系统误差测量值更新卡尔曼增益、机器人系统状态及协方差。6技术领域[0001]本发明属于管道机器人定位技术领域，特别是涉及一种基于EKF的管道巡检机器人激光雷达辅助实时定位方法。背景技术[0002]具有增强清洁和过滤的通风系统已成为预防疫情的重要途径，通风管道的消毒和清洁是保证空气质量的关键，而管道机器人的应用可以代替人工进行消毒和清洁作业，可以极大的减少劳动力成本和潜在的生物感染风险。对于自主化的管道机器人，定位是必不可少的功能，目前管道机器人现有定位技术分为两类，一类利用基于预先埋设于管内的磁电标记点进行定位，然而这种方法必须在铺设管道时人工布置信号源点，极大的增加了成本。另一类不依赖于信号源点的定位技术通常采用里程计和惯性测量单元组合进行定位，此类方法对于传感器性能要求较高，狭小的通风管道对于传感器的选型有较大的限制，而且由于通风管道内环境复杂，极易造成车轮滑动而产生累积误差，同时低成本的惯性测量单元对于角度测量检测误差较大。因此急需一种适用于通风管道巡检机器人的，能够不依赖于外部定位数据源的定位方法。发明内容[0003]本发明目的在于解决管道机器人在复杂密闭的通风管道自主工作中的定位易产生累积误差的问题，实现对管壁的点云反射去畸变处理与特征提取，并通过扩展卡尔曼滤波器对机器人系统动力学模型进行融合以对机器人进行更为准确的实时定位。[0004]为了实现本发明目的，本发明公开了一种基于扩展卡尔曼滤波EKF(ExtendedKalmanFilter)的通风管道巡检机器人激光雷达辅助实时定位方法，包括以下步骤：[0006]步骤2、将机器人放置在通风管道中采集点云数据并人工标注，训练激光雷达点云关于通风管道管壁的反射畸变模型人工神经网络；[0009]步骤5、通过安装于机器人侧面的激光雷达采集点云数据，并基于反射畸变校正人工神经网络对于在通风管道环境下的畸变点云进行去畸变处理；[0010]步骤6、基于激光雷达点云强度与密度提取直线特征进行逆解算计算机器人朝向角与横向偏移；[0012]进一步地，步骤1中的建立通风管道巡检机器人系统动力学模型.具体步骤如下：[0013]步骤1-1、建立管道机器人陀螺仪与编码器的传感器模型：7[0015]其中，①为管道机器人角速度，①²为机器人陀螺仪输出vx与v,分别为机器人在导航坐标系下沿x轴与y轴速度，v与θ分别为机器人在导航坐标系下编码器输出速度与航向角；[0016]步骤1-2、利用机器人陀螺仪与编码器的传感器模型建立机器人系统动力学模型分别表示机器人在导航坐标系下k时刻的沿x轴方向坐标，沿y轴方向坐标，沿x轴方向速度，沿y轴速度及朝向角。[0019]进一步地，步骤2中的将机器人放置在通风管道中采集点云数据并人工标注，训练激光雷达点云关于通风管道管壁的反射畸变模型人工神经网络，具体步骤如下：[0020]步骤2-1、将机器人放置在工作环境中，对于采集的点云数据的距离属性根据人工测量真值进行人工标注修改后与点云强度与角度重新组合构建训练集t;[0023]其中，向量x、y分别为网络的输入层和输出层，x向量由点云的角度和强度组成，y中激活函数分别为tansig函数和purelin函数；[0024]步骤2-3:根据损失函数J计算网络M输出结果与真实值的误差，迭代训练网络参[0027]进一步地，步骤3中的在开始工作点初始化机器人状态及协方差，具体步骤如下：[0028]定义机器人系统动力学状态与真实状态的误差状态矢量为：8德漂移误差；系统协方差由机器人装配的编码器及陀螺仪性能确定，T为矩阵转置符号。刻度因子的自相关时间常数，o2a为对应的方差；β₂为陀螺仪随机漂移的自相关时间常数，o2为对应的方差。[0043]进一步地，步骤5中的激光雷达点云距离范围为[0.03m,0.3m],角度范围为,采集频率为30Hz。[0044]进一步地，步骤6中基于激光雷达点云强度与密度提取直线特征进行解算计算机[0045]步骤6-1、对于步骤3中经去畸变处理的点云，选取具有最大反射强度的点p;[0050]步骤6-4、在剩余点中重复步骤4-1至4-3直到所有的点都被判别过；9线特征m6est,best;[0065]其中，8Z为由步骤6中激光雷达解算出的位姿与机器人动力学模型估计的位姿之在导航坐标系下机器人的朝向角与机器人系统动力学模型解算的在导航坐标系下机器人云因材料反射的畸变现象，同时本发明的特征提取方法显著减少了运行时间与资源消耗，可以实现在嵌入式机器人上的应用；3)本发明能够在复杂的管道环境背景下取得良好效附图说明[0070]此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：[0071]图1为一种基于EKF的管道巡检机器人激光雷达辅助实时定位方法的原理图。具体实施方式[0072]下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例；基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实[0073]如图1所示，一种基于EKF的管道巡检机器人激光雷达辅助实时定位方法，包括以[0075]步骤2、将机器人放置在通风管道中采集点云数据并人工标注，训练激光雷达点云关于通风管道管壁的反射畸变模型人工神经网络；[0078]步骤5、通过安装于机器人侧面的激光雷达采集点云数据，并基于反射畸变校正人工神经网络对于在通风管道环境下的畸变点云进行去畸变处理；[0079]步骤6、基于激光雷达点云强度与密度提取直线特征进行逆解算计算机器人朝向角与横向偏移；[0081]具体地，在本实施例中，步骤1中的建立通风管道巡检机器人系统动力学模型，具体步骤如下：[0082]步骤1-1、建立管道机器人陀螺仪与编码器的传感器模型：[0084]其中，①为管道机器人角速度，①²为机器人陀螺仪输出角速度，b₂为陀螺仪零偏，vx与v,分别为机器人在导航坐标系下沿x轴与y轴速度，v与θ分别为机器人在导航坐标系下编码器输出速度与航向角；[0085]步骤1-2、利用机器人陀螺仪与编码器的传感器模型建立机器人系统动力学模型测量真值进行人工标注修改后与点云强度与角度重新组合[0093]步骤2-3、根据损失函数J计算网络M输出结果与真实值的误差，迭代训练网络参仪随机漂移的自相关时间常数，σ2为对应的方差。[0108]具体地，在本实施例中，步骤5