CN115816465A - 用于隧道检测的地质雷达测线选取与随动控制方法及系统 - Google Patents

Abstract

本发明提供一种用于隧道检测的地质雷达测线选取与随动控制方法及系统，涉及无损检测技术领域，方法包括：设定扫描路径，基于扫描路径，获取隧道测距信息；根据隧道测距信息构建隧道横断面内轮廓模型，并通过移动平台对隧道横断面内轮廓模型进行修正；选取目标测线位置，根据修正后的隧道横断面内轮廓模型进行机械臂调节余量评估，获得最优目标测线位置；基于最优目标测线位置与当前位置信息进行轨迹规划，完成地质雷达的自动贴合，并实时获取地质雷达与隧道衬砌间的相对位姿，基于相对位姿进行实时调控，进行检测。本发明能够实现移动平台不同移动速度下，地质雷达均能够及时稳定的随动贴合于隧道内壁，保证地质雷达采集数据的质量。

Description

用于隧道检测的地质雷达测线选取与随动控制方法及系统

技术领域

本发明涉及无损检测技术领域，特别是涉及一种用于隧道检测的地质雷达测线选取与随动控制方法及系统。

背景技术

我国大量隧道已建成并投入使用，随着使用时间的增长，其结构健康服役的重要性日渐突显，隧道结构理论设计寿命可达百年，然而在其长期服役过程中，长期环境侵蚀、结构劣化、受工程扰动、等因素共同作用，隧道运营期结构会出现管片错台、脱空、剥落、裂缝、等病害，导致隧道结构性能退化，造成隧道寿命折减，甚至引起运营事故，严重威胁结构运营安全。

针对隧道结构内部检测，目前主流的检测方式是通过人工手持地质雷达、或伸缩杆托举地质雷达的方法对基础设施结构进行巡检探测，这种方式既费时又费力，随着信息技术与自动化技术的发展，巡检机器人以其高效、智能等特点，近年来逐渐应用于隧道结构检测。然而巡检机器人在应用过程中仍然存在诸多弊端，难以满足自动化巡检的需求。

已有的隧道巡检机器人多采用各种伸缩装置搭载地质雷达进行隧道结构检测，存在以下问题：1)地质雷达依赖人工选择测线，难以自动选择目标测线位置，并无法由初始位置自动贴合到目标测线位置；2)现有地质雷达贴合方法抖动性强，尤其对于复杂表面，地质雷达难以在移动过程中以固定距离实时稳定的贴合于在隧道内壁。

发明内容

为了解决上述现有技术中存在的问题，本发明提供了用于隧道检测的地质雷达测线选取与随动控制方法及系统，能够快速完成隧道横断面内轮廓建模，并根据所建模型完成机械臂最优可调节余量的测线自动选取策略，提高了自动化程度，还能够在地质雷达随动贴合过程中减少抖动，保证地质雷达数据质量。

为实现上述目的，本发明提供了如下方案：

用于隧道检测的地质雷达测线选取与随动控制方法，包括：

设定扫描路径，基于所述扫描路径，获取隧道测距信息；

根据所述隧道测距信息构建隧道横断面内轮廓模型，并通过移动平台对所述隧道横断面内轮廓模型进行修正；

选取目标测线位置，根据修正后的所述隧道横断面内轮廓模型进行机械臂调节余量评估，获得最优目标测线位置；

基于所述最优目标测线位置与当前位置信息进行轨迹规划，完成地质雷达的自动贴合，并根据机械臂末端传感器，实时获取地质雷达与隧道衬砌间的相对位姿，基于所述相对位姿进行实时调控，进行检测。

优选地，设定所述扫描路径，包括：

根据隧道形状，以机械臂基坐标原点为中心，于所述隧道横断面所在平面设定机械臂末端运动路径，获得所述扫描路径；其中，所述机械臂末端运动路径包括弧形、矩形路径；所述机械臂末端法兰的法线位于所述隧道横断面所在平面内，且垂直于所述设定路径。

