连续体机器人及其运动学模型的修改方法和其控制方法与流程

本公开涉及连续体机器人、连续体机器人的运动学模型的修改方法、以及连续体机器人的控制方法。

背景技术：

连续体机器人由柔性结构的多个可弯曲区段(可弯曲部分)组成，并且其整个形状通过变换或移动可弯曲区段来控制。这种机器人在两个方面优于由刚性连杆构成的机器人。首先，连续体机器人可以在狭窄空间中或在具有分散物体的环境中沿着曲线移动，在这种地方，具有刚性连杆的机器人可能被卡住。其次，由于连续体机器人基本上是柔性的，因此可以在不损坏脆弱的目标对象的情况下操作机器人。因此，在使用具有刚性连杆的机器人的情况下所需的外力检测可能变得不必要。根据这个特征，预期将连续体机器人应用于医疗领域，诸如用于内窥镜或导管的护套，以及应用于危险环境，诸如救援机器人。但是，由于连续体机器人由于其柔性结构而具有无限的自由度，因此难以推导其运动学模型。即，当提供可弯曲区段中的目标位置的数据时，导出适当地计算致动器的驱动量并且呈现要驱动可弯曲区段以实现目标位置的方式的过程是不容易的。

npl1描述了运动学模型的推导，其中假设可弯曲区段的曲率是分段常数。这个方法适用于许多连续体机器人。还有可能通过使用这个运动学模型来计算用于形状控制的致动器的驱动量。另外，在ptl1中公开的技术中，为了导出运动学模型并修改其误差，通过使用在可弯曲区段的端部处的位移和角度传感器在可弯曲区段中执行反馈控制来提高定位控制的精度。

引用列表

专利文献

ptl1

美国专利申请公开no.2013/0300537

非专利文献

npl1

designandkinematicmodelingofconstantcurvaturecontinuumrobots:areview，robertj.websteriii和bryana.jones，theinternationaljournalofroboticsresearch29(13)1661-1683

技术实现要素：

技术问题

但是，使用npl1中描述的运动学模型的方法不考虑建模误差，诸如机器人的摩擦和扭曲，或者用作致动器的线的伸展和收缩，并且形状控制的精度会由于误差而降低。在ptl1中，通过使用磁传感器来补偿三个可弯曲区段的位置，但是，存在在窄直径连续体机器人中难以安装磁传感器的问题。另外，对于磁传感器，检测系统有必要安装在连续体机器人的外部，这会限制连续体机器人的使用。

解决问题的方法

本公开一方面的连续体机器人包括连续体机器人的控制方法，连续体机器人包括能够弯曲的第一可弯曲部分、被设置为与第一可弯曲部分相邻并且能够弯曲的至少一个第二可弯曲部分、连接到第一可弯曲部分的第一线和连接到第二可弯曲部分的第二线，被配置为通过控制第一线和第二线的驱动来控制第一可弯曲部分和第二可弯曲部分的弯曲的控制单元，其中基于考虑到伴随为了使第一可弯曲部分弯曲而对第一线的驱动的第二可弯曲部分的弯曲以及伴随为了使第二可弯曲部分弯曲而对第二线的驱动的第一可弯曲部分的弯曲的运动学模型，控制单元控制第一线和第二线的驱动。可替代地，控制单元控制第一线和第二线的驱动，使得第一可弯曲部分的弯曲目标值通过第一可弯曲部分和第二可弯曲部分的弯曲量的总和来实现。

本公开另一方面的连续体机器人的运动学模型的修改方法包括：通过使用考虑到伴随为了使第一可弯曲部分弯曲而对第一线的驱动的第二可弯曲部分的弯曲以及伴随为了使第二可弯曲部分弯曲而对第二线的驱动的第一可弯曲部分的弯曲的模型，修改运动学模型。可替代地，通过使用用于控制第一线和第二线的驱动的模型使得第一可弯曲部分的弯曲目标值通过第一可弯曲部分和第二可弯曲部分的弯曲量的总和来实现，修改运动学模型。

本公开另一方面的连续体机器人的控制方法包括获得通过将可弯曲部分的目标位置的数据乘以通过使用修改方法的修改值的倒数而修改的目标位置，其中用于修改运动学模型的修改值是通过使用优化技术的算法获得的，其中运动学模型表示可弯曲部分的目标位置的数据与由根据连续体机器人的机制从该数据导出的第一线和第二线的驱动位移得到的可弯曲部分的位置之间的关系，以减小当连续体机器人获得预定曲率时可弯曲部分的目标位置的数据与和可弯曲部分的实际位置相关的测量值之间的误差，并通过根据修改后的目标位置计算的线的驱动位移来控制可弯曲部分。

发明的有益效果

根据本公开的一方面，通过修改在提供连续体机器人的可弯曲部分的目标位置的数据时获得的致动器的驱动量，可以减小关于连续体机器人的可弯曲部分的目标位置的误差。

参考附图，根据示例性实施例的以下描述，本发明的其它特征将变得清楚。

附图说明

[图1]图1图示了根据第一实施例的装置。

[图2]图2图示了根据第一实施例的运动学模型。

[图3]图3图示了根据第一实施例的运动学模型。

[图4]图4图示了根据第一实施例的运动学模型。

[图5]图5是图示根据第一实施例的优化算法的框图。

[图6]图6是图示根据第一实施例的控制系统的框图。

[图7]图7图示了根据第一实施例的位移测量装置。

[图8a]图8a图示了根据第一实施例的实验结果。

[图8b]图8b图示了根据第一实施例的实验结果。

[图8c]图8c图示了根据第一实施例的实验结果。

[图8d]图8d图示了根据第一实施例的实验结果。

[图8e]图8e图示了根据第一实施例的实验结果。

[图8f]图8f图示了根据第一实施例的实验结果。

[图8g]图8g图示了根据第一实施例的实验结果。

[图8h]图8h图示了根据第一实施例的实验结果。

