CN110244717B - 基于既有三维模型的港口起重机攀爬机器人自动寻路方法 - Google Patents

Abstract

本发明提供基于既有三维模型的港口起重机攀爬机器人自动寻路方法，包括：步骤1.将搭载有摄像头的攀爬机器人放置于起重机上；步骤2.攀爬机器人按照规划的运动轨迹进行运动，并通过摄像头获取图像，筛选出可以拟合出比较长的直线的点；步骤3.基于筛选出的边界点拟合出边界线；步骤4.基于边界线修正攀爬机器人的运动轨迹；步骤5.基于攀爬机器人上安装的定位器，根据起重机的三维模型，确定攀爬机器人所处的模型位置坐标和姿态；步骤6.根据预设运动路径，基于攀爬机器人所处的模型位置坐标和姿态，规划下一个时段内的运动轨迹；步骤7.重复上述步骤2至6直至攀爬机器人移动至预设运动路径的终点。

Description

基于既有三维模型的港口起重机攀爬机器人自动寻路方法

技术领域

本发明属于攀爬机器人寻路识别技术领域，具体涉及一种基于既有三维模型的港口起重机攀爬机器人自动寻路方法。

背景技术

港口起重机械作为一种起重量大、工作条件恶劣的工程机械，国家规定必须进行定时检修。目前行业主流的检测方法是通过人工检查金属结构裂纹的方法来评估港口起重机械的工作状态，工作量大，无法保证操作人员的安全。因此，开发一款针对港口起重机械的攀爬机器人来代替人工对金属结构进行检查是很有必要的，而设计的难点就是攀爬机器人如何在港口起重机械上自动寻路，到达指定的测量点。

现有的自动寻路算法大部分都是在路径上预先设定标定点或者在机器人上设置大量传感器的方式来实现机器人的自动寻路。比如上海港第一代自动化码头就是通过在码头埋设大量磁钉的方式实现AGV小车在港区的定位和导航；很多仓储运输机器人则是通过识别地面上预先设置的二维码实现定位和导航；无人驾驶汽车在车身上安装多种传感器不断探测周围环境来实现定位和导航。而这些方法不适用于港口起重机械，港口起重机械多为金属结构，工作条件恶劣，无法长期设置标记；攀爬机器人负载能力有限，无法安装大量传感器。

另外，在机器人运行过程中，为了安全问题，防止驶出边界，同时为了确定自己的工作姿态，保证左右两条边界线必须充分显示在摄像头视野中，能够更好地拟合边界线对于机器人正常工作非常重要。

但是，由于光学仪器的加工误差，以及后期使用的影响，摄像头存在畸变，使得拍摄的图像相对于真实图像有一定的误差。同时，由于长期暴露于恶劣工作条件中，港口起重机械的表面受到磨损、锈蚀、老化等因素的影响，表面往往凹凸不平，而现有的边界线识别方法，没有考虑金属表面的不平整。在攀爬机器人这种需要对图像进行实时处理的领域，如果不考虑畸变误差、以及表面不平整性在边界线识别过程中的影响，将会使得识别的边界线和真实的边界相比出现较大的累积误差，容易导致攀爬机器人运行过程中出现安全隐患，发生高空坠落等事故。

发明内容

本发明是为了解决上述问题而进行的，目的在于提供一种基于既有三维模型的港口起重机攀爬机器人自动寻路方法，无需安装大量传感器，而且能够精确识别边界线，切实保证攀爬过程的安全性。

本发明为了实现上述目的，采用了以下方案：

本发明提供一种基于既有三维模型的港口起重机攀爬机器人自动寻路方法，其特征在于，包括以下步骤：

步骤1.将搭载有摄像头的攀爬机器人放置于起重机上的攀爬起始位置处；

步骤2.攀爬机器人按照规划的运动轨迹进行运动，在运动过程中不断通过摄像头获取攀爬前方视野的图像，并对获取到的图像进行降噪处理；将降噪处理后的图像中的低频区域和高频区域的分界处视为起重机主体金属结构与其他部件或背景环境的分界处；再通过Hough变换提取出边界点，进而根据边界点在图像中的位置关系筛选出可以拟合出比较长的直线的点，作为筛选出的边界点；

