CN112356020B - 一种变截面柔性臂反馈控制的线长定位方法、装置和介质 - Google Patents

一种变截面柔性臂反馈控制的线长定位方法、装置和介质 Download PDF

Info

Publication number
CN112356020B
CN112356020B CN202010992971.4A CN202010992971A CN112356020B CN 112356020 B CN112356020 B CN 112356020B CN 202010992971 A CN202010992971 A CN 202010992971A CN 112356020 B CN112356020 B CN 112356020B
Authority
CN
China
Prior art keywords
section
variable cross
air bag
equivalent
section air
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010992971.4A
Other languages
English (en)
Other versions
CN112356020A (zh
Inventor
黄奕勇
许嘉毅
刘红卫
张翔
付康佳
韩伟
熊丹
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
National Defense Technology Innovation Institute PLA Academy of Military Science
Original Assignee
National Defense Technology Innovation Institute PLA Academy of Military Science
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by National Defense Technology Innovation Institute PLA Academy of Military Science filed Critical National Defense Technology Innovation Institute PLA Academy of Military Science
Priority to CN202010992971.4A priority Critical patent/CN112356020B/zh
Publication of CN112356020A publication Critical patent/CN112356020A/zh
Application granted granted Critical
Publication of CN112356020B publication Critical patent/CN112356020B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Air Bags (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)

Abstract

本发明公开了一种变截面柔性臂反馈控制的线长定位方法、装置和介质，所述方法包括步骤：S1、将所测的实际线长
Figure DDA0002691559330000011
作为当前变截面气囊段的等效线长
Figure DDA0002691559330000015
的初值；S2、根据所述初值得到弯曲角度θ和弯曲程度
Figure DDA00026915593300000110
S3、根据弯曲角度θ和弯曲程度
Figure DDA0002691559330000012
分别得到位差角βi；S4、根据
Figure DDA0002691559330000013
βi、d0、d1得到等效线长
Figure DDA0002691559330000016
的计算值；S5、将等效线长
Figure DDA0002691559330000019
的计算值作为当前变截面气囊段的三条等效线长
Figure DDA0002691559330000018
的初值后重复步骤S1～S4迭代得到最终的
Figure DDA0002691559330000017
θ、和βi；S6、重复步骤S1～S5，得到各变截面气囊段末端的相对坐标；S7、将每一个变截面气囊段的相对坐标转换到绝对坐标系下相累加，得到每一个变截面气囊段和变截面柔性臂末端在绝对坐标系下的精确坐标位置。本发明结构简单、精度高，适应性强。

Description

一种变截面柔性臂反馈控制的线长定位方法、装置和介质
技术领域
本发明涉及机器人控制领域，特别地，涉及一种变截面柔性臂反馈控制的线长定位方法、装置和介质。
背景技术
当前，软体机械臂属于机器人研究的前沿领域，由于其柔性的特点，无法从驱动环节获得准确的位置坐标信息，因此需要独立于驱动系统的测量系统进行：
1)激光/超声波测距定位。激光和超声波在测量长度、速度等方面精度高、响应快已经在工业和日常各个领域有大量应用，其核心方法是建立相对坐标，测量相对坐标下测量点的速度和距离，进而得到相对坐标下的位置，但是这种方法对于环境有较高要求，例如在狭窄、曲折的环境中，激光无法绕过障碍物获得位置信息，再例如在太空环境下，超声波由于缺乏介质，完全失去作用。
2)机器视觉方法。机器视觉方法利用光学原理，从成像环节获取仪器信息，例如成像对焦点焦距等。双目视觉方法核心原理与传统光学测距仪类似，核心测量角度信息，解算三角函数从而获得位置信息。由于需要外置摄像头等装置，且由于要保证机械臂定位点处于相场中，因此在实际使用中，机械臂系统实际占用空间较大，且传感器布置在机械臂活动范围外，从而大大限制了机械臂系统的使用范围。
另外，目前很多软体机械臂除了恒截面外，例如Festo仿生机械臂并非恒截面，而是有4.5°渐收。该类机械臂在无外力施加的情况下并非恒曲率变形，而是随着距离根部越远曲率逐渐增加，因此所有基于恒曲率模型的位置坐标获取方法均不可采用。
发明内容
本发明一方面提供了一种变截面柔性臂反馈控制的线长定位方法，以解决现有软体机械臂定位时易受环境影响、定位系统复杂、定位精度低、忽略变截面的技术问题。
本发明采用的技术方案如下：
一种变截面柔性臂反馈控制的线长定位方法，包括步骤：
S1、测量变截面柔性臂的任一变截面气囊段弯曲后三条线缆的实际线长
Figure GDA0003029179740000021
将所测的三条实际线长
Figure GDA0003029179740000022
作为当前变截面气囊段的三条等效线长
Figure GDA0003029179740000023
的初值，i＝1，2，3；所述三条线缆在变截面气囊段未弯曲前以变截面气囊段中心线为旋转中心两两相隔120度；
S2、根据所述三条等效线长
Figure GDA0003029179740000024
的初值得到当前变截面气囊段的弯曲角度θ和弯曲程度
Figure GDA0003029179740000025
其中，所述弯曲程度
Figure GDA0003029179740000026
为当前变截面气囊段两端截面之间的夹角；所述弯曲角度θ的定义为：设当前变截面气囊段基座截面的圆心为o，所测量的三条实际线长
Figure GDA0003029179740000027
中最短的实际线长
Figure GDA0003029179740000028
与变截面气囊段基座截面的外圆的交点为z，圆心o与交点z的连线所形成的x轴与变截面气囊段的弯曲方向的夹角即为弯曲角度θ，其中，所述变截面气囊段的弯曲方向定义为：变截面气囊段弯曲后其侧面上最短母线与变截面气囊段基座截面外圆具有交点，将圆心o与该交点的连线形成x'轴，则所述x'轴所指的方向即为变截面气囊段的弯曲方向；
S3、根据当前变截面气囊段的弯曲角度θ和弯曲程度
Figure GDA0003029179740000029
分别得到三条等效线长
Figure GDA00030291797400000210
各自对应的位差角βi，i＝1，2，3；其中，三条等效线长
Figure GDA00030291797400000211
各自对应的位差角βi具体为三条线缆在未弯曲前的变截面气囊段基座截面上的投影线分别与对应的等效线长
Figure GDA00030291797400000212
之间的夹角；
S4、根据三条实际线长
Figure GDA0003029179740000031
三条等效线长
Figure GDA0003029179740000032
各自对应的位差角βi、当前变截面气囊段末端截面半径d0、当前变截面气囊段基座截面半径d1得到当前变截面气囊段的三条等效线长
Figure GDA0003029179740000033
的计算值；
S5、将三条等效线长
Figure GDA0003029179740000034
的计算值作为当前变截面气囊段的三条等效线长
Figure GDA0003029179740000035
的初值后重复步骤S1～S4，直到相邻两次所得的三条等效线长
Figure GDA0003029179740000036
的计算值的差小于设定阈值时，输出当前变截面气囊段最终的三条等效线长
Figure GDA0003029179740000037
弯曲角度θ、弯曲程度
Figure GDA0003029179740000038
和三条等效线长各自对应的位差角βi；
S6、重复步骤S1～S5，计算变截面柔性臂所有变截面气囊段最终的等效线长
Figure GDA0003029179740000039
弯曲角度θ、弯曲程度
Figure GDA00030291797400000310
和三条等效线长各自对应的位差角βi，得到各变截面气囊段末端在其变截面气囊段基座坐标系下的相对坐标；
S7、将每一个变截面气囊段的相对坐标转换到绝对坐标系下相累加，得到每一个变截面气囊段末端和变截面柔性臂末端在绝对坐标系下的精确坐标位置。
进一步地，所述步骤S2具体包括步骤：
根据所述三条等效线长
Figure GDA00030291797400000311
的初值计算当前变截面气囊段的弯曲角度θ：
Figure GDA00030291797400000312
其中，
Figure GDA00030291797400000313
为分别为三条等效线长，且
Figure GDA00030291797400000314
最短；
根据所述三条等效线长
Figure GDA00030291797400000315
的初值计算当前变截面气囊段的弯曲程度
Figure GDA00030291797400000316
Figure GDA00030291797400000317
其中，
Figure GDA00030291797400000318
为当前变截面气囊段中心弧线实际长度，k为当前变截面气囊段曲率，d0为当前变截面气囊段末端截面半径。
进一步地，所述步骤S3具体包括步骤：
根据当前变截面气囊段的弯曲角度θ和弯曲程度
Figure GDA0003029179740000041
分别计算三条等效线长
Figure GDA0003029179740000042
各自对应的位差角βi：
Figure GDA0003029179740000043
进一步地，所述步骤S4具体包括步骤：
根据余弦公式得实际线长
Figure GDA0003029179740000044
和等效线长
Figure GDA0003029179740000045
之间的关系式：
Figure GDA0003029179740000046
将测量的三条实际线长
Figure GDA0003029179740000047
三条等效线长
Figure GDA0003029179740000048
各自对应的位差角βi、当前变截面气囊段末端截面半径d0、当前变截面气囊段基座截面半径d1代入所述关系式，计算得到当前变截面气囊段的三条等效线长
Figure GDA0003029179740000049
的计算值。
进一步地，所述步骤S7中具体包括步骤：
根据各变截面气囊段的弯曲角度θ和弯曲程度
Figure GDA00030291797400000411
确定各变截面气囊段的基座对应的转换矩阵T；
将当前变截面气囊段的基座的相对坐标系沿靠近变截面柔性臂底座的方向依次累乘各变截面气囊段的基座对应的转换矩阵T，得到当前变截面气囊段末端在绝对坐标系下的指向矢量；
计算所有变截面气囊段的指向矢量并依次累加，得到变截面柔性臂末端在绝对坐标系下的精确坐标位置。
进一步地，各变截面气囊段的基座对应的转换矩阵T具体为：
Figure GDA00030291797400000410
转换时，相对坐标系先绕z轴旋转-θ，接着绕y轴旋转
Figure GDA0003029179740000051
再绕z轴旋转θ。
进一步地，步骤S1中，所述三条线缆为变截面柔性臂的驱动线缆、用于辅助控制变截面柔性臂的线缆或仅用于定位的线缆。
本发明另一方面还提供了一种变截面柔性臂反馈控制的线长定位装置，包括：
初始赋值模块，用于测量变截面柔性臂的任一变截面气囊段弯曲后三条线缆的实际线长
Figure GDA0003029179740000052
将所测的三条实际线长
Figure GDA0003029179740000053
作为当前变截面气囊段的三条等效线长
Figure GDA0003029179740000054
的初值，i＝1，2，3；所述三条线缆在变截面气囊段未弯曲前以变截面气囊段中心线为旋转中心两两相隔120度；
弯曲角度及程度计算模块，用于根据所述三条等效线长
Figure GDA0003029179740000055
的初值得到当前变截面气囊段的弯曲角度θ和弯曲程度
Figure GDA0003029179740000056
其中，所述弯曲程度
Figure GDA00030291797400000513
为当前变截面气囊段两端截面之间的夹角；所述弯曲角度θ的定义为：设当前变截面气囊段基座截面的圆心为o，所测量的三条实际线长
Figure GDA0003029179740000057
中最短的实际线长
Figure GDA0003029179740000058
与变截面气囊段基座截面的外圆的交点为z，圆心o与交点z的连线所形成的x轴与变截面气囊段的弯曲方向的夹角即为弯曲角度θ，其中，所述变截面气囊段的弯曲方向定义为：变截面气囊段弯曲后其侧面上最短母线与变截面气囊段基座截面外圆具有交点，将圆心o与该交点的连线形成x'轴，则所述x'轴所指的方向即为变截面气囊段的弯曲方向；
位差角计算模块，用于根据当前变截面气囊段的弯曲角度θ和弯曲程度
Figure GDA00030291797400000514
分别得到三条等效线长
Figure GDA0003029179740000059
各自对应的位差角βi，i＝1，2，3，其中，三条等效线长
Figure GDA00030291797400000510
各自对应的位差角βi具体为三条线缆在未弯曲前的变截面气囊段基座截面上的投影线分别与对应的等效线长
Figure GDA00030291797400000511
之间的夹角；
等效线长计算模块，用于根据三条等效线长
Figure GDA00030291797400000512
各自对应的位差角βi、三条实际线长
Figure GDA0003029179740000061
当前变截面气囊段末端截面半径d0、当前变截面气囊段基座截面半径d1得到当前变截面气囊段的三条等效线长
Figure GDA0003029179740000062
的计算值；
迭代计算模块，用于将三条等效线长
Figure GDA0003029179740000063
的计算值作为当前变截面气囊段的三条等效线长
Figure GDA0003029179740000064
的初值后重复计算等效线长
Figure GDA0003029179740000065
直到相邻两次所得的三条等效线长
Figure GDA0003029179740000066
的计算值的差小于设定阈值时，输出当前变截面气囊段最终的三条等效线长
Figure GDA0003029179740000067
弯曲角度θ、弯曲程度
Figure GDA0003029179740000069
和三条等效线长各自对应的位差角βi；
相对坐标计算模块，用于重复计算变截面柔性臂所有变截面气囊段最终的等效线长
Figure GDA0003029179740000068
弯曲角度θ、弯曲程度
Figure GDA00030291797400000610
和三条等效线长各自对应的位差角βi，得到各变截面气囊段末端在其变截面气囊段基座坐标系下的相对坐标；
坐标系转换模块，用于将每一个变截面气囊段的相对坐标转换到绝对坐标系下相累加，得到每一个变截面气囊段末端和变截面柔性臂末端在绝对坐标系下的精确坐标位置。
本发明的另一方面提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现所述的变截面柔性臂反馈控制的线长定位方法。
本发明的另一方面提供了一种存储介质，所述存储介质包括存储的程序，在所述程序运行时控制所述存储介质所在的设备执行所述的变截面柔性臂反馈控制的线长定位方法。
本发明具有以下有益效果：
本发明提出的变截面柔性臂反馈控制的线长定位方法，采用了基于测量变截面气囊段线长从而完成截面气囊段末端坐标定位的求解方法，具体通过测量驱动线缆或测量仅是用于定位线缆的长度，代入定位方法解算可获得精度较高的变截面柔性臂末端位置。由于线缆来源于线控柔性臂自身驱动线缆，亦可以是用于辅助控制柔性臂的线缆或是仅采用线缆以完成定位，本发明的定位方法不需要在机械臂外部设置安装其他定位装置，有效控制了定位系统的体积，避免了在臂外设置测量装置；与此同时，本发明利用相应的数值计算方法解决了几何解算方法中精度低、忽略变截面的问题，其利用几何方法得到等效长度和位置之间的关系式，对于实际线长和等效线长之间的关系式，则通过迭代求解方法解算数值解，进而解决无法得到解析解造成误差较大的问题，另外，对于相对坐标系下的相对坐标进行向绝对坐标系转换，依次累加即可得到在绝对坐标系下的机械臂模态和末端坐标，具有适用范围广、精度较高的技术特征，具有很好的市场应用前景。
除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照附图，对本发明作进一步详细的说明。
附图说明
构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1是本发明优选实施例的一种变截面柔性臂反馈控制的线长定位方法流程示意图。
图2是变截面气囊段弯曲时的俯视示意图。
图3是变截面气囊段弯曲时的侧视示意图。
图4是本发明图1中步骤S2的详细步骤流程示意图。
图5是本发明图1中步骤S3的详细步骤流程示意图。
图6是本发明图1中步骤S4的详细步骤流程示意图。
图7是本发明图1中步骤S7的详细步骤流程示意图。
图8是本发明优选实施例的变截面柔性臂反馈控制的线长定位装置框图。
图9是本发明优选实施例的电子设备实体示意框图。
具体实施方式
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。
参照图1，本发明的优选实施例提供了一种变截面柔性臂反馈控制的线长定位方法，包括步骤：
S1、测量变截面柔性臂的任一变截面气囊段弯曲后三条线缆的实际线长
Figure GDA0003029179740000081
将所测的三条实际线长
Figure GDA0003029179740000082
作为当前变截面气囊段的三条等效线长
Figure GDA0003029179740000083
的初值，i＝1，2，3；所述三条线缆在变截面气囊段未弯曲前以变截面气囊段中心线为旋转中心两两相隔120度；
S2、根据所述三条等效线长
Figure GDA0003029179740000084
的初值得到当前变截面气囊段的弯曲角度θ和弯曲程度
Figure GDA0003029179740000085
其中，所述弯曲程度
Figure GDA0003029179740000088
为当前变截面气囊段两端截面之间的夹角；所述弯曲角度θ的定义为：设当前变截面气囊段基座截面的圆心为o，所测量的三条实际线长
Figure GDA0003029179740000086
中最短的实际线长
Figure GDA0003029179740000087
与变截面气囊段基座截面的外圆的交点为z，圆心o与交点z的连线所形成的x轴与变截面气囊段的弯曲方向的夹角即为弯曲角度θ，其中，所述变截面气囊段的弯曲方向定义为：变截面气囊段弯曲后其侧面上最短母线与变截面气囊段基座截面外圆具有交点，将圆心o与该交点的连线形成x'轴，则所述x'轴所指的方向即为变截面气囊段的弯曲方向；
S3、根据当前变截面气囊段的弯曲角度θ和弯曲程度
Figure GDA0003029179740000089
分别得到三条等效线长
Figure GDA0003029179740000091
各自对应的位差角βi，i＝1，2，3；其中，三条等效线长
Figure GDA0003029179740000092
各自对应的位差角βi具体为三条线缆在未弯曲前的变截面气囊段基座截面上的投影线分别与对应的等效线长
Figure GDA0003029179740000093
之间的夹角；
S4、根据三条实际线长
Figure GDA0003029179740000094
三条等效线长
Figure GDA0003029179740000095
各自对应的位差角βi、当前变截面气囊段末端截面半径d0、当前变截面气囊段基座截面半径d1得到当前变截面气囊段的三条等效线长
Figure GDA0003029179740000096
的计算值；
S5、将三条等效线长
Figure GDA0003029179740000097
的计算值作为当前变截面气囊段的三条等效线长
Figure GDA0003029179740000098
的初值后重复步骤S1～S4，直到相邻两次所得的三条等效线长
Figure GDA0003029179740000099
的计算值的差小于设定阈值时，输出当前变截面气囊段最终的三条等效线长
Figure GDA00030291797400000910
弯曲角度θ、弯曲程度
Figure GDA00030291797400000912
和三条等效线长各自对应的位差角βi；
S6、重复步骤S1～S5，计算变截面柔性臂所有变截面气囊段最终的等效线长
Figure GDA00030291797400000911
弯曲角度θ、弯曲程度
Figure GDA00030291797400000913
和三条等效线长各自对应的位差角βi，得到各变截面气囊段末端在其变截面气囊段基座坐标系下的相对坐标；
S7、将每一个变截面气囊段的相对坐标转换到绝对坐标系下相累加，得到每一个变截面气囊段末端和变截面柔性臂末端在绝对坐标系下的精确坐标位置。
本实施例中，由于该变截面柔性臂每根波纹管由数段变截面气囊段单独构成，因此可以选取每一个变截面气囊段进行研究，并且可以认为每个变截面气囊段不受变曲率影响。为了得到变截面线长和等效线长之间的关系式，以弯曲方向为x'轴，在该气囊段底部平面建立坐标系x'oy'，如图2和图3所示。
本实施例的变截面柔性臂反馈控制的线长定位方法，采用了基于测量变截面气囊段线长从而完成截面气囊段末端坐标定位的求解方法，具体通过测量驱动线缆或测量仅是用于定位线缆的长度，代入定位方法解算可获得精度较高的变截面柔性臂末端位置。由于线缆来源于线控柔性臂自身驱动线缆，亦可以是用于辅助控制柔性臂的线缆或是仅采用线缆以完成定位，本发明的定位方法不需要在机械臂外部设置安装其他定位装置，有效控制了定位系统的体积，避免了在臂外设置测量装置；与此同时，本发明利用相应的数值计算方法解决了几何解算方法中精度低、忽略变截面的问题，其利用几何方法得到等效长度和位置之间的关系式，对于实际线长和等效线长之间的关系式，则通过迭代求解方法解算数值解，进而解决无法得到解析解造成误差较大的问题，另外，对于相对坐标系下的相对坐标进行向绝对坐标系转换，依次累加即可得到在绝对坐标系下的机械臂模态和末端坐标，具有适用范围广、精度较高的技术特征，具有很好的市场应用前景。
如图4所示，在本发明的优选实施例中，所述步骤S2具体包括步骤：
S21、根据所述三条等效线长
Figure GDA0003029179740000101
的初值计算当前变截面气囊段的弯曲角度θ：
Figure GDA0003029179740000102
其中，
Figure GDA0003029179740000103
为分别为三条等效线长，且
Figure GDA0003029179740000104
最短；
S22、根据所述三条等效线长
Figure GDA0003029179740000105
的初值计算当前变截面气囊段的弯曲程度
Figure GDA0003029179740000106
Figure GDA0003029179740000107
其中，
Figure GDA0003029179740000108
为当前变截面气囊段中心弧线实际长度，k为当前变截面气囊段曲率，d0为当前变截面气囊段末端截面半径。
如图5所示，在本发明的优选实施例中，所述步骤S3具体包括步骤：
S31、根据当前变截面气囊段的弯曲角度θ和弯曲程度
Figure GDA00030291797400001115
分别计算三条等效线长
Figure GDA0003029179740000111
各自对应的位差角βi：
Figure GDA0003029179740000112
下面对三条等效线长
Figure GDA0003029179740000113
各自对应的位差角βi的推导过程做简要说明，如图2所示，针对位差角β2，引出两向量
Figure GDA0003029179740000114
则在x'oy'中的坐标分别是：
Figure GDA0003029179740000115
因此可以写出位差角β2的表达式：
Figure GDA0003029179740000116
位差角β1和β3的求解过程相类似，进而得到上述适用于所有位差角βi的计算公式。
如图6所示，在本发明的优选实施例中，所述步骤S4具体包括步骤：
S41、根据余弦公式得实际线长
Figure GDA0003029179740000117
和等效线长
Figure GDA0003029179740000118
之间的关系式：
Figure GDA0003029179740000119
S42、将测量的三条实际线长
Figure GDA00030291797400001110
三条等效线长
Figure GDA00030291797400001111
各自对应的位差角βi、当前变截面气囊段末端截面半径d0、当前变截面气囊段基座截面半径d1代入所述关系式，计算得到当前变截面气囊段的三条等效线长
Figure GDA00030291797400001112
的计算值。
本实施例通过上述步骤获得当前变截面气囊段的三条等效线长
Figure GDA00030291797400001113
的计算值，然后将三条等效线长
Figure GDA00030291797400001114
的计算值作为当前变截面气囊段的三条等效线长
Figure GDA0003029179740000121
的初值后重复步骤S1～S4，直到相邻两次所得的三条等效线长
Figure GDA0003029179740000122
的计算值的差小于设定阈值时，输出当前变截面气囊段最终的三条等效线长
Figure GDA0003029179740000123
弯曲角度θ、弯曲程度
Figure GDA0003029179740000127
和三条等效线长各自对应的位差角βi。
在通常解算方法中，由于实际线长和等效线长之间的关系式中包含位差角β，而位差角β需要根据含等效线长的弯曲角度θ、弯曲程度
Figure GDA0003029179740000128
解得，无法得到解析解，从而将位差角β近似为90°进而将实际线长和等效线长之间的关系式省去最后两项。
本实施例为了提高求解精度，采用不动点迭代的方法，计测量得到的实际长度为迭代初值，根据实际线长
Figure GDA0003029179740000124
和等效线长
Figure GDA0003029179740000125
之间的关系式进行迭代，最终可以得到变截面柔性臂确定末端位置所需的θ、
Figure GDA0003029179740000126
三个参数的精确解，进而解算变截面柔性臂的状态。
如图7所示，在本发明的优选实施例中，所述步骤S7中具体包括步骤：
S71、根据各变截面气囊段的弯曲角度θ和弯曲程度
Figure GDA0003029179740000129
确定各变截面气囊段的基座对应的转换矩阵T；
S72、将当前变截面气囊段的基座的相对坐标系沿靠近变截面柔性臂底座的方向依次累乘各变截面气囊段的基座对应的转换矩阵T，得到当前变截面气囊段末端在绝对坐标系下的指向矢量；
S73、计算所有变截面气囊段的指向矢量并依次累加，得到变截面柔性臂末端在绝对坐标系下的精确坐标位置。
本实施例中，由于各变截面气囊段的弯曲角度θ和弯曲程度
Figure GDA00030291797400001210
有所不同，因此，不同的变截面气囊段相对应不同的转换矩阵T，为了获得当前变截面气囊段末端在绝对坐标系下的指向矢量，本实施例将当前变截面气囊段的基座的相对坐标系沿靠近变截面柔性臂底座的方向依次累乘各变截面气囊段的基座对应的转换矩阵T，如当前变截面气囊段沿靠近变截面柔性臂底座的方向一共连接有六个变截面气囊段时，则将当前变截面气囊段的基座的相对坐标系依次累乘所述六个变截面气囊段的基座对应的转换矩阵T，即可得到当前变截面气囊段末端在绝对坐标系下的指向矢量，最后只要将所有变截面气囊段的指向矢量并依次累加，即可得到变截面柔性臂末端在绝对坐标系下的精确坐标位置。
在本发明的优选实施例中，各变截面气囊段的基座对应的转换矩阵T具体为：
Figure GDA0003029179740000131
转换时，相对坐标系先绕z轴旋转-θ，接着绕y轴旋转
Figure GDA0003029179740000132
再绕z轴旋转θ。
在本发明的优选实施例中，步骤S1中所述三条线缆为变截面柔性臂的驱动线缆、用于辅助控制变截面柔性臂的线缆或仅用于定位的线缆。
本实施例利用变截面柔性臂已有的线缆进行定位，实施过程中不需要在变截面柔性臂外部设置安装其他定位装置，有效控制了定位系统的体积，避免了在变截面柔性臂外部设置测量装置，大幅简化设备体积和成本，与此同时，本实施例可结合数值计算方法解决了几何解算方法中精度低、忽略变截面的问题，提高变截面柔性臂定位的适应性和精确性。
如图8所示，本发明的另一优选实施例提供了一种变截面柔性臂反馈控制的线长定位装置，包括：
初始赋值模块，用于测量变截面柔性臂的任一变截面气囊段弯曲后三条线缆的实际线长
Figure GDA0003029179740000133
将所测的三条实际线长
Figure GDA0003029179740000134
作为当前变截面气囊段的三条等效线长
Figure GDA0003029179740000141
的初值，i＝1，2，3；所述三条线缆在变截面气囊段未弯曲前以变截面气囊段中心线为旋转中心两两相隔120度；
弯曲角度及程度计算模块，用于根据所述三条等效线长
Figure GDA0003029179740000142
的初值得到当前变截面气囊段的弯曲角度θ和弯曲程度
Figure GDA0003029179740000143
其中，所述弯曲程度
Figure GDA00030291797400001416
为当前变截面气囊段两端截面之间的夹角；所述弯曲角度θ的定义为：设当前变截面气囊段基座截面的圆心为o，所测量的三条实际线长
Figure GDA0003029179740000144
中最短的实际线长
Figure GDA0003029179740000145
与变截面气囊段基座截面的外圆的交点为z，圆心o与交点z的连线所形成的x轴与变截面气囊段的弯曲方向的夹角即为弯曲角度θ，其中，所述变截面气囊段的弯曲方向定义为：变截面气囊段弯曲后其侧面上最短母线与变截面气囊段基座截面外圆具有交点，将圆心o与该交点的连线形成x'轴，则所述x'轴所指的方向即为变截面气囊段的弯曲方向；
位差角计算模块，用于根据当前变截面气囊段的弯曲角度θ和弯曲程度
Figure GDA00030291797400001417
分别得到三条等效线长
Figure GDA0003029179740000146
各自对应的位差角βi，i＝1，2，3，其中，三条等效线长
Figure GDA0003029179740000147
各自对应的位差角βi具体为三条线缆在未弯曲前的变截面气囊段基座截面上的投影线分别与对应的等效线长
Figure GDA0003029179740000148
之间的夹角；
等效线长计算模块，用于根据三条等效线长
Figure GDA0003029179740000149
各自对应的位差角βi、三条实际线长
Figure GDA00030291797400001410
当前变截面气囊段末端截面半径d0、当前变截面气囊段基座截面半径d1得到当前变截面气囊段的三条等效线长
Figure GDA00030291797400001411
的计算值；
迭代计算模块，用于将三条等效线长
Figure GDA00030291797400001412
的计算值作为当前变截面气囊段的三条等效线长
Figure GDA00030291797400001413
的初值后重复计算等效线长
Figure GDA00030291797400001414
直到相邻两次所得的三条等效线长
Figure GDA00030291797400001415
的计算值的差小于设定阈值时，输出当前变截面气囊段最终的三条等效线长
Figure GDA0003029179740000151
弯曲角度θ、弯曲程度
Figure GDA0003029179740000153
和三条等效线长各自对应的位差角βi；
相对坐标计算模块，用于重复计算变截面柔性臂所有变截面气囊段最终的等效线长
Figure GDA0003029179740000152
弯曲角度θ、弯曲程度
Figure GDA0003029179740000154
和三条等效线长各自对应的位差角βi，得到各变截面气囊段末端在其变截面气囊段基座坐标系下的相对坐标；
坐标系转换模块，用于将每一个变截面气囊段的相对坐标转换到绝对坐标系下相累加，得到每一个变截面气囊段末端和变截面柔性臂末端在绝对坐标系下的精确坐标位置。
如图9所示，在本发明的优选实施例还提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现所述的变截面柔性臂反馈控制的线长定位方法。
具体地，在本发明的优选实施例还提供了一种存储介质，所述存储介质包括存储的程序，在所述程序运行时控制所述存储介质所在的设备执行所述的变截面柔性臂反馈控制的线长定位方法。
本发明采用实际线长向等效线长过渡以测量变截面柔性臂状态的方法，对于任意形状柔性机械臂均可采用该方法测算，无需对具体机械臂建模。
本发明面向柔性机械臂系统，可以实现任意柔性机械臂状态下的末端定位和机械臂模态测量，本发明提出基于线长测量任意构型机械臂状态和末端位置的方法，利用几何方法推导等效长度和位置之间的关系式，对于实际线长和等效线长之间的关系式，则通过迭代求解方法解算数值解，进而解决无法得到解析解造成误差较大的问题。与此同时，对于相对坐标系下的相对坐标进行向绝对坐标系转换，依次累加即可得到在绝对坐标系下的机械臂模态和末端坐标；对环境要求低，结构简单体积小。
需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
本实施例方法所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个或者多个计算设备可读取存储介质中。基于这样的理解，本发明实施例对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该软件产品存储在一个存储介质中，包括若干指令用以使得一台计算设备(可以是个人计算机，服务器，移动计算设备或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)，磁碟或者光盘等各种可以存储程序代码的介质。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种变截面柔性臂反馈控制的线长定位方法，其特征在于，包括步骤：
S1、测量变截面柔性臂的任一变截面气囊段弯曲后三条线缆的实际线长
Figure FDA0003029179730000011
将所测的三条实际线长
Figure FDA0003029179730000012
作为当前变截面气囊段的三条等效线长
Figure FDA0003029179730000013
的初值，i＝1，2，3；所述三条线缆在变截面气囊段未弯曲前以变截面气囊段中心线为旋转中心两两相隔120度；
S2、根据所述三条等效线长
Figure FDA0003029179730000014
的初值得到当前变截面气囊段的弯曲角度θ和弯曲程度
Figure FDA0003029179730000015
其中，所述弯曲程度
Figure FDA0003029179730000016
为当前变截面气囊段两端截面之间的夹角；所述弯曲角度θ的定义为：设当前变截面气囊段基座截面的圆心为o，所测量的三条实际线长
Figure FDA0003029179730000017
中最短的实际线长
Figure FDA0003029179730000018
与变截面气囊段基座截面的外圆的交点为z，圆心o与交点z的连线所形成的x轴与变截面气囊段的弯曲方向的夹角即为弯曲角度θ，其中，所述变截面气囊段的弯曲方向定义为：变截面气囊段弯曲后其侧面上最短母线与变截面气囊段基座截面外圆具有交点，将圆心o与该交点的连线形成x'轴，则所述x'轴所指的方向即为变截面气囊段的弯曲方向；
S3、根据当前变截面气囊段的弯曲角度θ和弯曲程度
Figure FDA0003029179730000019
分别得到三条等效线长
Figure FDA00030291797300000110
各自对应的位差角βi，i＝1，2，3；其中，三条等效线长
Figure FDA00030291797300000111
各自对应的位差角βi具体为三条线缆在未弯曲前的变截面气囊段基座截面上的投影线分别与对应的等效线长
Figure FDA00030291797300000112
之间的夹角；
S4、根据三条等效线长
Figure FDA00030291797300000113
各自对应的位差角βi、三条实际线长
Figure FDA00030291797300000114
当前变截面气囊段末端截面半径d0、当前变截面气囊段基座截面半径d1得到当前变截面气囊段的三条等效线长
Figure FDA00030291797300000115
的计算值；
S5、将三条等效线长
Figure FDA0003029179730000021
的计算值作为当前变截面气囊段的三条等效线长
Figure FDA0003029179730000022
的初值后重复步骤S1～S4，直到相邻两次所得的三条等效线长
Figure FDA0003029179730000023
的计算值的差小于设定阈值时，输出当前变截面气囊段最终的三条等效线长
Figure FDA0003029179730000024
弯曲角度θ、弯曲程度
Figure FDA0003029179730000025
和三条等效线长各自对应的位差角βi；
S6、重复步骤S1～S5，计算变截面柔性臂所有变截面气囊段最终的等效线长
Figure FDA0003029179730000026
弯曲角度θ、弯曲程度
Figure FDA0003029179730000027
和三条等效线长各自对应的位差角βi，得到各变截面气囊段末端在其变截面气囊段基座坐标系下的相对坐标；
S7、将每一个变截面气囊段的相对坐标转换到绝对坐标系下相累加，得到每一个变截面气囊段末端和变截面柔性臂末端在绝对坐标系下的精确坐标位置。
2.根据权利要求1所述的变截面柔性臂反馈控制的线长定位方法，其特征在于，所述步骤S2具体包括步骤：
根据所述三条等效线长
Figure FDA0003029179730000028
的初值计算当前变截面气囊段的弯曲角度θ：
Figure FDA0003029179730000029
其中，
Figure FDA00030291797300000210
为分别为三条等效线长，且
Figure FDA00030291797300000211
最短；
根据所述三条等效线长
Figure FDA00030291797300000212
的初值计算当前变截面气囊段的弯曲程度
Figure FDA00030291797300000213
Figure FDA00030291797300000214
其中，
Figure FDA00030291797300000215
为当前变截面气囊段中心弧线实际长度，k为当前变截面气囊段曲率，d0为当前变截面气囊段末端截面半径。
3.根据权利要求1所述的变截面柔性臂反馈控制的线长定位方法，其特征在于，所述步骤S3具体包括步骤：
根据当前变截面气囊段的弯曲角度θ和弯曲程度
Figure FDA0003029179730000031
分别计算三条等效线长
Figure FDA0003029179730000032
各自对应的位差角βi：
Figure FDA0003029179730000033
4.根据权利要求1所述的变截面柔性臂反馈控制的线长定位方法，其特征在于，所述步骤S4具体包括步骤：
根据余弦公式得实际线长
Figure FDA0003029179730000034
和等效线长
Figure FDA0003029179730000035
之间的关系式：
Figure FDA0003029179730000036
将测量的三条实际线长
Figure FDA0003029179730000037
三条等效线长
Figure FDA0003029179730000038
各自对应的位差角βi、当前变截面气囊段末端截面半径d0、当前变截面气囊段基座截面半径d1代入所述关系式，计算得到当前变截面气囊段的三条等效线长
Figure FDA0003029179730000039
的计算值。
5.根据权利要求1所述的变截面柔性臂反馈控制的线长定位方法，其特征在于，所述步骤S7中具体包括步骤：
根据各变截面气囊段的弯曲角度θ和弯曲程度
Figure FDA00030291797300000310
确定各变截面气囊段的基座对应的转换矩阵T；
将当前变截面气囊段的基座的相对坐标系沿靠近变截面柔性臂底座的方向依次累乘各变截面气囊段的基座对应的转换矩阵T，得到当前变截面气囊段末端在绝对坐标系下的指向矢量；
计算所有变截面气囊段的指向矢量并依次累加，得到变截面柔性臂末端在绝对坐标系下的精确坐标位置。
6.根据权利要求5所述的变截面柔性臂反馈控制的线长定位方法，其特征在于，各变截面气囊段的基座对应的转换矩阵T具体为：
Figure FDA0003029179730000041
转换时，相对坐标系先绕z轴旋转-θ，接着绕y轴旋转-
Figure FDA0003029179730000042
再绕z轴旋转θ。
7.根据权利要求1所述的变截面柔性臂反馈控制的线长定位方法，其特征在于，步骤S1中，所述三条线缆为变截面柔性臂的驱动线缆、用于辅助控制变截面柔性臂的线缆或仅用于定位的线缆。
8.一种变截面柔性臂反馈控制的线长定位装置，其特征在于，包括：
初始赋值模块，用于测量变截面柔性臂的任一变截面气囊段弯曲后三条线缆的实际线长
Figure FDA0003029179730000043
将所测的三条实际线长
Figure FDA0003029179730000044
作为当前变截面气囊段的三条等效线长
Figure FDA0003029179730000045
的初值，i＝1，2，3；所述三条线缆在变截面气囊段未弯曲前以变截面气囊段中心线为旋转中心两两相隔120度；
弯曲角度及程度计算模块，用于根据所述三条等效线长
Figure FDA0003029179730000046
的初值得到当前变截面气囊段的弯曲角度θ和弯曲程度
Figure FDA0003029179730000047
其中，所述弯曲程度
Figure FDA0003029179730000048
为当前变截面气囊段两端截面之间的夹角；所述弯曲角度θ的定义为：设当前变截面气囊段基座截面的圆心为o，所测量的三条实际线长
Figure FDA0003029179730000049
中最短的实际线长
Figure FDA00030291797300000410
与变截面气囊段基座截面的外圆的交点为z，圆心o与交点z的连线所形成的x轴与变截面气囊段的弯曲方向的夹角即为弯曲角度θ，其中，所述变截面气囊段的弯曲方向定义为：变截面气囊段弯曲后其侧面上最短母线与变截面气囊段基座截面外圆具有交点，将圆心o与该交点的连线形成x'轴，则所述x'轴所指的方向即为变截面气囊段的弯曲方向；位差角计算模块，用于根据当前变截面气囊段的弯曲角度θ和弯曲程度
Figure FDA00030291797300000411
分别得到三条等效线长
Figure FDA00030291797300000412
各自对应的位差角βi，i＝1，2，3，其中，三条等效线长
Figure FDA00030291797300000413
各自对应的位差角βi具体为三条线缆在未弯曲前的变截面气囊段基座截面上的投影线分别与对应的等效线长
Figure FDA0003029179730000051
之间的夹角；
等效线长计算模块，用于根据三条等效线长
Figure FDA0003029179730000052
各自对应的位差角βi、三条实际线长
Figure FDA0003029179730000053
当前变截面气囊段末端截面半径d0、当前变截面气囊段基座截面半径d1得到当前变截面气囊段的三条等效线长
Figure FDA0003029179730000054
的计算值；
迭代计算模块，用于将三条等效线长
Figure FDA0003029179730000055
的计算值作为当前变截面气囊段的三条等效线长
Figure FDA0003029179730000056
的初值后重复计算等效线长
Figure FDA0003029179730000057
直到相邻两次所得的三条等效线长
Figure FDA0003029179730000058
的计算值的差小于设定阈值时，输出当前变截面气囊段最终的三条等效线长
Figure FDA0003029179730000059
弯曲角度θ、弯曲程度
Figure FDA00030291797300000510
和三条等效线长各自对应的位差角βi；
相对坐标计算模块，用于重复计算变截面柔性臂所有变截面气囊段最终的等效线长
Figure FDA00030291797300000511
弯曲角度θ、弯曲程度
Figure FDA00030291797300000512
和三条等效线长各自对应的位差角βi，得到各变截面气囊段末端在其变截面气囊段基座坐标系下的相对坐标；
坐标系转换模块，用于将每一个变截面气囊段的相对坐标转换到绝对坐标系下相累加，得到每一个变截面气囊段末端和变截面柔性臂末端在绝对坐标系下的精确坐标位置。
9.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1至7中任一项所述的变截面柔性臂反馈控制的线长定位方法。
10.一种存储介质，所述存储介质包括存储的程序，其特征在于，在所述程序运行时控制所述存储介质所在的设备执行如权利要求1至7中任一项所述的变截面柔性臂反馈控制的线长定位方法。
CN202010992971.4A 2020-09-21 2020-09-21 一种变截面柔性臂反馈控制的线长定位方法、装置和介质 Active CN112356020B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010992971.4A CN112356020B (zh) 2020-09-21 2020-09-21 一种变截面柔性臂反馈控制的线长定位方法、装置和介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010992971.4A CN112356020B (zh) 2020-09-21 2020-09-21 一种变截面柔性臂反馈控制的线长定位方法、装置和介质

