连续体机器臂末端位置的控制方法、系统及机器臂系统
1.本发明涉及机器人技术领域，更具体地，涉及一种连续体机器臂末端位置的控制方法、系统及机器臂系统。
背景技术：
2.连续体机器人是一种基于章鱼触手、象鼻等生物器官仿生的新型机器人，其不具有刚性连杆和离散关节，因而具有优越的柔顺性和灵活性，在微创介入手术、狭小环境探测救援、空间操作等领域具有非常广阔的应用前景，逐渐成为新的研究热点。然而它的柔顺性和超冗余的特点也给连续体机器人的建模和控制带来了极大的考验。
3.与运动可预测刚体机器人不一样，连续体机器人具有无限自由度，但是其在受到外部载荷时却容易变形。因此与传统机器手相比，连续体机器人的控制任务更具有挑战性、且更具有难度。如今，连续体机器人的精确位置控制成为了连续体机器人最重要的研究课题之一。
4.目前，连续体控制方法主要是有模型的闭环反馈控制。研究人员为连续体机器人发现了新的逆运动学模型，这些模型在一定程度上可以捕捉弯曲以及弯曲的能力。这类逆运动学模型就是恒曲率模型。而依赖于这特定的逆运动学模型的控制方法就是有模型的控制。基于连续体机器人逆运动学模型的反馈控制方法有着比较明显的缺点。当连续体机器人在受限环境下工作或者是受到外力干扰下时，它基于逆运动学模型的雅可比矩阵是不正确的。所以，在未知环境下，我们无法对连续体机器人的配置和雅可比矩阵进行有效建模。
5.因此，开发一种连续体机器臂末端位置的精确控制方法、系统是一个亟待解决的技术问题。
技术实现要素：
6.由于现有技术存在上述缺陷，本发明提供了一种连续体机器臂末端位置的控制方法、系统及机器臂系统，采取在线训练神经网络方案，通过神经网络输出值达到更新雅可比矩阵的目的。通过更新后的雅可比矩阵计算出当前连续体机器人的驱动补偿量，进而实现连续体机器人高精度控制。
7.为实现上述目的，一方面，本发明提供一种连续体机器臂末端位置的控制方法，包括以下步骤：
8.步骤s1、训练出连续体机器臂逆运动学的神经网络模型；
9.步骤s2、输入给定控制的期望位置，通过预先训练的所述神经网络得到控制所需的驱动变化量；根据所述神经网络输出的驱动变化量控制所述连续体机器臂的末端位置向所述期望位置运动；
10.其特征在于，所述步骤s1和s2执行后，还包括以下步骤：
11.步骤s3、通过位置传感器实时地测量出连续体机器臂末端的实际位置，从而计算出末端位置控制误差；
12.步骤s4、判断所述末端位置控制误差是否大于设定的阈值；若是，说明控制精度不足，则进入下一步；否则，控制进度达到控制要求，可以输入下一个目标位置进行控制；
13.步骤s5、通过在线训练神经网络预测实时的雅可比矩阵变化量，进而通过计算出的雅可比矩阵和当前时刻的位置控制误差计算出驱动补偿量，并根据所述驱动补偿量驱使机器人的末端位置发生变化，从而使控制精度满足控制要求。
14.本方法结合预训练的神经网络、实时末端位置测量和在线神经网络训练，及时精确地获得连续体机器臂末端位置的驱动补偿量。在线神经网络训练无需训练集和测试集，输入和输出都是来源于上一时刻以及当前时刻位置传感器的测量值，预测标签值则为所设的位置控制目标值。
15.进一步地，驱动电机旋转相应圈数带动所述连续体机械臂内的两根驱动绳索发生直线位移；随着驱动绳索直线位移的变化，所述连续体机械臂末端发生运动；所述驱动电机根据所获取的驱动变化量指令执行操作。
16.进一步地，所述步骤s3中，所述连续体机械臂前端设有磁传感器，通过所述磁传感器与外部磁场发生器的交互实时测量出连续体机器臂末端的实际位置。
17.进一步地，所述步骤s5中，所述在线训练神经网络为输入层、隐藏层和输出层的3层结构：输入层设有4个神经元，隐藏层设有12个神经元，输出层设有4个神经元；采用bp算法的所述神经网络得到雅可比矩阵变化量和驱动补偿量。
18.进一步地，所述在线训练神经网络选取的损失函数l：式中p(k)为k时刻连续体机械臂末端的实际位置，pd为期望控制位置，[p(k)-pd]
t
为k时刻位置控制误差长度向量。
[0019]
进一步地，通过求解所述损失函数l对在线训练神经网络参数的偏导，计算出每一次优化时所述参数的变化量，对所述参数进行优化，从而循环地训练所述参数，更新出新的雅可比矩阵，得到所需的驱动补偿量，最终使得连续体机器臂末端位置控制误差小于设定值为止。
[0020]
另一方面，本发明提供一种连续体机器臂末端位置的控制系统，其特征在于，包括：预训练的神经网络模块，被配置为预先训练并获得连续体机器臂逆运动学模型，得到连续体机器臂末端目标位置的所需驱动变化量；
[0021]
位置传感器感知模块，被配置为获得连续体机器臂末端的实际位置；
[0022]
在线训练的神经网络模块，被配置为实时训练并获得连续体机器臂末端的实时位移与逆运动学模型之间的驱动补偿量；
[0023]
驱动模块，被配置为根据预训练的神经网络模块获得的驱动变化量、以及在线训练的神经网络模块获得的驱动补偿量驱动连续体机器臂动作。
[0024]
再一方面，本发明提供一种电子设备，其特征在于，包括存储器和处理器以及存储在存储器上并在处理器上运行的计算机指令，所述计算机指令被处理器运行时，完成上述连续体机器臂末端位置的控制方法。
[0025]
最后一方面，本发明提供一种机械臂系统，包括基座、一端固定连接于所述基座上的连续体机械臂本体、所述连续体机械臂内的驱动绳索、所述驱动绳索的驱动电机和所述连续体机械臂末端的位置传感器，其特征在于，还包括上述连续体机器臂末端位置的控制
系统。
[0026]
与现有技术相比，上述发明具有如下优点或者有益效果：
[0027]
本发明结合实时测量的机器臂末端位置以及在线训练神经网络获得连续体机器臂末端位置的驱动补偿量，是一种基于无模型在线训练神经网络进而更新雅可比矩阵，实现连续体机器臂闭环控制的方法。本方法克服了连续体机器人在受限环境下的控制问题，同时也进一步提高了此类机器人末端位置控制的精度，提升了控制准确度，为连续体机器人在控制领域提供了一个较好的控制策略。
附图说明
[0028]
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明及其特征、外形和优点将会变得更加明显。在全部附图中相同的标记指示相同的部分。并未可以按照比例绘制附图，重点在于示出本发明的主旨。
[0029]
图1为本发明一实施例中的连续体机器臂的结构俯视图；
[0030]
图2为本发明一实施例的实验布置侧视图；
[0031]
图3为本发明一实施例中连续体机器臂末端位置控制方法的步骤流程图；
[0032]
图4描述了本发明一实施例中的连续体机器臂末端位置控制误差的含义；
[0033]
图5为本发明一实施例中连续体机器臂末端位置控制方法的控制流程图；
[0034]
其中，1、连续体机械臂本体；2、第一驱动绳索；3、第二驱动绳索；4、基座；5、磁传感器；6、磁场发生器。
具体实施方式
[0035]
下面结合附图和具体的实施例对本发明作进一步的说明，但是不作为本发明的限定。
[0036]
实施例1
[0037]
参见图1，一种连续体机器臂的基本结构包括连接在基座4上的连续体机械臂本体1和驱动绳索。作为一种优选的实施例，假定驱动绳索的数量为两根，即第一驱动绳索2、第二驱动绳索3，其驱动原理是通过改变两根驱动绳索在连续体机械臂本体内的长度，使连续体机械臂本体1发生弯曲。由于只有2根驱动绳索，这里的驱动空间是指穿过连续体机器人的驱动绳索的长度向量，即q＝[q1,q2]
t
，其中q为驱动长度向量，q1为第一根驱动绳索长度(cm)，q2为第二根驱动绳索长度(cm)。由此可见，所述连续体机器臂运动时仅有2个自由度。这里任务空间指的是连续体机器臂末端的二维位置，此二维位置坐标的原点位于连续体机械臂底部中心。
[0038]
参见图2和图3，基于以上模型，一种连续体机器臂末端位置的控制方法包括以下步骤：
[0039]
步骤s1、训练出连续体机器臂逆运动学的神经网络模型；
[0040]
步骤s2、输入给定控制的期望位置，通过预先训练的所述神经网络得到控制所需的驱动变化量；根据所述神经网络输出的驱动变化量控制所述连续体机器臂的末端位置向所述期望位置运动。
[0041]
输入给定控制的期望位置，即给所设定的期望位置变量赋值；将预先训练的神经
网络作为此实施例连续体机械臂的逆运动学模型。期望位置变量通过预先训练的神经网络转变为控制所需的驱动变化量。该神经网络训练过程为：首先需要通过实验获得各个位置任务空间(p1,p2,p3....pn)和其对应的驱动空间(q1,q2,q3....qn)，将任务空间数据作为训练输入、驱动空间作为目标输出。使用bp算法对数据做回归预测，直到测试误差低于设定值。神经网络参数训练完成后可以近似为一个映射函数q＝φ(p)，其中p是期望控制位置、q是该期望位置对应的驱动量的预测值。
[0042]
本实施例的步骤s2中，首先获取由上一步神经网络输出的驱动变化量δq，再将其作为控制信号输入，紧接着驱动电机发生相应变化，驱动电机旋转相应圈数带动驱动绳索发生直线位移。最后，随着驱动绳索长度变化，连续机器臂末端位置向期望位置运动。
[0043]
步骤s3、通过位置传感器实时地测量出连续体机器臂末端的实际位置，从而计算出末端位置控制误差。所述位置传感器可选择设于所述连续体机械臂前端的磁传感器5，通过磁传感器5与外部磁场发生器6的交互实时测量出连续体机器臂末端的实际位置。磁场发生器6可以是ndiaurorav3系列医用磁场发生器，磁传感器5可以是磁场发生器配套的磁性定位线圈。可以理解的是，所述位置传感器也可以是其他类型或型号的体内定位传感器，以实现连续体机械臂末端位置的定位。
[0044]
参见图4，pd是期望控制位置，δp(k)指的是k时刻的控制误差值，δpr(k)指的k时刻机械臂末端的实际位移，通过位置传感器测量出连续体机器臂末端实际位置，并实时上传位置信息且计算出实际末端位置控制误差δp(k)，其中k指的是当前时间线。
[0045]
步骤s4、判断所述末端位置控制误差δp(k)是否大于设定的阈值l；若是，说明控制精度不足，需要进行驱动量补偿控制，则进入下一步；否则，控制进度达到控制要求，可以输入下一个目标位置进行控制。
[0046]
步骤s5、通过在线训练神经网络预测实时的雅可比矩阵变化量，进而通过计算出的雅可比矩阵和当前时刻的位置控制误差计算出驱动补偿量，并根据所述驱动补偿量驱使机器人的末端位置发生变化，从而使控制精度满足控制要求。
[0047]
参见图5，为了获得驱动补偿量，所述方法通过在线训练神经网络预测当时的雅可比矩阵变化量，进而通过计算出的雅可比矩阵和当前时刻的位置控制误差δp(k)获取驱动补偿量δqc，δqc计算公式如下：
[0048]
δp(k)＝p(k)-pd[0049]
δpr(k)＝p(k+1)-p(k)
[0050]
j(k)＝j(k-1)+δj(k)
[0051]
δqc(k)＝j(k)δp(k)
[0052]
其中j(k)是k时刻神经网络预测出的雅可比矩阵值，pd是期望控制位置，δp(k)指的是k时刻的控制误差值，δpr(k)指的k时刻机械臂末端的实际位移。
[0053]
在通过神经网络预测雅可比矩阵变化量前，需要进行雅可比矩阵初始化，从而获得初始化雅可比矩阵j0。
[0054]
雅可比矩阵初始化方法如下：
[0055]
在实验控制前，启动连续体机器臂，给定一个微小的任务空间移动量p＝[δx,δz]
t
,从而获得相应驱动空间的变化量q＝[δq1,δq2]
t
。
[0056]
通过上述的任务空间移动量以及其对应的驱动空间变化量，计算出它们的映射关
系，并将映射矩阵作为初始化的雅可比矩阵j0如下。
[0057]
q＝j0p
[0058]
在线实时训练神经网络训练过程如下：
[0059]
在线训练的神经网络结构为3层，输入层设有4个神经元，输入值为p1(k)-p
d(1)
，p2(k)-p
d(2)
，p1(k)-p1(k-1)，p2(k)-p2(k-1)。隐藏层设有12个神经元。输出层设有4个神经元，输出值分别为δj
11
,δj
12
,δj
21
,δj
22
。
[0060]
其中
[0061]
隐藏层的激活函数为：g[
·
]＝[exp(x)-exp(-x)]/[exp(x)+exp(-x)]
[0062]
输出层的激活函数为：h[
·
]＝[exp(x)]/[exp(x)+exp(-x)]
[0063]
选取的损失函数l：其中期望控制位置pd为标签。
[0064]
由神经网络bp算法可以得到：
[0065]
隐藏层输出为：
[0066]
其中：x1,x2,x3,x4，w
ji(1)
和b
j(1)
为隐藏层的权重和偏置。
[0067]
输出层输出为：
[0068]
其中：w
rj(2)
和b
r(2)
为输出层的权重和偏置。
[0069]
雅可比矩阵变化量：
[0070][0071]
驱动补偿量为：
[0072]
其中，
[0073]
k时刻驱动量为：
[0074]
驱动补偿量δqc(k)作用于连续体机械臂上，此时末端执行器位置为p(k+1)，其变
化量为δpr(k)。且令在线神经网络训练边界条件：l(k)＜l或者训练次数epoches＞＝n，其中l为误差临界值，n为设定的训练次数大小。
[0075]
接下来，为了减小误差，需要优化神经网络参数，即w
ji(1)
,b
j(1)
,w
ri(2)
,b
r(2)
,。从而使损失函数l小于临界值，损失函数l：
[0076]
求损失函数l对这些参数的偏导，进而计算出每一次优化时参数变化量。
[0077]
在线神经网络的参数优化公式如下：
[0078][0079][0080][0081][0082][0083][0084][0085][0086][0087]
其中：当r＝1时，
[0088]
当r＝2时，
[0089]
当r＝3时，
[0090]
当r＝4时，
[0091]
其中
[0092]
其中
[0093]
如此，使用上述参数优化公式，循环地训练神经网络参数，更新出新的雅可比矩阵，从而得到所需的驱动补偿量，最终使得连续体机器臂末端位置发生改变，直到位置控制误差小于设定值l为止。
[0094]
实施例2
[0095]
一种连续体机器臂末端位置的控制系统，包括：预训练的神经网络模块，被配置为预先训练并获得连续体机器臂逆运动学模型，得到连续体机器臂末端目标位置的所需驱动变化量；
[0096]
位置传感器感知模块，被配置为获得连续体机器臂末端的实际位置；
[0097]
在线训练的神经网络模块，被配置为实时训练并获得连续体机器臂末端的实时位移与逆运动学模型之间的驱动补偿量；
[0098]
驱动模块，被配置为根据预训练的神经网络模块获得的驱动变化量、以及在线训练的神经网络模块获得的驱动补偿量驱动连续体机器臂动作。
[0099]
所述连续体机器臂末端位置的控制系统可实施实施例1中所述的控制方法。
[0100]
实施例3
[0101]
一种电子设备，包括存储器和处理器以及存储在存储器上并在处理器上运行的计算机指令，所述计算机指令被处理器运行时，完成实施例1所述的连续体机器臂末端位置的控制方法。
[0102]
实施例4
[0103]
参见图2，一种机械臂系统，包括基座4、一端固定连接于所述基座上的连续体机械臂本体1、所述连续体机械臂内的驱动绳索(仅示出第一驱动绳索2)、所述驱动绳索的驱动电机(未示出)和连续体机械臂末端的磁传感器5，还包括实施例1所述的连续体机器臂末端位置的控制系统。
[0104]
综上所述，本发明提供了一种连续体机器臂末端位置的控制方法、系统及机器臂系统，包括训练出连续体机器臂逆运动学的神经网络模型；通过预先训练的所述神经网络得到控制所需的驱动变化量，根据所述神经网络输出的驱动变化量控制所述连续体机器臂的末端位置向所述期望位置运动；通过位置传感器实时地测量出连续体机器臂末端的实际位置，计算出末端位置控制误差；若所述末端位置控制误差大于设定的阈值，则通过在线训练神经网络预测实时的雅可比矩阵变化量和驱动补偿量等步骤。本发明通过在线训练神经网络输出值达到更新雅可比矩阵的目的，通过更新后的雅可比矩阵计算出当前连续体机器人的驱动补偿量，进而实现连续体机器人高精度控制。
[0105]
本领域技术人员应该理解，本领域技术人员在结合现有技术以及上述实施例可以实现变化例，在此不做赘述。这样的变化例并不影响本发明的实质内容，在此不予赘述。
[0106]
以上对本发明的较佳实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，其中未尽详细描述的设备和结构应该理解为用本领域中的普通方式予以实
施；任何熟悉本领域的技术人员，在不脱离本发明技术方案范围情况下，都可利用上述揭示的方法和技术内容对本发明技术方案做出许多可能的变动和修饰，或修改为等同变化的等效实施例，这并不影响本发明的实质内容。因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本发明技术方案保护的范围内。
[0107]
本领域普通技术人员可以意识到，结合本实施例描述的各示例的单元即算法步骤，能够以电子硬件或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本技术的范围。
技术特征：
1.一种连续体机器臂末端位置的控制方法，其特征在于，包括以下步骤：步骤s1、训练出连续体机器臂逆运动学的神经网络模型；步骤s2、输入给定控制的期望位置，通过预先训练的所述神经网络得到控制所需的驱动变化量；根据所述神经网络输出的驱动变化量控制所述连续体机器臂的末端位置向所述期望位置运动；步骤s3、通过位置传感器实时地测量出连续体机器臂末端的实际位置，从而计算出末端位置控制误差；步骤s4、判断所述末端位置控制误差是否大于设定的阈值；若是，说明控制精度不足，则进入下一步；否则，控制进度达到控制要求，可以输入下一个目标位置进行控制；步骤s5、通过在线训练神经网络预测实时的雅可比矩阵变化量，进而通过计算出的雅可比矩阵和当前时刻的位置控制误差计算出驱动补偿量，并根据所述驱动补偿量驱使机器人的末端位置发生变化，从而使控制精度满足控制要求。2.根据权利要求1所述的一种连续体机器臂末端位置的控制方法，其特征在于，驱动电机旋转相应圈数带动所述连续体机械臂内的两根驱动绳索发生直线位移；随着驱动绳索直线位移的变化，所述连续体机械臂末端发生运动；所述驱动电机根据所获取的驱动变化量指令执行操作。3.根据权利要求1所述的一种连续体机器臂末端位置的控制方法，其特征在于，所述步骤s3中，所述连续体机械臂前端设有磁传感器，通过所述磁传感器与外部磁场发生器的交互实时测量出连续体机器臂末端的实际位置。4.根据权利要求1所述的一种连续体机器臂末端位置的控制方法，其特征在于，所述步骤s5中，所述在线训练神经网络为输入层、隐藏层和输出层的3层结构：输入层设有4个神经元，隐藏层设有12个神经元，输出层设有4个神经元；采用bp算法的所述神经网络得到雅可比矩阵变化量和驱动补偿量。5.根据权利要求4所述的一种连续体机器臂末端位置的控制方法，其特征在于，所述在线训练神经网络选取的损失函数l：式中p(k)为k时刻连续体机械臂末端的实际位置，p
d
为期望控制位置，[p(k)-p
d
]
t
为k时刻位置控制误差长度向量。6.根据权利要求5所述的一种连续体机器臂末端位置的控制方法，其特征在于，通过求解所述损失函数l对在线训练神经网络参数的偏导，计算出每一次优化时所述参数的变化量，对所述参数进行优化，从而循环地训练所述参数，更新出新的雅可比矩阵，得到所需的驱动补偿量，最终使得连续体机器臂末端位置控制误差小于设定值为止。7.一种连续体机器臂末端位置的控制系统，其特征在于，包括：预训练的神经网络模块，被配置为预先训练并获得连续体机器臂逆运动学模型，得到连续体机器臂末端目标位置的所需驱动变化量；位置传感器感知模块，被配置为获得连续体机器臂末端的实际位置；在线训练的神经网络模块，被配置为实时训练并获得连续体机器臂末端的实时位移与逆运动学模型之间的驱动补偿量；驱动模块，被配置为根据预训练的神经网络模块获得的驱动变化量、以及在线训练的神经网络模块获得的驱动补偿量驱动连续体机器臂动作。
8.一种电子设备，其特征在于，包括存储器和处理器以及存储在存储器上并在处理器上运行的计算机指令，所述计算机指令被处理器运行时，完成权利要求1-6任一项所述的控制方法。9.一种机械臂系统，包括基座、一端固定连接于所述基座上的连续体机械臂、所述连续体机械臂内的驱动绳索、所述驱动绳索的驱动电机和所述连续体机械臂末端的位置传感器，其特征在于，还包括权利要求7所述的控制系统。
技术总结
本发明公开了一种连续体机器臂末端位置的控制方法、系统及机器臂系统，包括训练出连续体机器臂逆运动学的神经网络模型；通过预先训练的所述神经网络得到控制所需的驱动变化量，根据所述神经网络输出的驱动变化量控制所述连续体机器臂的末端位置向所述期望位置运动；通过位置传感器实时地测量出连续体机器臂末端的实际位置，计算出末端位置控制误差；若所述末端位置控制误差大于设定的阈值，则通过在线训练神经网络预测实时的雅可比矩阵变化量和驱动补偿量等步骤。本发明通过在线训练神经网络输出值达到更新雅可比矩阵的目的，通过更新后的雅可比矩阵计算出当前连续体机器人的驱动补偿量，进而实现连续体机器人高精度控制。制。制。