步骤3.基于筛选出的边界点拟合出边界线

对于每一侧的边界线，均按照如下拟合方法获得：

步骤3-1.用步骤2筛选出的位于该侧的n个点来拟合一条直线y＝kx+b，先对直线斜率k拟合，将相邻的两点两两相连，得到n-1条直线，可以求得每条直线的斜率k₁，k₂，k₃，……k_n-1；

步骤3-2.求出所有直线段斜率的算术平均值

求出每条直线段对应的斜率与平均值的差值

认为Δk_i越小的区域越平滑，而Δk_i越大的区域存在较多的影响观测结果的因素；

步骤3-3.根据Δk_i的大小确定第一个权重Q_i：Q_i和Δk_i的大小负相关，即Q_i和

的大小正相关，

Δk_i越大，所代表的区域的权重越小；归一化之后Q_i和Δk_i的关系如下所示：

式中：1≤i≤n-1，

步骤3-4.根据畸变情况确定拟合过程中的第二个权重M_i：

镜头存在多种畸变，其中径向畸变的影响最为显著：

δ_x＝s₁x(x²+y²)，

δ_y＝s₁y(x²+y²)，

式中：x、y为点在图像坐标系的坐标；δ_x、δ_y分别为x、y两个方向的畸变量；s₁为径向畸变参数，由摄像头标定得到；

每个点总的畸变量

M_i和畸变量δ_i的大小负相关，即

其中1≤i≤n-1；权重M_i和畸变δ_i的关系如下所示：

式中：1≤i≤n-1，

在计算过程中，由每一条直线段的中点坐标(x_j ， y_j ) 来确定δ_x，δ_y，最终确定畸变δ_i和对应的权重M_i；

步骤3-5.对任意一条直线段的斜率k赋予两个权重Q_i和M_i，将两个权重统一为一个总的权重N_i：

N_i＝qQ_iM_i，

式中q为归一化常数，需满足如下关系：

对每一条直线段的斜率k_i赋予权重，最终求得拟合的边界线的斜率k：

k＝N₁k₁+N₂k₂+…+N_n-1k_n-1，

步骤3-6.求解直线方程的常数b：

对于筛选出的n个点，使用Δk_i和畸变参数对每个点进行加权；第一个点和最终的点对应的只有一条直线段，将Q₁和Q_n-1直接作为第一个点和第n个点的权重U₁和U_n；中间第i个点的权重为相邻两条直线段的权重的平均值，

第一个权重确定；

第二个权重根据畸变情况确定，对每个点的坐标计算出δ_x，δ_y，将总畸变δ_i的倒数作为第二个权重

确定每个点坐标的最终权重为W_i＝pU_iV_i，式中

求出每一点到直线y＝kx+b的距离d_i 1≤i≤n，对距离d_i赋予权重W_i，使所有的d_i之和最小的常数b即为所求，常数b满足下式：

通过以上方法，即可确定每一侧的边界线所对应的直线方程y＝kx+b；

步骤4.基于边界线修正攀爬机器人的运动轨迹，使攀爬机器人始终处于边界线所围成的区域的正中间；

步骤5.在攀爬机器人的运动过程中，基于攀爬机器人上安装的定位器，根据起重机的三维模型，确定攀爬机器人所处的模型位置坐标和姿态，进而确定攀爬机器人在起重机具体结构上的运动情况；

步骤6.根据预设运动路径，基于攀爬机器人所处的模型位置坐标和姿态，规划下一个时段内的运动轨迹；

步骤7.重复上述步骤2至6直至攀爬机器人移动至预设运动路径的终点。

进一步，本发明提供的基于既有三维模型的港口起重机攀爬机器人自动寻路方法，还可以具有以下特征：

在步骤4中，通过在边界线捕捉特征点，计算出攀爬机器人到边界线的距离：

