软连续体机器人的三维形状融合重构方法及装置

本技术涉及机器人，特别涉及一种软连续体机器人的三维形状融合重构方法及装置。

背景技术：

1、大长径比软连续体机器人的形状感知与三维重构，通常基于机器人嵌入式感知技术或体外感知技术实现，体外感知是指用于感知机器人形状或其它物理信息的传感器布置在机器人的体外，或机器人体内的传感元件必须由机器人体外辅助检测设备近距离监测或被监测，才能完成信息获取的测量方案。与体外感知相对应的测量方案的是嵌入式感知技术，嵌入式感知是指用于检测机器人形状或其他物理信息的传感器完全布置在机器人体内，且无需外部辅助设备对其近距离监测，即可实现机器人的形状感知。

2、相关技术中，可以通过体外感知技术在机器人体外使用动态捕捉摄像头监测机器人表面的标记物实现位置感知、将多个相机布置在机器人的表面共同监测机器人体外的敏感标记物实现自感知、或使用电磁追踪技术使用体外电磁场追踪埋置于机器人体内的电磁线圈位置和姿态等；也可以通过嵌入式感知技术使用光纤传感器、柔性电子技术或霍尔元件对机器人局部变形进行测量。

3、然而，相关技术中，使用体外的传感系统进行辅助，导致系统的感知信息的获取受限于外部辅助设备，其应用场景十分受限，使用嵌入式感知技术以获得准确的机器人形状感知，传感元件的布置密度不能过低，形状重构的累积误差也将随机器人的长度增加而不断增大，难以彻底消除，亟待改善。

技术实现思路

1、本技术提供一种软连续体机器人的三维形状融合重构方法及装置，完全基于嵌入式感知技术，以解决相关技术中，机器人系统体外感知技术信息的获取受限于外部辅助设备，及嵌入式感知技术对传感元件的布置密度要求高，形状重构存在空间累积误差的问题。

2、本技术第一方面实施例提供一种软连续体机器人的三维形状融合重构方法，包括以下步骤：基于嵌入式传感器惯性测量单元imu获取软连续体机器人沿轴向离散分布的姿态信息，基于嵌入式传感器布拉格光纤光栅fbg获取所述软连续体机器人沿轴向离散分布的曲率信息；设计由预测函数控制参数控制的沿机器人轴向连续变化的曲率形变预测函数，根据所述已知曲率信息作为形变预测函数的约束条件，在所述约束条件下，根据所述形变预测函数获取所述软连续体机器人的预测姿态信息，将所述预测姿态信息与所述姿态信息的差作为目标函数，并对所述形变预测函数的控制参数进行寻优运算，确定最优控制参数，以得到最优重构结果。

3、可选地，在本技术的一个实施例中，所述获取所述软连续体机器人沿轴向离散分布的曲率信息，包括：根据纤芯截面上的弯曲曲率与各纤芯机械应变之间的几何关系计算所述截面的弯曲曲率，其中，所述光纤布拉格光栅传感器使用四芯光纤，中心纤芯位于所述四芯光纤的中心位置，其余纤芯均匀分布在所述中心纤芯的四周；根据所述截面的弯曲曲率的计算结果得到所述曲率信息。

4、可选地，在本技术的一个实施例中，所述将所述曲率信息作为形变预测函数的约束条件的计算公式为：

5、

6、其中，p为预测形变的控制参数组，和分别为所述软连续体机器人杆长sj处的弯曲曲率信息，j为各曲率测量点的编号。

7、可选地，在本技术的一个实施例中，所述将所述预测姿态信息与所述姿态信息的差作为目标函数，并对所述形变预测函数的控制参数进行寻优运算的目标函数计算式为：

8、

9、其中，f为和之间差异的任意函数，i为各姿态测量点的编号，m为姿态测量点的总数，r为机器人实际弯曲姿态。

10、可选地，在本技术的一个实施例中，所述对所述目标函数进行优化的公式为：

11、

12、其中，trace(a)表示求矩阵a的迹，i为所述各姿态测量点的编号，r为所述软连续体机器人实际弯曲姿态，p为预测形变的所述控制参数组。

13、本技术第二方面实施例提供一种软连续体机器人的三维形状融合重构装置，包括：获取模块，用于基于嵌入式传感器惯性测量单元imu获取软连续体机器人沿轴向离散分布的姿态信息，基于嵌入式传感器布拉格光纤光栅fbg获取所述软连续体机器人沿轴向离散分布的曲率信息；形状重构模块，用于设计由预测函数控制参数控制的沿机器人轴向连续变化的曲率形变预测函数，并将所述曲率信息作为形变预测函数的约束条件，在所述约束条件下，根据所述形变预测函数获取所述软连续体机器人的预测姿态信息，将所述预测姿态信息与所述姿态信息的差作为目标函数，并对所述形变预测函数的控制参数进行寻优运算，确定最优控制参数，以得到最优重构结果。

14、可选地，在本技术的一个实施例中，所述获取模块包括：计算单元，用于根据纤芯截面上的弯曲曲率与各纤芯机械应变之间的几何关系计算所述截面的弯曲曲率，其中，所述光纤布拉格光栅传感器使用四芯光纤，中心纤芯位于所述四芯光纤的中心位置，其余纤芯均匀分布在所述中心纤芯的四周；生成单元，用于根据所述截面的弯曲曲率的计算结果得到所述曲率信息。

15、可选地，在本技术的一个实施例中，所述根据所述曲率信息得到形变预测函数的约束条件的计算公式为：

16、

17、其中，p为预测形变的控制参数组，和分别为所述软连续体机器人杆长sj处的弯曲曲率信息，j为各曲率测量点的编号。

18、可选地，在本技术的一个实施例中，所述对所述软连续体机器人的三维形状进行最优重构的目标函数计算式为：

19、

20、其中，f为和之间差异的任意函数，i为各姿态测量点的编号，m为姿态测量点的总数，r为机器人实际弯曲姿态。

21、可选地，在本技术的一个实施例中，所述对所述目标函数进行优化的公式为：

22、

23、其中，trace(a)表示求矩阵a的迹，i为所述各姿态测量点的编号，r为所述软连续体机器人实际弯曲姿态，p为预测形变的所述控制参数组。

24、本技术第三方面实施例提供一种电子设备，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序，以实现如上述实施例所述的软连续体机器人的三维形状融合重构方法。

25、本技术第四方面实施例提供一种计算机可读存储介质，所述计算机可读存储介质存储计算机程序，该程序被处理器执行时实现如上的软连续体机器人的三维形状融合重构方法。

26、本技术实施例可以通过惯性测量单元和布拉格光纤光栅获取机器人沿轴向离散分布的姿态和曲率信息，根据形变预测函数获取软连续体机器人的预测姿态信息，将预测姿态信息与姿态信息的差作为目标函数，并对形变预测函数的控制参数进行寻优运算，确定最优控制参数，得到最优重构结果，从而实现了机器人三维形貌的最优形状重构，提高了形状重构结果的准确性，有利于消除连续体机器人嵌入式感知的累积误差，具有较大的应用空间。由此，解决了相关技术中，机器人系统体外感知技术信息的获取受限于外部辅助设备，及嵌入式感知技术对传感元件的布置密度要求高，形状重构存在空间累积误差等问题。

27、本技术附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本技术的实践了解到。