优选地，根据所述隧道测距信息构建隧道横断面内轮廓模型，包括：

通过距离检测单元获取所述隧道测距信息，并以机械臂基坐标进行建模；其中，所述隧道横断面内轮廓模型与机械臂运动于同一坐标系内；

根据所述隧道横断面大小将所述路径划分为若干段，机械臂末端沿已规划的路径 每移动一段采集n次隧道测距数据，并对所述隧道测距数据取平均值

，同时获 取此时所述机械臂各关节角的角度信息

；

根据传感器在机械臂末端的安装位置、机械臂参数以及传感器距离信息，建立变参数D-H模型；

根据所述D-H模型进行正运动学求解，获得机械臂测量点坐标；

将所有所述测量点坐标在同一坐标系下显示，获得所述隧道横断面内轮廓模型。

优选地，对所述隧道横断面内轮廓模型进行修正，包括：

获取所述移动平台位于不同位置时前后两端距离隧道内壁的水平距离，计算所述移动平台相对于所述隧道内壁的距离和偏航角度，构建坐标变换矩阵，通过所述坐标变换矩阵对已建的所述隧道横断面内轮廓模型的测量点坐标进行坐标变换，完成对所述隧道横断面内轮廓模型的修正。

优选地，根据修正后的所述隧道横断面内轮廓模型进行机械臂调节余量评估，包括：

S1.1、根据所述目标测线位置的相邻测点的坐标信息，计算得到所述目标测线位置的斜率，基于所述斜率获得目标位姿信息；

S1.2、设定机械臂各关节角调节余量的上限值与下限值，并根据所述上限值和所述下限值设置评估函数和用于测线选取的评估函数参考值，根据所述目标位姿信息逆向求解，获得机械臂各关节角度，将所述机械臂各关节角度作为评估函数的输入，将输出结果与所述评估函数参考值进行比较；

若所述输出结果小于等于所述评估函数参考值，则选取所述目标测线位置；若所述输出结果大于所述评估函数参考值，则选取相邻测线位置并进行评估，重复所述S1.1-S1.2，直至选取到所述最优目标测线位置。

优选地，完成所述地质雷达的自动贴合，包括：

根据所述最优目标测线位置的位姿信息与所述机械臂当前位姿信息进行若干次多项式轨迹规划，同时加入约束条件使所述机械臂在所建隧道横断面模型内部运动，避免与隧道内壁产生碰撞，使所述地质雷达贴合于隧道内壁。

优选地，基于所述相对位姿进行实时调控，包括：

采用单闭环负反馈控制策略，将卡尔曼滤波与PID控制算法相串联，控制所述机械臂搭载所述地质雷达以固定距离随动贴合于所述隧道内壁。

优选地，控制所述机械臂搭载所述地质雷达以固定距离随动贴合于所述隧道内壁，包括：

通过距离检测单元获得地质雷达与隧道内壁间的距离x、机械臂末端工具坐标系X轴偏转角α和Y轴偏转角β；

以固定时间间隔获取若干组x、α、β值，计算x、α、β值的变换速度v和加速度a；

将所述x、α、β、v、a作为地质雷达状态变量，根据所述地质雷达运动状态，采用卡尔曼滤波对所述地质雷达运动状态进行预测；

根据x、α、β的设定值与卡尔曼滤波后状态变量的偏差进行求解，获得所述机械臂各关节角的角度偏差，引入经典PID控制，并将所述移动平台运行速度作为参数调节的依据，根据各关节的角度偏差和移动平台运行速度控制机械臂各关节的调节速度，保证地质雷达随动贴合的稳定性与及时性。

为了实现上述目的，本发明还提供了一种用于隧道检测的地质雷达测线选取与随动控制系统，包括：

移动平台，用于搭载机械臂，作为移动机构实现对隧道的巡检；

