CN117245663A - 绳驱空间柔性机械臂力位型融合柔顺控制方法及*** - Google Patents
绳驱空间柔性机械臂力位型融合柔顺控制方法及*** Download PDFInfo
- Publication number
- CN117245663A CN117245663A CN202311364302.2A CN202311364302A CN117245663A CN 117245663 A CN117245663 A CN 117245663A CN 202311364302 A CN202311364302 A CN 202311364302A CN 117245663 A CN117245663 A CN 117245663A
- Authority
- CN
- China
- Prior art keywords
- arm
- force
- angle
- expected
- control
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 55
- 230000004927 fusion Effects 0.000 title claims abstract description 52
- 239000011159 matrix material Substances 0.000 claims abstract description 37
- 238000004364 calculation method Methods 0.000 claims abstract description 8
- 239000013598 vector Substances 0.000 claims description 35
- 230000009466 transformation Effects 0.000 claims description 13
- 238000004590 computer program Methods 0.000 claims description 7
- 238000000418 atomic force spectrum Methods 0.000 claims description 5
- 101100234547 Caenorhabditis elegans rod-1 gene Proteins 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 9
- 230000008569 process Effects 0.000 description 7
- 238000013461 design Methods 0.000 description 5
- 230000007613 environmental effect Effects 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 230000007246 mechanism Effects 0.000 description 3
- 230000001360 synchronised effect Effects 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 239000012636 effector Substances 0.000 description 2
- 230000000737 periodic effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 230000001747 exhibiting effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 210000003857 wrist joint Anatomy 0.000 description 1
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
- B25J9/1615—Programme controls characterised by special kind of manipulator, e.g. planar, scara, gantry, cantilever, space, closed chain, passive/active joints and tendon driven manipulators
-
- 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/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
- B25J9/104—Programme-controlled manipulators characterised by positioning means for manipulator elements with cables, chains or ribbons
-
- 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
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Manipulator (AREA)
Abstract
本发明公开了绳驱空间柔性机械臂力位型融合柔顺控制方法及***，包括如下步骤：建立DH坐标系，并根据任务规划得到期望力/力矩、以及期望位姿和期望臂型角；利用关节角度编码器采集当前位姿和当前臂型角；利用六维力传感器采集当前力/力矩；通过将期望力/力矩、期望位姿和期望臂型角分别与当前力/力矩、当前位姿和当前臂型角作差，分别得到力/力矩误差、位姿误差和臂型角误差，并通过力位型融合控制器计算得到控制率；将控制率通过扩展雅可比矩阵得到关节运动期望角度；根据关节运动期望角度，通过关节控制器得到下一个控制周期关节运动指令，驱动驱动箱内电机运动完成力位型融合柔顺控制，实现在狭小、多障碍环境中实现安全、柔顺的操控。
Description
技术领域
本发明涉及空间机器人在轨柔顺控制技术领域，特别是涉及一种绳驱空间柔性机械臂力位型融合柔顺控制方法及***。
背景技术
绳驱空间柔性机械臂采用绳索进行远程驱动，实现了机电分离。这种设计具有大长径比和高冗余自由度，使得它能够适应恶劣的太空环境，并在狭小、非结构化环境中展现出独特的优势。这种机械臂可在轨道上执行狭缝穿越、故障维修、部件装配等精细作业任务。
然而，传统的柔顺控制方法仅能对机械臂末端位姿和/或末端操作力/力矩进行控制，无法对机械臂臂型进行同步控制。当机械臂工作于狭小、多障碍环境中时，若不对机械臂臂型进行控制使其与作业环境相匹配，操作臂将与作业环境发生接触碰撞，导致机械臂无法正常工作，严重的还可能损坏机械臂或其他设备。
发明内容
为了解决针对现有技术中柔性机械臂在狭小空间和非结构化环境中的操控能力受限、操作效率较低和安全性较低的技术问题，本发明的首要目的在于提供一种绳驱空间柔性机械臂力位型融合柔顺控制方法。
本发明的再一目的是提供一种绳驱空间柔性机械臂力位型融合柔顺控制***，包括处理器和存储器，所述存储器中存储有计算机程序，所述计算机程序可被处理器执行以实现上述的控制方法。
本发明的技术问题通过以下的技术方案予以解决：
一种绳驱空间柔性机械臂力位型融合柔顺控制方法，所述绳驱空间柔性机械臂包括驱动箱、柔性臂臂杆、六维力传感器以及内设有关节角度编码器的中心块，包括如下步骤：
S1、建立所述柔性臂臂杆以及中心块的DH坐标系，并根据任务规划得到所述柔性臂臂杆的期望力/力矩、以及所述中心块的期望位姿和期望臂型角；
S2、利用所述关节角度编码器采集所述中心块的当前位姿和当前臂型角；利用六维力传感器采集所述柔性臂臂杆的当前力/力矩；
S3、通过将所述期望力/力矩、期望位姿和期望臂型角分别与所述当前力/力矩、当前位姿和当前臂型角作差，分别得到力/力矩误差、位姿误差和臂型角误差，并通过力位型融合控制器计算得到闭环控制组的控制率；
S4、将所述闭环控制组的控制率通过扩展雅可比矩阵得到关节运动期望角度；
S5、根据所述关节运动期望角度，通过关节PID控制器得到下一个控制周期关节运动指令，驱动所述驱动箱内电机运动完成整个周期的力位型融合柔顺控制。
在一些实施例中，还包括：判断所述绳驱空间柔性机械臂是否完成设定的操作运动轨迹，若未完成，则重复步骤S2至步骤S5以继续进入下一个控制周期的力位型融合柔顺控制，否则结束力位型融合柔顺控制。
在一些实施例中，步骤S1中，所述DH坐标系是通过D-H方法建立D-H参数表并得到各个点笛卡尔空间下的齐次变换矩阵表达式为：
式中：为从基座坐标系到连杆i之间的齐次变换矩阵的表达式，0代表基座坐标系；/>和/>分别为第i-1连杆尾端与第i连杆首端的齐次变换矩阵；每个齐次变换矩阵包括了旋转和平移信息，描述了一个连杆如何相对于前一个连杆移动和旋转。由于中心块处具有两个自由度，所以两个连杆间的其次变换矩阵为两个相乘的形式。
在一些实施例中，所述期望臂型角和所述当前臂型角中的臂型角ψ1的表达式为：
其中，V1为任意固定单位矢量；向量h1为垂直于向量w1且为臂型面A内的辅助向量；k1为垂直于w1且垂直于臂型面A的辅助向量；和/>分别为向量w1和V1的单位向量；T右上角标表示对应矩阵的转置矩阵。。
在一些实施例中，步骤S3中，所述位姿误差的表达式为：XedS＝ep；其中，ep为位姿误差，Xed为位姿差，S为矩阵；
所述力/力矩误差的表达式为：其中，eF为力/力矩误差，Fed为力/力矩差值，，矩阵/>
所述臂型角误差的表达式为：
ψed＝ψd-ψs(θs)＝[ψ1,d-ψ1,s(θs)ψ2,d-ψ2,s(θs)]；
其中，其中，ψed为臂型角差；ψd臂型角期望值，ψs(θs)为通过关节角度编码器采集的当前关节角度值θs算得到的臂型角当前值；ψ1,d和ψ2,d分别为臂型角1和臂型角2的期望值；ψ1,s(θs)和ψ2,s(θs)分别为通过关节角度编码器采集的当前关节角度值计算得到的臂型角1与臂型角2的当前值。
在一些实施例中，步骤S3中，所述闭环控制组包括力/力矩闭环控制、位置闭环控制和臂型角闭环控制。
在一些实施例中，步骤S4中，所述雅可比矩阵为Jex(θ)＝[J|Jev]；其中，所述臂型角雅可比矩阵Jev的表达式为：
其中，J为末端到关节的雅可比矩阵；θ为关节角度θi的集合，即θ＝[θ1,θ2,…,θi]；n为柔性臂连杆数量；ψs(θs)为当前臂型角，而和/>分别为臂型角1和臂型角2对θi的偏微分。
在一些实施例中，步骤S4中，所述关节运动期望角度θcmd的表达式为：θcmd＝θd+Δθ；
其中，θd为期望的关节角度，θd＝[θ1,d,θ2,d,…,θi,d]；Δθ为关节运动偏移量。
在一些实施例中，步骤S5中，所述下一个控制周期关节运动指令的表述式为：
其中，θ(s)为下一个控制周期关节运动指令；Kp为比例增益系数、Ki为积分增益系数和Kd为微分增益系数；s为控制周期。
本发明还提出了一种绳驱空间柔性机械臂柔顺控制***，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现上述的控制方法。
本发明与现有技术对比的有益效果包括：
本发明通过结合绳驱空间柔性机械臂自身结构机构特性和闭环控制，实现绳驱空间柔性机械臂在接触操作过程中顺应外部作用力/力矩和环境约束，以确保全***的安全性；同时通过设计融合全***力/力矩、位置、臂型角三要素的多目标融合控制率，实现绳驱空间柔性机械臂的操作力、位姿以及整臂接触力、臂型角按期望特性变化，以满足狭小空间大范围操控的精度、速度、操作力和刚度需求，从而实现能够在狭小、多障碍环境中实现安全、柔顺的操控，为空间机器人在轨柔顺操控提供重要支撑。
本发明实施例中的其他有益效果将在下文中进一步述及。
附图说明
图1是本发明实施例中绳驱空间柔性机械臂力位型融合柔顺控制方法的流程图。
图2是本发明实施例中绳驱空间柔性机械臂的侧视图。
图3是本发明实施例中柔性机械臂的中心块坐标示意图。
图4是本发明实施例中臂型角计算示意图。
图5是本发明实施例中绳驱空间柔性机械臂力位型控制原理图。
图6是本发明实施例中绳驱空间柔性机械臂力位型控制流程图。
图7是本发明实施例的双闭环伺服控制流程图。
图8是本发明实施例的绳驱空间柔性机械臂实物图。
图9是本发明实施例的绳驱空间柔性机械臂三维图。
附图标记：
臂杆100、驱动电机200、中心块300、驱动箱400、驱动绳索500、六维力传感器600、关节角度编码器700。
具体实施方式
下面对照附图并结合优选的实施方式对本发明作进一步说明。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。
需要说明的是，本实施例中的左、右、上、下、顶、底等方位用语，仅是互为相对概念，或是以产品的正常使用状态为参考的，而不应该认为是具有限制性的。
目前只有少量的涉及机械臂进行力-位混合控制的研究。虽然刚性关节机械臂采用刚性关节机械臂进行力-位混合控制，在精度和速度方面具有一定优势，但其在狭小空间和非结构化环境中的操控能力受到限制。此外，刚性关节机械臂的自由度较低，无法适应复杂的太空环境。与本发明相比，刚性关节机械臂控制技术方案在狭小空间和非结构化环境中的操控能力较弱，且无法实现高冗余自由度，具体包括以下方面缺点：
1.无法实现高冗余自由度，导致在复杂环境中的操控能力受限。
2.在狭小空间和非结构化环境中，刚性关节机械臂的力-位混合控制方法可能无法充分利用机械臂的柔性特性，无法同时对机械臂的末端位姿、操作力和臂型三个要素进行控制，从而导致操作效率降低和安全性降低。
3.传统柔顺控制方法只能对绳驱空间柔性机械臂的末端位姿和/或末端操作力/力矩进行控制，刚性关节机械臂缺乏对机械臂臂型的同步控制，在狭小、多障碍环境中操作时，如果不对机械臂的臂型进行控制以使其与作业环境相匹配，机械臂可能与作业环境发生接触碰撞，影响机械臂的正常工作，甚至可能损坏机械臂或其他设备。
为了解决现有柔顺控制技术中仅考虑机械臂末端位姿或/和末端操作力控制带来的柔性机械臂在狭小环境中的臂型控制问题，并为了在狭小、多障碍的环境中操作，机械臂不仅需要沿着期望轨迹运动和产生期望的操作力/力矩，还需要调整臂型以适应操作环境，避免碰撞和冲击，因此本发明实施例提出了一种绳驱空间柔性机械臂力位型融合柔顺控制方法和相应的控制***。
该方法将机器人自身结构机构特性与闭环控制相结合，实现接触操作过程中空间机器人顺应外部作用力/力矩和环境约束，以确保全***安全。这是绳驱空间柔性机械臂在轨柔顺操控能力的保证。绳驱空间柔性机械臂力位型融合柔顺控制方法还结合操作路径和任务特性，设计了融合全***力、位、型三要素的多目标融合控制律。以整臂驱动代价最小为目标进行多重空间运动分解，构建了驱动空间-任务空间双闭环联合控制框架。
上述设计实现了机械臂的操作力、位姿及整臂接触力、臂型按期望特性变化，能够同时控制机械臂的末端位姿、操作力和臂型，满足狭小空间大范围操控的精度、速度、操作力和刚度的需求。通过这种力位型融合柔顺控制方法和***，绳驱空间柔性机械臂能够在狭小、多障碍环境中实现安全、柔顺的操控，确保机械臂能够适应外部环境并保持安全工作，为空间机器人在轨柔顺操控提供重要支撑。
本发明解决现有柔顺控制技术中仅考虑机械臂末端位姿或/和末端操作力控制带来的技术问题的原理如下：
本发明实施例提出的绳驱空间柔性机械臂力位型融合柔顺控制方法能够对机械臂末端位姿、操作力及机械臂臂型三个要素同时进行控制，使得机械臂在狭小、多障碍环境中操作时不仅其末端沿着期望轨迹运动，接触环境、产生期望的操作力/力矩，还使得运动过程中机械臂臂型适应操作环境而不产生过大的碰撞和冲击，具有更高的安全性和柔顺性，为空间机器人在轨柔顺操控提供了重要支撑。
如图2所示，本发明实施例的绳驱空间柔性机械臂包括柔性臂臂杆100、驱动电机200、中心块300、驱动箱400、驱动绳索500、六维力传感器600、关节角度编码器700。其中上驱动电机200选型配置，柔性臂臂杆100长度等设计参数是可调整的。其中，驱动箱400和串联连接的N段柔性臂臂杆段，N段柔性臂臂杆段包括依次连接的柔性臂臂杆100和内设有关节角度编码器的中心块300，每个柔性臂臂杆段通过三条驱动绳索500连接到驱动箱400中，每个中心块300内设置有用于采集中心块关节的角度信息的关节角度编码器700，每根驱动绳索500设置有用于实时采集驱动绳索拉力的六维力传感器600，本实施例的六维力传感，600是绳索拉力传感器，其设置在驱动箱400内。
本发明实施例的绳驱空间柔性机械臂力位型融合柔顺控制方法如图1所示，其包括以下步骤：
S1：建立绳驱空间中柔性臂臂杆和中心块的DH坐标系，并根据任务规划得到柔性臂臂杆的期望力/力矩、以及中心块的期望位姿和期望臂型角。
S2：利用关节角度编码器采集绳驱柔性机械臂的中心块的当前位姿和当前臂型角；具体是采集的中心块的角度，并计算N段臂杆段上中心块位置、末端位置及臂型角参数；通过末端六维力传感器，采集到柔性臂臂杆的当前力/力矩。
S3：通过将期望力/力矩、期望位姿和期望臂型角分别与当前力/力矩、当前位姿和当前臂型角作差，分别得到力/力矩误差、位姿误差和臂型角误差，并通过力位型融合控制器计算得到闭环控制组的控制率；具体是根据选择矩阵对操作力、位置和臂型进行选择规划及控制。
S4：将所述闭环控制组的控制率通过扩展雅可比矩阵得到关节运动期望角度；
S5：根据关节运动期望角度，通过关节空间闭环控制器(即关节PID控制器)获得下一周期关节运动指令，驱动所述驱动箱内电机运动完成整个周期的力位型融合柔顺控制。
S6：通过关节空间到绳索空间的映射将输出的关节控制量进一步转换为绳索的运动速度量。通过绳索空间到驱动空间的映射将绳索的运动速度量转化为电机的运动速度，以驱动绳索完成下一周期的机械臂操作。判断轨迹是否走完，若未走完则返回步骤S2进入下一个控制周期，否则结束控制。
柔性机械臂中心块的中心点坐标如图3所示，其中P0至PN+1各个点代表了各个中心块中心处的位置的三维坐标信息。
在本发明实施例中的步骤S1还包括计算各个中心块位置和臂型角，具体操作如下：
其中DH坐标系是通过D-H方法(一种为关节链中的每一杆件建立坐标系的矩阵方法)，建立D-H参数表(相邻各个关节坐标的变换关系表)，并且带入当前关节角度θi，得到各个点笛卡尔空间下的齐次变换矩阵其表达式如下：
其中，该公式中和/>分别为“坐标系2i-2到2i-1的齐次变换矩阵”和“坐标系2i-1到2i的齐次变换矩阵”。/>为连乘符号。因此，齐次变换矩阵/>及机械臂基座原点P0可以获得P1至PN+1各个点的笛卡尔空间坐标。进一步为了获得臂角相关计算，局部坐标系中建立了两个辅助向量w1＝P3–P1；e1＝P2-P1。本发明实施例中的臂型角计算原理如图4所示，再令V1表示任意固定单位矢量，辅助向量d1是向量e1在向量w1上的投影。可以通过几何导出其他参数如下：
其中向量h1为垂直于向量w1(或者向量d1)且为臂型面A内的辅助向量；k1为垂直于w1(或者d1)且垂直于臂型面A的辅助向量；I为单位向量，即为3×3对角线全为1的单位向量；和/>分别为向量w1和V1的单位向量。
S3、通过将所述期望力/力矩、期望位姿和期望臂型角分别与所述当前力/力矩、当前位姿和当前臂型角作差，分别得到力/力矩误差、位姿误差和臂型角误差，并通过力位型融合控制器计算得到闭环控制组的控制率；根据上述原理进一步推导出期望臂型角和当前臂型角中的臂型角ψ1为：
其中，V1为任意固定单位矢量；向量h1为垂直于向量w1且为臂型面A内的辅助向量；k1为垂直于w1且垂直于臂型面A的辅助向量；和/>分别为向量w1和V1的单位向量。
同样的根据PN-3、PN-2和PN-1三点的迪卡空间下的坐标(图3展示为N＝6的情况)也可以推导得到腕部关节的臂角ψ2。其中具体的推导和等式关系与ψ1相同，不同的是w2＝P4–P3；e4＝O5–P3。然后再去求解d2、h2、k1和ψ2即可。
本发明实施例的步骤S3包括，需要将反馈得到的力/力矩信息进行分解，得到当前与期望的力差值Fed。同样的结合步骤S1，也可以获得当前位姿的差值Xed。考虑到力-位混合控制的位置控制包含k个位置约束，6-k个力约束。将下一个周期的期望位姿与当前得到的位姿进行做差得到位姿差Xed，再与位姿选择矩阵S相乘得到位置控制率。其中位姿误差ep如下所示：
XedS＝ep
其中ep为位置控制的误差值。同样的，对于末端操作力也要进行选择矩阵的定义，因此力选择矩阵同样的可以得到力误差为：
其中eF为力控制的误差值。柔性臂的期望臂型角主要是由不同任务规划的期望值，因此期望柔性臂臂型角参数ψd＝[ψ1,d,ψ2,d]。同样的利用期望值(即期望臂型角)与当前值(即当前臂型角)ψs(θs)＝[ψ1,s(θs),ψ2,s(θs)]的差值得到臂型角的误差值ψed(即臂型角误差)如下所示：
ψed＝ψd-ψs(θs)＝[ψ1,d-ψ1,s(θs)ψ2,d-ψ2,s(θs)]；
其中当前臂型角ψs(θs)为通过关节角度编码器700采回的当前关节角度值θs＝[θ1,s,θ2,s,…,θi,s]计算得到的臂型角ψs(θs)。绳驱空间柔性机械臂力位型控制框图如图5所示：
根据规划值可以得到期望的力/力矩、期望位姿和期望臂型角，并利用六维力传感器和关节角度编码器的值可以得到当前的力/力矩、当前位姿和当前臂型角。通过将期望值与当前值做差可以得到：力/力矩的误差、位置误差和臂型的误差，并将其通过力位型融合控制器(一种PID——比例-积分-微分控制器)计算得出闭环控制组的控制率，并将其发送给伺服驱动器驱动电机运动，从而完成整个周期的力位型融合柔顺控制。
闭环控制组包括力/力矩、位置和臂型的三个闭环控制。
在力/力矩闭环控制中，首先需要从六维力传感器中获取机械臂末端的实时受力信息，并将其与期望值进行比较。将期望受力与实际受力之间的误差作为输入，通过力位型融合控制器计算出下一周期的力/力矩控制律，将其作为末端执行器的输出，从而使机械臂末端输出力/力矩达到期望值。
在位置闭环控制中，首先需要将规划的位姿作为期望值，将期望位姿与实际位姿之间的误差作为输入，通过力位型融合控制器计算出下一周期的位姿控制律，来实现对末端运动的跟踪，以进一步提高运动控制的特性和精度。
在臂型闭环控制中，主要是针对空间机械臂的臂型参数进行控制，以保持机械臂在运动过程中具有一定的臂型形状，从而适应于一些特定工作场景。需要将获得的臂型角作为臂型***的期望，设置臂角的PID控制器，来实现对末端运动的跟踪，以进一步提高其控制特性。
S4、将所述闭环控制组的控制率通过扩展雅可比矩阵得到关节运动期望角度；具体操作如下：
将得到力位型融合控制器输出的控制率乘以扩展雅可比矩阵(即臂型角雅可比矩阵)Jex(θ)＝[J|Jev]目的是将力位型空间分解至关节空间，其中J是末端到关节的雅可比矩阵；θ为关节角度θi的集合，即θ＝[θ1,θ2,…,θi]；而Jev是臂型角雅可比矩阵。
其中J为末端到关节的雅可比矩阵；θ为关节角度θi的集合，即θ＝[θ1,θ2,…,θi]；ψs(θs)为当前臂型角，而和/>分别为臂型角1和臂型角2对θi的偏微分。通过上述计算过程可以得到各个关节角度的期望值。
结合臂角的扩展雅可比矩阵其是机械臂末端操作力-位姿控制任务的扩充任务，进而可以填充机械臂的关节冗余的自由度，使得机械臂在扩展任务(臂角+位置+姿态+力)特定的情况下，其关节角度有唯一的增量即关节运动偏移量Δθ。需要指出的是，从物理意义上看，极端情况下，当机械臂快伸直时，臂角退化为一条直线，其无法有效表示机器人的构型，由此臂角是有一定范围的，否则容易出现奇异。
因而结合图5的绳驱空间柔性机械臂力位型控制框图，得到如图6所示的绳驱空间柔性机械臂力位型流程图，其说明了整个控制的流程关系，其中部分内容进一步说明：当前周期的关节角指令是由任务规划而得到的，主要包括期望位姿、期望力/力矩和期望臂型角；需期望值要与通过六维力传感器和关节编码器得到当前值做差才能进一步发送给控制器；三个控制器融合在一起作为力位型融合控制器；(扩展)雅可比矩阵的作用是将控制器输出的控制率转化到关节角的增量Δθ；关节空间到电机空间的映射主要是将关节角度与电机的运动进行转化，最终转换到电机的偏移量，从而实现整个闭环的控制。
本发明实施例中的步骤S4包括，将机械臂末端的力位型偏移量转化为关节的关节运动偏移量Δθ后，将当前周期关节运动指令(运动层规划的指令)与关节运动偏移量相加，得到关节运动期望角度θcmd。其中θd为期望的关节角度的集合即：θd＝[θ1,d,θ2,d,…,θi,d]。进一步说明：θd主要是由任务规划的期望的关节角度，即通过期望位姿计算得到的期望关节角度，而关节运动期望角度θcmd是考虑了力/力矩与臂型角后的运动期望角度，即为下一层位置控制内环的输入指令。进一步说明，关节运动期望角度θcmd计算如下：
θcmd＝θd+Δθ；
式中，θd表示为期望的关节角度，θd＝[θ1,d,θ2,d,…,θi,d]；Δθ表示关节运动偏移量。
在本发明实施例的步骤S5具体操作如下：
将获得的关节运动期望角度作为关节运动***的期望，设置一个关节位置的关节PID控制器，来实现对关节期望运动的跟踪，以进一步提高其控制特性。具体地，以关节运动期望角度θcmd以及关节编码器反馈的当前关节角度信息θs的差值作为控制器的输入，通过一个PID控制器，输出下一控制周期关节运动反馈控制规划值θ(s)(即关节运动指令)，控制器的输入输出计算的下一个控制周期关节运动指令，其表达式如下：
式中，θ(s)表示下一周期关节运动指令；Kp为比例增益系数、Ki积分增益系数和Kd微分增益系数；s为控制周期。
本实施例的步骤S6在获得下一周期关节运动指令后，通过柔性机械臂关节空间运动到驱动空间运动的转换关系，得到机械臂驱动空间的期望运动θ，并控制驱动电机运动，完成一个力位型控制的规划周期。在驱动电机运动的过程中，可以根据电机编码器的反馈值θd进行闭环控制，进一步保证驱动电机对期望运动的跟随性能。
本发明实施例中的绳驱空间柔性机械臂力位型融合柔顺控制方法整体主要包括了双闭环伺服控制，因此将***的控制框图绘制如图7所示。其双闭环控制主要包含了“力位型闭环控制”及“关节闭环控制”两个伺服控制。根据规划值可以得到期望的力/力矩、期望位姿和期望臂型角，并利用六维力传感器和关节角度编码器的值可以得到当前的力/力矩、当前位姿和当前臂型角。通过将期望值与当前值做差可以得到：力/力矩的误差、位置误差和臂型的误差，并将其通过力位型融合控制器计算得出闭环控制组的控制率，并通过扩展雅可比矩阵计算得到关节运动偏移量Δθ及关节运动期望角度θcmd，并且通过关节PID控制器得到关节角度控制率，进一步转化至电机的期望值完成一整个周期控制。
经过上述步骤S1至S6完成了一个力位型控制周期，若此时绳驱空间柔性机械臂完成设定的操作运动指令(即操作运动轨迹)，即给绳驱空间柔性机械臂的运动指令没有停止(即未完成)，则重复步骤S2至S6以继续进行另一个控制周期(即下一个控制周期)的规划与运动(即力位型融合柔顺控制)；否则结束力位型融合柔顺控制。
本发明实施例提出了一种绳驱空间柔性机械臂柔顺控制***，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现绳驱空间柔性机械臂力位型融合柔顺控制方法。
本发明实施例提出的绳驱空间柔性机械臂力位型融合柔顺控制***充分利用了柔性关节机械臂的高冗余自由度，使其能够在复杂的太空环境中实现更灵活的操控。这有助于解决现有技术中刚性关节机械臂在狭小空间和非结构化环境中操控能力受限的问题。并结合机器人自身结构机构特性与闭环控制，实现了空间机器人在接触操作过程中顺应外部作用力/力矩和环境约束，确保全***安全。这有助于解决现有技术中刚性关节机械臂在狭小空间和非结构化环境中可能导致操作效率降低和安全性降低的问题。本发明实施例还设计了合全***力、位、型三要素的多目标融合控制律，实现了操作力、位姿及整臂接触力、臂型按期望特性变化。这有助于解决现有技术中刚性关节机械臂缺乏对机械臂臂型的同步控制，可能导致在狭、多障碍环境中与作业环境发生接触碰撞的问题。
综上，本发明实施例通过高冗余自由度、柔顺控制和同步控制等技术特征，有效解决了现有技术中刚性关节机械臂在狭小空间和非结构化环境中的操控能力受限、操作效率降低和安全性降低等技术问题，为狭小空间大范围操控的精度、速度、操作力和刚度的需求提供了有力保障。
在不同的实施例中，设计一款类似上述实施例的柔性臂，并安装有六维力传感器和关节编码器等；双闭环控制中的PID参数根据具体性能进行调参，在其他实施例中力位型融合控制器的比例系数为80，积分系数为0.2，微分系数为100。关节闭环控制的比例系数为30，积分系数为0.05，微分系数为0.1。利用本实施例建立起力位型控制模型，并设计一组轨迹进行具体运动，通过实验验证如下参数：位置精度，力/力矩精度和臂型角的精度。本实施例的绳驱空间柔性机械臂实物图如图8所示，三维图如图9所示。
以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。
Claims (10)
1.一种绳驱空间柔性机械臂力位型融合柔顺控制方法，其特征在于，所述绳驱空间柔性机械臂包括驱动箱、柔性臂臂杆、六维力传感器以及内设有关节角度编码器的中心块，包括如下步骤：
S1、建立所述柔性臂臂杆以及中心块的DH坐标系，并根据任务规划得到所述柔性臂臂杆的期望力/力矩、以及所述中心块的期望位姿和期望臂型角；
S2、利用所述关节角度编码器采集所述中心块的当前位姿和当前臂型角；利用六维力传感器采集所述柔性臂臂杆的当前力/力矩；
S3、通过将所述期望力/力矩、期望位姿和期望臂型角分别与所述当前力/力矩、当前位姿和当前臂型角作差，分别得到力/力矩误差、位姿误差和臂型角误差，并通过力位型融合控制器计算得到闭环控制组的控制率；
S4、将所述闭环控制组的控制率通过扩展雅可比矩阵得到关节运动期望角度；
S5、根据所述关节运动期望角度，通过关节PID控制器得到下一个控制周期关节运动指令，驱动所述驱动箱内电机运动完成整个周期的力位型融合柔顺控制。
2.如权利要求1所述的绳驱空间柔性机械臂力位型融合柔顺控制方法，其特征在于，还包括：判断所述绳驱空间柔性机械臂是否完成设定的操作运动轨迹，若未完成，则重复步骤S2至步骤S5以继续进入下一个控制周期的力位型融合柔顺控制，否则结束力位型融合柔顺控制。
3.如权利要求1所述的绳驱空间柔性机械臂力位型融合柔顺控制方法，其特征在于，步骤S1中，所述DH坐标系是通过D-H方法建立D-H参数表并得到各个点笛卡尔空间下的齐次变换矩阵表达式为：
其中：为从基座坐标系到连杆i之间的齐次变换矩阵的表达式，0代表基座坐标系；和/>分别为第i-1连杆尾端与第i连杆首端的齐次变换矩阵。
4.如权利要求1所述的绳驱空间柔性机械臂力位型融合柔顺控制方法，其特征在于，所述期望臂型角和所述当前臂型角中的臂型角ψ1的表达式为：
其中，V1为任意固定单位矢量；向量h1为垂直于向量w1且为臂型面A内的辅助向量；k1为垂直于w1且垂直于臂型面A的辅助向量；和/>分别为向量w1和V1的单位向量；T右上角标表示对应矩阵的转置矩阵。
5.如权利要求1所述的绳驱空间柔性机械臂力位型融合柔顺控制方法，其特征在于，步骤S3中，所述位姿误差的表达式为：XedS＝ep；其中，ep为位姿误差，Xed为位姿差，S为矩阵；
所述力/力矩误差的表达式为：其中，eF为力/力矩误差，Fed为力/力矩差值，矩阵/>
所述臂型角误差的表达式为：
ψed＝ψd-ψs(θs)＝[ψ1,d-ψ1,s(θs)ψ2,d-ψ2,s(θs)]；
其中，ψed为臂型角差；ψd臂型角期望值，ψs(θs)为通过关节角度编码器采集的当前关节角度值θs算得到的臂型角当前值；ψ1,d和ψ2,d分别为臂型角1和臂型角2的期望值；ψ1,s(θs)和ψ2,s(θs)分别为通过关节角度编码器采集的当前关节角度值计算得到的臂型角1与臂型角2的当前值。
6.如权利要求1所述的绳驱空间柔性机械臂力位型融合柔顺控制方法，其特征在于，步骤S3中，所述闭环控制组包括力/力矩闭环控制、位置闭环控制和臂型角闭环控制。
7.如权利要求1所述的绳驱空间柔性机械臂力位型融合柔顺控制方法，其特征在于，步骤S4中，所述雅可比矩阵为Jex(θ)＝[J|Jev]；其中，所述臂型角雅可比矩阵Jev的表达式为：
其中，J为末端到关节的雅可比矩阵；θ为关节角度θi的集合，即θ＝[θ1,θ2,…,θi]；n为柔性臂连杆数量；ψs(θs)为当前臂型角，而和/>分别为臂型角1和臂型角2对θi的偏微分。
8.如权利要求1所述的绳驱空间柔性机械臂力位型融合柔顺控制方法，其特征在于，步骤S4中，所述关节运动期望角度θcmd的表达式为：θcmd＝θd+Δθ；
其中，θd为期望的关节角度，θd＝[θ1,d,θ2,d,…,θi,d]；Δθ为关节运动偏移量。
9.如权利要求1所述的绳驱空间柔性机械臂力位型融合柔顺控制方法，其特征在于，步骤S5中，所述下一个控制周期关节运动指令的表述式为：
其中，θ(s)为下一个控制周期关节运动指令；Kp为比例增益系数、Ki为积分增益系数和Kd为微分增益系数；s为控制周期。
10.一种绳驱空间柔性机械臂力位型融合柔顺控制***，其特征在于，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至9中任意一项所述的控制方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311364302.2A CN117245663A (zh) | 2023-10-20 | 2023-10-20 | 绳驱空间柔性机械臂力位型融合柔顺控制方法及*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311364302.2A CN117245663A (zh) | 2023-10-20 | 2023-10-20 | 绳驱空间柔性机械臂力位型融合柔顺控制方法及*** |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117245663A true CN117245663A (zh) | 2023-12-19 |
Family
ID=89135029
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311364302.2A Pending CN117245663A (zh) | 2023-10-20 | 2023-10-20 | 绳驱空间柔性机械臂力位型融合柔顺控制方法及*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117245663A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118083144A (zh) * | 2024-04-18 | 2024-05-28 | 中国空气动力研究与发展中心计算空气动力研究所 | 飞行器空中对接装置及方法 |
-
2023
- 2023-10-20 CN CN202311364302.2A patent/CN117245663A/zh active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118083144A (zh) * | 2024-04-18 | 2024-05-28 | 中国空气动力研究与发展中心计算空气动力研究所 | 飞行器空中对接装置及方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US5341459A (en) | Generalized compliant motion primitive | |
| Nguyen et al. | Adaptive control of a Stewart platform‐based manipulator | |
| JP3207728B2 (ja) | 冗長マニピュレータの制御方法 | |
| CN111390872B (zh) | 一种舱外机器人双臂协同柔性拖拽与对接逆操作方法 | |
| JP2006236031A (ja) | ロボット軌跡制御方法及び装置並びにロボット軌跡制御方法のプログラム | |
| JP2001157975A (ja) | ロボットの制御装置 | |
| JP6170285B2 (ja) | マニピュレータ及びその経路生成方法 | |
| CN105598968B (zh) | 一种并联机械臂的运动规划与控制方法 | |
| Li et al. | A hybrid visual servo control method for simultaneously controlling a nonholonomic mobile and a manipulator | |
| CN117245663A (zh) | 绳驱空间柔性机械臂力位型融合柔顺控制方法及*** | |
| Mu et al. | Dynamic feedforward control of spatial cable-driven hyper-redundant manipulators for on-orbit servicing | |
| Abou Moughlbay et al. | Error regulation strategies for model based visual servoing tasks: Application to autonomous object grasping with nao robot | |
| Liu et al. | Contact Force/Motion Hybrid Control for a Hydraulic Legged Mobile Manipulator via a Force-Controlled Floating Base | |
| Sanders et al. | Dynamically Feasible Trajectory Generation for Soft Robots | |
| CN113084797A (zh) | 一种基于任务分解的双臂冗余机械臂动态协同控制方法 | |
| Fernandez et al. | Multi-surface admittance control approach applied on robotic assembly of large-scale parts in aerospace manufacturing | |
| Hasunuma et al. | The tele-operation of the humanoid robot-workspace extension of the arm with step motion | |
| Connolly et al. | Cooperating manipulator control using dual quaternion coordinates | |
| JP4647919B2 (ja) | 制御方法および制御装置 | |
| JPH08141951A (ja) | ロボットの協調制御方法および協調制御装置 | |
| Simetti et al. | Distributed control and coordination of cooperative mobile manipulator systems | |
| JP6057283B2 (ja) | 多関節ロボット及び半導体ウェハ搬送装置 | |
| Gao et al. | Adaptive velocity planning for 6-DOF Robots with fixed tracks | |
| WO2024142289A1 (ja) | プログラミング装置、プログラミング方法及びプログラム | |
| US20220219316A1 (en) | Modular robotic device and method to operate a modular robotic device |
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 |