Publications (2)

Publication Number Publication Date
CN112356020A CN112356020A (zh) 2021-02-12
CN112356020B true CN112356020B (zh) 2021-06-04

Family

ID=74516615

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010992971.4A Active CN112356020B (zh) 2020-09-21 2020-09-21 一种变截面柔性臂反馈控制的线长定位方法、装置和介质

Country Status (1)

Country Link
CN (1) CN112356020B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104814792A (zh) * 2015-04-01 2015-08-05 上海交通大学 可分离多臂软体机械臂装置
CN106584507A (zh) * 2016-05-19 2017-04-26 北京卫星环境工程研究所 全柔性气动机械臂结构
CN109176494A (zh) * 2018-09-28 2019-01-11 哈尔滨工业大学（深圳） 绳驱多关节柔性机器人自标定方法及系统、存储介质
CN110076775A (zh) * 2019-04-25 2019-08-02 清华大学深圳研究生院 一种绳驱动连续型机械臂的三维静力学建模方法
JP2020044216A (ja) * 2018-09-21 2020-03-26 国立大学法人東京工業大学 ワイヤ駆動マニピュレータ装置
CN111015737A (zh) * 2019-12-25 2020-04-17 长安大学 一种单连杆柔性机械臂的减振方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104814792A (zh) * 2015-04-01 2015-08-05 上海交通大学 可分离多臂软体机械臂装置
CN106584507A (zh) * 2016-05-19 2017-04-26 北京卫星环境工程研究所 全柔性气动机械臂结构
JP2020044216A (ja) * 2018-09-21 2020-03-26 国立大学法人東京工業大学 ワイヤ駆動マニピュレータ装置
CN109176494A (zh) * 2018-09-28 2019-01-11 哈尔滨工业大学（深圳） 绳驱多关节柔性机器人自标定方法及系统、存储介质
CN110076775A (zh) * 2019-04-25 2019-08-02 清华大学深圳研究生院 一种绳驱动连续型机械臂的三维静力学建模方法
CN111015737A (zh) * 2019-12-25 2020-04-17 长安大学 一种单连杆柔性机械臂的减振方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
变截面线驱动柔性机械臂运动学建模与分析;王涛等;《机械工程与自动化》;20170430(第2期);第49-50页 *
线驱动柔性机械臂的运动学分析;俞晓瑾;《微型电脑应用 研究与设计》;20130420;第29卷(第4期);第8-11页 *