[图8i]图8i图示了根据第一实施例的实验结果。

[图8j]图8j图示了根据第一实施例的实验结果。

[图9a]图9a图示了根据第一实施例的实验结果。

[图9b]图9b图示了根据第一实施例的实验结果。

[图9c]图9c图示了根据第一实施例的实验结果。

[图10a]图10a图示了根据第一实施例的实验结果。

[图10b]图10b图示了根据第一实施例的实验结果。

[图11a]图11a图示了根据第一实施例的实验结果。

[图11b]图11b图示了根据第一实施例的实验结果。

[图12]图12图示了根据第二实施例的跟随领导者控制。

[图13]图13图示了根据第二实施例的运动学模型。

[图14]图14是图示根据第二实施例的控制系统的框图。

[图15a]图15a是图示根据第二实施例的轨迹和机械误差的框图。

[图15b]图15b是图示根据第二实施例的轨迹和机械误差的框图。

[图16a]图16a图示了根据第二实施例的实验结果。

[图16b]图16b图示了根据第二实施例的实验结果。

[图17]图17图示了根据第二实施例的控制指令。

[图18a]图18a图示了根据第二实施例的实验结果。

[图18b]图18b图示了根据第二实施例的实验结果。

[图18c]图18c图示了根据第二实施例的实验结果。

[图18d]图18d图示了根据第二实施例的实验结果。

[图19a]图19a图示了根据第二实施例的实验结果。

[图19b]图19b图示了根据第二实施例的实验结果。

[图19c]图19c图示了根据第二实施例的实验结果。

[图19d]图19d图示了根据第二实施例的实验结果。

[图20]图20是图示根据第三实施例的控制系统的框图。

[图21a]图21a是图示根据第三实施例的轨迹和机械误差的框图。

[图21b]图21b是图示根据第三实施例的轨迹和机械误差的框图。

[图22a]图22a图示了根据第三实施例的优化算法。

[图22b]图22b图示了根据第三实施例的优化算法。

[图23a]图23a图示了根据第三实施例的实验结果。

[图23b]图23b图示了根据第三实施例的实验结果。

[图23c]图23c图示了根据第三实施例的实验结果。

[图23d]图23d图示了根据第三实施例的实验结果。

[图24a]图24a图示了根据第三实施例的实验结果。

[图24b]图24b图示了根据第三实施例的实验结果。

[图24c]图24c图示了根据第三实施例的实验结果。

[图24d]图24d图示了根据第三实施例的实验结果。

具体实施方式

在本公开中，可以通过使用考虑可弯曲部分之间的连续性的模型来修改运动学模型，并且可以根据修改后的运动学模型来配置连续体机器人并且可以配置连续体机器人的控制方法。在该控制方法中，通过在修改后的运动学模型中使用可弯曲部分的目标位置的数据(弯曲目标值)和修改值的倒数(在乘以修改值时产生1的值)获得致动器的驱动量以获得目标位置，来控制可弯曲部分中对应的一个可弯曲部分。不仅先前设置的弯曲量的控制而且通过实时操作的曲率的实时控制都可以通过将曲率的附加目标值顺序地添加到弯曲目标值来执行。这种技术使得能够执行称为跟随领导者控制的控制，并且关于具有非恒定曲率的轨迹的跟随领导者控制也可以通过将修改值的倒数乘以附加增益来执行。

第一实施例

在第一实施例中，除了其中作为可弯曲部分的可弯曲区段的曲率被假设为分段恒定的运动学模型之外，还描述了一种通过引入相邻可弯曲区段之间的连续性的模型来修改上述运动学模型的方法，其中相邻可弯曲区段的形状由多个机械因素引起。另外，将描述通过该方法改进连续体机器人的驱动和控制的方法。连续体机器人的运动学模型呈现可弯曲区段的位置，该可弯曲区段的位置通过当提供可弯曲区段的目标位置的数据时根据连续体机器人的机制从数据导出的致动器(驱动单元)中对应一个致动器的驱动量来实现。

通过实验测量连续体机器人的曲率并通过使用迭代优化技术的算法获得用于修改运动学模型的系数(修改值)来执行运动学模型的修改。可以从运动学修改系数获得运动学修改矩阵，并且通过使用运动学修改矩阵的逆矩阵，可以减小目标形状与由连续体机器人的控制系统实际实现的曲率之间的误差。

图5是用于获得在本实施例中使用的运动学修改系数向量γ的优化算法的框图。p表示连续体机器人，k表示用于获得线的驱动位移量的块，其中每个可弯曲区段的远端的角度作为目标值。首先，设置作为预定代表性位置的第一代表性形状向量θrep[1]，并且通过线位移操作块k获得线的驱动位移lp[1]。接下来，通过应用线的驱动位移来控制机器人p，并且通过使用图7中所示的可弯曲区段的远端的位移测量系统来获得受控机器人的曲率的测量位移xex[1]和zex[1]。通过使用这些测量位移、代表性形状向量θrep[1]和修改系数的初始向量γ0，通过最速下降法操作块sdm获得关于第一代表性形状的修改系数向量γ[1]1。接下来，以相同的方式获得关于第二代表性形状的修改系数γ[2]1。在本实施例中，关于第一代表性形状获得的修改系数γ[1]1被用作关于第二代表性形状的最速下降法的初始值。这避免了将修改系数向量设置为局部最小值。

接下来，针对“a”个类型的代表性形状中的每一个(“a”表示数字)获得修改系数γ[a]1。上面提到的过程是第一试验。然后，执行第二试验，其中将修改系数的初始值定义为关于在第一试验中获得的每个代表性形状的修改系数γ[a]1。因此，第二试验以与第一试验相同的方式进行。试验迭代h次，并且在块cjb中确定修改系数是否充分收敛。通过平均运算块1/(ah)获得所有修改系数的均值，并且将所获得的均值定义为修改系数向量γ。均值可以由通过组合所有修改系数而获得的另一个值代替。例如，另一个值可以是中值、考虑到取决于代表性形状的模式而确定的权重的加权均值。可以使用提供充分修改效果的任何组合值。修改系数γ[a]1等可以用作初始值，用于通过使用多个代表性形状(位置)中的至少一个中的修改结果来修改另一个代表性形状(位置)中的运动学模型。

