用于冷源隧道清理的大型多自由度机械臂轨迹规划方法

技术领域

本发明属于工业机器人领域，具体涉及用于冷源隧道清理的大型多自由度机械臂轨迹规划方法。

背景技术

核电机组运行时的最终热阱是海水冷源，负责带走停堆时的堆芯预热、冷却各种核安全设备，在核电运行中具有重要作用，取水口运行状态的好坏直接影响电厂的安全运行性和可靠性。机组运行时产生的热量会导致海生物在冷源隧道附着，目前主要通过机组运行时喷氯、隧道壁喷防护涂层和大修时人工或机械清理三种方式减少隧道中海生物的附着，但隧道壁喷防护涂层对环境存在影响，单靠机组运行时喷氯并不能完全阻止海生物的生长，大修时人工或机械清理成为维持海水冷源隧道清洁的必要方法。人工清理冷源隧道存在密闭空间作业、高处作业等风险，且单人清理效率较低；现有的机械清理把隧道截面当作一个标准的圆来处理，但由于施工误差，隧道截面并不是标准圆，且搭载机械臂的底盘存在定位误差，会导致机械臂作业截面变成椭圆，上述原因会导致现有机械清理效果不佳，还有可能损坏隧道壁面。

发明内容

本发明的目的在于提供一种用于冷源隧道清理的大型多自由度机械臂轨迹规划方法，能够有效地解决核电厂冷源隧道壁面清理过程中的机械臂轨迹规划和控制问题，充分考虑平底和圆底隧道、隧道壁面的不规则性、机械臂载体定位信息和作业安全性，达到全自主清理隧道壁面的目的。

本发明的技术方案如下：用于冷源隧道清理的大型多自由度机械臂轨迹规划方法，包括如下步骤：

步骤1：确认隧道固有属性，车体位置信息，机械臂位置信息，被控量和运动目标；

步骤2：轨迹规划；

步骤3：使用激光测距实时测量G点到壁面的实际距离，或通过压力传感器测量得到实际压力，以此作为伸缩缸的控制项，控制伸缩臂的伸缩；

步骤4：通过规划轨迹限制传感器壁面适配，同时避免完全的传感器壁面适配由于传感器测量误差而失控。

所述的隧道固有属性包括隧道直径R、O点为隧道中心点、平底到隧道中心的高度H。

所述的车体位置信息包括：O

所述的机械臂位置信息包括：当前机械臂相位角θ

所述的被控量ΔO

所述的运行目标保持G点与内壁或平底保持接触。

所述的步骤2包括如下：

步骤21：首先计算刀头在隧道坐标系下的坐标点，D点坐标表达式

步骤22：根据底盘的偏航角度，内壁椭圆曲线极坐标表达式如下

步骤23：由实际机械臂离地面的高度H，可得平底直线表达式

y＝-H

步骤24：将G点表达式分别带入曲线和直线表达式中，得O

所述的步骤3包括如下：

步骤31：通过传感器测量得到G点到壁面的实际距离或者实际压力，对所测数据进行低通滤波，减少数据的震荡；

步骤32：滤波之后的数据结合距离或者压力的标定数据，以恒压力或者恒距离为控制目标，计算实时O

步骤33：通过调整预设距离或者压力的大小，实现刀头对隧道壁面不同的压力。

所述的步骤4包括如下：

步骤41：分别通过理论轨迹规划和传感器壁面适配获取对应的伸缩臂O

步骤42：计算理论轨迹和实际适配轨迹之间的差值，与安全阈值进行对比，当差值小于阈值时，以实际适配轨迹运行，若差值大于阈值，则机械臂按照理论轨迹行走。

本发明的有益效果在于：理论轨迹规划考虑载体朝向偏差，转化机械臂作业截面为椭圆，在此基础上融合测量得到的机械臂中心与隧道截面中心之间的偏置信息，使机械臂的行走路径与实际情况更加符合；实际壁面适配能够根据激光测量实际壁面的起伏做出实时轨迹规划，通过设置一定的预压缩量(或预压力)可以保证清理效果的同时保护隧道壁面；安全冗余设计能够通过判断理论轨迹和实际适配轨迹之间的差值，判断作业的安全性，当传感器测量数据不稳定时，依据轨迹规划运行，保证自主作业安全。

附图说明

图1为圆底平底隧道截面轨迹规划示意图；

图2为本发明所提供的用于冷源隧道清理的大型多自由度机械臂轨迹规划方法控制流程图。

具体实施方式

下面结合附图及具体实施例对本发明作进一步详细说明。

本发明所提供的用于冷源隧道清理的大型多自由度机械臂轨迹规划方法兼顾隧道壁面的清理效率和清理效果，能够针对冷源圆底和平底隧道全自主规划轨迹并执行清理作业。全自主的轨迹规划主要包括理论轨迹计算、实际壁面适配和安全冗余设计三个步骤。理论轨迹计算综合隧道直径信息、机械臂载体定位信息和机械臂旋转中心偏置信息，规划出相对于理想圆形隧道时在机械臂坐标系下机械臂清理刀头的运动坐标轨迹，根据机械臂固有尺寸，可计算出机械臂伸缩臂的伸长量以及翻折臂的翻折量；实际壁面适配主要以理论轨迹计算为基础，通过激光测距信息(或者拉压力测量信息)计算机械臂伸长量和翻折量，反馈控制机械臂以一定的压缩量贴近实际隧道壁，保证清理效果的同时保护隧道壁面；最后，在理论轨迹和实际壁面适配轨迹之间设置阈值，当两个轨迹差别大于阈值后，依据轨迹规划运行，保证自主作业安全。