式中Z_c为缩放系数；

为特征点在图像像素坐标系上的坐标，可以直接从图像中提取；

为摄像头内参矩阵；

为两个摄像头相对的位置和姿态信息；

为特征点在物方坐标系的坐标；

综合摄像头采集的左右两侧边界处的图像信息，可以求解出特征点在物方的坐标，攀爬机器人本身在物方坐标系中的位置已知，坐标已知，即可求解出距离；分别在左、右边界上选择特征点，即可求得机器人到左、右边界的距离 L₁、L₂；

比较L₁、L₂的大小，可确定此时机器人相对于两条边界的距离，若L₁大于L₂，则可以确定攀爬机器人右偏，需要向左修正；若L₁小于L₂，则需要向右修正，直至L₁等于L₂。

进一步，本发明提供的基于既有三维模型的港口起重机攀爬机器人自动寻路方法，还可以具有以下特征：采用的摄像头为双目摄像头，定位器为高精度陀螺仪。

进一步，本发明提供的基于既有三维模型的港口起重机攀爬机器人自动寻路方法，还可以具有以下特征：攀爬机器人内设有控制模块，用于控制攀爬机器人执行上述步骤2至步骤7的内容。

进一步，本发明提供的基于既有三维模型的港口起重机攀爬机器人自动寻路方法，还可以具有以下特征：攀爬机器人内设有通信模块，用于将摄像头获取的图像信息和定位器获取的位置信息传输给外部控制中心，并接收控制中心发送来的指令信息。

发明的作用与效果

本发明提供的基于既有三维模型的港口起重机攀爬机器人自动寻路方法，在运动过程中不断通过摄像头获取攀爬前方视野的图像，并将降噪处理后的图像中的低频区域和高频区域的分界处视为起重机主体金属结构与其他部件或背景环境的分界处，再通过Hough变换提取并筛选出边界点；接着，将畸变作为权重赋予到所有的边界点，畸变大的点得到更小的权重，畸变小的点得到更大的权重；同时为了更好地符合金属表面的实际情况，使用相邻边界点的斜率 k作为平整度的评价指标，越平整的部分得到更大的权重，不平整的部分权重较小；将两个畸变相乘，进行归一化处理，得到最终的权重来拟合边界线；本方法对于图像的畸变问题并没有对整幅图像进行畸变校正，只是对于最终的边界点根据畸变赋予权重，在保证比较高的精度前提下提高了图像的处理速度，简化了工作量；对于金属表面平整度的问题，确定了将斜率k作为评价指标，引入权重，更多地考虑平整表面对于最终拟合直线的贡献，相对于直接线性拟合更加符合实际情况；将两个畸变统一，兼顾处理速度和最终直线拟合的精度。因而可以精确识别边界线，能够切实保证攀爬过程的安全性。并且，本方法中无需安装大量传感器，不用在行进路径上设置标记点，就能够确定攀爬机器人在港口起重机械上的位置，根据既有的港口起重机械三维模型实现自动寻路。

附图说明

图1为本发明实施例中涉及的基于既有三维模型的港口起重机攀爬机器人自动寻路方法的流程图；

图2为本发明实施例中涉及的攀爬机器人的结构示意图；

图3为本发明实施例中攀爬机器人在起重机上起点位置的示意图；

图4为本发明实施例中低频区域(灰度变化缓慢的纯色区域)和高频区域(灰度变化交大区域)的分界处示意图；

图5为本发明实施例中基于筛选出的边界点拟合出边界线的流程图；

图6为本发明实施例中提取的点两两相连拟合直线的示意图；

图7为本发明实施例中纠偏原理示意图。

在上述图2和3中各标号含义如下：

1-攀爬机器人，2-定位器，3-摄像头，4-路基的边界，Z-起重机。

具体实施方式

以下结合附图对本发明涉及的基于既有三维模型的港口起重机攀爬机器人自动寻路方法的具体实施方案进行详细地说明。

<实施例>