在下文中，将详细描述连续体机器人的运动学模型的推导、修改系数优化算法和控制系统，并且将描述通过实验获得的控制结果。

(1.1连续体机器人的建模)

(1.1.1运动学模型的推导)

图1图示了在本实施例中使用的连续体机器人100的示意图，其包括六个可弯曲区段101至106。在连续体机器人100中，驱动线111至116分别连接到可弯曲区段101至106的远端121至126，并且通过由分别安装在机器人基部140中的致动器131至136推拉线111至116来控制可弯曲区段101至106的位置。在图1所示的示例中，驱动线111至116在中心轴的相对侧上交替地连接到可弯曲区段101至106的远端121至126，但是，线连接到可弯曲区段的位置不限于此。如果可以通过推拉线来控制可弯曲区段的位置，那么可以任意确定连接位置。基部140在z轴方向上具有自由度并且可以检测位移。图2是由壳体和线111组成的第一弯曲区段101的结构的示意图。在图1和2中，壳体的中心轴线由断线或点划线描绘。连续体机器人还包括控制单元(未示出)。当输入目标位置时，控制单元输出用于通过致动器控制对应的一根线的驱动的信号，使得每个可弯曲区段获得目标位置。即，控制单元根据稍后将描述的运动学模型控制第一线和第二线的驱动。目标位置可以由用户输入，或者可以根据由目标位置输入单元(未示出)预先存储的程序输入。另外，目标位置可以作为每个可弯曲区段的角度被输入。例如，控制单元可以由加载和执行程序的计算机中的一个或多个处理器实现，或者可以被实现为专用电路(fpga和asic)。

以下描述中的标号的定义如下。ln：第n个可弯曲区段的臂壳体长度；rn：从臂壳体的中心轴线到第n个可弯曲区段中的线的位移；e：机器人的可弯曲区段的数量；θn：第n个可弯曲区段的远端的角度；ρn：第n个可弯曲区段的曲率半径；θrefn：第n个可弯曲区段的远端的目标角度；lpn：第n个可弯曲区段中线的驱动位移；xtn、ztn：第n个可弯曲区段的远端的坐标；c：机器人的评估点；xi、zi：机器人在纵向上被分成c时的第i个坐标；以及zb：基部的位移。

基于以下假设导出图3中所示的具有n个可弯曲区段的连续体机器人的运动学模型(也参见图4)。

1.壳体和线仅在绘图平面上水平变形。

2.在每个可弯曲区段中，壳体和线以恒定的曲率变形。

3.不考虑壳体和线的扭曲变形。

4.壳体和线不在纵向方向上变形。

5.导线器(wireguide)在壳体中提供，并且壳体的中心轴线的圆弧的中心和由线制成的圆弧的中心始终彼此重合。

6.不考虑壳体和线之间的摩擦。

首先，导出线的驱动位移与可弯曲区段的远端的角度之间的关系。仅考虑第一弯曲区段，线的驱动量lp1与第一弯曲区段的远端的角度θ1之间的关系由表达式(1)表达。在这里，由于表达式(2)成立，因此从表达式(1)和(2)获得表达式(3)。接下来，导出线的驱动位移lpn与第n个可弯曲区段的远端的角度θn之间的关系。在这里，n为2或更大。作为第n个可弯曲区段中的弯曲相对角度的θn(带波浪号的θ)由表达式(4)定义。如图3中所示，建立具有原点(xtn-1，ztn-1)并且具有在n-θ1方向和与n-θ1方向正交的方向上延伸的坐标轴的相对坐标系xn-zn。然后，作为相对坐标系xn-zn中的线的驱动位移的lpn(带波浪号的l)与作为第n个可弯曲区段的远端的相对角度的θn(带波浪号的θ)之间的关系由表达式(5)表达。在第n个可弯曲区段中的线的驱动位移lpn变为用于在相对坐标系中从第一到第(n-1)个区段驱动第n个可弯曲区段的线的位移的总和，并且由表达式(6)表达。

(ρ1-r1)θ1+lp1＝l1(1)

l1＝ρ1θ1(2)

lp1＝r1θ1(3)

[数学表达式1]

[数学表达式2]

[数学表达式3]

这指示第n个可弯曲区段的远端的角度θn仅根据线的驱动位移lpn而不根据第一到第(n-1)个区段的角度来确定。

接下来，导出第n个可弯曲区段的远端处的角度与远端处的坐标之间的关系。首先，将考虑第一个弯曲区段。如果ρ被定义为曲率半径，那么第一弯曲区段的远端的坐标(xt1，zt1)由表达式(7)和(8)表达。当表达式(2)被代入表达式(7)和(8)时，给出表达式(9)和(10)。在这里，导出远端处的角度与第n个可弯曲区段的远端处的坐标之间的关系。在这里，n为2或更大。作为相对坐标系xn-zn中可弯曲区段的远端的坐标的xtn(带波浪号的x)和ztn(带波浪号的z)由表达式(11)和(12)表达。

因此，绝对坐标系中远端的坐标(xtn，ztn)通过使用旋转变换矩阵由表达式(13)表达。在以下一节中，将整个机器人划分为α次的坐标用作优化算法的评估点。评估点的总数是c＝αe，并且第i个评估点的坐标(xi，zi)由表达式(14)给出。q是通过q＝[i/α]获得的商，并且r是通过r＝imodα获得的余数。

xt1＝ρ1(1-cosθ1)(7)

zt1＝ρ1sinθ1(8)

[数学表达式4]

[数学表达式5]

[数学表达式6]

[数学表达式7]

[数学表达式8]

[数学表达式9]

(1.1.2通过相邻区段连续性模型的运动学模型修改)

在前一节中，基于1至5的假设推导出运动学模型，但是，在结构设计中在弯曲方向上柔软并且在纵向方向上高度刚硬的机器人的壳体和线是困难的，因此实际上很难满足假设3和4。因此，相对于线的驱动位移的弯曲角度的关系表达式(6)不成立，并且在控制后的实际弯曲角度与目标角度之间将产生误差。这种影响不仅会在线被驱动的可弯曲区段中而且还在其它区段中造成弯曲角度的误差，并且会产生连续性问题。于是，在本实施例中，添加了与可弯曲区段的连续性相关的以下假设，并且修改了运动学误差。

当将第n个可弯曲区段驱动到弯曲角度θn(带波浪号的θ)时，发生以下连续性。即，第(n-1)个可弯曲区段和第(n+1)个可弯曲区段的弯曲角度分别增加γpn*(θn)(带波浪号的θ)和γdn*(θn)(带波浪号的θ)，第n个可弯曲区段的弯曲角度减小了(γpn+γdn)*(θn)(带有波浪号的θ)。在这里，γpn和γdn是修改系数。即，通过将第i个可弯曲部分中的曲线相对角度乘以系数γp[i]而获得的角度被加到相邻的第(i-1)个可弯曲部分中的曲线相对角度。另外，通过将第i个可弯曲部分中的曲线相对角度乘以另一个系数γd[i]而获得的角度被加到相邻的第(i+1)个可弯曲部分中的曲线相对角度，并且通过将第i个可弯曲部分中的弯曲相对角度乘以(γp[i]+γd[i])而获得的角度从第i个可弯曲部分中的弯曲相对角度减去。通过使用这种模型获得修改值(在这里，i为2或更大并且等于或小于(可弯曲部分的数量)-1)。要基于这个假设修改的第n个可弯曲区段中的弯曲角度被定义为θgn(具有波浪号的θ)并且由表达式(15)表达。在作为最近端的第一弯曲区段中，弯曲角度由表达式(16)表达，并且在作为最远端的第e个可弯曲区段中，弯曲角度由表达式(17)表达。这些表达式可以通过表达式(18)表达为矩阵，表达式(18)在本实施例中被定义为修改矩阵γ。在修改矩阵γ中，第i行第i列被定义为1-γd[i]-γp[i]，第i行第(i-1)列被定义为γd[i-1]]，并且第i行第(i+1)列被定义为γp[i+1]。

由运动学修改系数构成的向量γ由表达式(19)定义，并且被称为修改系数向量。因此，要修改的可弯曲区段的每个远端的坐标(xgtn，zgtn)由表达式(20)表达。在用于将整个机器人划分为α次的第i个评估点的坐标(xgi，zgi)中，θ和θ(带波浪号的θ)可以分别在表达式(14)中由θg和θg(带波浪号的θ)代替。因此，将省略其描述。

[数学表达式10]

[数学表达式11]

[数学表达式12]

[数学表达式13]

[数学表达式14]

γ＝[γp2…γpeγd1…γde-1](19)

[数学表达式15]

(1.1.3修改系数的优化)

难以分析地获得前一节中描述的运动学修改系数。这是因为弯曲角度的连续性是由于在纵向上壳体和线的变形引起的，这种变形是由于压缩力和张力，并且压缩力和张力由于穿过每个可弯曲区段的线的不同数量而变化，或者壳体的刚度取决于弯曲角度而非线性地变化。另外，假设1、2、5和6不完全满足，并且这会造成弯曲角度的误差。于是，在本实施例中，通过实验测量曲率，并且通过使用作为优化方法的最速下降法从测量值与运动学模型的值之间的差异来获得修改系数γ。由于不能选择合适的一个曲率来优化修改系数γ，因此在本实施例中通过使用a个类型(a是多个)的代表性形状迭代h次(h是多个)来执行优化。这种方法被称为扩展的最速下降法。

扩展的最速下降法的算法在图5中作为框图示出。框图中的标号定义如下：[k]表示第k个(小于或等于a)代表性形状，并且j表示第j次(小于或等于h)迭代试验。

第k个代表性形状向量θrep[k]由表达式(21)表达，线相对于第k个代表性形状的驱动位移lp[k]由表达式(22)表达，并且机器人的测量位移xex[k]和zex[k]分别由表达式(23)和(24)表达。sdm表示最速下降法算法，并且是用于获得修改系数向量γ[k]j的块，其通过优化来最小化由表达式(25)表达的评估函数。修改系数向量γ[k]j是相对于第k个代表性形状的迭代次数j的修改系数向量。

如上所述，为了通过使用这些块通过扩展的最速下降法获得修改系数向量γ，首先，设置第一代表性形状向量θrep[1]并且通过操作表达式(6)的线位移块k获得线的驱动位移lp[1]。接下来，通过施加线的驱动位移来控制机器人p，并且获得受控机器人的曲率的测量位移xex[1]和zex[]1。通过使用这些测量位移、代表性形状θrep[1]和修改系数的初始向量γ0，通过最速下降法操作块sdm获得关于第一代表性形状的修改系数向量γ[1]1。接下来，以相同的方式获得关于第二代表性形状的修改系数γ[2]1。在本实施例中，关于第一代表性形状获得的修改系数γ[1]1被用作关于第二代表性形状的最速下降法的初始值。这避免了将修改系数向量设置为局部最小值。然后，如图5中所示，针对a个类型的代表性形状中的每一个获得修改系数γ[k]1。上面提到的过程是第一迭代试验。接下来，执行第二迭代试验，其中将修改系数的初始值定义为在第一迭代试验中获得的修改系数。因此，第二试验以与第一试验相同的方式进行。试验迭代h次，并在块cjb中确定修改系数是否充分收敛。可替代地，可以通过试错法预先确定迭代次数。通过表达式(26)获得所有修改系数向量的均值，并且将所获得的均值定义为修改系数γ。