机械臂，用于调节地质雷达位姿；

夹持装置，用于安装所述地质雷达；

距离检测模块，用于获取距离信息；

控制模块：用于完成隧道横断面内轮廓建模，并根据隧道横断面内轮廓模型完成机械臂最优可调节余量的测线自动选取策略，以及用于地质雷达的随动控制；

其中，所述机械臂安装于所述移动平台顶部，所述夹持装置安装于所述机械臂末端，所述距离检测模块安装于所述夹持装置上；

所述距离检测模块包括若干传感器，且所述若干传感器采用“#”形布设方式，且传感器测量方向平行于所述机械臂末端法兰的法线方向，所述控制模块与所述移动平台、所述机械臂和所述距离检测模块并行连接。

优选地，所述控制模块包括：

扫描路径设定单元，用于根据隧道横断面的形状，设定机械臂进行隧道横断面建模的扫描路径；

建模控制和模型数据处理单元，用于控制机械臂沿所述扫描路径运动，通过所述距离检测模块获取距离信息，并对所述距离信息进行处理，完成隧道横断面内轮廓建模以及补偿修正；

地质雷达自动贴合单元，用于在所建隧道横断面内轮廓模型选取目标测线位置，控制所述机械臂自动选取最优目标测线，并将地质雷达贴合于所述最优目标测线位置；

地质雷达随动控制单元，用于在所述移动平台运行时，控制所述机械臂托载所述地质雷达随动贴合于隧道内壁；

其中，所述扫描路径设定单元、所述建模控制和模型数据处理单元、所述地质雷达自动贴合单元和所述地质雷达随动控制单元依次连接。

本发明的有益效果为：

(1)本发明提出的用于隧道检测的地质雷达测线选取与随动控制方法，将机械臂和激光测距传感器组合作为建模装置，使传感器具有多种用途，通过机械臂对隧道横断面内轮廓进行仿形扫描获取距离数据，于机械臂基坐标系下求解各测点坐标，求解过程简单，建模速度快，最终将所有坐标于基坐标下进行显示，便于进行测线选取、实用性强；

(2)本发明提出的用于隧道检测的地质雷达测线选取与随动控制方法，通过安装于移动平台的传感器获取移动平台在隧道中的位置，当移动平台处于不同于建模时所处的位置时，可根据当前位置信息对已建模型进行补偿修正，从而避免重复建模，实现一次建模多次使用，提高检测效率；

(3)本发明提出的方法，对所选目标测线位置信息进行评估，判断机械臂在当前测线各关节是否具有最优调节余量，若不具备，则控制系统根据最优调节余量测线选取算法，自动选择与目标测线相邻的测线，该方法能够解决实际应用过程中机械臂调节余量小导致的调节速度慢及碰撞的问题；

(4)本发明提出的方法，采用卡尔曼滤波对地质雷达运动状态进行预测，减少地质雷达在检测过程中的抖动现象，采用经典PID控制算法，将移动平台速度作为参数调节的依据，能够实现移动平台通移动速度下，地质雷达均能够及时稳定的随动贴合于隧道内壁，保证地质雷达采集数据的质量。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例中方法整体控制流程图；

图2为本发明实施例中隧道横断面内轮廓建模示意图；

图3为本发明实施例中夹持装置示意图；

图4为本发明实施例中移动平台传感器布局图；

图5为本发明实施例中隧道横断面内轮廓模型建模结果图；

图6为本发明实施例中测线选取控制流程图；

图7为本发明实施例中随动贴合控制框图；

其中，2-1、隧道；2-2、地质雷达；2-3、机械臂；2-4、升降台；3-1、距离检测模块；3-2、夹持装置；3-3、“#”形布设右前测距传感器；3-4、“#”形布设右后测距传感器；3-5、“#”形布设左后测距传感器；3-6、“#”形布设左前测距传感器；4-1、移动平台前置测距传感器；4-2、移动平台后置测距传感器。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例一、

本实施例公开了一种用于隧道检测的地质雷达测线选取与随动控制方法，整体控制流程如图1，具体包括：

流程1：隧道横断面机械臂多点仿形扫描路径设定，用于隧道横断面内轮廓建模，具体包括：

隧道断面形状有圆形、拱形、矩形等，根据隧道大致形状，在控制器内部以机械臂基坐标原点为中心于隧道横断面所在平面设定机械臂末端运动路径，包括但不限于弧形、矩形路径；

将不同形状隧道所设定的路径进行保存，以便重复使用，所设路径用于进行隧道横断面内轮廓建模；

机械臂末端法兰的法线始终位于隧道横断面所在平面内且垂直于所设定的路径。

流程2：基于机械臂末端测距信息的隧道横断面内轮廓建模，具体包括：

机械臂作为执行机构，通过距离检测单元获取数据，并以机械臂基坐标进行建模，建模过程示意图如图2，集隧道横断面内轮廓模型与机械臂运动于同一坐标系，减小建模误差，包括：

根据隧道横断面大小将路径划分为m段，机械臂末端沿已规划的路径每移动一段 采集n次传感器数据并取平均值

，同时获取此时机械臂各关节角的角度信息

；

根据传感器在机械臂末端的安装位置、机械臂固有尺寸参数以及传感器距离信 息，建立变参数D-H模型，其中传感器距离信息

作为变化参数；

根据所建D-H模型进行正运动学求解，即可获得传感器每个测点位置相对于机械 臂基坐标的坐标信息

，且无需进行其他坐标转换，简化建模过程；

将所有坐标信息在同一坐标系下显示，如图5，便于目标测线位置选取。

流程3：隧道横断面内轮廓模型补偿修正，当移动平台所处位置与第一次进行隧道横断面内轮廓建模时位置不同，且需要进行测线选取时，需要对已建模型进行补偿修正，不需进行重复建模，具体为：

通过移动平台前置测距传感器4-1、移动平台后置测距传感器4-2获取移动平台位 于不同位置时前后两端距离隧道内壁的水平距离

，计算移动平台相对于隧道内壁 的距离

和偏航角度

：

其中

为两传感器之间的距离，进而构建坐标变换矩阵T，进行坐标变换：

其中，

为补偿修正前第m个测点的坐标，

为修正前移动平台与隧道内壁的 水平距离，

为经坐标变换后的坐标，即完成模型补偿修正。

流程4：机械臂最优调节余量的测线自动选取，测线选取流程如图6，具体包括：

根据需求于隧道横断面内轮廓模型选取目标测线位置

，根据选取的目标测线 位置相邻测点的坐标信息，计算得到目标测线位置的斜率k，也就是地质雷达目标测线位置 X轴偏转角，地质雷达Y轴、Z轴偏转角为0，从而获得目标位姿信息

；

其中，

分别表示第n+1个测点的X轴Y轴坐标值，

、

分别表示第n个 测点的X轴Y轴坐标值，

分别表示第n-1个测点的X轴Y轴坐标值。

设定机械臂各关节角调节余量的上限值

、下限值

，并根据上下限值设 置评估函数：

以及用于测线选取的评估函数参考值MIN，根据目标位姿信息

逆向求解获得 机械臂各关节角度

，将各关节角度输入评估函数，获得输出值与参考值进行 比较。若输出值小于等于参考值，则选取该测线位置；若大于，则选取相邻测线并进行评估， 重复此过程直至选取到最优测线位置

。

根据最优测线位置的位姿信息

与机械臂当前位姿信息

进行逆运动学求 解得机械臂目标关节角度

和当前关节角度

，然后进行五次多 项式轨迹规划；

每次机械臂运动前，都要进行正运动学求解，判断机械臂末端坐标是否在所建隧道横断面模型内部运动，避免与隧道内壁产生碰撞，使地质雷达快速贴合于隧道内壁。

