CN114578820A - 一种核电站检测爬壁机器人的行进轨迹控制方法 - Google Patents
一种核电站检测爬壁机器人的行进轨迹控制方法 Download PDFInfo
- Publication number
- CN114578820A CN114578820A CN202210204465.3A CN202210204465A CN114578820A CN 114578820 A CN114578820 A CN 114578820A CN 202210204465 A CN202210204465 A CN 202210204465A CN 114578820 A CN114578820 A CN 114578820A
- Authority
- CN
- China
- Prior art keywords
- robot
- gate
- crawling
- laser range
- direction angle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 49
- 238000001514 detection method Methods 0.000 title claims abstract description 19
- 238000005259 measurement Methods 0.000 claims abstract description 81
- 230000009193 crawling Effects 0.000 claims description 114
- 229910000831 Steel Inorganic materials 0.000 claims description 12
- 239000010959 steel Substances 0.000 claims description 12
- 238000001914 filtration Methods 0.000 claims description 10
- 230000008569 process Effects 0.000 claims description 9
- 238000005070 sampling Methods 0.000 claims description 9
- 238000011065 in-situ storage Methods 0.000 claims description 8
- 230000004927 fusion Effects 0.000 claims description 7
- 238000009434 installation Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 4
- 238000003384 imaging method Methods 0.000 claims description 4
- BULVZWIRKLYCBC-UHFFFAOYSA-N phorate Chemical compound CCOP(=S)(OCC)SCSCC BULVZWIRKLYCBC-UHFFFAOYSA-N 0.000 claims description 4
- 238000012937 correction Methods 0.000 claims description 3
- 230000003068 static effect Effects 0.000 claims description 2
- 238000013461 design Methods 0.000 abstract description 2
- 238000011161 development Methods 0.000 abstract description 2
- 239000000498 cooling water Substances 0.000 description 5
- 238000012360 testing method Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000007689 inspection Methods 0.000 description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 3
- 230000017525 heat dissipation Effects 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000009194 climbing Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000004804 winding Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
-
- G—PHYSICS
- G21—NUCLEAR PHYSICS; NUCLEAR ENGINEERING
- G21C—NUCLEAR REACTORS
- G21C17/00—Monitoring; Testing ; Maintaining
- G21C17/003—Remote inspection of vessels, e.g. pressure vessels
- G21C17/013—Inspection vehicles
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02E—REDUCTION OF GREENHOUSE GAS [GHG] EMISSIONS, RELATED TO ENERGY GENERATION, TRANSMISSION OR DISTRIBUTION
- Y02E30/00—Energy generation of nuclear origin
- Y02E30/30—Nuclear fission reactors
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Electromagnetism (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- General Engineering & Computer Science (AREA)
- High Energy & Nuclear Physics (AREA)
- Optics & Photonics (AREA)
- Plasma & Fusion (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种核电站检测爬壁机器人的行进轨迹控制方法，步骤如下：步骤1、对传感器进行配置；步骤2、约束测算；步骤3、开启方向角测量；步骤4、机器人方向角初对准；步骤5、保持机器人方向行进；步骤6、机器人转向。本发明行进方向控制和倾覆检测采用同一套传感器，结构简单，便于设计，同时降低了研制成本。
Description
技术领域
本发明涉及核电站安全壳安全检测技术领域，具体为一种核电站检测爬壁机器人的行进轨迹控制方法。
背景技术
核电站安全壳通常采用内外双层结构，外层为混凝土结构的屏蔽层，内层为钢制安全壳；其中钢制安全壳的主要作用是将核电站内部的温度传到到外部，利用控器对流扩散到外界；为了提升散热效率，钢制安全壳外层会设置空气导流板，用以提高空气对流速度，带走多余热量；同时安全壳顶部设置有冷却水箱，是核电站重要非能动保险装置，必要时会自动释放冷却水，冷却水在重力作用下沿安安全壳外壁流动，带走大量热量，极大减缓安全壳内部升温速度；冷却水箱释放冷却水后，冷却水在安全壳外壁流动时能否充分覆盖安全壳外壁表面，是制约散热效率的关键因素；因此钢制安全壳水膜覆盖率测量试验是在核电站投入运行前调试阶段的一项重要试验，同时为了保证运行安全，在核电站运行期间也需要定期开展水膜覆盖率测量试验；除了水膜覆盖率测试以外，定期对安全壳外壁和空气导流板进行检视，也是保证核电站安全、稳定运行的必要补充手段；
目前常用的测量和检视手段多为人工，需要测量人员进入核电站内外安全壳之间的狭窄空间，拆除内层钢制安全壳外侧的空气导流板后进行测量或检查，测量结果主观性强，拆装空气导流板工序复杂，时间长，且带有一定危险性；采用爬壁机器人在钢制安全壳表面和空气导流板之间的狭缝中爬行并采集表面图像进行测量和检视，可以避免空气导流板拆装，有效提升效率，降低风险；
由于安全壳上部环廊上方有人员闸门和设备闸门处于机器人水平行进路线上，需要机器人绕行，因此需要解决机器人在狭窄空间电磁屏蔽严重的环境下按预定路线行进的问题。
发明内容
本发明的目的在于提供一种核电站检测爬壁机器人的行进轨迹控制方法，以解决上述背景技术中提出的机器人在狭窄空间电磁屏蔽严重的环境下按预定路线行进的问题。
为实现上述目的，本发明提供如下技术方案：
一种核电站检测爬壁机器人的行进轨迹控制方法，用于核电站安全壳表面爬行作业时的行进方向和路线控制，为方便后续描述，定义机器人固连坐标系OrXrYrZr，其中坐标系原点Or位于爬行机器人形心，机器人直行前进方向为+Xr方向，垂直于机器人底盘平面向上方向为+Zr方向，+Yr轴方向根据右手螺旋法确定，考虑爬行机器人在安全壳表面行进过程中需要向上走“几”字形路线绕过人员闸门和设备闸门，爬行机器人的行进路线分为水平直行和闸门绕行两类，步骤如下：
步骤1、对传感器进行配置；
步骤2、约束测算：根据爬行机器人的相机成像参数和任务约束，测算出路径控制的约束以及传感器选型参数约束；
步骤3、开启方向角测量：在将爬行机器人放置在安全壳表面后，根据实际任务路线需求，开启对应传感器，进行方向角测量；
步骤4、机器人方向角初对准：爬行机器人行进前对方向角进行调整，实现初始对准；
步骤5、保持机器人方向行进：在机器人初始方向对准的基础上，控制其保持当前方向继续行进，直至下一转弯点；
步骤6、机器人转向：在转向点，控制机器人方向角
旋转，实现直行和闸门绕行的情况切换。
优选的，所述步骤1中，配置方案如下：
配置激光测距仪：配置在机器人前段、机器人后端和机器人侧端，机器人前端的激光测距仪之间的安装距离为WLpx，靠近+Yr方向的激光测距仪为Lpx1，另一个激光测距仪为Lpx2，机器人后端的激光测距仪之间的安装距离为WLnx，靠近+Yr方向的激光测距仪为Lnx1，另一个激光测距仪为Lnx2，机器人侧端记为Ly，若Ly位于+Yr方向，则记侧端激光测距仪方向标识λ＝-1，反之则记为λ＝1；
配置惯性测量器件：单轴、双轴和三轴均可，需要能够测量爬行机器人+Xr轴方向的与重力方向绕+Zr之间的夹角，定义为方向角；
配置驱动电机编码器：+Yr侧电机和-Yr侧电机，分别记为Epy和Eny。
优选的，所述步骤2中，根据爬行机器人相机参数，安装位置和方向，测算爬行机器人相机有效工作视场宽度Wc；
根据核电站安全壳型号，确定钢制安全壳的周长Ls，爬行机器人沿上部环廊上方距离爬行时最小离地高度h；
爬行机器人路线控制的约束为：为保证爬行机器人在安全壳上爬行一周后拼接程闭合图像，需要保证爬行机器人水平爬行距离Ls后保证高度偏差小于Wc；
根据核电站安全壳设备闸门和人员闸门直径和位置，确定钢制安全壳设备闸门和人员闸门绕行距离De和Dh，记最大绕行距离D＝max(De,Dh)；
根据爬行机器人选型型号，确定爬行机器人工作情况下的爬行速度Vr；
根据下式计算爬行机器人闸门绕行时间T，D为爬行机器人闸门绕行距离，Vr为爬行机器人闸门绕行速度；
根据下式计算惯性测量器件在T时间内的测角飘移量的测角偏差上限；
为保证后续方法的有效性，分别计算惯性测量器件的选型参数约束：
角度精度pI<0.2elim，角度分辨率rI<0.2elim，考虑飘移情况，在3σ置信域内，T时间内无矫正角度测量偏差εT<elim；另外，更新时间间隔需小于1s；
为保证后续方法的有效性，分别计算激光测距仪的选型参数约束：
最大测量距离DL>D，最小测量距离dL<h，距离测量精度
距离测量分辨率
其中WLx＝min(WLpx,WLnx)。
优选的，所述步骤3中，开启后持续为后续各步骤提供测量信息；
首先设置传感器采样间隔：设置惯性测量器件的采样间隔为ΔI，ΔI由惯性器件具体型号的更新间隔决定；设置激光测距仪的采样间隔为ΔL(ΔI≤ΔL≤3s)；
然后根据实际任务路线需求，确定起始段路线为垂直向上绕行闸门或水平行进，分下述两种情况开启方向角测量；
情况一：起始段为垂直向上绕行闸门
1)启用激光测距仪Lnx1和Lnx2，其余激光测距仪关闭，启用惯性测量器件，令当前时刻t＝0；
2)判断当前时刻t是否可读取激光测距仪数据，若可则读取t时刻Lnx1和Lnx2的测距结果分别为dnx1(t)和dnx2(t)，并根据下式计算t时刻由激光测距仪计算的方向角θNL(t)：
若不可则进行下一步；
3)判断当前时刻t是否可读取惯性测量器件，若可则读取t时刻惯性器件测得方向角θI(t)，若不可则进行下一步；
4)根据步骤2)和步骤3)的测量结果，采用卡尔曼滤波融合得到当前方向角θ(t)或利用下式直接优先采用激光测距仪计算的方向角，得到当前方向角θ(t)；
5)令t＝t+1，返回步骤2)。
优选的，情况二：起始段为水平行进
机器人保持静止10s以上后判断θ(t)：
若
则原地调整机器人行进方向，减小方向角，直至
方向调整完成后，机器人保持静止，直到Ly连续测得2组相同的激光测距数据，记录为起始数据dy0；编码器Epy和Eny的t时刻读数sny(t)和sny(t)均设为0，进行步骤5；
若
则保持机器人前进方向静止，直到Ly连续测得2组相同的激光测距数据，记录为起始数据dy0；编码器Epy和Eny的t时刻读数sny(t)和sny(t)均设为0，进行步骤5。
优选的，所述步骤5中，根据起始段的两种情况，本步中保持机器人方向行进方法步骤分别如下：
情况一：起始段为垂直向上绕行闸门
i)控制机器人行进，期间调整左右电机速度，保持方向角测量θ(t)始终满足|θ(t)-π|<ε；
ii)判断当前时刻t是否可读取激光测距仪数据，若不可读取则返回步骤i)；若可读取则读取Lnx1和Lnx2的测距结果分别为dnx1(t)和dnx2(t)，跟据绕行闸门为人员闸门或设备闸门计算min(dnx1(t),dnx2(t))-dx0是否大于对应的闸门绕行距离Dh或De，若大于则抵达进行步骤6，否则返回步骤i)。
优选的，情况二：起始段为水平行进
A)控制机器人行进，期间调整左右电机速度，保持方向角测量θ(t)始终满足
B)判断当前时刻t是否可读取激光测距仪数据，若不可读取则返回步骤A)；若可读取则读Ly的测距结果为dy(t)；
C)判断dy(t)与初始高度dy0偏差是否满足|dy(t)-dy0|<εh，其中εh为水平保持高度偏差阈值εh取值满足εh∈(0，Wc)，将εh设置与激光测距仪的分辨率一致；若满足则继续，若不满足则调整左右电机速度，使得|dy(t)-dy0|<εh；
D)人员在环廊判断爬行机器人是否爬行至人员闸门或设备闸门绕行位置，若是则进行步骤6，否则返回A)。
优选的，所述步骤6中，根据起始段的两种情况，将本步骤分为两种情况:
情况一：起始段为垂直向上绕行闸门
步骤5完成后，爬行机器人垂直向上爬行到闸门上方，机器人转向步骤如下：
(I)关闭激光测距仪Ly；停止步骤3中的方向角计算；
(II)仅利用惯性测量器件测量方向角，控制爬行机器人转向
将爬行机器人调整为水平行进方向；
(III)利用编码器测量值计算爬行距离，仅利用惯性测量器件测量方向角，在保证方向角满足
的情况下，根据所绕行闸门类型为人员闸门或设备闸门，分别行进距离Dh和De；
(IV)控制爬行机器人转向
将爬行机器人调整为垂直向下方向；
(V)开启Lpx1和Lpx2；
(VI)判断当前时刻t是否可读取激光测距仪数据，若可则读取t时刻Lpx1和Lpx2的测距结果分别为dpx1(t)和dpx2(t)，并根据下式计算t时刻由激光测距仪计算的方向角θPL(t)：
若不可则进行下一步；
(VII)判断当前时刻t是否可读取惯性测量器件，若可则读取t时刻惯性器件测得方向角θI(t)，若不可则进行下一步；
(VIII)根据步骤(VI)和步骤(VIII)的测量结果，采用卡尔曼滤波融合得到当前方向角θ(t)或利用下式直接优先采用激光测距仪计算的方向角，得到当前方向角θ(t)；
(IX)根据所绕行闸门类型为人员闸门或设备闸门，分别行进距离Dh和De；
(X)控制爬行机器人转向
将爬行机器人调整为水平行进，后续可按步骤5情况二继续行进，直至爬行一周。
优选的，情况二：起始段为水平行进
步骤5完成后，爬行机器人水平爬行到闸门绕行点，机器人转向步骤如下：
控制爬行机器人转向
将爬行机器人调整为垂直向上，关闭Ly，转为闸门绕行工况；后续可按步骤3情况一测量方向角，并按步骤5情况一继续行进至闸门顶部，按步骤6情况一完成闸门绕行后，继续水平行进，直至爬行一周。
与现有技术相比，本发明的有益效果是：
1.本发明可以实现自动行进方向判定，无需认为设置机器人行进方向；
2.本发明行进方向控制和倾覆检测采用同一套传感器，结构简单，便于设计，同时降低了研制成本。
附图说明
图1为本发明爬行机器人激光测距仪配置图；
图2为本发明爬行机器人相机倾斜安装时视场示意图；
图3为本发明爬行机器人相机垂直安装时视场示意图；
图4为本发明爬行机器人人员闸门和设备闸门绕行路线图；
图5为本发明爬行机器人水平行进激光测距示意图；
图6为本发明爬行机器人垂直向上行进激光测距示意图；
图7为本发明爬行机器人闸门上方绕行示意图；
图8为本发明爬行机器人垂直向下行进激光测距示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
请参阅图1-8，本发明提供一种技术方案：一种核电站检测爬壁机器人的行进轨迹控制方法，用于核电站安全壳表面爬行作业时的行进方向和路线控制，为方便后续描述，定义机器人固连坐标系OrXrYrZr，其中坐标系原点Or位于爬行机器人形心，如图3、4所示：机器人直行前进方向为+Xr方向，垂直于机器人底盘平面向上方向为+Zr方向，+Yr轴方向根据右手螺旋法确定，考虑爬行机器人在安全壳表面行进过程中需要向上走“几”字形路线绕过人员闸门和设备闸门，如图4所示；爬行机器人的行进路线分为水平直行和闸门绕行两类，步骤如下：
步骤1、对传感器进行配置；
如图1所示：配置激光测距仪5个以上：机器人前端+Xr方向配备2个，二者安装距离WLpx，分别记为Lpx1和Lpx2，其中靠近+Yr方向的激光测距仪为Lpx1；机器人后端-Xr方向配备2个，二者安装距离WLnx，分别记为Lnx1和Lnx2，其中靠近+Yr方向的激光测距仪为Lnx1；机器人侧端+Yr方向或-Yr方向配备1个，记为Ly，同时若Ly位于+Yr方向，则记侧端激光测距仪方向标识λ＝-1，反之则记为λ＝1；
配置惯性测量器件1个以上：单轴、双轴和三轴均可，需要能够测量爬行机器人+Xr轴方向的与重力方向绕+Zr之间的夹角，定义为方向角；
配置驱动电机编码器2个：+Yr侧电机和-Yr侧电机分别记为Epy和Eny
如图2、3所示：步骤2、约束测算：根据爬行机器人的相机成像参数和任务约束，测算出路径控制的约束以及传感器选型参数约束；
步骤2中，根据爬行机器人相机参数，安装位置和方向，测算爬行机器人相机有效工作视场宽度Wc；
根据核电站安全壳型号，确定钢制安全壳的周长Ls，爬行机器人沿上部环廊上方距离爬行时最小离地高度h；
爬行机器人路线控制的约束为：为保证爬行机器人在安全壳上爬行一周后拼接程闭合图像，需要保证爬行机器人水平爬行距离Ls后保证高度偏差小于Wc；
根据核电站安全壳设备闸门和人员闸门直径和位置，确定钢制安全壳设备闸门和人员闸门绕行距离De和Dh，记最大绕行距离D＝max(De,Dh)；
根据爬行机器人选型型号，确定爬行机器人工作情况下的爬行速度Vr；
根据下式计算爬行机器人闸门绕行时间T，D为爬行机器人闸门绕行距离，Vr为爬行机器人闸门绕行速度；
根据下式计算惯性测量器件在T时间内的测角飘移量的测角偏差上限；
为保证后续方法的有效性，分别计算惯性测量器件的选型参数约束：
角度精度pI<0.2elim，角度分辨率rI<0.2elim，考虑飘移情况，在3σ置信域内，T时间内无矫正角度测量偏差εT<elim；另外，更新时间间隔需小于1s；
为保证后续方法的有效性，分别计算激光测距仪的选型参数约束：
最大测量距离DL>D，最小测量距离dL<h，距离测量精度
距离测量分辨率
其中WLx＝min(WLpx,WLnx)
步骤3、开启方向角测量：目的在于在将爬行机器人放置在安全壳表面后，根据实际任务路线需求，开启对应传感器，进行方向角测量；
如图8所示：步骤3中，开启后持续为后续各步骤提供测量信息；
首先设置传感器采样间隔：设置惯性测量器件的采样间隔为ΔI，ΔI由惯性器件具体型号的更新间隔决定；设置激光测距仪的采样间隔为ΔL(ΔI≤ΔL≤3s)；
然后根据实际任务路线需求，确定起始段路线为垂直向上绕行闸门或水平行进，分下述两种情况开启方向角测量；
情况一：起始段为垂直向上绕行闸门
1)启用激光测距仪Lnx1和Lnx2，其余激光测距仪关闭，启用惯性测量器件，令当前时刻t＝0；
2)判断当前时刻t是否可读取激光测距仪数据，若可则读取t时刻Lnx1和Lnx2的测距结果分别为dnx1(t)和dnx2(t)，并根据下式计算t时刻由激光测距仪计算的方向角θNL(t)：
若不可则进行下一步；
3)判断当前时刻t是否可读取惯性测量器件，若可则读取t时刻惯性器件测得方向角θI(t)，若不可则进行下一步；
4)根据步骤2)和步骤3)的测量结果，采用卡尔曼滤波融合得到当前方向角θ(t)或利用下式直接优先采用激光测距仪计算的方向角，得到当前方向角θ(t)；
5)令t＝t+1，返回步骤2)；
情况二：起始段为水平行进
a)启用激光测距仪Ly，其余激光测距仪关闭，启用惯性测量器件，令当前时刻t＝0；
b)判断当前时刻t是否可读取激光测距仪数据，若可则读取t时刻Ly的测距结果分别为dy(t)；若不可则进行下一步；
c)判断当前时刻t是否可读取惯性测量器件，若可则读取t时刻惯性器件测得方向角θI(t)，若不可则进行下一步；
d)根据步骤b)和步骤c)的测量结果，经卡尔曼滤波后得到θ(t)，t卡尔曼滤波方法为控制领域常见方法，不属于本发明保护范围；
e)令t＝t+1，返回步骤b)；
步骤3中，开启方向角测量后持续循环，用于后续步骤随时获得θ(t)，直至步骤6更新方向角计算方式
步骤4、机器人方向角初对准：目的在于爬行机器人行进前对方向角进行调整，实现初始对准；
步骤4中，设置方向对准偏差阈值ε，取值范围为ε∈(0elim)；根据步骤3中的情况区分，机器人方向角初始对准步骤分别如下
情况一：起始段为垂直向上绕行闸门
机器人保持静止10s以上后判断θ(t)：
若|θ(t)-π|>ε，则原地调整机器人行进方向，直至|θ(t)-π|≤ε；方向调整完成后，机器人保持静止，直到Lnx1为Lnx2均连续测得2组相同的激光测距数据，记录为起始数据dnx10和dnx20，记起始点距离环廊平面的距离为dx0＝(dnx10+dnx20)/2；编码器Epy和Eny的t时刻读数sny(t)和sny(t)均设为0；进行步骤5；
若|θ(t)-π|≤ε，则保持机器人前进方向静止，直到Lnx1为Lnx2均连续测得2组相同的激光测距数据，记录为起始数据dnx10和dnx20，记起始点距离环廊平面的距离为dx0＝(dnx10+dnx20)/2；编码器Epy和Eny的t时刻读数sny(t)和sny(t)均设为0；进行步骤5；
其中调整机器人方向方法取决于机器人选型及控制方法，不属于本发明保护范围；
情况二：起始段为水平行进
机器人保持静止10s以上后判断θ(t)：
若
则原地调整机器人行进方向，减小方向角，直至
方向调整完成后，机器人保持静止，直到Ly连续测得2组相同的激光测距数据，记录为起始数据dy0；编码器Epy和Eny的t时刻读数sny(t)和sny(t)均设为0，进行步骤5；
若
则保持机器人前进方向静止，直到Ly连续测得2组相同的激光测距数据，记录为起始数据dy0；编码器Epy和Eny的t时刻读数sny(t)和sny(t)均设为0，进行步骤5；
其中调整机器人方向方法取决于机器人选型及控制方法，不属于本发明保护范围
步骤5、保持机器人方向行进：目的是在机器人初始方向对准的基础上，控制其保持当前方向继续行进，直至下一转弯点；
步骤5中，根据起始段的两种情况，本步中保持机器人方向行进方法步骤分别如下：
情况一：起始段为垂直向上绕行闸门
i)控制机器人行进，期间调整左右电机速度，保持方向角测量θ(t)始终满足|θ(t)-π|<ε；
ii)判断当前时刻t是否可读取激光测距仪数据，若不可读取则返回步骤i)；若可读取则读取Lnx1和Lnx2的测距结果分别为dnx1(t)和dnx2(t)，跟据绕行闸门为人员闸门或设备闸门计算min(dnx1(t),dnx2(t))-dx0是否大于对应的闸门绕行距离Dh或De，若大于则抵达进行步骤6，否则返回步骤i)；
情况二：起始段为水平行进
A)控制机器人行进，期间调整左右电机速度，保持方向角测量θ(t)始终满足
B)判断当前时刻t是否可读取激光测距仪数据，若不可读取则返回步骤A)；若可读取则读Ly的测距结果为dy(t)；
C)判断dy(t)与初始高度dy0偏差是否满足|dy(t)-dy0|<εh，其中εh为水平保持高度偏差阈值εh取值满足εh∈(0，Wc)，本方法强烈建议将εh设置与激光测距仪的分辨率一致；若满足则继续，若不满足则调整左右电机速度，使得|dy(t)-dy0|<εh；
D)人员在环廊判断爬行机器人是否爬行至人员闸门或设备闸门绕行位置，若是则进行步骤6，否则返回A)
步骤6、机器人转向：主要目的是在转向点，控制机器人方向角
旋转，实现直行和闸门绕行的情况切换；
步骤6中，根据起始段的两种情况，将本步骤分为两种情况:
情况一：起始段为垂直向上绕行闸门
如图7所示：步骤5完成后，爬行机器人垂直向上爬行到闸门上方，机器人转向步骤如下：
(I)关闭激光测距仪Ly；停止步骤3中的方向角计算；
(II)仅利用惯性测量器件测量方向角，控制爬行机器人转向
将爬行机器人调整为水平行进方向；
(III)利用编码器测量值计算爬行距离，仅利用惯性测量器件测量方向角，在保证方向角满足
的情况下，根据所绕行闸门类型为人员闸门或设备闸门，分别行进距离Dh和De；
(IV)控制爬行机器人转向
将爬行机器人调整为垂直向下方向；
(V)开启Lpx1和Lpx2；
(VI)判断当前时刻t是否可读取激光测距仪数据，若可则读取t时刻Lpx1和Lpx2的测距结果分别为dpx1(t)和dpx2(t)，并根据下式计算t时刻由激光测距仪计算的方向角θPL(t)：
若不可则进行下一步；
(VII)判断当前时刻t是否可读取惯性测量器件，若可则读取t时刻惯性器件测得方向角θI(t)，若不可则进行下一步；
(VIII)根据步骤(VI)和步骤(VIII)的测量结果，采用卡尔曼滤波融合得到当前方向角θ(t)或利用下式直接优先采用激光测距仪计算的方向角，得到当前方向角θ(t)；
其中卡尔曼滤波为多传感器融合领域公共知识，不属于本专利保护范围；(IX)根据所绕行闸门类型为人员闸门或设备闸门，分别行进距离Dh和De；(X)控制爬行机器人转向
将爬行机器人调整为水平行进，后续可按步骤5情况二继续行进，直至爬行一周；
情况二：起始段为水平行进
步骤5完成后，爬行机器人水平爬行到闸门绕行点，机器人转向步骤如下：
控制爬行机器人转向
将爬行机器人调整为垂直向上，关闭Ly，转为闸门绕行工况；后续可按步骤3情况一测量方向角，并按步骤5情况一继续行进至闸门顶部，按步骤6情况一完成闸门绕行后，继续水平行进，直至爬行一周。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。
Claims (10)
1.一种核电站检测爬壁机器人的行进轨迹控制方法，用于核电站安全壳表面爬行作业时的行进方向和路线控制，为方便后续描述，定义机器人固连坐标系OrXrYrZr，其中坐标系原点Or位于爬行机器人形心，机器人直行前进方向为+Xr方向，垂直于机器人底盘平面向上方向为+Zr方向，+Yr轴方向根据右手螺旋法确定，考虑爬行机器人在安全壳表面行进过程中需要向上走“几”字形路线绕过人员闸门和设备闸门，爬行机器人的行进路线分为水平直行和闸门绕行两类，其特征在于，步骤如下：
步骤1、对传感器进行配置；
步骤2、约束测算：根据爬行机器人的相机成像参数和任务约束，测算出路径控制的约束以及传感器选型参数约束；
步骤3、开启方向角测量：将爬行机器人放置在安全壳表面后，根据实际任务路线需求，开启对应传感器，进行方向角测量；
步骤4、机器人方向角初对准：爬行机器人行进前对方向角进行调整，实现初始对准；
步骤5、保持机器人方向行进：在机器人初始方向对准的基础上，控制其保持当前方向继续行进，直至下一转弯点；
步骤6、机器人转向：在转向点，控制机器人方向角
旋转，实现直行和闸门绕行的情况切换。
2.根据权利要求1所述的一种核电站检测爬壁机器人的行进轨迹控制方法，其特征在于，所述步骤1中，配置方案如下：
配置激光测距仪：配置在机器人前段、机器人后端和机器人侧端，机器人前端的激光测距仪之间的安装距离为WLpx，靠近+Yr方向的激光测距仪为Lpx1，另一个激光测距仪为Lpx2，机器人后端的激光测距仪之间的安装距离为WLnx，靠近+Yr方向的激光测距仪为Lnx1，另一个激光测距仪为Lnx2，机器人侧端记为Ly，若Ly位于+Yr方向，则记侧端激光测距仪方向标识λ＝-1，反之则记为λ＝1；
配置惯性测量器件：单轴、双轴和三轴均可，需要能够测量爬行机器人+Xr轴方向的与重力方向绕+Zr之间的夹角，定义为方向角；
配置驱动电机编码器：+Yr侧电机和-Yr侧电机，分别记为Epy和Eny。
3.根据权利要求1所述的一种核电站检测爬壁机器人的行进轨迹控制方法，其特征在于，所述步骤2中，根据爬行机器人相机成像参数，安装位置和方向，测算爬行机器人相机有效工作视场宽度Wc；
根据核电站安全壳型号，确定钢制安全壳的周长Ls，爬行机器人沿上部环廊上方距离爬行时最小离地高度h；
爬行机器人路线控制的约束为：为保证爬行机器人在安全壳上爬行一周后拼接程闭合图像，需要保证爬行机器人水平爬行距离Ls后保证高度偏差小于Wc；
根据核电站安全壳设备闸门和人员闸门直径和位置，确定钢制安全壳设备闸门和人员闸门绕行距离De和Dh，记最大绕行距离D＝max(De,Dh)；
根据爬行机器人选型型号，确定爬行机器人工作情况下的爬行速度Vr；
根据下式计算爬行机器人闸门绕行时间T，D为爬行机器人闸门绕行距离，Vr为爬行机器人闸门绕行速度；
根据下式计算惯性测量器件在T时间内的测角飘移量的测角偏差上限；
为保证后续方法的有效性，分别计算惯性测量器件的选型参数约束：
角度精度pI<0.2elim，角度分辨率rI<0.2elim，考虑飘移情况，在3σ置信域内，T时间内无矫正角度测量偏差εT<elim；另外，更新时间间隔需小于1s；
为保证后续方法的有效性，分别计算激光测距仪的选型参数约束：
最大测量距离DL>D，最小测量距离dL<h，WLx＝min(WLpx,WLnx)，距离测量精度
距离测量分辨率
4.根据权利要求1所述的一种核电站检测爬壁机器人的行进轨迹控制方法，其特征在于，所述步骤3中，开启后持续为后续各步骤提供测量信息；
首先设置传感器采样间隔：设置惯性测量器件的采样间隔为ΔI，ΔI由惯性器件具体型号的更新间隔决定；设置激光测距仪的采样间隔为ΔL(ΔI≤ΔL≤3s)；
然后根据实际任务路线需求，确定起始段路线为垂直向上绕行闸门或水平行进，分下述两种情况开启方向角测量；
情况一：起始段为垂直向上绕行闸门
1)启用激光测距仪Lnx1和Lnx2，其余激光测距仪关闭，启用惯性测量器件，令当前时刻t＝0；
2)判断当前时刻t是否可读取激光测距仪数据，若可则读取t时刻Lnx1和Lnx2的测距结果分别为dnx1(t)和dnx2(t)，并根据下式计算t时刻由激光测距仪计算的方向角θNL(t)：
若不可则进行下一步；
3)判断当前时刻t是否可读取惯性测量器件，若可则读取t时刻惯性器件测得方向角θI(t)，若不可则进行下一步；
4)根据步骤2)和步骤3)的测量结果，采用卡尔曼滤波融合得到当前方向角θ(t)或利用下式直接优先采用激光测距仪计算的方向角，得到当前方向角θ(t)；
5)令t＝t+1，返回步骤2)；
情况二：起始段为水平行进
a)启用激光测距仪Ly，其余激光测距仪关闭，启用惯性测量器件，令当前时刻t＝0；
b)判断当前时刻t是否可读取激光测距仪数据，若可则读取t时刻Ly的测距结果分别为dy(t)；若不可则进行下一步；
c)判断当前时刻t是否可读取惯性测量器件，若可则读取t时刻惯性器件测得方向角θI(t)，若不可则进行下一步；
d)根据步骤b)和步骤c)的测量结果，经卡尔曼滤波后得到θ(t)；
e)令t＝t+1，返回步骤b)；
步骤3中，开启方向角测量后持续循环，用于后续步骤随时获得θ(t)，直至步骤6更新方向角计算方式。
5.根据权利要求1所述的一种核电站检测爬壁机器人的行进轨迹控制方法，其特征在于，所述步骤4中，设置方向对准偏差阈值ε，取值范围为ε∈(0elim)；根据步骤3中的情况区分，机器人方向角初始对准步骤分别如下
情况一：起始段为垂直向上绕行闸门
机器人保持静止10s以上后判断θ(t)：
若|θ(t)-π|>ε，则原地调整机器人行进方向，直至|θ(t)-π|≤ε；方向调整完成后，机器人保持静止，直到Lnx1为Lnx2均连续测得2组相同的激光测距数据，记录为起始数据dnx10和dnx20，记起始点距离环廊平面的距离为dx0＝(dnx10+dnx20)/2；编码器Epy和Eny的t时刻读数sny(t)和sny(t)均设为0；进行步骤5；
若|θ(t)-π|≤ε，则保持机器人前进方向静止，直到Lnx1为Lnx2均连续测得2组相同的激光测距数据，记录为起始数据dnx10和dnx20，记起始点距离环廊平面的距离为dx0＝(dnx10+dnx20)/2；编码器Epy和Eny的t时刻读数sny(t)和sny(t)均设为0；进行步骤5。
6.根据权利要求5所述的一种核电站检测爬壁机器人的行进轨迹控制方法，其特征在于，情况二：起始段为水平行进
机器人保持静止10s以上后判断θ(t)：
若
则原地调整机器人行进方向，减小方向角，直至
方向调整完成后，机器人保持静止，直到Ly连续测得2组相同的激光测距数据，记录为起始数据dy0；编码器Epy和Eny的t时刻读数sny(t)和sny(t)均设为0，进行步骤5；
若
则保持机器人前进方向静止，直到Ly连续测得2组相同的激光测距数据，记录为起始数据dy0；编码器Epy和Eny的t时刻读数sny(t)和sny(t)均设为0，进行步骤5。
7.根据权利要求1所述的一种核电站检测爬壁机器人的行进轨迹控制方法，其特征在于，所述步骤5中，根据起始段的两种情况，本步中保持机器人方向行进方法步骤分别如下：
情况一：起始段为垂直向上绕行闸门
i)控制机器人行进，期间调整左右电机速度，保持方向角测量θ(t)始终满足|θ(t)-π|<ε；
ii)判断当前时刻t是否可读取激光测距仪数据，若不可读取则返回步骤i)；若可读取则读取Lnx1和Lnx2的测距结果分别为dnx1(t)和dnx2(t)，跟据绕行闸门为人员闸门或设备闸门计算min(dnx1(t),dnx2(t))-dx0是否大于对应的闸门绕行距离Dh或De，若大于则抵达进行步骤6，否则返回步骤i)。
8.根据权利要求7所述的一种核电站检测爬壁机器人的行进轨迹控制方法，其特征在于，情况二：起始段为水平行进
A)控制机器人行进，期间调整左右电机速度，保持方向角测量θ(t)始终满足
B)判断当前时刻t是否可读取激光测距仪数据，若不可读取则返回步骤A)；若可读取则读Ly的测距结果为dy(t)；
C)判断dy(t)与初始高度dy0偏差是否满足|dy(t)-dy0|<εh，其中εh为水平保持高度偏差阈值εh取值满足εh∈(0，Wc)，将εh设置与激光测距仪的分辨率一致；若满足则继续，若不满足则调整左右电机速度，使得|dy(t)-dy0|<εh；
D)人员在环廊判断爬行机器人是否爬行至人员闸门或设备闸门绕行位置，若是则进行步骤6，否则返回A)。
9.根据权利要求1所述的一种核电站检测爬壁机器人的行进轨迹控制方法，其特征在于，所述步骤6中，根据起始段的两种情况，将本步骤分为两种情况:
情况一：起始段为垂直向上绕行闸门
步骤5完成后，爬行机器人垂直向上爬行到闸门上方，机器人转向步骤如下：
(I)关闭激光测距仪Ly；停止步骤3中的方向角计算；
(II)仅利用惯性测量器件测量方向角，控制爬行机器人转向
将爬行机器人调整为水平行进方向；
(III)利用编码器测量值计算爬行距离，仅利用惯性测量器件测量方向角，在保证方向角满足
的情况下，根据所绕行闸门类型为人员闸门或设备闸门，分别行进距离Dh和De；
(IV)控制爬行机器人转向
将爬行机器人调整为垂直向下方向；
(V)开启Lpx1和Lpx2；
(VI)判断当前时刻t是否可读取激光测距仪数据，若可则读取t时刻Lpx1和Lpx2的测距结果分别为dpx1(t)和dpx2(t)，并根据下式计算t时刻由激光测距仪计算的方向角θPL(t)：
若不可则进行下一步；
(VII)判断当前时刻t是否可读取惯性测量器件，若可则读取t时刻惯性器件测得方向角θI(t)，若不可则进行下一步；
(VIII)根据步骤(VI)和步骤(VIII)的测量结果，采用卡尔曼滤波融合得到当前方向角θ(t)或利用下式直接优先采用激光测距仪计算的方向角，得到当前方向角θ(t)；
(IX)根据所绕行闸门类型为人员闸门或设备闸门，分别行进距离Dh和De；
(X)控制爬行机器人转向
将爬行机器人调整为水平行进，后续可按步骤5情况二继续行进，直至爬行一周。
10.根据权利要求9所述的一种核电站检测爬壁机器人的行进轨迹控制方法，其特征在于，情况二：起始段为水平行进
步骤5完成后，爬行机器人水平爬行到闸门绕行点，机器人转向步骤如下：
控制爬行机器人转向
将爬行机器人调整为垂直向上，关闭Ly，转为闸门绕行工况；后续可按步骤3情况一测量方向角，并按步骤5情况一继续行进至闸门顶部，按步骤6情况一完成闸门绕行后，继续水平行进，直至爬行一周。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210204465.3A CN114578820B (zh) | 2022-03-02 | 2022-03-02 | 一种核电站检测爬壁机器人的行进轨迹控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210204465.3A CN114578820B (zh) | 2022-03-02 | 2022-03-02 | 一种核电站检测爬壁机器人的行进轨迹控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114578820A true CN114578820A (zh) | 2022-06-03 |
| CN114578820B CN114578820B (zh) | 2024-05-07 |
Family
ID=81775601
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210204465.3A Active CN114578820B (zh) | 2022-03-02 | 2022-03-02 | 一种核电站检测爬壁机器人的行进轨迹控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114578820B (zh) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN204772513U (zh) * | 2015-05-18 | 2015-11-18 | 浙江理工大学 | 一种船舶立面作业多功能爬行机器人 |
| CN111090282A (zh) * | 2019-12-19 | 2020-05-01 | 安克创新科技股份有限公司 | 机器人的避障方法、机器人以及装置 |
| CN112045679A (zh) * | 2020-08-21 | 2020-12-08 | 上海核工程研究设计院有限公司 | 一种爬壁机器人防倾覆行进控制方法 |
| CN113448325A (zh) * | 2020-03-24 | 2021-09-28 | 青岛塔波尔机器人技术股份有限公司 | 一种扫地机器人控制方法及扫地机器人 |
| CN113804184A (zh) * | 2020-06-15 | 2021-12-17 | 上海知步邦智能科技有限公司 | 基于多传感器的地面机器人定位方法 |
| CN114019986A (zh) * | 2022-01-04 | 2022-02-08 | 北京史河科技有限公司 | 一种爬壁机器人的路径规划方法 |
-
2022
- 2022-03-02 CN CN202210204465.3A patent/CN114578820B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN204772513U (zh) * | 2015-05-18 | 2015-11-18 | 浙江理工大学 | 一种船舶立面作业多功能爬行机器人 |
| CN111090282A (zh) * | 2019-12-19 | 2020-05-01 | 安克创新科技股份有限公司 | 机器人的避障方法、机器人以及装置 |
| CN113448325A (zh) * | 2020-03-24 | 2021-09-28 | 青岛塔波尔机器人技术股份有限公司 | 一种扫地机器人控制方法及扫地机器人 |
| CN113804184A (zh) * | 2020-06-15 | 2021-12-17 | 上海知步邦智能科技有限公司 | 基于多传感器的地面机器人定位方法 |
| CN112045679A (zh) * | 2020-08-21 | 2020-12-08 | 上海核工程研究设计院有限公司 | 一种爬壁机器人防倾覆行进控制方法 |
| CN114019986A (zh) * | 2022-01-04 | 2022-02-08 | 北京史河科技有限公司 | 一种爬壁机器人的路径规划方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114578820B (zh) | 2024-05-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104699104A (zh) | 一种自适应agv视觉导航视线调整装置与线迹跟踪方法 | |
| CN106200647A (zh) | 自动导航系统和自动导航方法 | |
| CN109900273B (zh) | 一种室外移动机器人导引方法及导引系统 | |
| BRPI1015268B1 (pt) | Método para derramamento automático de metal fundido e sistema de controle de inclinação que utiliza o método | |
| CN104120656B (zh) | 桥梁高墩施工垂直度自动控制方法 | |
| CN110307834B (zh) | 基于低精度gps、激光测距传感器与方位传感器信息融合的蟹塘自动作业船组合导航方法 | |
| CN106946049A (zh) | 集装箱码头流动设备自动化行走方法 | |
| CN207407864U (zh) | 一种自动测斜仪装置 | |
| CN114578820A (zh) | 一种核电站检测爬壁机器人的行进轨迹控制方法 | |
| CN106442014A (zh) | 水质取样装置 | |
| JP5984491B2 (ja) | ケーソン誘導管理システム及びこれを利用したケーソンの誘導管理方法 | |
| CN206258288U (zh) | 水质取样装置 | |
| CN112045679A (zh) | 一种爬壁机器人防倾覆行进控制方法 | |
| JP2015148496A (ja) | 施工調査装置および施工調査方法 | |
| CN104822026A (zh) | 用于核电站安全壳缺陷检测的相机姿态控制方法及装置 | |
| CN113064430A (zh) | 一种基于Android手机的质量巡检小车避障及路径规划算法 | |
| JPH02154148A (ja) | 構造物の検査装置 | |
| CN204480047U (zh) | 一种自适应agv视觉导航视线调整装置 | |
| JP2017204043A (ja) | 自律移動システムおよび自律移動方法 | |
| CN113358094B (zh) | 一种房屋建筑承重墙垂直检测方法 | |
| CN111024313A (zh) | 一种主动搜索定位化学气体泄漏点的方法和设备 | |
| CN111055647B (zh) | 一种可在水泥回转窑变径弧面行走小车的控制系统 | |
| CN110864663B (zh) | 一种基于无人机技术的房屋面积测量方法 | |
| CN106629394B (zh) | 应用于轨道吊吊具位姿检测的相机外参数标定系统及方法 | |
| JP2011018150A (ja) | 無人走行システム |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| CB02 | Change of applicant information |
Address after: No. 29 Hong Cao Road, Xuhui District, Shanghai Applicant after: Shanghai Nuclear Engineering Research and Design Institute Co.,Ltd. Address before: No. 29 Hong Cao Road, Xuhui District, Shanghai Applicant before: SHANGHAI NUCLEAR ENGINEERING RESEARCH & DESIGN INSTITUTE Co.,Ltd. |
|
| CB02 | Change of applicant information | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |