CN113334381A - 一种可运动解耦的连续体机器人控制方法 - Google Patents

Abstract

本发明涉及一种可运动解耦的连续体机器人控制方法，包括以下步骤：建立机器人正运动学末端位姿矩阵；通过对机器人正运动学末端位姿矩阵的求逆解得到机器人关节向量；将关节向量对驱动关节的驱动变量进行映射；根据驱动变量对机器人进行控制。可以实现远端连续体段与近端连续体段的驱动解耦，以及末端位姿的分离，并针对该构型的机器人设计了一种可单独求解出机器人逆运动控制的每个驱动变量的直接逆运动学求解算法，简化了控制计算过程。该直接逆运动学求解算法无需多次迭代，稳定可靠，避免了传统的雅克比迭代求运动学逆解中存在的精度不高、不稳定、收敛慢等问题。

Description

一种可运动解耦的连续体机器人控制方法

技术领域

本发明涉及一种连续体机器人的运动控制方法，该连续体机器人利用自身形变可以实现多个自由度的位姿解耦运动，主要用于微创手术机器人以及工业应用中狭小空间探测等领域。

背景技术

连续体机器人具有尺寸小、重量轻、运动灵活、负载自重比大和环境顺应性强等一系列优点，在手术机器人及狭小空间探测领域具有广泛的应用。连续体机器人为了实现末端操作器的灵活运动，需要多个运动自由度，一般需要采用多个形变段叠加实现。形变段多采用腱进行拖拽驱动，驱动腱贯穿于形变段中。而当多个形变段叠加在一起时，驱动腱会贯穿于多个形变段中，这就导致了在驱动过程中，多个形变段之间的驱动会发生耦合，给控制带来困难。由于连续体机器人构型特殊，一般不能采用常规机器人的运动控制方法进行控制，而机器人的末端位置与姿态高度耦合，一般无法直接求出逆解，需要采用迭代算法求解(如速度雅克比矩阵迭代)，计算效率低，稳定性差(特别是在边界位置)，精度也有限。

发明内容

针对目前连续体机器人中存在驱动与控制耦合等问题，如果能够从构型出发解除形变段之间的耦合关系，并且实现机器人的位姿分离，将为连续体机器人的建模与控制带来诸多便利。本发明设计一种拓扑解耦的连续体机器人控制方法，该连续体机器人可以实现多段连续体段的驱动解耦，以及末端位姿的分离，机器人的直接逆运动学求解控制方法，可以实现机器人高效、稳定与准确的控制。

本发明为实现上述目的所采用的技术方案是：一种可运动解耦的连续体机器人控制方法，包括以下步骤：

建立机器人正运动学末端位姿矩阵；

通过对机器人正运动学末端位姿矩阵的求逆解得到机器人关节向量；

将关节向量对驱动关节的驱动变量进行映射；

根据驱动变量对机器人进行控制。

所述机器人正运动学末端位姿矩阵如下：

其中，n_x、n_y、n_z、o_x、o_y、o_z、a_x、a_y、a_z、p_x、p_y、p_z均为为齐次变换矩阵参数；其中，p_x、p_y、p_z表示机器人末端位置；n_x、n_y、n_z表示末端x轴的单位矢量；o_x、o_y、o_z表示末端y轴的单位矢量；a_x、a_y、a_z表示末端z轴的单位矢量。

建立机器人整体运动学关系模型，具体如下：

其中，

表示第i+1个坐标系O_i+1在第i个坐标系O_i的位姿转换矩阵，i＝0、1、…、4；Z_j表示第j个坐标系O_j的z轴，j＝0、1、…、4；d表示第0个坐标系沿其z轴方向运动的距离；L₂、L₄分别表示近端段与中间段之间的连接段长度、中间段与远端段之间的连接段长度；α₁、R₁、θ₁分别表示近端段的旋转角、近端段的弯曲半径和近端段的弯曲角，α₂、R₂、θ₂分别表示远端段的旋转角、远端段的弯曲半径和远端段的弯曲角，α₃表示末端执行器的旋转角；X₁₁、Y₁₁、Z₁₁分别表示第1个坐标系O₁绕其z轴旋转α₁之后得到新坐标系O₁₁的x轴、y轴和z轴，Z₁₂表示坐标系O₁₁沿其z轴运动R₁sinθ₁，再沿其x轴运动R₁(1-cosθ₁)得到的新坐标O₁₂的z轴；X₂₁、Y₂₁、Z₂₁分别表示第2个坐标系O₂绕其z轴旋转π+α₁-π/6之后得到新坐标系O₂₁的x轴、y轴和z轴，Z₂₂表示坐标系O₂₁沿其z轴运动R₁sinθ₁，再沿其x轴运动R₁(1-cosθ₁)得到的新坐标O₂₂的z轴；X₃₁、Y₃₁、Z₃₁分别表示第3个坐标系O₃绕其z轴旋转α₂之后得到新坐标系O₃₁的x轴、y轴和z轴，Z₃₂表示坐标系O₃₁沿其z轴运动R₂sinθ₂，再沿其x轴运动R₂(1-cosθ₂)得到的新坐标O₃₂的z轴。

