CN115256376B - 基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法 - Google Patents

Abstract

本发明涉及一种基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法，其包括以下步骤，步骤一：确定变直径攀爬的背脊曲线，获得蛇形机器人攀爬路径；步骤二：离散化背脊曲线，计算蛇形机器人与背脊曲线对应的关节角度；步骤三：结合移位控制与滚动控制，控制蛇形机器人实现变直径攀爬运动。本发明将具有高度灵活性贝塞尔曲线用于蛇形机器人攀爬步态的设计，通过改变贝塞尔曲线自由控制点的位置可以匹配不同尺寸管道的过渡需求，具有很好的调整灵活性，提高了蛇形机器人在复杂管道结构中的通行能力。

Description

基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法

技术领域

本申请涉及机器人控制技术领域，具体地涉及一种基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法。

背景技术

蛇形机器人是一种高冗余度的移动机器人，具有可以在管道等非结构化的复杂环境中工作的潜力。与此同时，高自由度的复杂结构也给机器人的运动控制带来了很大的挑战。

蛇形机器人目前常用的步态控制方法可分为两类。第一类是控制函数法，通过参数化方程即步态方程直接生成关节角轨迹，只需要调节步态方程的振幅、频率和相移参数即可控制蛇形机器人实现蜿蜒、螺旋滚动等运动形式，然而该方法是通过减少蛇形机器人的有效自由度来简化控制，导致其运动的灵活性受到了很大的限制。另一种方法是背脊曲线法，利用蛇形机器人的背脊曲线去拟合实际任务中所需要实现的运动姿态的空间曲线形状，极大地提高了蛇形机器人环境适应性，但它需要合适的方法对所设计的曲线进行关节离散化处理，生成步态控制所需的关节角度。近年来，为了满足管道检测、探索救援等复杂环境的工作需求，基于背脊曲线的步态设计方法被广泛用于蛇形机器人复杂三维运动的步态设计。

目前针对直径突变管道结构的攀爬问题，基于背脊曲线的步态设计方法主要有利用锥形螺旋线和将曲率、挠率为常数的简单曲线平滑连接用于设计变径管道的过渡部分两种方法。由于锥形螺旋线和螺旋线之间不能保证在连接点处二阶导数连续，会导致蛇形机器人攀爬过程中在连接点处关节角度发生突变；而将曲率、挠率为常数的曲线平滑连接设计过渡部分的方法，所设计曲线形状的可调节性差，而且需要将各段曲线平滑连接，设计过程十分的繁琐。针对上述问题，考虑到贝塞尔曲线具有非常高的调整灵活性，可以简单直观地设计出想要的目标形式，用于曲线段连接时还能使连接点处二阶导数连续，本发明将建立基于空间贝塞尔曲线的分段背脊曲线，通过调节贝塞尔曲线的控制点位置来控制蛇形机器人通过各种尺寸的变直径管道。

发明内容

为了克服现有技术的不足，本发明将具有高度灵活性贝塞尔曲线用于蛇形机器人攀爬步态的设计，通过改变贝塞尔曲线自由控制点的位置生成准确的背脊曲线作为蛇形机器人攀爬路径；可以匹配不同尺寸管道的过渡需求，具有更高灵活性；通过曲率积分对生成的背脊曲线进行离散化处理，结合移位控制与滚动控制，提高蛇形机器人在复杂管道结构中的通行能力。

为实现上述目的，本发明提供一种基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法，其包括以下步骤：

步骤1：确定变直径攀爬的背脊曲线，获得蛇形机器人攀爬路径；

所述的背脊曲线α(t)包括至少六次贝塞尔曲线和两条不同螺旋半径的螺旋线，通过构建贝塞尔曲线的控制点约束方程来确定贝塞尔曲线的多个控制点，确定贝塞尔曲线到管道中心轴距离d、背脊曲线的曲率k(t)和挠率τ(t)，并利用最优化方法获得贝塞尔曲线的自由控制点的最优位置，从而确定背脊曲线α(t)的位置和形状；最终获得蛇形机器人攀爬路径；

步骤2：离散化背脊曲线，计算蛇形机器人与背脊曲线对应的关节角度；