[数学表达式16]

θrep[k]＝[θrep1[k]θrep2[k]…θrepe[k]]^t(21)

[数学表达式17]

lp[k]＝[lp1[k]lp2[k]…lpe[k]]^t(22)

[数学表达式18]

xex[k]＝[xex1[k]xex2[k]…xexc[k]]^t(23)

[数学表达式19]

zex[k]＝[zex1[k]zex2[k]…zexc[k]]^t(24)

[数学表达式20]

[数学表达式21]

(1.2控制系统设计)

运动学修改系数不仅适用于修改机器人的运动学模型及其曲率的计算，而且还适用于控制曲率。由相对于第n个可弯曲区段的目标角度θrefn组成的目标曲率向量θref由表达式(27)定义，并且由相对于第n个可弯曲区段的修改目标角度θmodn组成的修改曲率向量θmod由表达式(28)定义。

为了将机器人控制到目标曲率，通过使用修改矩阵γ的逆矩阵，如表达式(29)所表达的那样获得修改曲率向量。将修改目标角度转换为绝对坐标系，并将其代入表达式(6)的角度θ，以获得线的驱动位移。在本实施例的控制系统中，运动学修改不仅适用于控制先前设置的曲率，而且还适用于通过将曲率的附加目标向量θcom添加到目标曲率向量而通过实时操作控制曲率。这个控制系统的框图在图6中不出。

[数学表达式22]

θref＝[θref1θref2…θrefe]^t(27)

[数学表达式23]

θmod＝[θmod1θmod2…θmode]^t(28)

[数学表达式24]

(1.3实验)

在这一节中，将描述使用利用第二和第三节说明的运动学模型的修改系数γ来修改运动学模型的有效性以及曲率控制。在实验中使用的机器人的参数是臂壳体的长度l1至l6＝0.010m，并且机器人的可弯曲区段的数量e＝6。从臂壳体的中心轴线到第n个可弯曲区段中的线的位移是r1＝r3＝1.32*10^-3m、r2＝r4＝-1.32*10^-3m、r5＝1.4*10^-3m以及r6＝-1.4*l0^-3m。在该实验中，如图7中所示，标记201至206在连续体机器人100的可弯曲区段的远端处提供，并且每个可弯曲区段中的远端的位移(由表达式(30)表达)由图像拾取设备210获得。在扩展的最速下降法中，评估点c被定义为c＝60。因此，基于可弯曲区段具有恒定曲率的假设，通过内插获得除可弯曲区段中的远端以外的评估点。作为代表性的弯曲位置，选择k＝2种类型的位置，即，所有可弯曲区段均匀弯曲的形状和具有由表达式(31)和(32)表达的两个弯曲点的形状。

在扩展的最速下降法算法中，迭代次数h被定义为h＝10，并且修改系数γ是正数。因此，如果元素变为负，那么将值设置为0.1并继续优化。除了代表性曲率之外，还准备由表达式(33)表达的用于评估运动学模型的修改和控制性能的形状θcit。

图8a至8j图示了通过(1.1.3)一节中描述的扩展的最速下降法算法相对于迭代试验次数的对修改系数γ的每个元素优化的响应。图8a至8e分别图示了修改系数γp2至p6，并且图8f至8j分别图示了修改系数γd1至d5。指示修改系数γp3、γp5、γd2和γd4在约5次的迭代中收敛。γp4和γd3是振动的，但是表现出收敛的趋势。其余的修改系数是振动的，但是，指示通过本实施例的方法，超过一半的修改系数避免了局部最小值。通过代入表达式(26)获得的修改系数向量是表达式(34)。

[数学表达式25]

xed＝[xed1…xed6]^t，zed＝[zed1…zed6]^t(30)

[数学表达式26]

[数学表达式27]

θcit＝[-0.38440.24760.90441.04720.66570.2718]^t(33)

γ＝[0.10480.06490.13460.06000.04650.03490.01500.01590.10880.1511](34)

图9a至9c图示了通过使用表达式(34)修改运动学模型的弯曲位置响应。图9a和9b分别图示了由表达式(31)和(32)表达的代表性形状的响应，并且图9c图示了由表达式(33)表达的用于评估的形状的响应。为了比较，通过修改后的运动学模型的响应用实线描绘，通过实验测得的可弯曲区段远端的位移用星号描绘，并且修改前运动学模型的响应用虚线描绘。如图9a和9b中所示，指示虽然实验响应与基于(1.1.1)一节的假设1至6导出的未经修改的运动学模型的响应之间引起差异，但是通过修改通过使用实验响应来优化修改系数γ的运动学模型,相对于实验响应的差异可以减小。另外，如图9c中所示，指示由于运动学模型在针对不用于修改系数γ的优化的评估的曲率中也可以以与代表性形状的精度基本相同的精度进行修改，因此通过使用多个代表性形状执行多个迭代操作的扩展的最速下降法是有效的。

接下来，将描述控制系统(下文中，称为“提出的控制系统”)使用在(1.2)一节中描述的修改矩阵γ的响应。图10a和10b图示了哪个目标曲率是由表达式(31)表达的第一代表性形状的响应，并且图11a和11b图示了哪个目标曲率是由表达式(33)表达的用于评估的形状的响应。在图10a和11a中，为了比较，由提出的控制系统在可弯曲区段的远端处的响应由圆形标记描绘，由不使用修改矩阵γ的控制系统(在下文中，称为“常规控制系统”)的响应用星号表示，并且目标曲率用断线描绘。另外，在目标曲率的可弯曲区段中的远端处描绘了点(在下文中，称为“远端处的目标坐标”)。另外，在图10b和11b中关于每个可弯曲区段中的每个远端处的目标曲率与控制响应的位移之间的差异，提出的控制系统的响应的差异用实线表示，并且常规控制系统的响应的差异用断线表示。指示提出的控制系统通过使用修改矩阵γ的逆矩阵修改线的驱动位移的控制量减小目标曲率与曲率之间的差异。在代表性形状和用于评估的形状中，减少差异的性能的差别不大。这指示使用多个代表性形状的修改系数的优化技术是有效的。

根据本实施例，除了连续体机器人的运动学模型之外，还通过考虑相邻可弯曲区段(可弯曲部分)的连续性以呈现模型来修改运动学模型。该模型涉及用于修改运动学模型的修改值(修改系数、修改系数向量和修改矩阵)的推导。为了获得修改值，首先，使连续体机器人根据未经修改的运动学模型获得代表性形状，并且测量那个可弯曲区段中的任意位置(例如，远端)处的实际位移。然后，使用利用该模型消除目标值与测量值之间的差异的优化算法。另外，通过修改当通过使用这个运动学模型的修改值的倒数(诸如逆矩阵)提供可弯曲区段中的目标位置的数据时所获得的致动器的驱动量，减小连续体机器人的可弯曲区段中的目标位置与实际位置之间的差异。通过使用修改值，可以按预期进一步驱动和控制连续体机器人。

[第二实施例]

在第二实施例中，本公开应用于跟随领导者控制。如图12中所示，跟随领导者控制是控制后续的可弯曲区段以通过与最远端的可弯曲区段经过的轨迹相同的轨迹。这使得连续体机器人能够在狭窄的空间中向前移动而不会卡住。

(2.1跟随领导者控制的目标角度的优化)

在跟随领导者控制中没有必要预先定义轨迹，但是，最远端的弯曲角度可以以一定的时间差连续地传播到后续可弯曲区段。但是，如果如图12中的断线所描绘的那样预先定义整个轨迹，那么可以通过取决于基部的z方向上的位移优化曲率来执行控制。与其中传播弯曲角度的跟随领导者控制相比，这可以减少连续体机器人中的轨迹误差。该过程将在下面描述。

在本实施例中，如图13中的点划线所示，将描述轨迹与机器人的总长度相同的示例。首先，如图13中的断线所示，将切线添加到轨迹的近端，并且在其终点处建立坐标系的原点。切线的长度可以被设置为与机器人的总长度相同。在本实施例中，所添加的切线和轨迹被称为整个轨迹。接下来，例如，通过恒定速度样条插值将整个轨迹划分为2c个相等的区段，并且在划分的节点处设置轨迹评估点p1(xtr1，ztr1)至p2c(xtr2c，ztr2c)。与第一实施例中一样，连续体机器人在纵向上被划分为c个相等的部分，并且划分的点被定义为评估点。在跟随领导者控制中，基部的位移从zb＝ztr1＝0m开始，并在zb＝ztrc＝nlm处完成(在这里，n是e或更小的整数，l是被假设长度相同的可弯曲区段的长度)。

然后，为了获得随着基部位置向前移动而符合整个轨迹的连续体机器人的曲率，采取以下过程。用轨迹评估点pζ(1<ζ小于或等于c)从整个轨迹中提取c轨迹评估点作为起点，并且连续体机器人的c轨迹评估点与c评估点之间的距离之和被定义为评估函数(由表达式(35)表达)。然后，通过最速下降法获得使评估函数最小化的弯曲角度目标值向量。如果当基部位于第ζ个轨迹评估点pζ时的弯曲目标角度被定义为θrefζ，那么第e行第c列的弯道目标角度矩阵θref(由表达式(36)表达)可以为上述过程中的跟随领导者控制获得。

由于可弯曲区段的数量是有限的，因此表达式(35)的评估函数在一些轨迹中不一定变为0。然后，将通过优化获得的弯曲角度目标值的形状与整个轨迹的形状之间的差异定义为本实施例中的机械误差，并且将弯曲角度目标值与由线驱动控制的弯曲角度之间的差异定义为控制误差。

如果整个轨迹比机器人短，那么可以通过使用上述算法通过以下方式生成弯曲角度目标值：(1)延伸切线以添加到最近端，或者(2)将切线添加到远端。在(1)中，要添加到轨迹的最近端的切线的长度可以被设置为(机器人的总长度+机器人的总长度-轨迹的长度)。在(2)中，可以将切线添加到轨迹的远端并将长度设置为(总长度-机器人的轨迹的长度)。在(2)中，跟随领导者控制需要在基部的位移变得等于轨迹的长度的坐标处完成。

(2.2运动学模型的修改)

虽然可以在跟随领导者控制中采用各种轨迹，但是目标曲率随着基部位置也相对于单个轨迹向前移动而改变。难以选择一个合适的曲率作为代表性形状，并且选择所有形状作为代表性形状需要非常长的时间来进行优化。然后，同样在跟随领导者控制中，通过使用如第一实施例中的a个类型的代表性形状的扩展的最速下降法来优化运动学修改系数γ。

(2.3控制系统设计)

在跟随领导者控制中，可以取决于基部的位移zb从如(2.1)一节中描述获得的曲线目标角度矩阵θref中提取目标曲率向量θrefζ，可以通过使用与第一实施例中一样的修改矩阵γ的逆矩阵获得修改曲率向量，并且可以获得线的驱动位移。在跟随领导者控制中，运动学修改还适用于通过将曲率的附加目标向量θcom添加到目标曲率向量来以实时操作控制曲率。控制系统的框图在图14中示出。

(2.4实验)

(2.4.1运动学修改的评估)

将验证通过扩展的最速下降法相对于跟随领导者控制的运动学修改算法的有效性。使用由图15a中的断线描绘的c形轨迹和由图15b中的断线描绘的s形轨迹。可弯曲区段的数量被定义为e＝6，并且通过使用表达式(35)相对于基部的位移进行优化而获得的所有曲率被叠加为灰线。指示灰色区域与断线之间的偏差变为机械差异，并且最大机械差异出现在作为轨迹的入口的坐标(0，n1)附近。由扩展的最速下降法使用的代表性形状是由表达式(31)和(32)表达的两个形状，如第一实施例中那样。因此，修改系数向量γ与第一实施例的相同。