通过机器人整体运动学关系模型中的

与机器人正运动学末端位姿矩阵T^E的相等关系，得到齐次变换矩阵参数n_z、o_x、o_y、o_z、o_z：

所述通过对机器人正运动学末端位姿矩阵的求逆解得到机器人关节向量，包括以下步骤：

根据齐次变换矩阵参数得到θ₂,α₂,α₃：

再通过机器人整体运动学关系模型中的

与机器人正运动学末端位姿矩阵T^E的相等关系，得到在第0个坐标系O₀(机器人基坐标系)中机器人末端位置为：

L₁、L₃表示近端段与形变段之间的长度、远端段与形变段之间的长度；

进而求出：

其中，P_dx、P_dy、P_dz分别表示机器人远端段在第0个坐标系O₀即机器人基坐标系x、y、z方向上产生的位移；

根据上述计算得到关节向量q＝[d,θ₁,α₁,θ₂,α₂,α₃]^T。

所述将关节向量对驱动关节的驱动变量进行映射，包括以下步骤：

对于每个形变段，将关节变量映射为各个驱动腱所对应的驱动变量，将驱动腱从过第1个关节坐标系O₁ X轴的第一根开始，沿逆时针定义为1,2,3,4；横截面上驱动腱到中心点位置距离为r，得到关节变量到驱动的映射变换为：

其中，L_0,j表示第j段形变段中轴线的长度，R表示形变段的弯曲半径，L_i,j为第j段形变段中第i根驱动腱的长度，ΔL_i,j为第j段形变段中第i根驱动腱的长度变化，也就是电机运动控制所需的驱动量；θ_j表示第1、3个的角度，即近端段的弯曲角、远端段的弯曲角。

各个形变段驱动腱的驱动量，通过控制各个形变段的驱动装置按照所求解的驱动量运动，使机器人到达目标位姿。

一种可运动解耦的连续体机器人控制方法，用于多个形变段的连续体机器人。

对于单个变形段，具有至少两个以上的自由度数目，具有至少三根驱动腱。

所述坐标系O_i+1构建如下：以连续形变段近端端面圆心为原点建立连续形变段的基坐标系O_i，垂直连续形变段端面并指向远端为Z_i，由原点指向驱动腱中的一根为X_i，建立右手坐标系；以连续形变段远端端面圆心为原点建立末端坐标系O_i+1。

本发明具有以下有益效果及优点：

本发明利用一种可运动解耦的连续体机器人构型，通过配置一个中间的被动连续体段，将远端驱动与近端驱动的解耦，同时机器人的末端姿态仅取决于远端形变段，实现了部分位姿分离。可以实现远端连续体段与近端连续体段的驱动解耦，以及末端位姿的分离，并针对该构型的机器人设计了一种可单独求解出机器人逆运动控制的每个驱动变量的直接逆运动学求解算法，简化了控制计算过程。该直接逆运动学求解算法无需多次迭代，稳定可靠，避免了传统的雅克比迭代求运动学逆解中存在的精度不高、不稳定、收敛慢等问题。

附图说明

图1运动解耦连续体机器人构型原理图；

图2单个形变段坐标系及关节变量示意图；

图3运动解耦连续体机器人整体运动学坐标系及关节参数定义示意图；

图4运动解耦连续体机器人的运动控制流程图。

具体实施方式

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

目前的连续体机器人大多采用多段柔性段串联作为运动机构，多段间的驱动与机器人末端位姿运动存在高度耦合，导致其精确控制一直是难以解决的问题。如果能够将连续体机器人的位姿分离，同时将多形变段之间的耦合解除，将极大简化连续体机器人的建模，并且获得良好的控制性能。为此，本发明针对连续体机器人通过配置一个中间的被动形变段，将远端驱动与近端驱动的解耦，同时机器人的末端姿态仅取决于远端形变段，实现了位姿分离。

本实施例中的连续机器人结构与公开号为CN111317570A的中国发明专利中公开的形变联动机构结构基本相同，区别仅在于本实施例近端形变段与远端形变段之间增加了一段联动变形部。一对驱动腱通过驱动装置实现一拉一推，驱动装置为现有技术。