步骤21：使用波纹管模型对蛇形机器人进行建模，获取步骤1中背脊曲线的曲率k(t)计算背脊曲线的背曲率k_d和侧曲率k_l；如下所示：

式中：k_d(t)表示背脊曲线的背曲率；k_l(t)表示背脊曲线的侧曲率；k(t)表示背脊曲线的曲率；φ(t)表示弗莱纳坐标系与波纹管坐标系之间的相位角；t表示贝塞尔曲线方程的参数；

步骤22：对步骤21中确定的背曲率和侧曲率进行积分运算，计算出蛇形机器人与背脊曲线α(t)对应的俯仰关节pitch的关节角度和偏航关节yaw的关节角度，具体计算公式如下所示：

式中：θ_pitch表示俯仰关节的关节角度；θ_yaw表示偏航关节的关节角度；α'表示背脊曲线α(t)关于t的一阶导数；i表示蛇形机器人的关节编号；pitch表示蛇形机器人的俯仰关节；yaw表示蛇形机器人的偏航关节；t_i+1和t_i-1分别表示第i+1和i-1个关节处的参数方程的参数；

步骤23：对积分表达式进行离散化后再累加求和；取离散步长为Δt，计算其对应的弧长Δs，如下所示：

Δs＝|α'(t_j)|Δt；

式中：Δs表示单位离散步长对应的弧长；Δt表示离散步长；α'(t_j)表示背脊曲线α(t)在离散点t_j处的一阶导数；j表示当前离散点的序号；

计算离散化后的弧长Δs内背曲率k_d和侧曲率k_l的对应角度θ_d和θ_l，如下所示：

式中：θ_d表示弧长Δs内背曲率的对应角度；θ_l表示弧长Δs内侧曲率的对应角度；

对蛇形机器人关节长度l内的θ_d、θ_l进行累加求和得到

和/>

然后将相邻俯仰关节和偏航关节内的/>

和/>

分别相加，得到该俯仰关节和偏航关节所对应的关节角度θ_pitch和θ_yaw，实现了对所设计背脊曲线的离散化过程；

步骤3：结合移位控制与滚动控制，控制蛇形机器人实现变直径攀爬运动；

所述移位控制包括使蛇形机器人按照所设计的背脊曲线形状产生变化，所述滚动控制包括使蛇形机器人关节产生翻滚运动；

构建移位控制与滚动控制的计算关系，使得移位长度ΔS_L与滚动角度Δψ满足以下关系式：

式中：Δψ表示蛇形机器人沿着背脊曲线移动时每步的滚动角度；ΔS_L表示每步的移位长度；d_robot为蛇形机器人的关节直径；α_h表示螺旋线与贝塞尔曲线连接点处切线的倾斜角度；

将每步的滚动角度Δψ进行累加，每次累加的结果作为下一步相位角的初始值φ₀，从而使蛇形机器人的贝塞尔曲线相对于管径发生变化的位置保持相对恒定。

可优选的是，所述步骤1中蛇形机器人为关节类型为正交关节的蛇形机器人。

可优选的是，所述步骤1中背脊曲线α(t)包括一条至少六次贝塞尔曲线和两条分别位于所述贝塞尔曲线的两端的具有不同螺旋半径的螺旋线平滑连接构成，本发明后续选用六次贝塞尔曲线对步态控制过程进行说明，具体为：

所述背脊曲线α(t)的表达式为：

式中：M₁表示第一段函数；M₂表示第二段函数；M₃表示第三段函数；x₁、y₁和z₁分别表示第一段函数的横坐标、纵坐标和竖直方向坐标；x₃、y₃和z₃分别表示第三段函数的横坐标、纵坐标和竖直方向坐标；r₁和r₃分别表示第一段函数和第三段函数的螺旋半径；p₁和p₃分别表示第一段函数和第三段函数的单位弧长的螺距；N表示控制点的编号；P_N表示贝塞尔曲线第N个控制点；T₁和T₂分别表示第一段函数区间的右端点和第三段函数区间的右端点。

可优选的是，所述步骤1中的构建控制点约束方程来确定六次贝塞尔曲线P₀、P₁、P₂、P₄、P₅和P₆六个控制点，具体为：