流程5：地质雷达自动贴合完成后进行机械臂末端搭载地质雷达贴合随动控制，具体包括：

采用单闭环负反馈控制策略，如图7，卡尔曼滤波与PID控制算法相串联，控制机械臂搭载地质雷达以固定距离随动贴合于隧道内壁，包括：

通过夹持装置上的距离检测单元获得地质雷达与隧道内壁间的距离x、机械臂末端工具坐标系X轴偏转角α和Y轴偏转角β：

其中b ₁ 、b ₂ 、b ₃ 、b ₄ 分别为测距传感器3-3、测距传感器3-4、测距传感器3-5、测距传感器3-6的测量数据，width为测距传感器3-3与测距传感器3-4之间的距离，length为测距传感器3-4与测距传感器3-5之间的距离；

以固定时间间隔ΔT获取多组x、α、β值，计算x、α、β值的变换速度v和加速度a，将x、 α、β、v、a作为地质雷达状态变量，根据地质雷达运动状态，采用卡尔曼滤波对地质雷达运动状态进行预测，其中，观测噪声的协方差矩阵R根据传感器测量误差获得，过程噪声的协方差矩阵Q根据机械臂运动获得。达到减小粗糙隧道内壁导致的地质雷达抖动并保证地质雷达采集数据的清晰度的目的；

根据x、α、β的设定值

与卡尔曼滤波后状态变量的偏差

，经求解获得各关节角的角度偏差

，引入经典PID控制：

其中，

分别为比例、积分、微分参数，

为第k时刻第i个关节 角的角度偏差。

并将移动平台运行速度作为参数调节的依据：

其中

为系数，

为移动平台速度，根据

控制机械臂各关节的 调节速度，保证地质雷达随动贴合的稳定性与及时性。

流程6：检测完毕后，机械臂自动恢复至初始位姿。

实施例二、

本实施例还提供一种用于隧道检测的地质雷达测线选取与随动控制系统，具体包括：

移动平台、机械臂2-3、夹持装置3-2、距离检测模块3-1和控制模块；夹持装置如图3。

所述移动平台可采用皮卡、升降车、AGV等，包括但不限于以上几项，作为一种实施例，采用升降车作为移动平台，用于搭载机械臂，作为移动机构实现对隧道的巡检；

所述机械臂2-3安装于移升降车的升降台2-4，作为调节地质雷达位姿的执行机构，所述夹持装置3-2安装于机械臂2-3末端，用于安装地质雷达，所述距离检测模块3-1安装于夹持装置3-2，所述距离检测单元包括多个测距传感器，并采用“#”形布设方式，且传感器测量方向平行于机械臂末端法兰的法线方向，用于获取距离信息；图4为移动平台传感器布局图。

控制模块包括：

扫描路径设定单元：用于根据隧道横断面的大致形状，设定机械臂进行隧道横断面建模的扫描路径；

建模控制和模型数据处理单元：用于控制机械臂沿已设定的扫描路径运动，通过距离检测单元获取距离信息，并对获得的信息进行处理完成隧道横断面内轮廓建模以及补偿修正；

地质雷达自动贴合单元用于：在所建隧道横断面内轮廓模型选取目标测线大致位置，控制机械臂进行最优调节余量的测线自动选取并将地质雷达贴合于目标测线位置；

地质雷达随动控制单元：用于地质雷达贴合于目标测线位置后，在移动平台运行时控制机械臂托载地质雷达随动贴合于隧道内壁；

各模块既可单独执行，又可串行执行。

实施例三、

本实施例提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现入上述实施例一中的用于隧道结构探伤的地质雷达测线自动选取与贴合随动控制方法中的流程。

本实施例中的计算机程序所执行的用于隧道结构探伤的地质雷达测线自动选取与贴合随动控制方法可应用于同类机器人系统，普适性强。

实施例四、

本实施例提供了一种计算机设备，包括存储器，处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行所述程序时实现如上述实施例一提出的用于隧道结构探伤的地质雷达测线自动选取与贴合随动控制方法中的流程。

本实施例处理器执行的用于隧道结构探伤的地质雷达测线自动选取与贴合随动控制方法可应用于同类机器人系统，普适性强。

以上所述的实施例仅是对本发明优选方式进行的描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (10)

1.用于隧道检测的地质雷达测线选取与随动控制方法，其特征在于，包括：

设定扫描路径，基于所述扫描路径，获取隧道测距信息；

根据所述隧道测距信息构建隧道横断面内轮廓模型，并通过移动平台对所述隧道横断面内轮廓模型进行修正；

选取目标测线位置，根据修正后的所述隧道横断面内轮廓模型进行机械臂调节余量评估，获得最优目标测线位置；

基于所述最优目标测线位置与当前位置信息进行轨迹规划，完成地质雷达的自动贴合，并根据机械臂末端传感器，实时获取地质雷达与隧道衬砌间的相对位姿，基于所述相对位姿进行实时调控，进行检测。

2.根据权利要求1所述的用于隧道检测的地质雷达测线选取与随动控制方法，其特征在于，设定所述扫描路径，包括：

根据隧道形状，以机械臂基坐标原点为中心，于所述隧道横断面所在平面设定机械臂末端运动路径，获得所述扫描路径；其中，所述机械臂末端运动路径包括弧形、矩形路径；所述机械臂末端法兰的法线位于所述隧道横断面所在平面内，且垂直于设定路径。

3.根据权利要求1所述的用于隧道检测的地质雷达测线选取与随动控制方法，其特征在于，根据所述隧道测距信息构建隧道横断面内轮廓模型，包括：

通过距离检测单元获取所述隧道测距信息，并以机械臂基坐标进行建模；其中，所述隧道横断面内轮廓模型与机械臂运动于同一坐标系内；

根据所述隧道横断面大小将所述路径划分为若干段，机械臂末端沿已规划的路径每移 动一段采集n次隧道测距数据，并对所述隧道测距数据取平均值

，同时获取此 时所述机械臂各关节角的角度信息

；

根据传感器在机械臂末端的安装位置、机械臂参数以及传感器距离信息，建立变参数D-H模型；

根据所述D-H模型进行正运动学求解，获得机械臂测量点坐标；

将所有所述测量点坐标在同一坐标系下显示，获得所述隧道横断面内轮廓模型。

4.根据权利要求1所述的用于隧道检测的地质雷达测线选取与随动控制方法，其特征在于，对所述隧道横断面内轮廓模型进行修正，包括：

获取所述移动平台位于不同位置时前后两端距离隧道内壁的水平距离，计算所述移动平台相对于所述隧道内壁的距离和偏航角度，构建坐标变换矩阵，通过所述坐标变换矩阵对已建的所述隧道横断面内轮廓模型的测量点坐标进行坐标变换，完成对所述隧道横断面内轮廓模型的修正。

5.根据权利要求1所述的用于隧道检测的地质雷达测线选取与随动控制方法，其特征在于，根据修正后的所述隧道横断面内轮廓模型进行机械臂调节余量评估，包括：

S1.1、根据所述目标测线位置的相邻测点的坐标信息，计算得到所述目标测线位置的斜率，基于所述斜率获得目标位姿信息；

S1.2、设定机械臂各关节角调节余量的上限值与下限值，并根据所述上限值和所述下限值设置评估函数和用于测线选取的评估函数参考值，根据所述目标位姿信息逆向求解，获得机械臂各关节角度，将所述机械臂各关节角度作为评估函数的输入，将输出结果与所述评估函数参考值进行比较；