在图16a和16b中分别图示了在通过实验的曲率与关于图15a和15b的轨迹在跟随领导者控制中根据运动学模型的曲率之间的差异响应。由于在本节中验证了运动学模型的修改，因此不应用(2.3)一节的线补偿的驱动位移。在本实施例中，在基部zb的位移中，通过实验的曲率与根据运动学模型的形状之间的运动学差异被定义为由表达式(37)表达的可弯曲区段的远端的位移的差异之和。

[数学表达式28]

[数学表达式29]

[数学表达式30]

根据通过扩展的最速下降法获得的修改矩阵γcs的运动学模型的差异由实线描绘，而根据用于比较的未经修改的运动学模型的误差由点线描绘。另外，在本实施例中，为了通过迭代指示修改系数向量的优化的有效性，根据仅通过使用表达式(31)的代表性形状而不执行迭代所获得的修改矩阵γc的运动学误差由断线描绘。根据使用由表达式(31)和(32)表达的两种类型的代表性形状而不执行迭代所获得的修改矩阵γs的运动学误差由点划线描绘。图16a和16b都图示了与根据未修改的运动学模型的响应相比，根据修改矩阵γcs的响应在跟随领导者控制的所有区段中都减小了运动学误差。另外，当基部的位移为zb＝0.045m至0.06m时，在整个机器人进入轨迹并且所有区段都是弯曲的情况下，运动学误差在区段中最大程度地减小。但是，当基部的位移zb＝0m至0.022m时，与未经修改的运动学模型相比，根据修改矩阵γc的响应具有更大的误差。另外，当基部的位移zb＝0m至0.013m时，与未经修改的运动学模型相比，根据修改矩阵γs的响应具有更大的误差。这指示通过本实施例中的扩展的最速下降法的修改系数优化算法对由跟随引导控制伴随的运动学修改是有效的。

(2.4.2跟随领导者控制响应)

接下来，将描述使用在(2.3)一节中描述的修改矩阵γ的跟随领导者控制系统(在下文中，称为“提出的跟随领导者控制系统”)的响应。轨迹是由图15a中的断线描绘的c形轨迹和图15b中的断线描绘的s形轨迹，如上一节中的那些。图17的实线和断线如下。实线描绘了与由提出的跟随领导者控制系统相对于图15a的轨迹计算的基部的位移zb对应的每个可弯曲区段的校正目标角度θmod1至θmod6中的对应的一个。断线描绘了由未经修改的控制系统(在下文中，称为“常规的跟随领导者控制系统”)获得的目标角度θref1至θref6中的对应的一个。例如，在第三可弯曲区段中，当第四可弯曲区段在zb＝0.02m处进入轨迹时开始弯曲时，第三可弯曲区段导致弯曲的连续性并且在正方向上弯曲，从而在常规的跟随领导者控制系统中发生轨迹误差。为了减小轨迹误差，指示提出的跟随领导者控制系统修改目标角度，使得第三可弯曲区段的弯曲角度在zb＝0.03m至0.04m内变为负。

图18a至18d图示了跟随领导者控制相对于c形轨迹的实验响应，并且图19a至19d图示了跟随领导者控制相对于s形轨迹的实验响应。图18a和19a各自图示了相对于基部的位移zb在每个可弯曲区段的远端处的坐标与远端处的目标坐标之间的差异距离。图18b和19b各自图示了所有可弯曲区段的距离差异的总和。图18c和18d各自图示了基部的位移zb＝0.03m和0.06m处的曲率。为了比较，由提出的跟随领导者控制系统对可弯曲区段的远端的响应由圆形标记描绘，由常规的跟随领导者控制系统对可弯曲区段的远端的响应由x标记描绘：基部的位移由方形标记描绘，并且整个轨迹由断线描绘。

图19c和19d中的标记是相同的。

图18a中所示的相对于c形轨迹的响应指示，与常规的跟随领导者控制系统相比，在提出的跟随领导者控制系统中，第六可弯曲区段中的距离差异在基部的位移zb＝0.02m附近变得稍微更大。在图18b所示的距离差异的总和中，在基部的位移zb＝0.02m至0.06m的区段中，相对于常规的跟随领导者控制系统，差异几乎减小到一半。图18c指示根据提出的跟随领导者控制系统的距离差异很小，但是，在常规的跟随领导者控制系统中，第三和第四可弯曲区段朝着轨迹的曲率中心偏离。在到达图18d的轨迹终止时的形状中，指示在常规的跟随领导者控制系统中，第五和第六可弯曲区段朝着轨迹的曲率中心的相对侧偏离。

图19a中所示的相对于s形轨迹的响应指示，与常规的跟随领导者控制系统相比，在提出的跟随领导者控制系统中，在第六可弯曲区段中基部的位移zb＝0.04m附近并且在第四可弯曲区段中基部的位移zb＝0.06m附近的距离差异更大。但是，在图19a所示的距离差异的总和中，相对于常规的跟随领导者控制系统，在基部的位移zb＝0.02m至0.058m的区段中差异显著减小。图19c指示根据提出的跟随领导者控制系统的距离差异小，但是，在常规的跟随领导者控制系统中，第三到第五可弯曲区段朝着轨迹的曲率中心偏离。在到达图18d的轨迹终止时的形状中，指示在常规的跟随领导者控制系统中，第六可弯曲区段朝着轨迹的曲率中心的相对侧偏离。因此，与常规的跟随领导者控制系统相比，指示根据使用多个代表性形状的修改矩阵γcs的提出的跟随领导者控制系统具有与目标角度更小的距离差异，并且能够减少与轨迹的偏离。另外，由于误差减小性能不限于轨迹中的具体轨迹或具体区段，因此指示提出的跟随领导者控制系统对于连续体机器人的跟随领导者控制是有效的。

第三实施例