式中：M₁(T₁)、M₁'(T₁)和M₁”(T₁)分别表示第一段函数在T₁位置的一阶、二阶和三阶导数；M₂(T₁)、M₂'(T₁)和M₂”(T₁)分别表示第二段函数在T₁位置的一阶、二阶和三阶导数；M₂(T₁+1)、M₂'(T₁+1)和M₂”(T₁+1)分别表示第二段函数在T₁+1位置的一阶、二阶和三阶导数；M₃(T₁+1)、M₃'(T₁+1)和M₃”(T₁+1)分别表示第三段函数在T₁+1位置的一阶、二阶和三阶导数。

可优选的是，所述步骤1中的六次贝塞尔曲线到管道中心轴距离d、背脊曲线的曲率k(t)和挠率τ(t)，具体为：

所述六次贝塞尔曲线到管道中心轴距离d的获取方法如下所示：

式中：d表示六次贝塞尔曲线到管道中心轴距离；C_6x(t)和C_6y(t)分别表示六次贝塞尔曲线的x、y方向分量；

所述背脊曲线的曲率k(t)和挠率τ(t)的获取方法如下所示：

式中：k(t)表示背脊曲线的曲率；τ(t)表示背脊曲线的挠率；α'、α”和α”分别表示背脊曲线α(t)关于t的一阶、二阶和三阶导数。

可优选的是，所述步骤1利用最优化方法计算六次贝塞尔曲线自由控制点P₃，具体为：

当六次贝塞尔曲线到管道中心轴的距离d的最大值满足所设置的安全距离的同时，使得最大值点处的曲率最小，最优化方法的目标函数和约束条件如下所示：

式中：d_max表示背脊曲线过渡部分到管道中心轴距离d的最大值；t_max表示该最大值点对应六次贝塞尔曲线参数方程的参数；m表示所设置过渡时的安全距离；k表示最大值点处的曲率；a表示自由控制点P₃的横坐标；b表示自由控制点P₃纵坐标。

可优选的是，所述步骤21中弗莱纳坐标系与波纹管坐标系之间的相位角的获取方法如下：

式中：φ(t)表示弗莱纳坐标系与波纹管坐标系之间的相位角；φ₀表示弗莱纳坐标系与波纹管坐标系之间的初始相位角；t_i表示第i个关节处的参数方程的参数；

第i个关节处的参数方程的参数t_i的获取方法如下所示：

式中：l表示蛇形机器人的关节长度。

与现有技术相比，本发明的有益效果在于：

(1)本发明提供的基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法，利用六次或者更多次贝塞尔曲线连接两条不同螺旋半径的螺旋线用于规划蛇形机器人的攀爬路径，与基于锥形螺旋线的变直径攀爬步态设计方法相比，本发明可以保证蛇形机器人在攀爬过程中关节角度不会发生突变，具有好的稳定性；与基于简单曲线段连接的步态设计方法相比，本发明的步态设计过程更为简单；

(2)本发明通过调整贝塞尔曲线自由控制点的位置，可以匹配不同尺寸管道的过渡需求，具有很好的调整灵活性，提高了蛇形机器人在复杂管道结构中的通行能力。

附图说明

图1为本发明实施例基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法流程图；

图2为本发明实施例S1所设计用于变直径攀爬的目标背脊曲线；

图3为本发明实施例S2中离散化背脊曲线的方法流程图；

图4为本发明实施例S2目标背脊曲线的离散结果图；

图5为本发明实施例S3移位长度ΔS_L与滚动角度Δψ的关系图；

图6为本发明实施例蛇形机器人变直径攀爬仿真过程图；

图7为本发明实施例蛇形机器人变直径攀爬时部分关节的关节角度随时间的变化图。

具体实施方式

以下，参照附图对本发明的实施方式进行说明。

如图1的流程图所示，本发明基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法，具体包括如下步骤：

S1：设计变直径攀爬的背脊曲线，规划蛇形机器人攀爬路径；

背脊曲线α(t)由一条六次或者六次以上的贝塞尔曲线两端分别和两条不同螺旋半径的螺旋线平滑连接组成；在本实施例中以六次贝塞尔曲线为例，背脊曲线α(t)的表达式为一个三段的分段函数，如下所示：

M₂＝C₆(t-T₁)；t∈[T₁,T₁+1]；

式中：M₁表示第一段函数；M₂表示第二段函数；M₃表示第三段函数；x₁、y₁和z₁分别表示第一段函数的横坐标、纵坐标和竖直方向坐标；x₃、y₃和z₃分别表示第三段函数的横坐标、纵坐标和竖直方向坐标；r₁和r₃分别表示第一段函数和第三段函数的螺旋半径；p₁和p₃分别表示第一段函数和第三段函数的单位弧长的螺距；T₁和T₂分别表示第一段函数区间的右端点和第三段函数区间的右端点；

六次贝塞尔曲线的获取方法如下式所示：

式中：C₆(t)表示六次贝塞尔曲线参数方程；P_N表示六次贝塞尔曲线第N个控制点；N表示贝塞尔曲线控制点编号；t表示参数方程的参数；

为了避免蛇形机器人在连接点处运行时关节角发生突变，保证在连接点处二阶导数连续，构建控制点约束方程来确定六次贝塞尔曲线P₀、P₁、P₂、P₄、P₅和P₆六个控制点，如下所示：

式中：M₁(T₁)、M₁'(T₁)和M₁”(T₁)分别表示第一段函数在T₁位置的一阶、二阶和三阶导数；M₂(T₁)、M₂'(T₁)和M₂”(T₁)分别表示第二段函数在T₁位置的一阶、二阶和三阶导数；M₂(T₁+1)、M₂'(T₁+1)和M₂”(T₁+1)分别表示第二段函数在T₁+1位置的一阶、二阶和三阶导数；M₃(T₁+1)、M₃'(T₁+1)和M₃”(T₁+1)分别表示第三段函数在T₁+1位置的一阶、二阶和三阶导数。

进一步确定六次贝塞尔曲线到管道中心轴距离d的获取方法如下所示：

式中：d表示六次贝塞尔曲线到管道中心轴距离；C_6x(t)和C_6y(t)分别表示六次贝塞尔曲线的x、y方向分量；

背脊曲线的曲率k(t)和挠率τ(t)的获取方法如下所示：

式中：k(t)表示背脊曲线的曲率；τ(t)表示背脊曲线的挠率；α'、α”和α”分别表示背脊曲线α(t)关于t的一阶、二阶和三阶导数。

然后利用最优化方法计算六次贝塞尔曲线自由控制点P₃的最优位置，从而确定背脊曲线α(t)的位置和形状；使六次贝塞尔曲线到管道中心轴的距离d的最大值满足所设置的安全距离的同时，保证最大值点处的曲率最小，最优化方法的目标函数和约束条件如下所示：

式中：d_max表示背脊曲线过渡部分到管道中心轴距离d的最大值；t_max表示该最大值点对应六次贝塞尔曲线参数方程的参数；m表示所设置过渡时的安全距离；k表示最大值点处的曲率；a表示自由控制点P₃的横坐标；b表示自由控制点P₃纵坐标。

在本实施例中，设置初始管径D₁＝160mm，最终管径D₂＝200mm，蛇形机器人关节直径d_robot＝40mm，计算出初始螺旋线半径r₁＝100mm，最终螺旋线半径r₂＝120mm，利用上述最优化方法计算六次贝塞尔曲线的最优自由控制点为P₃(-0.28,-0.07,0.52)，所设计用于变直径攀爬的目标背脊曲线如图2所示，为本发明实施例S1所设计用于变直径攀爬的目标背脊曲线。

最终获得蛇形机器人攀爬路径。

S2：采用曲率积分的方法离散化背脊曲线，计算蛇形机器人与之对应的关节角度；

S21：使用波纹管模型对蛇形机器人进行建模，获取S1中背脊曲线的曲率k(t)计算背脊曲线的背曲率k_d和侧曲率k_l；如下所示：

式中：k_d9t)表示背脊曲线的背曲率；k_l(t)表示背脊曲线的侧曲率；k(t)表示背脊曲线的曲率；φ(t)表示弗莱纳坐标系与波纹管坐标系之间的相位角；

其中，弗莱纳坐标系与波纹管坐标系之间的相位角获取方法如下所示：

式中：φ(t)表示弗莱纳坐标系与波纹管坐标系之间的相位角；φ₀表示弗莱纳坐标系与波纹管坐标系之间的初始相位角；t_i表示第i个关节处的参数方程的参数；