Also Published As

Publication number Publication date
CN112356020A (zh) 2021-02-12

Similar Documents

Publication Publication Date Title
Rao et al. How to model tendon-driven continuum robots and benchmark modelling performance
CN107738254B (zh) 一种机械臂坐标系的转换标定方法与系统
CN112975973B (zh) 一种应用于柔性机器人的混合标定方法及装置
CN111168719B (zh) 一种基于定位工装的机器人校准方法及系统
CN113160334B (zh) 一种基于手眼相机的双机器人系统标定方法
CN105807712B (zh) 一种六自由度并联机器人正向运动学的对偶四元数求解的方法
CN107339987B (zh) 一种基于函数迭代积分的刚体姿态解算方法
CN111216164A (zh) 一种机器人在线校准方法、系统、存储介质及校准设备
CN113618742B (zh) 一种机器人避障方法、装置和机器人
Andersson Discretization of a continuous curve
Lu et al. Kinematic modeling of a class of n-tendon continuum manipulators
CN113500599A (zh) 一种基于神经网络的绳驱机械臂的轨迹跟踪方法及系统
CN116038647A (zh) 代步车、机械臂的控制方法、电子设备及存储介质
Wiese et al. Kinematic modeling of a soft pneumatic actuator using cubic hermite splines
CN110125933B (zh) 软体机械臂的力学模型的建立方法及控制方法
CN112356020B (zh) 一种变截面柔性臂反馈控制的线长定位方法、装置和介质
JP7250089B2 (ja) ソフトロボットマニピュレータの特異点のない運動パラメータ化
CN109434838B (zh) 线驱动连续机器人内窥操作的协同运动规划方法及系统
CN108534774B (zh) 基于函数迭代积分的刚体姿态解算方法及系统
CN110900608B (zh) 基于最优测量构型选择的机器人运动学标定方法
Gu et al. Dexterous obstacle-avoidance motion control of Rope Driven Snake Manipulator based on the bionic path following
CN111796520A (zh) 软体机器人的建模方法和建模系统、电子设备及存储介质
US20220092316A1 (en) Method, device, computer program and computer program product for providing a trajectory of an object for a vehicle
Singh et al. Towards accurate shape reconstruction of compact bionic handling arm
Chen et al. Modeling of continuum robots with environmental constraints

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant