CN113253745A - 一种四足管道机器人周向运动位姿规划方法 - Google Patents

Abstract

本发明公开了一种四足管道机器人周向运动位姿规划方法，属于运动学的研究领域，包括以下步骤：S1、建立管道坐标系和四足机器人本体坐标系，确立机器人起始姿态，同时建立机器人的位姿方程；S2、调整足姿、机身旋转和机身平移的周向运动，对运动过程中位姿的角度、方向、位移进行规划；S3、通过足端运动轨迹实现周向运动位姿的规划，获得周向运动的各步骤的关节角度运动曲线。为保证周向运动的连续性，对运动过程中位姿的角度、方向、位移进行规划；并通过足端运动轨迹实现周向运动位姿的规划；利用MATLAB计算得到的周向运动的各个步骤的关节角度运动曲线。

Description

一种四足管道机器人周向运动位姿规划方法

技术领域

本申请涉及四足机器人运动控制领域，特别是涉及一种四足管道机器人周向运动位姿规划方法。

背景技术

近年来，随着社会不断进步和科学技术的不断发展，移动机器人广泛应用于军事工业、抢险救灾等危险性高、劳动强度大领域，代替人类完成特种环境下的工作。对机器人的功能要求也不断提高，智能机器人的研究是近年来研究的重要领域。

管道广泛应用在生产生活中，比如石油运输、水资源调动、天然气输送和GIS高压开关等方面。然而使用过程中管道会出现不同的故障，需要定期进行检查和维修。并且管道狭小，人工不易到达，并且传统的人工检修效率较低，因此需要研究特定的管道机器人代替人工在管道内进行工作。

CN107065867A中设计了一种面向崎岖地形的四足机器人，可以适应不同表面的地形，但是缺少在管道中运动的规划，也缺少对周向运动位姿的规划，

而管道机器人主要以轮式为主，运动控制较为简单，但运动空间有限，不能沿着管道周向进行运动。四足机器人可以沿着周向运动，但是现有的文献都没有给出具体的位姿控制方法。

为了解决上述论文中存在的不足，本文研究了一种四足机器人沿着管道内壁运动的周向运动的位姿分析方法。

发明内容

本发明的目的是提供一种四足管道机器人周向运动位姿规划方法，对四足管道机器人周向运动位姿进行规划。

本发明至少通过如下技术方案之一实现。

一种四足管道机器人周向运动位姿规划方法，包括以下步骤：

S1、建立管道坐标系和四足机器人本体坐标系，确立机器人起始姿态，同时建立机器人的位姿方程；

S2、调整足姿、机身旋转和机身平移的周向运动，对运动过程中位姿的角度、方向、位移进行规划；

S3、通过足端运动轨迹实现周向运动位姿的规划，获得周向运动的各步骤的关节角度运动曲线。

优选的，所述四足机器人包括机身和与机身连接的四条腿足机构，每条腿足机构包括髋关节、腿关节和踝关节串联，关节和关节、关节和机身之间通过连接杆连接，足部末端连接真空吸盘；所述机身腰部携带真空泵，所述机身上安装真空传感器。

优选的，所述四足机器人的每条腿足均具有六个自由度，分别是：髋关节自由度、腿关节自由度、踝关节自由度和球关节的三个自由度。

优选的，所述管道坐标系Σ_O，是以管道中心为原点，以管道轴向方向为x轴，以重力的反方向为z轴；

所述机器人本体坐标系Σ_E，是以机器人腰部中心为原点，前进方向为x轴，侧向运动方向为y轴。

优选的，机器人运动的起始位姿为：机器人髋关节角度不为零，足端垂直于管道壁面；管道坐标系和机器人本体坐标系的三轴方向互相平行，机器人本体坐标系原点位于管道坐标系原点的正下方。

优选的，步骤S2的周向运动包括以下步骤：

(1)足姿调整：机器人四足分别在管道内壁运动相同的步长到达目标足端位置，对足姿调整进行足端圆弧轨迹规划，在机身本体坐标系的轨迹方程为：

式中：

式中:0＜α＜π，α表示运动圆弧轨迹实时弧度；r_步表示运动圆弧轨迹半径； (x_p，y_p，z_p)表示足端起始位置坐标；α₀表示圆弧路径直径和y_E轴的夹角，L_F表示步长；R表示管道内壁半径；H表示机身坐标系原点到管道中心的距离；

(2)机身旋转：以腰部中心为旋转中心，机身旋转使得机身滚转角达到目标角度；

(3)机身平移：机身沿着某一方向平移一定距离使得机器人中心到达目标中心，最后使得机器人完成一个周向运动周期后关节角度和运动前相同。

优选的，对足姿调整目标位置进行足端轨迹规划，而对机身旋转机身平移进行等效的足端运动，具体如下：