第i个关节处的参数方程的参数t_i的获取方法如下所示：

式中：l表示蛇形机器人的关节长度；i表示蛇形机器人的关节序号；

S22：对S21中确定的背曲率和侧曲率进行积分运算，计算出蛇形机器人与背脊曲线α(t)对应的俯仰关节pitch和偏航关节yaw的角度，具体计算公式如下所示：

式中：θ_pitch表示俯仰关节的关节角度；θ_yaw表示偏航关节的关节角度；α'表示背脊曲线α(t)关于t的一阶导数；t_i+1和t_i-1分别表示第i+1和i-1个关节处的参数方程的参数；pitch表示蛇形机器人的俯仰关节；yaw表示蛇形机器人的偏航关节；

S23：对积分表达式进行离散化后再累加求和；取离散步长为Δt，计算其对应的弧长Δs，如下所示：

Δs＝|α'(t_j)|Δt；

式中：Δs表示离散化后的弧长；Δt表示离散步长；α'(t_j)表示背脊曲线α(t)在离散点t_j处的一阶导数；j表示当前离散点的编号；

计算离散化后的弧长Δs内背曲率k_d和侧曲率k_l对应的角度θ_d和θ_l，如下所示：

式中：θ_d表示弧长Δs内背曲率的对应角度；θ_l表示弧长Δs内侧曲率的对应角度；

对蛇形机器人关节长度l内的θ_d、θ_l进行累加求和得到

和/>

然后将相邻俯仰关节和偏航关节内的/>

和/>

分别相加，得到该俯仰关节和偏航关节所对应的关节角度θ_pitch和θ_yaw，实现了对所设计背脊曲线的离散化过程；

在本实施例中，具体离散化流程图如图3所示为本发明实施例S2中离散化背脊曲线的方法流程图，取离散步长Δt＝0.001，关节长度l＝0.0725m，图2中目标背脊曲线的离散结果如图4所示为本发明实施例S2目标背脊曲线的离散结果图。

S3：结合移位控制与滚动控制，控制蛇形机器人实现变直径攀爬运动；

移位控制是使蛇形机器人按照所设计的背脊曲线形状不断地产生变化；滚动控制是控制蛇形机器人关节产生翻滚运动，从而达到前进的目的；

构建移位控制与滚动控制的计算关系，需要保证每步的移位长度ΔS_L与滚动角度Δψ应满足以下关系式：

式中：Δψ表示每步的滚动角度；ΔS_L表示每步的移位长度；d_robot为蛇形机器人的关节直径；α_h表示螺旋线M₁与六次贝塞尔曲线连接点处切线的倾斜角度。

将每步的滚动角度Δψ进行累加，每次累加的结果作为下一步相位角的初始值φ₀，即可保证攀爬过程中，过渡部分位置相对于管径变化的位置保持相对恒定。

在本实施例中，如图5所示为本发明实施例S3移位长度ΔS_L与滚动角度Δψ的关系图，其中R表示蛇形机器人在M₁与M₂的连接点P₀处滚动控制方向，M表示蛇形机器人在P₀处移位控制方向；在滚动控制与移位控制共同作用下，蛇形机器人在P₀处产生一个水平方向的运动速度，使蛇形机器人变直径攀爬过程中在水平方向完成由初始管道的解旋到最终管道的缠绕过程，避免蛇形机器人在攀爬过程中与管道发生碰撞。

利用三维仿真软件Webots对所提出的步态控制方法进行验证，如图6所示为本发明实施例蛇形机器人变直径攀爬仿真过程图。由图可知，蛇形机器人沿着所设计的攀爬路径前进，安全平稳地通过管径突变的管道结构，验证了所提出步态控制方法的正确性。

图7为本发明实施例蛇形机器人变直径攀爬时部分关节的关节角度随时间的变化图，由图可知蛇形机器人在攀爬过程中关节角度没有发生突变，说明该步态控制方法可以保证蛇形机器人攀爬过程中关节角度连续变化。

综上，本案例的基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法结果证明了具有好的应用效果。