本发明中的机械臂有两个对称的作业臂，主要包括三个自由度，第一个自由度为主旋转和翻折臂旋转，第二个自由度为作业壁伸缩缸伸缩，第三个自由度由载体前进后退实现。

首先针对机械臂在理论隧道中的刀头轨迹进行规划，通过底盘上搭载的测量设备可以得到机械臂作业截面的法线与隧道横截面法线之间的角度偏差，也可以得到机械臂旋转中心相对于隧道横截面的中心点之间的偏置，分别以隧道横截面中心和机械臂旋转中心为坐标原点，建立世界坐标系和机械臂坐标系；由于机械臂既存在伸缩运动，又存在旋转运动，在假定伸长量和翻折量的前提下，根据主旋转马达的旋转角度和偏置信息，可以计算得到机械臂刀头位置在世界坐标系中的位置，把该坐标带入椭圆方程中(值得注意的是，在平底隧道，当现在伸缩量和翻折量到头的位置的Y值小于直线的Y值时，则通过直线方程求解伸缩量和翻折量)，就可以得到关于伸长量和翻折量的方程，这个方程有两种那个求解方法，第一种求解方法时固定一个变量，求另外一个变量，另一种方法则是通过给伸长量和翻折量分配不同的权重，同时求解两个值；在得到两个控制值之后，就可以按照求解量对机械臂进行循迹控制。

但实际情况中，隧道壁面由于施工误差，会存在起伏，从而不符合理想状态的圆或者椭圆，若单纯按照理论轨迹规划的结果运行，会导致刀头与壁面的压缩量不一致，影响清理效果，而且有可能刮坏隧道壁面。基于上述原因，在机械臂刀头部位添加一个测距(或者测力)传感器，实时测量刀头与隧道壁面的距离(或压力)，通过控制伸缩量保持一定的压缩量(或压力)保证刀头一直与隧道壁面接触，可以达到保证清理效果的同时保护隧道壁面的目的。

基于上述两种方法的互补性，需要对上述两种轨迹规划方式进行综合，以达到安全作业的目的。具体做法是以理论轨迹规划为基础，与通过传感器反馈计算得到的伸缩量进行对比，设置安全阈值为机械臂刀头可压缩量的3/5，也就是±180mm，当理论轨迹规划得到的伸缩量和通过传感器反馈计算得到的伸缩量偏差大雨或着小雨阈值时，机械臂按照轨迹计算结果运行，保证机械臂作业安全。

用于冷源隧道清理的大型多自由度机械臂轨迹规划方法，包括如下步骤：

步骤1：确认隧道固有属性，车体位置信息，机械臂位置信息，被控量和运动目标；

隧道固有属性包括：隧道直径R、O点为隧道中心点、平底到隧道中心的高度H。

车体位置信息包括：O

机械臂位置信息包括：当前机械臂相位角θ

被控量：ΔO

运行目标：保持G点与内壁或平底保持接触。

步骤2：轨迹规划；

具体过程如下：

步骤21：首先计算刀头在隧道坐标系下的坐标点，D点坐标表达式

步骤22：根据底盘的偏航角度，内壁椭圆曲线极坐标表达式如下

步骤23：由实际机械臂离地面的高度H，可得平底直线表达式

y＝-H

步骤24：将G点表达式分别带入曲线和直线表达式中，可以得O

步骤3：使用激光测距实时测量G点到壁面的实际距离，或通过压力传感器测量得到实际压力，以此作为伸缩缸的控制项，控制伸缩臂的伸缩，具体步骤如下：

步骤31：通过传感器测量得到G点到壁面的实际距离或者实际压力，对所测数据进行低通滤波，减少数据的震荡；

步骤32：滤波之后的数据结合距离或者压力的标定数据，以恒压力或者恒距离为控制目标，计算实时O

步骤33：通过调整预设距离或者压力的大小，可以实现刀头对隧道壁面不同的压力，从而获得不同的清理效果。

步骤4：通过规划轨迹限制传感器壁面适配，同时避免完全的传感器壁面适配由于传感器测量误差而失控；

具体实现步骤如下：

步骤41：分别通过理论轨迹规划和传感器壁面适配获取对应的伸缩臂O

步骤42：计算理论轨迹和实际适配轨迹之间的差值，与安全阈值进行对比(设置安全阈值为机械臂刀头可压缩量的3/5，也就是±180mm)，当差值小于阈值时，以实际适配轨迹运行，以保证实时适配壁面，获得更好的清理效果，若差值大于阈值，则机械臂按照理论轨迹行走，保证机械臂运行的安全性。