在第二实施例中，轨迹具有弯曲点，而曲率是恒定的。在本实施例中，对曲率不恒定的轨迹执行跟随领导者控制。在上述实施例中使用的相邻区段连续性模型的修改系数相对于弯曲角度的变化是恒定的，但是，在实践中，可弯曲区段的连续性根据弯曲角度非线性地变化，并且连续性的影响相对于弯曲角度的增加趋于饱和。因此，在具有曲率大的部分的轨迹中，修改会变得过度。于是，本实施例描述了一种控制系统，其中用于修改运动学模型的矩阵的逆矩阵γ^-1乘以附加增益gm，如图20所示。

首先，通过图21a和21b中的断线图示本实施例中要讨论的轨迹。如在第二实施例中那样，可弯曲区段的数量被定义为e＝6，并且通过使用表达式(35)作为评估函数的优化而获得的所有曲率被叠加为灰线。由于在本实施例中机器人的目标弯曲角度的最大值被限制为60度，因此在图21b所示的轨迹中机械误差大。在本实施例中，通过使用图21a的轨迹获得附加增益gm。作为过程，首先将运动学修改系数乘以附加增益，并获得相对于跟随领导者控制的基部的位移而对运动学模型进行了修改的曲率，然后，运动学误差eki(zb)根据如第二实施例中的实验获得。接下来，利用运动学误差eki(zb)的整个轨迹中的均值和标准偏差作为评估指标来确定适当的附加增益gm。图22a图示了运动学误差的响应，其将附加增益gm按0.1为单位从0到1改变。指示在未经修改的运动学模型的附加增益gm＝0的响应中，轨迹终止处的运动学误差大。指示轨迹终止处的运动学误差随着附加增益gm接近1而减小，而运动学误差在基部的位移zb＝0.04mm附近增加。图22b图示了响应，其中在水平轴上绘制附加增益gm，并且在垂直轴上绘制运动学误差的均值和标准偏差。图22b图示了运动学误差的均值在附加增益gm＝0.6时变为最小，并且运动学误差的标准偏差在附加增益gm＝0.9时变为最小。例如，当图像拾取装置安装在机器人的最远端时，可以通过减小运动学误差的标准偏差来减少捕获图像的模糊。考虑到上面提到的情况，在本实施例中，附加增益gm被确定为0.7。

接下来，将描述根据使用附加增益的跟随领导者控制系统(在下文中，称为“经修改的跟随领导者控制系统”)的响应。图23a、23b、23c和23d图示了跟随领导者控制相对于图21a的轨迹的实验响应。图24a、24b、24c和24d图示了跟随领导者控制相对于图21b的轨迹的实验响应。轴的含义和每个图的标号与第二实施例的那些相同。

图23a所示的响应指示，在基部的位移zb＝0.02米至0.04米处，在经修改的跟随领导者控制系统中，第六可弯曲区段中的距离误差略大于常规的跟随领导者控制系统的距离误差。在图23b所示的距离误差之和中，在基部的位移zb＝0.045m到0.06m的区段中，与常规跟随领导者控制系统中的差异相比，经修改的跟随领导者控制系统中的差异减小。图23c和23d图示了在经修改的跟随领导者控制系统与常规的跟随领导者控制系统之间在基部的位移zb＝0.03m时的性能没有大的差异，而在到达轨迹终止时的形状中，经修改的跟随领导者控制系统减小了第六可弯曲区段中的距离差异。

图24a和24b中所示的响应指示，与常规的跟随领导者控制系统相比，经修改的跟随领导者控制系统显著地减小了所有可弯曲区段中的距离差异。图24c和24d图示了在经修改的跟随领导者控制系统与常规的跟随领导者控制系统之间在基部的位移zb＝0.03m时的性能没有大的差异，而在到达轨迹终止时的形状中，经修改的跟随领导者控制系统减小了第五和第六可弯曲区段中的距离差异。

因此，指示在相对于具有恒定曲率的轨迹的跟随领导者控制中，与常规的跟随领导者控制系统相比，相对于目标角度的距离差异能够变更小并且相对于轨迹的偏差能够通过引入附加的增益来减小。另外，由于差异减小性能不限于轨迹中基部的具体轨迹或具体位移，因此指示提出的跟随领导者控制系统对于连续体机器人的跟随领导者控制是有效的。

其它实施例

本发明的(一个或多个)实施例还可以通过读出并执行记录在存储介质(其也可以被更完整地称为“非瞬态计算机可读存储介质”)上的计算机可执行指令(例如，一个或多个程序)以执行上述(一个或多个)实施例中的一个或多个实施例的功能和/或包括用于执行上述(一个或多个)实施例中的一个或多个实施例的功能的一个或多个电路(例如，专用集成电路(asic))的系统或装置的计算机来实现，以及通过由系统或装置的计算机例如从存储介质读出并执行计算机可执行指令以执行上述(一个或多个)实施例中的一个或多个实施例的功能和/或控制一个或多个电路执行上述(一个或多个)实施例中的一个或多个实施例的功能而执行的方法来实现。计算机可以包括一个或多个处理器(例如，中央处理单元(cpu)、微处理单元(mpu))，并且可以包括单独计算机或单独处理器的网络，以读出并执行计算机可执行指令。计算机可执行指令可以例如从网络或存储介质提供给计算机。存储介质可以包括例如硬盘、随机存取存储器(ram)、只读存储器(rom)、分布式计算系统的存储装置、光盘(诸如紧凑盘(cd)、数字多功能盘(dvd)或蓝光盘(bd)^tm)、闪存设备、存储卡等中的一个或多个。

虽然已经参考示例性实施例描述了本发明，但是应当理解的是，本发明不限于所公开的示例性实施例。所附权利要求的范围将被赋予最广泛的解释，以便涵盖所有此类修改以及等效的结构和功能。

本申请要求于2016年7月13日提交的日本专利申请no.2016-138133的权益，由此该申请通过引用而整体并入本文。