若所述输出结果小于等于所述评估函数参考值，则选取所述目标测线位置；若所述输出结果大于所述评估函数参考值，则选取相邻测线位置并进行评估，重复所述S1.1-S1.2，直至选取到所述最优目标测线位置。

6.根据权利要求5所述的用于隧道检测的地质雷达测线选取与随动控制方法，其特征在于，完成所述地质雷达的自动贴合，包括：

根据所述最优目标测线位置的位姿信息与所述机械臂当前位姿信息进行若干次多项式轨迹规划，同时加入约束条件使所述机械臂在所建隧道横断面模型内部运动，避免与隧道内壁产生碰撞，使所述地质雷达贴合于隧道内壁。

7.根据权利要求1所述的用于隧道检测的地质雷达测线选取与随动控制方法，其特征在于，基于所述相对位姿进行实时调控，包括：

采用单闭环负反馈控制策略，将卡尔曼滤波与PID控制算法相串联，控制所述机械臂搭载所述地质雷达以固定距离随动贴合于所述隧道内壁。

8.根据权利要求7所述的用于隧道检测的地质雷达测线选取与随动控制方法，其特征在于，控制所述机械臂搭载所述地质雷达以固定距离随动贴合于所述隧道内壁，包括：

通过距离检测单元获得地质雷达与隧道内壁间的距离x、机械臂末端工具坐标系X轴偏转角α和Y轴偏转角β；

以固定时间间隔获取若干组x、α、β值，计算x、α、β值的变换速度v和加速度a；

将所述x、α、β、v、a作为地质雷达状态变量，根据所述地质雷达运动状态，采用卡尔曼滤波对所述地质雷达运动状态进行预测；

根据x、α、β的设定值与卡尔曼滤波后状态变量的偏差进行求解，获得所述机械臂各关节角的角度偏差，引入经典PID控制，并将所述移动平台运行速度作为参数调节的依据，根据各关节的角度偏差和移动平台运行速度控制机械臂各关节的调节速度，保证地质雷达随动贴合的稳定性与及时性。

9.用于隧道检测的地质雷达测线选取与随动控制系统，其特征在于，包括：

移动平台，用于搭载机械臂，作为移动机构实现对隧道的巡检；

机械臂，用于调节地质雷达位姿；

夹持装置，用于安装所述地质雷达；

距离检测模块，用于获取距离信息；

控制模块：用于完成隧道横断面内轮廓建模，并根据隧道横断面内轮廓模型完成机械臂最优可调节余量的测线自动选取策略，以及用于地质雷达的随动控制；

其中，所述机械臂安装于所述移动平台顶部，所述夹持装置安装于所述机械臂末端，所述距离检测模块安装于所述夹持装置上；

所述距离检测模块包括若干传感器，且所述若干传感器采用“#”形布设方式，且传感器测量方向平行于所述机械臂末端法兰的法线方向，所述控制模块与所述移动平台、所述机械臂和所述距离检测模块并行连接。

10.根据权利要求9所述的用于隧道检测的地质雷达测线选取与随动控制系统，其特征在于，所述控制模块包括：

扫描路径设定单元，用于根据隧道横断面的形状，设定机械臂进行隧道横断面建模的扫描路径；

建模控制和模型数据处理单元，用于控制机械臂沿所述扫描路径运动，通过所述距离检测模块获取距离信息，并对所述距离信息进行处理，完成隧道横断面内轮廓建模以及补偿修正；

地质雷达自动贴合单元，用于在所建隧道横断面内轮廓模型选取目标测线位置，控制所述机械臂自动选取最优目标测线，并将地质雷达贴合于所述最优目标测线位置；

地质雷达随动控制单元，用于在所述移动平台运行时，控制所述机械臂托载所述地质雷达随动贴合于隧道内壁；

其中，所述扫描路径设定单元、所述建模控制和模型数据处理单元、所述地质雷达自动贴合单元和所述地质雷达随动控制单元依次连接。

Images