连续体机器人构型由三段依次串联的形变段组成：近端段，联动段，远端段。近端段驱动腱连接到外部直线驱动上，可以主动驱动控制。联动段驱动腱穿过近端段，一端与近端段基座固连，另一端固定在联动段的末端。远端段驱动腱穿过联动段与近端段，连接到外部的直线驱动上，可以主动驱动控制。

对于单个变形段，具有至少两个或更多的自由度数目，具有三根或更多驱动腱，

对于一个给定的目标位姿，该机器人构型可以通过直接求逆解的方法求解出到达该目标位姿所需要的关节变量，并可以根据关节变量求解出各个形变段驱动腱的驱动量，通过控制各个形变段的驱动装置按照所求解的驱动变量运动可以让机器人准确到达该目标位姿。

通过构建该机器人构型的正运动学方法，可以得到该机器人构型末端位姿相对基座标系关于各个关节变量的齐次矩阵表达式，通过对比该齐次矩阵表达式与给定的目标位姿对应关系，可以按照一定步骤直接分别求解出各个关节变量。对于一种典型的六自由度该构型机器人，可以按照以下方法逐步求解出各个关节变量：

单个变形段的关节变量为描述该变形段变形到某个形状时的一组参数，通过该组参数可以计算出该变形段到达所描述的形状所需要的各个驱动腱的驱动量，一种典型的具有四个驱动腱的两自由度变形段的关节变量及其与驱动量间的计算方法如下：

可运动解耦的连续体机器人构型原理如图1所示，包含三段连形变段：近端段S1，联动段(也叫中间段)S2和远端段S3，每个变形段由多根腱驱动。其中近端段驱动腱连接到外部直线驱动上，可以主动驱动控制。中间联动段驱动腱穿过近端段，一端与近端段基座固连，另一端固定在联动段的远端。当近端段发生弯曲形变时，由于联动段的驱动腱总长不变，联动段会被动地朝相反方向弯曲相同角度。因此联动段末端姿态指向始终保持与基座轴向平行，只有位置发生变化。远端段驱动腱穿过联动段与近端段，连接到外部的直线驱动上，可以主动驱动控制。由于近端运动段与联动段是镜像运动，因此远端段驱动腱长度不会受近端段与联动段形状的影响，从而实现了远端驱动与近端驱动的解耦。同时机器人末端指向姿态只由远端段形状决定，可实现部分位姿分离。

对于可运动解耦连续体机器人的运动控制，本发明提供一种典型的基于运动解耦构型的六自由度连续体机器人控制方法。其自由度配置如下：整体直线运动自由度、近端变形段两个自由度、远端变形段两个自由度以及末端绕轴线的旋转自由度。首先需要建立其正运动学模型：

首先对单个变形段建立正运动学，对于一种典型的四根驱动腱驱动的两自由度变形段，采用圆弧对其变形形状进行模拟，可以建立其运动学关系如下：

图2中，以连续体段近端端面圆心为原点建立连续体段的基坐标系O_i，沿连续体段轴向为Z_i，由原点指向四根驱动丝中的一根为X_i，建立右手坐标系。以连续体段远端端面圆心为原点建立末端坐标系O_i+1，当连续体段不发生弯曲时，O_i+1与O_i保持平行。将连续体段的弯曲形状近似等效为圆弧形状，连续体段在与X_iZ_i平面成α角度的平面内弯曲，弯曲的圆心角为θ。α和θ分别为连续体段的两个运动关节变量。其中，i表示坐标系的序号；

基坐标系O_i到末端坐标系O_i+1变换可通过如下过程实现：

(1).O_i绕Z_i旋转α角度，得到坐标系O_i1；

(2).O_i1先沿Z_i1移动Rsinθ，再沿X_i1移动R(1-cosθ)，最后沿Y_i1旋转θ角度，得到坐标系O_i2(R＝L/θ)；R、L分别表示变形段的弯曲半径和变形段的长度；

(3).O_i2绕Z_i2旋转-α角度，得到坐标系O_i+1；

基于以上变换过程可以得到O_i+1在O_i的表达矩阵为：

其中rot(A,B)为绕A坐标轴旋转B角度的齐次坐标矩阵变换表达式，trans(A,B)为沿A坐标轴直线移动B距离的齐次坐标矩阵表达式。