机身旋转：足端固定时机身绕机身中心逆时针旋转等效为机身固定时足端以机身中心为旋转中心顺时针旋转相同的角度，对机身旋转进行等效的足端运动，在机身本体坐标系的轨迹方程为：

式中:β表示机身旋转实时角度，0＜β＜2arcsin(r_步/R)；(x_p，y₁，z₁)表示机器人机身旋转的起始足端坐标；β₀表示机身旋转开始时足端和机身中心连线和y_E轴的夹角；

机身平移：足端固定下机身沿着某一方向运动等效为机身固定时足端以反方向移动相同的距离。

优选的，根据足端运动轨迹，求解出在机身本体坐标系下的轨迹方程。

优选的，建立足部坐标系，进行单腿运动学求解：

原点在髋关节轴心、坐标轴方向与坐标系Σ_E方向相同建立原点坐标系Σ₁；原点在腿关节轴心、轴向方向为z轴、第一连杆方向为x轴建立第一坐标系Σ₂；原点在踝关节轴心、轴向为z轴、第二连杆方向为y轴建立第二坐标系Σ₃；以第三连杆和第四连杆交点为原点、第三连杆方向为x轴、第四连杆方向为z轴建立第三坐标系Σ₄；以足端为原点、三轴方向与第三坐标系Σ₄相同建立第四坐标系Σ₅；

根据坐标系之间的变换，求得足端在机身本体坐标系下的位置和三个关节转角的关系，得到正逆运动学解，得到第四坐标系Σ₅和坐标系Σ_E之间的坐标变换关系为：

式中：a、b分别表示机身长和宽的一半；L1、L2、L3、L4表示髋关节连杆、腿关节连杆、踝关节连杆、足端连杆的长度；

因此足端在机身本体坐标系Σ_E中的位置为：

式中:θ₁、θ₂、θ₃分别表示髋关节、腿关节、踝关节的转动角度，进行运动学逆解求解得：

式中：

q＝x-a-L₁均表示公式引入的中间变量。

优选的，根据求得的足端运动、机身旋转和机身平移运动轨迹，代入单腿运动学解中，求得对应的关节角度运动规律。

与现有的技术相比，本发明的有益效果为：机器人采用吸附方案，可以稳定的吸附在光滑的垂直或者负表面，并且通过周向运动规划可以实现机器人管道内壁的周向运动。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。

图1为本发明实施例中一四足管道机器人实体图；

图2为本发明实施例中起始姿态下机器人本体坐标系和管道坐标系示意图；

图3a为本发明实施例中足姿调整周向运动分解图；

图3b为本发明实施例中机身旋转周向运动分解图；

图3c为本发明实施例中机身平移周向运动分解图；

图4a为本发明实施例中足姿调整轨迹图；

图4b为本发明实施例中等效机身旋转轨迹图；

图4c为本发明实施例中等效机身平移轨迹图；

图5为本发明实施例中足部坐标系示意图；

图6a为本发明实施例中周向运动足姿调整关节角度曲线图；

图6b为本发明实施例中周向运动机身旋转关节角度曲线图；

图6c为本发明实施例中周向运动机身平移关节角度曲线图；

图7为本发明实施例管道与机器人的位置示意图；

其中：1-踝关节；2-腿关节；3-髋关节；4-机身腰部；5-连接杆；6-真空吸盘；7-摄像头；8-管道。

具体实施方式

以下结合实施例及其附图对本发明作进一步说明。

如图1所示，本发明的四足管道爬壁机器人包括机身和与机身连接的四条腿足机构，每条腿分别由髋关节3、腿关节2和踝关节1串联而成；关节和关节、关节和机身之间通过连接杆5连接；足部末端连接真空吸盘6，机身腰部4携带真空泵，机身上安装有真空传感器。机器人在管道内的运动如图7所示。

作为另一个实施例，可在机器人的机身上设置摄像头7，以便时刻观察机器人的运动情况。

一种四足管道机器人周向运动位姿规划方法，包括以下步骤：

S1、如图2所示，建立管道坐标系和机器人本体坐标系，用以描述机器人在管道8中所处的位置。根据管道8的特性和四足机器人结构的特性，建立机器人的起始位姿：机器人髋关节角度不为零，足端垂直于管道壁面；管道坐标系和机器人本体坐标系的三轴方向互相平行，机器人本体坐标系原点位于管道坐标系原点的正下方。

以管道中心为原点，以轴向方向为x轴，以重力的反方向为z轴建立管道坐标系Σ_O；以机器人腰部中心为原点，前进方向为x轴，侧向运动方向为y轴建立机器人本体坐标系Σ_E。

确立机器人运动的起始姿态：机器人髋关节角度不为零，足端垂直于管道壁面；管道坐标系和机器人本体坐标系的三轴方向互相平行，机器人本体坐标系原点位于管道坐标系原点的正下方。