(1)本发明提供的基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法，根据实际管道尺寸确定背脊曲线前后两段螺旋线的螺旋半径，利用六次贝塞尔曲线将前后两段螺旋线平滑连接用以规划蛇形机器人的攀爬路径，该背脊曲线不仅设计过程合理，还可以保证形机器人在攀爬过程中关节角度连续变化，不会发生突变；

(2)本发明基于一般参数使用曲率积分对生成的背脊曲线进行离散化处理，简化了通过离散背脊曲线求解蛇形机器人关节角度的计算过程。

(3)本发明结合移位控制与滚动控制，实现了蛇形机器人的变直径攀爬运动控制，并通过仿真验证了该步态控制方法的正确性，提高了蛇形机器人在管道中的通行能力，相比于现有的攀爬步态设计方法具有明显的优势。

以上所述的实施例仅是对本发明的优选实施方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (7)

1.一种基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法，其特征在于，其包括以下步骤：

步骤1：确定变直径攀爬的背脊曲线，获得蛇形机器人攀爬路径；

所述的背脊曲线α(t)包括至少六次贝塞尔曲线和两条不同螺旋半径的螺旋线，通过构建贝塞尔曲线的控制点约束方程来确定贝塞尔曲线的多个控制点，确定贝塞尔曲线到管道中心轴距离d、背脊曲线的曲率k(t)和挠率τ(t)，并利用最优化方法获得贝塞尔曲线的自由控制点的最优位置，从而确定背脊曲线α(t)的位置和形状；最终获得蛇形机器人攀爬路径；

步骤2：离散化背脊曲线，计算蛇形机器人与背脊曲线对应的关节角度；

步骤21：使用波纹管模型对蛇形机器人进行建模，获取步骤1中背脊曲线的曲率k(t)计算背脊曲线的背曲率k_d和侧曲率k_l；如下所示：

式中：k_d(t)表示背脊曲线的背曲率；k_l(t)表示背脊曲线的侧曲率；k(t)表示背脊曲线的曲率；φ(t)表示弗莱纳坐标系与波纹管坐标系之间的相位角；t表示贝塞尔曲线方程的参数；

步骤22：对步骤21中确定的背曲率和侧曲率进行积分运算，计算出蛇形机器人与背脊曲线α(t)对应的俯仰关节pitch的关节角度和偏航关节yaw的关节角度，具体计算公式如下所示：

式中：θ_pitch表示俯仰关节的关节角度；θ_yaw表示偏航关节的关节角度；α'表示背脊曲线α(t)关于t的一阶导数；i表示蛇形机器人的关节编号；pitch表示蛇形机器人的俯仰关节；yaw表示蛇形机器人的偏航关节；t_i+1和t_i-1分别表示第i+1和i-1个关节处的参数方程的参数；

步骤23：对积分表达式进行离散化后再累加求和；取离散步长为Δt，计算其对应的弧长Δs，如下所示：

Δs＝|α'(t_j)|Δt；

式中：Δs表示单位离散步长对应的弧长；Δt表示离散步长；α'(t_j)表示背脊曲线α(t)在离散点t_j处的一阶导数；j表示当前离散点的序号；

计算离散化后的弧长Δs内背曲率k_d和侧曲率k_l的对应角度θ_d和θ_l，如下所示：

式中：θ_d表示弧长Δs内背曲率的对应角度；θ_l表示弧长Δs内侧曲率的对应角度；

对蛇形机器人关节长度l内的θ_d、θ_l进行累加求和得到

和/>

然后将相邻俯仰关节和偏航关节内的/>

和/>

分别相加，得到所述俯仰关节和偏航关节所对应的关节角度θ_pitch和θ_yaw，实现了对所设计背脊曲线的离散化过程；

步骤3：结合移位控制与滚动控制，控制蛇形机器人实现变直径攀爬运动；

所述移位控制包括使蛇形机器人按照所设计的背脊曲线形状产生变化，所述滚动控制包括使蛇形机器人关节产生翻滚运动；

构建移位控制与滚动控制的计算关系，使得移位长度ΔS_L与滚动角度Δψ满足以下关系式：

式中：Δψ表示蛇形机器人沿着背脊曲线移动时每步的滚动角度；ΔS_L表示每步的移位长度；d_robot为蛇形机器人的关节直径；α_h表示螺旋线与贝塞尔曲线连接点处切线的倾斜角度；