基于单个变形段的运动学可以建立连续体整体的运动学模型，对机器人整体建立如图3所示的坐标系。其中，机器人整体的基坐标系为O₀；近端连续体基座标系为O₁，其在Z₀轴正方向上，定义O₀ O₁间的直线位移为关节变量d。近端段连续体长度为L₁，具有α₁和θ₁两个关节变量；联动段连续体基座标系为O₂，其与近端段连续体由一个长度为L₂的不可变形直段连接，且联动段连续体驱动腱位置与近端段连续体驱动腱位置的圆心角为π/6，因此沿近端连续体末端坐标系Z轴正方向移动L₂且绕其正方向旋转π/6即可得到O₂。联动段连续体长度同样为L₁，由于联动段与近端段弯曲方向相反，因此其具有π+α₁-π/6和θ₁两个关节变量；远端段连续体基座标系为O₃，其与联动段连续体由一个长度为L₂的不可变形直段连接，而且远端段连续体驱动腱位置与联动段连续体驱动腱位置的圆心角为π/6，因此沿联动连续体末端坐标系Z轴正方向移动L₂且绕其正方向旋转π/6即可得到O₃。远端段连续体长度为L₃，具有α₂和θ₂两个关节变量；远端段连续体末端坐标系定义为O₄，末端旋转腕关节坐标系为O₅，距离O₄的长度为L₄，关节变量为α₃。结合式(1)，可得到机器人整体运动学关系如下：

其中，R₁＝L₁/θ₁，R₂＝L₃/θ₂。

而对于每个变形段，需要将关节变量映射为各个驱动腱所对应的驱动变量，将驱动腱从过X轴的第一根开始，沿逆时针定义为1,2,3,4；横截面上驱动腱到中心点位置距离为r，可以容易得到关节变量到驱动驱动的映射变换为：

其中L_i,j为第j段形变段中第i根驱动腱的长度，ΔL_i,j为第j段形变段中第i根驱动腱的长度变化，也就是电机运动控制所需的驱动量。

当给定一个机器人的目标位姿T^E，可以按照如下直接求逆解的方法求解各个关节变量，令：

与机器人正运动学末端位姿矩阵

对比得到：

机器人末端坐标系的姿态只与机器人远端连续体段关节参数有关，若定义柔性段的弯曲角度为θ，θ∈[0,π/2]，根据式(4)可以求出：

再通过对比发现机器人末端位置为：

在式(6)中，p_p是近端和联动段(S1+S2)产生的移动，p_d是远端S3产生的移动。由于α₂和θ₂已经求解出，则p_d为已知，因此可求出：

式(8)中，α₁已在式(7)中求出，因此是一个关于θ₁的一元超越方程，可采用经典的牛顿迭代求解算法求解，至此，能够到达到达给定目标位姿的机器人各个关节变量均可以求解出。而对于两段连续体的驱动变量，可以依照(2)给出的由关节变量到驱动变量的转换公式求解。

因此，对于给定一个机器人目标位姿T^E后，即可采用上述计算方法首先计算对应关节向量q＝[d,θ₁,α₁,θ₂,α₂,α₃]^T，然后根据关节向量可以计算出各个驱动变量L＝[d,ΔL₁₁,ΔL₂₁,ΔL₃₁,ΔL₄₁,ΔL₁₃,ΔL₂₃,ΔL₃₃,ΔL₄₃,α₃]^T，随后控制电机按照相应驱动变量运动即可到达给定的目标位姿。其控制的流程如图4。

Claims (10)

1.一种可运动解耦的连续体机器人控制方法，其特征在于，包括以下步骤：

建立机器人正运动学末端位姿矩阵；

通过对机器人正运动学末端位姿矩阵的求逆解得到机器人关节向量；

将关节向量对驱动关节的驱动变量进行映射；

根据驱动变量对机器人进行控制。

2.根据权利要求1所述的一种可运动解耦的连续体机器人控制方法，其特征在于，所述机器人正运动学末端位姿矩阵如下：

其中，n_x、n_y、n_z、o_x、o_y、o_z、a_x、a_y、a_z、p_x、p_y、p_z均为为齐次变换矩阵参数；其中，p_x、p_y、p_z表示机器人末端位置；n_x、n_y、n_z表示末端x轴的单位矢量；o_x、o_y、o_z表示末端y轴的单位矢量；a_x、a_y、a_z表示末端z轴的单位矢量。

3.根据权利要求1所述的一种可运动解耦的连续体机器人控制方法，其特征在于，建立机器人整体运动学关系模型，具体如下：

其中，

表示第i+1个坐标系O_i+1在第i个坐标系O_i的位姿转换矩阵，i＝0、1、…、4；Z_j表示第j个坐标系O_j的z轴，j＝0、1、…、4；d表示第0个坐标系沿其z轴方向运动的距离；L₂、L₄分别表示近端段与中间段之间的连接段长度、中间段与远端段之间的连接段长度；α₁、R₁、θ₁分别表示近端段的旋转角、近端段的弯曲半径和近端段的弯曲角，α₂、R₂、θ₂分别表示远端段的旋转角、远端段的弯曲半径和远端段的弯曲角，α₃表示末端执行器的旋转角；X₁₁、Y₁₁、Z₁₁分别表示第1个坐标系O₁绕其z轴旋转α₁之后得到新坐标系O₁₁的x轴、y轴和z轴，Z₁₂表示坐标系O₁₁沿其z轴运动R₁sinθ₁，再沿其x轴运动R₁(1-cosθ₁)得到的新坐标O₁₂的z轴；X₂₁、Y₂₁、Z₂₁分别表示第2个坐标系O₂绕其z轴旋转π+α₁-π/6之后得到新坐标系O₂₁的x轴、y轴和z轴，Z₂₂表示坐标系O₂₁沿其z轴运动R₁sinθ₁，再沿其x轴运动R₁(1-cosθ₁)得到的新坐标O₂₂的z轴；X₃₁、Y₃₁、Z₃₁分别表示第3个坐标系O₃绕其z轴旋转α₂之后得到新坐标系O₃₁的x轴、y轴和z轴，Z₃₂表示坐标系O₃₁沿其z轴运动R₂sinθ₂，再沿其x轴运动R₂(1-cosθ₂)得到的新坐标O₃₂的z轴。

4.根据权利要求1、2或3所述的一种可运动解耦的连续体机器人控制方法，其特征在于：通过机器人整体运动学关系模型中的

与机器人正运动学末端位姿矩阵T^E的相等关系，得到齐次变换矩阵参数n_z、o_x、o_y、o_z、a_z：

5.根据权利要求1所述的一种可运动解耦的连续体机器人控制方法，其特征在于，所述通过对机器人正运动学末端位姿矩阵的求逆解得到机器人关节向量，包括以下步骤：

根据齐次变换矩阵参数得到θ₂,α₂,α₃：

再通过机器人整体运动学关系模型中的

与机器人正运动学末端位姿矩阵T^E的相等关系，得到在第0个坐标系O₀(机器人基坐标系)中机器人末端位置为：

L₁、L₃表示近端段与形变段之间的长度、远端段与形变段之间的长度；

进而求出：

其中，P_dx、P_dy、P_dz分别表示机器人远端段在第0个坐标系O₀即机器人基坐标系x、y、z方向上产生的位移；

根据上述计算得到关节向量q＝[d,θ₁,α₁,θ₂,α₂,α₃]^T。

6.根据权利要求1所述的一种可运动解耦的连续体机器人控制方法，其特征在于，所述将关节向量对驱动关节的驱动变量进行映射，包括以下步骤：

对于每个形变段，将关节变量映射为各个驱动腱所对应的驱动变量，将驱动腱从过第1个关节坐标系O₁X轴的第一根开始，沿逆时针定义为1,2,3,4；横截面上驱动腱到中心点位置距离为r，得到关节变量到驱动的映射变换为：

其中，L_0,j表示第j段形变段中轴线的长度，R表示形变段的弯曲半径，L_i,j为第j段形变段中第i根驱动腱的长度，ΔL_i,j为第j段形变段中第i根驱动腱的长度变化，也就是电机运动控制所需的驱动量；θ_j表示第1、3个的角度，即近端段的弯曲角、远端段的弯曲角。

7.根据权利要求1所述的一种可运动解耦的连续体机器人控制方法，其特征在于，各个形变段驱动腱的驱动量，通过控制各个形变段的驱动装置按照所求解的驱动量运动，使机器人到达目标位姿。

8.根据权利要求1所述的一种可运动解耦的连续体机器人控制方法，其特征在于，用于多个形变段的连续体机器人。

9.根据权利要求1所述的一种可运动解耦的连续体机器人控制方法，其特征在于，对于单个变形段，具有至少两个以上的自由度数目，具有至少三根驱动腱。

10.根据权利要求3所述的一种可运动解耦的连续体机器人控制方法，其特征在于：所述坐标系O_i+1构建如下：以连续形变段近端端面圆心为原点建立连续形变段的基坐标系O_i，垂直连续形变段端面并指向远端为Z_i，由原点指向驱动腱中的一根为X_i，建立右手坐标系；以连续形变段远端端面圆心为原点建立末端坐标系O_i+1。

Images