根据两个坐标系之间的关系确定机器人起始姿态的方程：

式中：^EP、^OP表示空间点在坐标系Σ_E和坐标系Σ_O下的坐标；R表示管道内壁半径；b表示机身宽度的一半；L₂、L₄表示腿关节连杆和足端连杆长度。

根据起始姿态的几何关系，可得BR足足端在机器人本体坐标系中的坐标：

式中：a、b分别表示机身长和宽的一半；L1、L3表示髋关节连杆、踝关节连杆的长度；H表示机身坐标系原点到管道中心的距离。

如图4所示，根据周向运动的连续性和周期性，结合管道内壁结构和机器人运动的特性，将一个周期内的周向运动位姿分解为三个步骤：

(1)如图3a所示，足姿调整：机器人四足分别在管道内壁运动相同的步长L_F达到目标足端位置。

(2)如图3b所示，机身旋转：以腰部中心为旋转中心，机身旋转角β_B使得机身滚转角达到目标角度。

(3)如图3c所示，机身平移：机身沿着与坐标系Σ_O的Y轴夹角y_O为

的方向平移距离

使得机器人中心到达目标中心。

为了满足运动的连续性，周向运动的位姿参数L_F、β_B、

和

应满足以下条件：

1)、一个运动周期内，足端相对管道轴心移动的角度应该与机身旋转的角度一致，因此机身旋转角度β_B＝2arcsin(L_F/2R)。

2)、一个运动周期内，机身中心绕管道轴线转过的角度应该和足姿调整时绕管道轴线转过的角度相同，且距离中心的位置不变，因此运动位移为

移动方向与y_O轴方向夹角为机身中心绕轴线转过的角度的一半

综合可得周向运动位姿参数关系为：

如图4a所示，对足姿调整进行足端圆弧轨迹规划，在机身本体坐标系的轨迹方程为：

式中:α表示圆弧轨迹实时角度，0＜α＜π；r_步表示运动圆弧轨迹半径； (x_p，y_p，z_p)表示足端起始位置坐标；α₀表示圆弧路径直径和y_E轴的夹角。

作为另一个实施例，如图4b所示，对机身旋转进行等效的足端运动，其等效运动在机身本体坐标系的轨迹方程为：

式中:0＜β＜2arcsin(r/R)，β表示机身旋转实时角度；(x_p，y₁，z₁)表示机器人机身旋转的起始足端坐标；β₀表示机身旋转开始时足端和机身中心连线和y_E轴的夹角。

作为另一个实施例，如图4c所示，对机身旋转进行等效的足端运动，其等效运动在机身本体坐标系的轨迹方程为：

式中：l表示运动的实时位移，0<l<2r_步H/R；(x_p,y₂,z₂)表示机身平移的起始坐标。

如图5所示，进行运动学分析，建立足部坐标系:原点在髋关节轴心、坐标轴方向与坐标系Σ_E方向相同建立原点坐标系Σ₁；原点在腿关节轴心、轴向方向为z轴、第一连杆方向为x轴建立第一坐标系Σ₂；原点在踝关节轴心、轴向为z 轴、第二连杆方向为y轴建立第二坐标系Σ₃；以第三连杆和第四连杆交点为原点、第三连杆方向为x轴、第四连杆方向为z轴建立第三坐标系Σ₄；以足端为原点、三轴方向与第三坐标系Σ₄相同建立第四坐标系Σ₅，可得坐标系Σ₅和坐标系Σ_E之间的坐标变换关系为：

式中：a、b分别表示机身长和宽的一半；L₁、L₂、L₃、L₄表示髋关节连杆、腿关节连杆、踝关节连杆、足端连杆的长度。

因此足端在机身本体坐标系Σ_E中的位置为：

式中:θ₁、θ₂、θ₃分别表示髋关节、腿关节、踝关节的转动角度，进行运动学逆解求解得：

式中：

q＝x-a-L₁均表示公式引入的中间变量。

如图6a、图6b、图6c所示，将足端运动轨迹代入运动学逆解中，在MATLAB 中求解得到足姿调整、机身旋转和机身平移所对应的关节运动角度，从而得到周向运动的各个步骤关节角度运动曲线。

以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制。

Claims (10)

1.一种四足管道机器人周向运动位姿规划方法，其特征是，包括以下步骤：

S1、建立管道坐标系和四足机器人本体坐标系，确立机器人起始姿态，同时建立机器人的位姿方程；

S2、调整足姿、机身旋转和机身平移的周向运动，对运动过程中位姿的角度、方向、位移进行规划；

S3、通过足端运动轨迹实现周向运动位姿的规划，获得周向运动的各步骤的关节角度运动曲线。