将每步的滚动角度Δψ进行累加，每次累加的结果作为下一步相位角的初始值φ₀，从而使蛇形机器人的贝塞尔曲线相对于管径发生变化的位置保持相对恒定。

2.根据权利要求1所述的基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法，其特征在于，所述步骤1中蛇形机器人为关节类型为正交关节的蛇形机器人。

3.根据权利要求1所述的基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法，其特征在于，所述步骤1中背脊曲线α(t)包括一条六次贝塞尔曲线和两条分别位于所述贝塞尔曲线的两端的具有不同螺旋半径的螺旋线平滑连接构成，具体为：

所述背脊曲线α(t)的表达式为：

式中：M₁表示第一段函数；M₂表示第二段函数；M₃表示第三段函数；x₁、y₁和z₁分别表示第一段函数的横坐标、纵坐标和竖直方向坐标；x₃、y₃和z₃分别表示第三段函数的横坐标、纵坐标和竖直方向坐标；r₁和r₃分别表示第一段函数和第三段函数的螺旋半径；p₁和p₃分别表示第一段函数和第三段函数的单位弧长的螺距；N表示控制点的编号；P_N表示贝塞尔曲线第N个控制点；T₁和T₂分别表示第一段函数区间的右端点和第三段函数区间的右端点。

4.根据权利要求3所述的基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法，其特征在于，所述步骤1中的构建控制点约束方程来确定六次贝塞尔曲线P₀、P₁、P₂、P₄、P₅和P₆六个控制点，具体为：

式中：M₁(T₁)、M₁'(T₁)和M₁”(T₁)分别表示第一段函数在T₁位置的一阶、二阶和三阶导数；M₂(T₁)、M₂'(T₁)和M₂”(T₁)分别表示第二段函数在T₁位置的一阶、二阶和三阶导数；M₂(T₁+1)、M₂'(T₁+1)和M₂”(T₁+1)分别表示第二段函数在T₁+1位置的一阶、二阶和三阶导数；M₃(T₁+1)、M₃'(T₁+1)和M₃”(T₁+1)分别表示第三段函数在T₁+1位置的一阶、二阶和三阶导数。

5.根据权利要求3所述的基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法，其特征在于，所述步骤1中的六次贝塞尔曲线到管道中心轴距离d、背脊曲线的曲率k(t)和挠率τ(t)，具体为：

所述六次贝塞尔曲线到管道中心轴距离d的获取方法如下所示：

式中：d表示六次贝塞尔曲线到管道中心轴距离；C_6x(t)和

分别表示六次贝塞尔曲线的x、y方向分量；

所述背脊曲线的曲率k(t)和挠率τ(t)的获取方法如下所示：

式中：k(t)表示背脊曲线的曲率；τ(t)表示背脊曲线的挠率；α'、α”和α”’分别表示背脊曲线α(t)关于t的一阶、二阶和三阶导数。

6.根据权利要求3所述的基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法，其特征在于，所述步骤1利用最优化方法计算六次贝塞尔曲线自由控制点P₃，具体为：

当六次贝塞尔曲线到管道中心轴的距离d的最大值满足所设置的安全距离的同时，使得最大值点处的曲率最小，最优化方法的目标函数和约束条件如下所示：

式中：d_max表示背脊曲线过渡部分到管道中心轴距离d的最大值；t_max表示该最大值点对应六次贝塞尔曲线参数方程的参数；m表示所设置过渡时的安全距离；k表示最大值点处的曲率；a表示自由控制点P₃的横坐标；b表示自由控制点P₃纵坐标。

7.根据权利要求3所述的基于贝塞尔曲线的蛇形机器人变直径攀爬步态控制方法，其特征在于，所述步骤21中弗莱纳坐标系与波纹管坐标系之间的相位角的获取方法如下：

式中：φ(t)表示弗莱纳坐标系与波纹管坐标系之间的相位角；φ₀表示弗莱纳坐标系与波纹管坐标系之间的初始相位角；t_i表示第i个关节处的参数方程的参数；

第i个关节处的参数方程的参数t_i的获取方法如下所示：

式中：l表示蛇形机器人的关节长度。

Images