如图1所示，本实施例提供的基于既有三维模型的港口起重机攀爬机器人自动寻路方法，包括以下步骤：

步骤1.如图2和3所示，将搭载有摄像头3的攀爬机器人1放置于起重机Z上的攀爬起始位置处，本实施例采用的摄像头3为双目摄像头。

步骤2.攀爬机器人1按照规划的运动轨迹进行运动，在运动过程中不断通过摄像头3获取攀爬前方视野的图像；对获取到的图像进行降噪处理，降低对比度，避免因褪色对结果造成的影响。

然后，如图4所示，将降噪处理后的图像中的低频区域(灰度变化缓慢的纯色区域)和高频区域(灰度变化交大区域)的分界处视为起重机Z主体金属结构与其他部件或背景环境的分界处。主体部分的边界线应该是两条相互平行的直线，由于机器人视角的问题，摄像头3捕捉到的画面里两条直线会在远方交汇(不平行只是由于视角的变化)。

接着，通过Hough变换中的边缘检测算子提取出边界点，进而根据边界点在图像中的位置关系筛选出可以拟合出比较长的直线的点，作为筛选出的边界点。

步骤3.基于筛选出的边界点拟合出边界线：

如图5所示，对于每一侧的边界线，均按照如下拟合方法获得：

步骤3-1.如图6所示，用步骤2筛选出的位于该侧的n个点来拟合一条直线y＝kx+b，先对直线斜率k拟合，将相邻的两点两两相连，得到n-1条直线，可以求得每条直线的斜率k₁，k₂，k₃，……k_n-1；

步骤3-2.求出所有直线段斜率的算术平均值

求出每条直线段对应的斜率与平均值的差值

认为Δk_i越小的区域越平滑，而Δk_i越大的区域存在较多的毛刺、焊点等影响观测结果的因素；

步骤3-3.根据Δk_i的大小确定第一个权重Q_i：Q_i和Δk_i的大小负相关，即Q_i和

的大小正相关，

Δk_i越大，所代表的区域的权重越小；归一化之后Q_i和Δk_i的关系如下所示：

直线拟合过程中第一个权重Q_i确定。

步骤3-4.根据畸变情况确定拟合过程中的第二个权重M_i。现有的镜头测量过程中存在畸变，一幅图像中畸变越大的位置误差越大，可信度越低，畸变越小的位置可信度越高。根据这一原则确定直线拟合过程中的第二个权重M_i。

镜头存在多种畸变，其中径向畸变的影响最为显著：

δ_x＝s₁x(x²+y²)，

δ_y＝s₁y(x²+y²)，

式中：x、y为点在图像坐标系的坐标；δ_x、δ_y分别为x、y两个方向的畸变量；s₁为径向畸变参数，由摄像头3标定得到；

每个点总的畸变量

运用总的畸变量δ_i对每一段直线的斜率进行加权，畸变量较小的区域权重较大，以此确定第二个权重M_i。M_i和畸变量δ_i的大小负相关，即

其中 1≤i≤n-1；权重M_i和畸变δ_i的关系如下所示：

在计算过程中，由每一条直线段的中点坐标(x_j ， y_j ) 来确定对应的畸变δ_i，最终确定这一条直线段的权重M_i。中点计算的畸变能够更好地代表直线段的误差。

由中点坐标确定δ_x，δ_y，最终确定畸变δ_i和对应的权重M_i。

步骤3-5.对任意一条直线段的斜率k赋予两个权重Q_i和M_i，将两个权重统一为一个总的权重N_i：

N_i＝qQ_iM_i，

式中q为归一化常数，q存在是为了保持最终总的权重N_i归一化，需满足如下关系：

对每一条直线段的斜率k_i赋予权重，最终求得拟合的边界线的斜率k：

k＝N₁k₁+N₂k₂+…+N_n-1k_n-1

步骤3-6.求解直线方程的常数b：

