受限空间监测机器人运动控制系统

摘要：针对监测机器人运动控制定位不精准等问题，本文研究了受限空间监测机器人运动控制系统的定位传感器、硬件和软件系统。本文研制了一款小尺寸激光位移传感器，提出基于直线线性变换（DLT）和非线性优化的外参计算方法以及改进灰度重心法的中心线提取算法。在硬件方面改进了机器人电器电路，提出基于大疆M3508设计的里程计程序和红外传感技术（RFID）联合轨道定位的机器人定位方法。在软件方面，根据受限空间工作要求设计了机器人软件系统。为隧道受限空间监测机器人的整体设计提供参考。

关键词：受限空间；监测机器人；定位传感器；运动控制；精确定位

中图分类号：TP242""""""""" 文献标志码：A

随着经济发展和社会进步，我国的隧道建设总长逐年增加，然而在隧道的使用过程中，受地质灾害的影响易产生横向变形和纵向不均匀沉降，容易造成隧道管片接缝张开与错动，从而影响隧道的防水效果[1-2]，严重威胁生命和财产安全[3]。因此，定期监测隧道病害必不可少。目前，部分国家采用智能监测机器人对隧道受限空间进行检测和维护，轮式管道机器人具有运行稳、移动快和效率高等优势，应用广泛。“巡游者”机器人利用图像识别进行机器人定位导航，高压电力廊道自动巡检机器人结合激光传感器和RFID红外传感技术进行地图构建、定位导航。由于目前研究多集中于对隧道内的运行情况进行监测，因此存在受限空间监测机器人的运动控制定位不精准、在轨运行不稳定等问题。因此，本文对受限空间监测机器人运动控制系统的定位传感器、硬件和软件系统进行了研究，为隧道受限空间监测机器人的整体设计提供参考。

1运动控制传感器

1.1线激光传感器

为监测管片接缝张开量，提高传感器效能，本文研制了一款小尺寸线激光传感器。其采用嵌入式迈德威视MV-SUM134GM相机，激光传感器为中科院长春光机所研制的MDL-SL激光模组，传感器样机如图1所示。线激光传感器和相机按照一定的角度摆放，线结构光发出一束线激光投射至被测物体上，相机捕捉线结构光条纹在被测物体上的投影，使用标定计算得线结构光平面在相机坐标系中的方程，提取线结构光投影的中心线，利用线结构光条纹在光平面上的约束得到线结构光条纹的三维空间坐标。扫描整个物体得到被测物体表面轮廓的三维坐标信息。

1.2线激光定位传感器标定原理

在样机研制过程中，本文提出一种基于直线线性变换（DLT）和非线性优化的外参计算方法以及改进灰度重心法的中心线提取算法。此外，在光平面的计算过程中，采用随机采样一致性算法（RANSAC）以及非线性优化得到精确的光平面方程。

1.2.1棋盘格平面方程计算

计算棋盘格平面在相机坐标系中的平面方程来标定结构光系统，本文采用张氏标定法进行计算。某个位姿中的棋盘格，其坐标系使用旋转矩阵Ri和平移向量ti转换至相机坐标系，已知3个棋盘格中非共线标定点坐标，如公式（1）所示。

（1）

式中：A、B、C为世界坐标系，A'、B'、C'为相机坐标系。ti为平移向量；Ri为旋转矩阵。

棋盘格在相机坐标系中的法向量如公式（2）所示。

ni=A'B'×A'C'（2）

记法向量表达式为ni=（nix，niy，niz），棋盘格在相机坐标系中的平面方程如公式（3）所示。

nix（x-X'ai）+niy（y-Y'ai）+niz（z-Z'ai）（3）

在试验过程中发现该方法有一定误差，因此，本文引入PnP问题的经典求解方法：DLT以及光束平差法（Bundle Adjustment）来得到比较精确的相机外参。在提取像素角点的过程中，采用亚像素角点提取算法来保证角点提取的精度。亚像素角点提取如图2所示，p为待计算的亚像素角点。如公式（4）所示。

（pi-p）·Gi=0（i=1，2）（4）

式中：Gi为点pi处的梯度；Pi为人工定义的坐标轴上第i个角点的三维坐标。

将公式（4）移项展开，使用最小二乘法进行计算，如公式（5）所示。

p=（GiTGi）-1GiTGipi（5）

式中：GiT为Gi的转置矩阵。

在原始角点处选择1个矩形窗口，计算窗口内所有像素点的和以及权重，得到亚像素角点，如公式（6）所示。

（6）

式中：ωi为第i个角点的权重；N为总角点数。

以亚像素角点为起始角点进行迭代计算，能获取更精确的角点坐标。

采用DLT求解3D点与2D点之间的运动（不通）。通过旋转矩阵R和平移向量t从世界坐标系转移到摄像机坐标系，角点可表示为公式（7）。

式中：s为相机对应距离参数；x，y为平面坐标系未知变量；[X，Y，Z]T为世界坐标系中的坐标；t1～t12为12个平移向量矩阵元素；R、t为外参。为了与前文变换矩阵区分，采用“T”增广矩阵，并定义增广矩阵表达式为T-[R|t]。

计算公式（7）可以得到2个约束，如公式（8）所示。

t1'TP-t3'TPx=0，t2'TP-t3'TPy=0（8）

式中：P为空间点的齐次坐标。

采用非线性优化算法进行迭代优化，得到更精确的位姿，如公式（9）所示。

式中：R*、t*为优化后外参；xi为第i个角点像素值的归一化坐标；n为迭代次数；si为pi处相机对应距离参数；K为内参矩阵。

1.2.2线结构光中心线提取

灰度重心法原理简单，计算速度较快，但是曲折形状结构光投影精度不高；Steger算法精度较高，但是后续处理烦琐，实时性差。本文根据这2种方法的特点，提出了一种快速计算法向量、改进灰度重心的中心线提取算法，使用RANSAC算法剔除外点，得到更加准确的中心线。

本文在室内光照环境中进行大量试验后发现，虽然光强具有近似高斯分布的特征，但是无法避免环境噪声等因素。如果在一定阈值下进行分割，就可能会造成光条纹不完整，从而直接影响后续中心线提取的结果。因此，在本文提出的中心线提取算法中，将原始图像只进行高斯滤波以消除高斯噪声，并不对其进行阈值分割，这样可以避免由于光条纹分割不完整导致中心线提取的误差。由于线结构光的光强比背景光强烈，因此沿着光条纹方向对图像的每一列进行遍历，将灰度值最高的像素点作为结构光的中心线的初始值。在此基础上，本文提出一种快速计算光条纹法向量的算法，并在法线方向上对其利用灰度重心法进行加权，得到更加精确的中心线。为了更加准确地确定中心线的坐标，在每个中心点的法向量方向上使用采取上述方法得到的法向量，以灰度重心法得到更精确的中心线坐标，如公式（10）所示。

（10）

式中：i为图像上像素点的横坐标；j为图像上像素点的纵坐标；W（i，j）为坐标的像素值；x0、y0分别为经过权重计算得到的中心线在像素坐标系下的横坐标和纵坐标。

利用极值计算近似的中心线，结合相邻点计算得到各中心点的法向量，在法向量方向上采用灰色重心法得到更精确的中心线。这样计算得到的中心点多数比较准确，使用最小二乘法进行拟合，得到精确的光条纹方程。本文利用MATLAB自带的曲线拟合工具箱对光条纹直线部分进行拟合计算，大多数中心点与拟合的中心线的距离偏差不超过0.35ppi。由于环境噪声或反光的影响，因此还有一些中心点会偏离拟合的直线。这些离群值会直接影响拟合结果，对线结构光的标定精度造成影响。为了消除这些离群值，提高标定精度，采用基于RANSAC的方法来得到光条纹的直线方程。其具体流程如下。

步骤一：将所有的中心点作为一个集合M，从中随机选取n个中心点作为集合N。

步骤二：使用最小二乘法将集合N中的点进行拟合，得到这些点的直线方程。

步骤三：计算集合N外的所有中心点至由步骤二拟合得到的直线方程的距离d。设定一个固定的阈值T，如果d小于T，则将该中心点当作1个内点，并将其添加至集合N中。计算更新后的集合N中点的数量。

步骤四：设定一个固定值Tc，重复步骤二和步骤三Tc次，得到Tc个不同的集合N，选择这些集合中具有最多中心点数量的集合，记作Nmax。使用最小二乘法将Nmax集合中的所有中心点拟合为最佳直线方程。

利用上述方法可以有效避免离群点导致的拟合直线误差，在后续光平面的计算过程中，提高了标定精度。

2运动控制系统硬件电路

隧道管片接缝张开与错动是隧道中容易出现的问题[4]，需要定期监测[5]。为了进行机器人远程控制以及自动化检测，对机器人电器电路进行改造，如图3（a）所示，机器人的主控板采用STM32F407单片机，搭载了红外热成像相机、激光位移传感器和视觉相机，对隧道管片接缝张开和渗漏水病害进行监测。为延长工作时间，选择了容量为10000MA36V10A的锂电池，并采用电源模块分压24V进行供电。

2.1无线通信模块

无线通信模块采用ESP8266芯片联立上机位和下机位，保证上机位对下机位的指令下达和下机位对上机位的数据反馈（包括位置信息和姿态信息），如图3（b）所示。ESP8266芯片包括电源电路、重启复位、指令、射频电路和串口5个模块。ESP8266芯片的接口采用UART方式，可以利用Wi-Fi对相关数据进行高速、高质传输，同时获得SOCKET和串口功能，STM32具有无线通信功能，电压为3.3V。ESP8266芯片用STM32的UART4通信，接收连接STM32的PC11，发送连接STM32的PC10。

2.2驱动电机控制模块

为了保证机器人移动的驱动力以及合理的功耗，电机采用大疆M3508直流无刷电机，F407和电机之间利用CAN通信，两路伺服电机和六路舵机利用PWM信号操控伞形机械臂工作。F407利用C620电调控制直流电机，同时电机也会向主控板反馈电机转速、角度和位置等相关信息，以达到精准定位的目的，驱动电机控制模块如图3（c）所示。

2.3定位模块

本文在上述2种机器人定位系统的启发下，提出一种基于M3508设计的里程计程序和RFID联合轨道定位的移动机器人的定位方法。利用CAN通信得到M3508直流伺服电机反馈数据，建立里程计模块；将RFID读取的信息融合至里程计模块中建立融合定位模块，对机器人位置进行精确定位，定位模块如图3（d）所示。CAN属于总线式串行通信网络，数据传送的速度快、可靠且灵活，CAN总线结构主要包括物理层以及数据链路层，数据链路层主要为媒体访问控制子层MAC和逻辑链路控制子层LLC，符合ISO/0SI的标准建设模型要求，具有更好的灵活性以及透明度。LLC子层可以提供数据传递服务以及远程数据请求服务；MAC子层可以提供控制帧结构、错误检验、执行仲裁、故障界定和出错标定等传送规则服务。由于STM32无法直接与CAN信号通信，因此在设计过程中添加TJA1050芯片进行数据传输，将CAN信号传递给STM32。

3软件系统开发

本文对监测机器人的嵌入式软件层面进行设计。其中，嵌入式编程利用C语言进行模块化编程。根据隧道巡检机器人现状以及当前隧道环境的工作要求，在机器人嵌入式系统架构中，主要功能包括机器人系统初始化、传感器实时采集、网络通信收发、运动控制和安全监视等，机器人软件模块组成如图4所示。

3.1系统初始化

系统初始化主要包括GPIO初始化、中断优先级设置、定时器设置和CAN初始化。

3.2传感器采集

分别采集机器人的电机温度数据、位置数据、速度数据、电流数据，可见光和热成像图像数据以及与障碍物之间的距离。

3.3网络通信

网络通信主要基于ESP8266模块的透传模式，STM32连接Wi-Fi，接收上位机控制指令，同时反馈当前状态给上位机。

3.4安全监视

安全监视主要包括机器人的电量监视和避障监视。如果出现受限空间的避障要求，就会发出信号，使机器人避障。

3.5运动控制

受限空间监测机器人主要有驱动机构运动和伸缩机构运动。在嵌入式软件中分别对驱动机构和伸缩机构进行模块化设计。机器人运动采用分层控制，分为决策、协调和执行3层（运动分层控制模型如图5所示）。决策层需要结合远程指令与融合定位信息对监测机器人做出运动指令；协调层根据传感器反馈的执行器数据协调决策层给出的决策信息发出最终指令；执行层由直行电机、升降电机、CHARGE继电器和刹车机构等单元执行上位机给出的指令。协调层利用运动控制电路控制执行层，例如，运动控制单元根据PMN信号为直行电机和升降电机供电，使用开关机构控制CHARGE继电器和刹车机构。

4结论

本文研究受限空间监测机器人运动控制系统的定位传感器、硬件和软件系统。本文研制了一款小尺寸激光位移传感器，提出基于DLT和非线性优化的外参计算方法以及改进灰度重心法的中心线提取算法。在硬件方面，改进了机器人电器电路，提出基于M3508设计的里程计程序和RFID联合轨道定位的机器人定位方法。在软件方面，根据受限空间工作要求设计了机器人软件系统。

参考文献

[1]TALMON AM，BEZUIJENA. Analytical model for the beam action of atunnel lining during construction[J].International journal for numerical and analytical methods in geomechanics，2013，37（2）：181-200.

[2]李靖铭，李小雨，王锲，等.Kalman-BP耦合模型预测下穿高铁桥梁盾构隧道形变[J]. 湖北民族大学学报（自然科学版），2022，40（4）：445-450.

[3]肖明清，封坤，周子扬，等. 盾构隧道施工期管片错台影响因素研究[J]. 岩土工程学报，2023，45（7）：1347-1356.

[4]张琨，陈定方，高铭鑫，等. 有监督学习下的市域铁路隧道结构裂缝边缘识别方法[J]. 中国机械工程，2021，32（4）：446-453.

[5]张志伟，梁荣柱，李忠超，等. 盾尾非对称推力作用下盾构隧道纵向变形分析[J]. 岩土力学，2023，44（1）：88-98.