[发明专利]一种潜油作业机器人线缆收放控制方法

权利要求书

1.一种潜油作业机器人线缆收放控制方法，包括放置在原油储罐(4)内的机器人(6)以及位于原油储罐(4)外部的线缆收放装置(1)，机器人(6)和线缆收放装置(1)通过线缆(5)连接，其特征在于：包括如下步骤：

步骤a，在原油储罐(4)的人孔处设置防缠绕管(3)，所述线缆(5)从防缠绕管(3)中穿过；定义机器人(6)在原油储罐(4)内的原点为机器人从原油储罐(4)人孔的下放位置，并进行系统初始化；

步骤b，定义迭代时间，控制器判断是否达到迭代时间，如果达到迭代时间，执行步骤1003，否则控制器继续进行迭代时间的判断；

步骤c，达到迭代时间后，控制器修正卷线辊筒线缆半径的时变函数l(θ)以及线缆长度误差的修正值e；

步骤d，根据步骤c中的时变函数l(θ)和修正值e，计算得到线缆收放装置(1)的补偿值c；

步骤e，在线缆(5)与机器人(6)的连接处设置拉力传感器，拉力传感器的信号输出端与控制器相连，控制器判断力传感器获取的受力F是否达到设置的线缆安全连接极限力Ft，若达到，则执行步骤f，若未达到，返回步骤b；

步骤f，控制器判断机器人(6)相邻两个迭代时间的位置半径r是否大于函数k(L)，如果大于k(L)，执行步骤h，否则执行步骤g，k(L)是关于机器人(6)长度的单值函数；

步骤g，控制器判断机器人(6)相邻两个迭代时间的位置半径r是否小于函数-k(L)，如果小于-k(L)，执行步骤i，否则返回步骤b；

步骤h，控制器确定线缆收放装置(1)执行对线缆(5)的收线操作，使线缆(5)达到目标长度；

步骤i，控制器确定线缆收放装置(1)执行对线缆(5)的放线操作，使线缆(5)达到目标长度。

2.根据权利要求1所述的潜油作业机器人线缆收放控制方法，其特征在于：在步骤h、步骤i中，所述的目标长度为：X+c，其中c为线缆收放装置(1)的补偿值，X为线缆收放装置(1)输出的线缆(5)的计算值。

3.根据权利要求1所述的潜油作业机器人线缆收放控制方法，其特征在于：所述的步骤c，包括如下步骤：

已知线缆总长度X是关于l(θ)与e的一个函数，因此另||X-X'||₂＝f(l(θ),e)。

假设处于某一时刻T＝k*0.5(k＝0，1，2，…，n)时，此时e视为一个常值，采用梯度下降法进行l(θ)的修正为：

此时将上式修正后的l(θ)视为一个常值，采用梯度下降法进行e的修正为：

4.根据权利要求3所述的潜油作业机器人线缆收放控制方法，其特征在于：在步骤d中，补偿值c为：c＝|X-X’|

其中，X为通过定位装置计算的线缆收放装置(1)输出的线缆(5)的计算值，X’为通过l(θ)计算的线缆收放装置放线总长度：X’＝l(θ)·θ·r_卷

θ为线缆收放装置(1)中卷线辊筒(15)的转角，r_卷为卷线辊筒(15)的半径。

5.根据权利要求3或4所述的潜油作业机器人线缆收放控制方法，其特征在于：线缆收放装置(1)输出的线缆(5)的计算值X为：

其中，h₁为防缠绕管(3)底部与原油储罐(4)顶面之间的距离，h₃为机器人(6)的线缆连接处与防缠绕管(3)底部之间的竖直距离，r为位置半径，l为线缆收放装置(1)的输出端与人孔之间的长度，e为线缆长度误差的修正值。