对于筛选出的n个点，使用Δk_i和畸变参数对每个点进行加权；第一个点和最终的点对应的只有一条直线段，将Q₁和Q_n-1直接作为第一个点和第n个点的权重U₁和U_n；中间第i个点的权重为相邻两条直线段的权重的平均值，

第一个权重确定；

第二个权重根据畸变情况确定，对每个点的坐标计算出δ_x，δ_y，将总畸变δ_i的倒数作为第二个权重

确定每个点坐标的最终权重为W_i＝pU_iV_i，式中

求出每一点到直线y＝kx+b的距离d_i(1≤i≤n)，对距离d_i赋予权重W_i，使所有的d_i之和最小的常数b即为所求，常数b满足下式：

通过以上方法，即可确定每一侧的边界线所对应的直线方程y＝kx+b；

步骤4.基于边界线修正攀爬机器人1的运动轨迹，使攀爬机器人1始终处于边界线所围成的区域的正中间。

通过在边界线捕捉特征点，计算出攀爬机器人1到边界线的距离：

式中Z_c为缩放系数；

为特征点在图像像素坐标系上的坐标，可以直接从图像中提取；

为摄像头3内参矩阵，实验室标定确定；

为两个摄像头3相对的位置和姿态信息；

为特征点在物方坐标系的坐标；

综合摄像头3采集的左右两侧边界处的图像信息，可以求解出特征点在物方的坐标，攀爬机器人1本身在物方坐标系中的位置已知，坐标已知，即可求解出距离；如图7所示，分别在左、右边界上选择特征点，即可求得机器人到左、右边界的距离L₁、L₂；

比较L₁、L₂的大小，可确定此时机器人相对于两条边界的距离，若L₁大于L₂，则可以确定攀爬机器人1右偏，需要向左修正；若L₁小于L₂，则需要向右修正，直至L₁等于L₂，确保机器人的实时位置位于两条边界线的中心。

步骤5.在攀爬机器人1的运动过程中，基于攀爬机器人1上安装的定位器，根据起重机Z的三维模型，确定攀爬机器人1所处的模型位置坐标和姿态，进而确定攀爬机器人1在起重机Z具体结构上的运动情况。

本实施例中，采用的定位器2为高精度陀螺仪，结合起重机Z的三维模型确定机器人在空间中的具体位置(具体到哪一根梁上)。港口起重机Z械的优势是结构相对固定，主要分为几类，同一种类的主要结构件尺寸不同，但形状变化不大，在系统内预先存储相应的结构模型，结合高精度陀螺仪和摄像头3 的数据，即可判断攀爬机器人1正在哪一根梁上运行，以及运行方向。

如图3所示，攀爬机器人1从立柱向上攀爬，在遇到第一个岔路(横梁和立柱的交界处)时，结合三维模型，若系统判断此次运行的目的地是前大梁，则需要向上运行，在道路的尽头转到门框的上横梁，最后到前大梁；若判断需要到横梁或者是需要通过横梁前往其他位置，则会在岔路处拐90度，在横梁上行驶，在拐90度的过程中，定位器读数应该在Z方向变化90度，辅助修正攀爬机器人1此时的姿态。结合三维模型和定位器数据，确定攀爬机器人1在港口起重机Z械上的位置坐标和姿态(运动方向)。

步骤6.根据预设运动路径，基于攀爬机器人1所处的模型位置坐标和姿态，规划下一个时段内的运动轨迹；

起重机Z的三维模型相当于机器人的运行环境，我们将模型预先存储在系统中，避免使用大量传感器来感知周围的环境。若需要前往前大梁，以起重机 Z接地处为起点，在已知三维模型的前提下，可以得到路径：立柱——门框上横梁——前大梁。每一个梁在空间中的姿态和相互关系由输入法的三维模型确定，结合陀螺仪数据和摄像头3数据，智能判断此时机器人运行的位置和姿态，规划下一个时间段机器人的运动轨迹，在下一个岔路是直行还是拐，拐的方向。完成整体路径的规划。

确定机器人在运行过程中的边界和相对于边界的距离，以及机器人的姿态和位置，通过既有的三维模型自动规划机器人下一步即将运行到起重机Z的哪一根梁上，如何才能最快地到达需要检测的位置，实现攀爬机器人1在港口起重机Z械上的自动寻路。

步骤7.重复上述步骤2至6直至攀爬机器人1移动至预设运动路径的终点。

本实施例中，攀爬机器人1内部设有控制模块，用于控制攀爬机器人1执行上述步骤2至步骤7的图像处理、计算、判断、规划、寻路以及控制运动灯内容。也就是说，所有的控制和处理工作都是在攀爬机器人1自带的控制模块中完成。

另外，攀爬机器人1内还应设有通信模块，用于将摄像头3获取的图像信息和定位器2获取的位置信息，以及控制模块的控制信息(包括处理、计算、判断、规划、寻路、运动指示等信息)，通过无线通信方式远程传输给外部控制中心，由控制中心的管理员对这些情况进行监控。

此外，为了简化攀爬机器人1，还可以直接通过通信模块将摄像头3获取的图像信息和定位器2获取的位置信息都直接传输给外部控制中心，由控制中心的控制处理系统来对这些信息按照上述步骤2至7中所描述的方法进行处理、分析和判断，并生成相应的控制指令传输给攀爬机器人1，攀爬机器人1 再根据接收到的指令信息进行运动。即、攀爬机器人1本身不对数据进行处理、计算、判断、规划、寻路和控制(不设置相应的模块)，仅接收并执行外部发送来的指令信息，这样也是可行的。

以上实施例仅仅是对本发明技术方案所做的举例说明。本发明所涉及的基于既有三维模型的港口起重机攀爬机器人自动寻路方法并不仅仅限定于在以上实施例中所描述的内容，而是以权利要求所限定的范围为准。本发明所属领域技术人员在该实施例的基础上所做的任何修改或补充或等效替换，都在本发明的权利要求所要求保护的范围内。

Claims (5)

1.一种基于既有三维模型的港口起重机攀爬机器人自动寻路方法，其特征在于，包括以下步骤：

步骤1.将搭载有摄像头的攀爬机器人放置于起重机上的攀爬起始位置处；

步骤2.攀爬机器人按照规划的运动轨迹进行运动，在运动过程中不断通过摄像头获取攀爬前方视野的图像；对获取到的图像进行降噪处理；将降噪处理后的图像中的低频区域和高频区域的分界处视为起重机主体金属结构与其他部件或背景环境的分界处；再通过Hough变换提取出边界点，进而根据边界点在图像中的位置关系筛选出可以拟合出比较长的直线的点，作为筛选出的边界点；

步骤3.基于筛选出的边界点拟合出边界线

对于每一侧的边界线，均按照如下拟合方法获得：

步骤3-1.用步骤2筛选出的位于该侧的n个点来拟合一条直线y＝kx+b，先对直线斜率k拟合，将相邻的两点两两相连，得到n-1条直线，可以求得每条直线的斜率k₁，k₂，k₃，……k_n-1；

步骤3-2.求出所有直线段斜率的算术平均值

求出每条直线段对应的斜率与平均值的差值

认为Δk_i越小的区域越平滑，而Δk_i越大的区域存在较多的影响观测结果的因素；

步骤3-3.根据Δk_i的大小确定第一个权重Q_i：Q_i和Δk_i的大小负相关，即Q_i和

的大小正相关，

Δk_i越大，所代表的区域的权重越小；归一化之后Q_i和Δk_i的关系如下所示：

式中：1≤i≤n-1，

步骤3-4.根据畸变情况确定拟合过程中的第二个权重M_i：

镜头存在多种畸变，其中径向畸变的影响最为显著：

δ_x＝s₁x(x²+y²)，

δ_y＝s₁y(x²+y²)，

式中：x、y为点在图像坐标系的坐标；δ_x、δ_y分别为x、y两个方向的畸变量；s₁为径向畸变参数，由摄像头标定得到；

每个点总的畸变量

M_i和畸变量δ_i的大小负相关，即

其中1≤i≤n-1；权重M_i和畸变δ_i的关系如下所示：

式中：1≤i≤n-1，

在计算过程中，由每一条直线段的中点坐标(x_j ， y_j ) 来确定δ_x，δ_y，最终确定畸变δ_i和对应的权重M_i；

步骤3-5.对任意一条直线段的斜率k赋予两个权重Q_i和M_i，将两个权重统一为一个总的权重N_i：

N_i＝qQ_iM_i，

式中q为归一化常数，需满足如下关系：

对每一条直线段的斜率k_i赋予权重，最终求得拟合的边界线的斜率k：

k＝N₁k₁+N₂k₂+…+N_n-1k_n-1，

步骤3-6.求解直线方程的常数b：

对于筛选出的n个点，使用Δk_i和畸变参数对每个点进行加权；第一个点和最终的点对应的只有一条直线段，将Q₁和Q_n-1直接作为第一个点和第n个点的权重U₁和U_n；中间第i个点的权重为相邻两条直线段的权重的平均值，

2≤i≤n-1，第一个权重确定；

第二个权重根据畸变情况确定，对每个点的坐标计算出δ_x，δ_y，将总畸变δ_i的倒数作为第二个权重

1≤i≤n；

确定每个点坐标的最终权重为W_i＝pU_iV_i，式中

求出每一点到直线y＝kx+b的距离d_i，1≤i≤n，对距离d_i赋予权重W_i，使所有的d_i之和最小的常数b即为所求，常数b满足下式：

通过以上方法，即可确定每一侧的边界线所对应的直线方程y＝kx+b；

步骤4.基于边界线修正攀爬机器人的运动轨迹，使攀爬机器人始终处于边界线所围成的区域的正中间；

步骤5.在攀爬机器人的运动过程中，基于攀爬机器人上安装的定位器，根据起重机的三维模型，确定攀爬机器人所处的模型位置坐标和姿态，进而确定攀爬机器人在起重机具体结构上的运动情况；

步骤6.根据预设运动路径，基于攀爬机器人所处的模型位置坐标和姿态，规划下一个时段内的运动轨迹；

步骤7.重复上述步骤2至6直至攀爬机器人移动至预设运动路径的终点。

2.根据权利要求1所述的基于既有三维模型的港口起重机攀爬机器人自动寻路方法，其特征在于：

其中，在步骤4中，通过在边界线捕捉特征点，计算出攀爬机器人到边界线的距离：

式中Z_c为缩放系数；

为特征点在图像像素坐标系上的坐标，可以直接从图像中提取；

为摄像头内参矩阵；

为两个摄像头相对的位置和姿态信息；

为特征点在物方坐标系的坐标；

综合摄像头采集的左右两侧边界处的图像信息，可以求解出特征点在物方的坐标，攀爬机器人本身在物方坐标系中的位置已知，坐标已知，即可求解出距离；分别在左、右边界上选择特征点，即可求得机器人到左、右边界的距离L₁、L₂；

比较L₁、L₂的大小，可确定此时机器人相对于两条边界的距离，若L₁大于L₂，则可以确定攀爬机器人右偏，需要向左修正；若L₁小于L₂，则需要向右修正，直至L₁等于L₂。

3.根据权利要求1所述的基于既有三维模型的港口起重机攀爬机器人自动寻路方法，其特征在于：

其中，采用的摄像头为双目摄像头，定位器为高精度陀螺仪。

4.根据权利要求1所述的基于既有三维模型的港口起重机攀爬机器人自动寻路方法，其特征在于：

其中，攀爬机器人内设有控制模块，用于控制攀爬机器人执行上述步骤2至步骤7的内容。

5.根据权利要求1所述的基于既有三维模型的港口起重机攀爬机器人自动寻路方法，其特征在于：

其中，攀爬机器人内设有通信模块，用于将摄像头获取的图像信息和定位器获取的位置信息传输给外部控制中心，并接收控制中心发送来的指令信息。

Images