2.如权利要求1所述的一种四足管道机器人周向运动位姿规划方法，其特征是，所述四足机器人包括机身和与机身连接的四条腿足机构，每条腿足机构包括髋关节、腿关节和踝关节串联，关节和关节、关节和机身之间通过连接杆连接，足部末端连接真空吸盘；所述机身腰部携带真空泵，所述机身上安装真空传感器。

3.如权利要求2所述的一种四足管道机器人周向运动位姿规划方法，其特征是，所述四足机器人的每条腿足均具有六个自由度，分别是：髋关节自由度、腿关节自由度、踝关节自由度和球关节的三个自由度。

4.如权利要求3所述的一种四足管道机器人周向运动位姿规划方法，其特征是，所述管道坐标系Σ_O，是以管道中心为原点，以管道轴向方向为x轴，以重力的反方向为z轴；

所述机器人本体坐标系Σ_E，是以机器人腰部中心为原点，前进方向为x轴，侧向运动方向为y轴。

5.如权利要求4所述的一种四足管道机器人周向运动位姿规划方法，其特征是，机器人运动的起始位姿为：机器人髋关节角度不为零，足端垂直于管道壁面；管道坐标系和机器人本体坐标系的三轴方向互相平行，机器人本体坐标系原点位于管道坐标系原点的正下方。

6.如权利要求5所述的一种四足管道机器人周向运动位姿规划方法，其特征是，步骤S2的周向运动包括以下步骤：

(1)足姿调整：机器人四足分别在管道内壁运动相同的步长到达目标足端位置，对足姿调整进行足端圆弧轨迹规划，在机身本体坐标系的轨迹方程为：

式中：

式中:0＜α＜π，α表示运动圆弧轨迹实时弧度；r_步表示运动圆弧轨迹半径；(x_p，y_p，z_p)表示足端起始位置坐标；α₀表示圆弧路径直径和y_E轴的夹角，L_F表示步长；R表示管道内壁半径；H表示机身坐标系原点到管道中心的距离；

(2)机身旋转：以腰部中心为旋转中心，机身旋转使得机身滚转角达到目标角度；

(3)机身平移：机身沿着某一方向平移一定距离使得机器人中心到达目标中心，最后使得机器人完成一个周向运动周期后关节角度和运动前相同。

7.如权利要求6所述的一种四足管道机器人周向运动位姿规划方法，其特征是，对足姿调整目标位置进行足端轨迹规划，而对机身旋转机身平移进行等效的足端运动，具体如下：

机身旋转：足端固定时机身绕机身中心逆时针旋转等效为机身固定时足端以机身中心为旋转中心顺时针旋转相同的角度，对机身旋转进行等效的足端运动，在机身本体坐标系的轨迹方程为：

式中:β表示机身旋转实时角度，0＜β＜2arcsin(r_步/R)；(x_p，y₁，z₁)表示机器人机身旋转的起始足端坐标；β₀表示机身旋转开始时足端和机身中心连线和y_E轴的夹角；

机身平移：足端固定下机身沿着某一方向运动等效为机身固定时足端以反方向移动相同的距离。

8.如权利要求7所述的一种四足管道机器人周向运动位姿规划方法，其特征是，根据足端运动轨迹，求解出在机身本体坐标系下的轨迹方程。

9.如权利要求8所述的一种四足管道机器人周向运动位姿规划方法，其特征是，建立足部坐标系，进行单腿运动学求解：

原点在髋关节轴心、坐标轴方向与坐标系Σ_E方向相同建立原点坐标系Σ₁；原点在腿关节轴心、轴向方向为z轴、第一连杆方向为x轴建立第一坐标系Σ₂；原点在踝关节轴心、轴向为z轴、第二连杆方向为y轴建立第二坐标系Σ₃；以第三连杆和第四连杆交点为原点、第三连杆方向为x轴、第四连杆方向为z轴建立第三坐标系Σ₄；以足端为原点、三轴方向与第三坐标系Σ₄相同建立第四坐标系Σ₅；

根据坐标系之间的变换，求得足端在机身本体坐标系下的位置和三个关节转角的关系，得到正逆运动学解，得到第四坐标系Σ₅和坐标系Σ_E之间的坐标变换关系为：

式中：a、b分别表示机身长和宽的一半；L1、L2、L3、L4表示髋关节连杆、腿关节连杆、踝关节连杆、足端连杆的长度；

因此足端在机身本体坐标系Σ_E中的位置为：

式中:θ₁、θ₂、θ₃分别表示髋关节、腿关节、踝关节的转动角度，进行运动学逆解求解得：

式中：

q＝x-a-L₁均表示公式引入的中间变量。

10.如权利要求9所述的一种四足管道机器人周向运动位姿规划方法，其特征是，根据求得的足端运动、机身旋转和机身平移运动轨迹，代入单腿运动学解中，求得对应的关节角度运动规律。

Images