一种三足支撑式管道机器人的运动定位控制方法与流程

1.本发明属于管道机器人技术领域，具体涉及一种三足支撑式管道机器人的运动定位控制方法。


背景技术：

2.由于管道所处环境的复杂性和工业需要，管道是多种形式并存的，如具有一定斜坡的管道、变径管道、圆弧管道、t型管道等，管道机器人要在这种特殊且空间有限的环境中作业的前提条件是在这些特殊类型的管道中的通过性。
3.目前在管道机器人的设计中，由于对弯道和管内障碍等限制性因素的考虑不足，机器人弯管转弯能力和越障能力较差，管内属性识别主要局限于典型水平的水平管道和垂直管道，且其控制策略均需要人员制定或者人员辅助，才能避免管道识别误判，同时三足支撑式管道机器人的动力学模型非常复杂，而且具有强耦合性，难以进行线性化，且管内摩擦力和滑动系数的估计不准确，导致控制器的性能恶化，产生很大的累积误差，机器人常常会出现跑偏现象，严重时候有可能会卡死在管内而无法收回。


技术实现要素：

4.本发明的目的在于提供一种三足支撑式管道机器人的运动定位控制方法，以解决上述背景技术中提出的问题。
5.为实现上述目的，本发明提供如下技术方案：一种三足支撑式管道机器人的运动定位控制方法，包括以下步骤：
6.步骤一：系统启动；
7.步骤二：获取姿态信息；
8.步骤三：建立静态姿态模型；
9.步骤四：获取测距信息；
10.步骤五：切换直/弯管控制方式；
11.步骤六：电机运行；
12.步骤七：电流pid模块运行；
13.步骤八：分段控制模式；
14.步骤九：系统停止。
15.作为本发明的一种优选的技术方案，在所述步骤二中建立管道、机器人坐标系；以管道轴心线为x轴，指向机器人运动的方向；以管道主平面与管道主截面的交线为y轴的右手坐标系xyzo，为静坐标系，原点o在管道中心轴线上，z轴由右手规则确定。
16.作为本发明的一种优选的技术方案，在所述步骤三中，定义摆动角α为管道机器人前进方向与xoz平面夹角；定义俯仰角β为管道机器人前进方向与xoy平面夹角；定义姿态角γ为管道机器人一足与z轴夹角，由于三足支撑式管道机器人结构上每120度为一个重复，因此只需选取三足中与z轴最小者即可，取逆时针为正，顺时针为负；根据摆动角α、俯仰角β
和姿态角γ建立出管道机器热静态姿态模型。
17.作为本发明的一种优选的技术方案，在所述步骤四中，根据测距传感器信息，判断机器人前方是否有弯道。
18.作为本发明的一种优选的技术方案，在所述步骤五中，根据步骤四的结果切换直管、弯管控制程序。
19.作为本发明的一种优选的技术方案，在所述步骤七中，pid根据驱动器反馈的电流实时调节电机速度。
20.作为本发明的一种优选的技术方案，在所述步骤八中，分段控制模式针对姿态信息而控制。
21.与现有技术相比，本发明的有益效果是：
22.本发明通过外部感知模块、定位模块和运动模块的信息反馈，经过处理后形成机器人的姿态信息，并由控制器决策采用最优控制策略，使得管道机器人具有一定的自主识别能力，减轻操作人员的负担和误操作风险，根据运动路径随时对机器人姿态进行调整，使其能够在管道内自主过弯以及在直管、弯管和障碍中的运行。
附图说明
23.图1为本发明中的双闭环控制框图；
24.图2为本发明中的控制流程框图；
25.图3为本发明中的管道、机器人坐标示意图；
26.图4为本发明中的机器人关内姿态角示意图。
具体实施方式
27.下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
28.请参照图1至图4，本发明提供一种技术方案：一种三足支撑式管道机器人的运动定位控制方法，包括以下步骤：
29.步骤一：系统启动；
30.步骤二：获取姿态信息；
31.步骤三：建立静态姿态模型；
32.步骤四：获取测距信息；
33.步骤五：切换直/弯管控制方式；
34.步骤六：电机运行；
35.步骤七：电流pid模块运行；
36.步骤八：分段控制模式；
37.步骤九：系统停止。
38.本实施例中，在所述步骤二中建立管道、机器人坐标系；以管道轴心线为x轴，指向机器人运动的方向；以管道主平面与管道主截面的交线为y轴的右手坐标系xyzo，为静坐标
系，原点o在管道中心轴线上，z轴由右手规则确定。
39.本实施例中，在所述步骤三中，定义摆动角α为管道机器人前进方向与xoz平面夹角；定义俯仰角β为管道机器人前进方向与xoy平面夹角；定义姿态角γ为管道机器人一足与z轴夹角，具体参照图4，由于三足支撑式管道机器人结构上每120度为一个重复，因此只需选取三足中与z轴最小者即可，取逆时针为正，顺时针为负；根据摆动角α、俯仰角β和姿态角γ建立出管道机器热静态姿态模型。
40.本实施例中，在所述步骤四中，根据测距传感器信息，判断机器人前方是否有弯道。
41.本实施例中，在所述步骤五中，根据步骤四的结果切换直管、弯管控制程序；
42.当切换到直管控制程序后，三个电机等速运行，即基准速度，具体公式为v
a
:v
b
:v
c
＝1:1:1。
43.当切换到弯管控制程序后，三个电机进行运送速度协调控制，只需控制3足驱动电机速度满足运动速度协调模型即可，具体公式为v
a
:v
b
:v
c
＝r cos(υ
‑
120
°
):r
‑
0.5dcos(υ 120
°
):r
‑
0.5dcosυ；其中d为管道直径，γ为姿态角，r为弯道中心曲率半径。
44.本实施例中，在所述步骤七中，pid根据驱动器反馈的电流实时调节电机速度，又分为以下两种控制模式：
45.①
、当实际电流与基准电流比较接近时，引入积分环节，采用pid控制，发挥积分作用，消除静态误差，保证了控制的精度又避免了震荡的产生；
46.②
、当实际电流与基准电流偏差较大时，去掉积分，采用pi控制，以避免积分饱和效应的产生，产生较大的超调量。
47.本实施例中，在所述步骤八中，分段控制模式针对姿态信息而控制，分为以下三种模式：
[0048]ⅰ、当摆动角α≤
‑
30
°
或α≥30
°
时，电机慢速左右转；当俯仰角β≤
‑
30
°
或β≥30
°
时，电机慢速上下转，调用慢速调整pid参数；
[0049]ⅱ、当摆动角
‑
30
°
≤α≤
‑
15
°
或15
°
≤α≤30
°
时，电机中速左右转；当俯仰角
‑
30
°
≤β≤
‑
15
°
或15
°
≤β≤30
°
时，电机中速上下转，调用中速调整pid参数；
[0050]ⅲ、当摆动角
‑
15
°
≤α≤15
°
时，电机高速左右转；当俯仰角
‑
15
°
≤β≤15
°
时，电机高速上下转，调用快速调整pid参数。
[0051]
管道机器人运动定位控制方法，最终是对电机驱动系统的控制，为了实现电机快速、准确的控制要求，控制系统采用的是一个姿态、电流负反馈双闭环系统，提供有电流环(内环)控制和姿态环(外环)控制，如图1所示，电流环由驱动器内部参数设置完成，作用是提高系统响应速度，姿态环由plc控制算法完成。
[0052]
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。