CN111702757B - 基于操作者意图的控制方法、装置、计算设备及存储介质 - Google Patents

基于操作者意图的控制方法、装置、计算设备及存储介质 Download PDF

Info

Publication number
CN111702757B
CN111702757B CN202010458443.0A CN202010458443A CN111702757B CN 111702757 B CN111702757 B CN 111702757B CN 202010458443 A CN202010458443 A CN 202010458443A CN 111702757 B CN111702757 B CN 111702757B
Authority
CN
China
Prior art keywords
operator
force
robot
intention
error compensation
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
CN202010458443.0A
Other languages
English (en)
Other versions
CN111702757A (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.)
Huazhong University of Science and Technology
Original Assignee
Huazhong University of Science and Technology
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 Huazhong University of Science and Technology filed Critical Huazhong University of Science and Technology
Priority to CN202010458443.0A priority Critical patent/CN111702757B/zh
Publication of CN111702757A publication Critical patent/CN111702757A/zh
Application granted granted Critical
Publication of CN111702757B publication Critical patent/CN111702757B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • 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/1612—Programme controls characterised by the hand, wrist, grip control
    • 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/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • 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/1679—Programme controls characterised by the tasks executed
    • B25J9/1692—Calibration of manipulator
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/04—Constraint-based CAD

Abstract

本发明公开了基于操作者意图的控制方法、装置、计算设备及存储介质，所述基于操作者意图的控制方法包括步骤：基于虚拟夹具引导路径构造理想运动方向矩阵与禁止运动方向矩阵；基于操作者作用于机器人末端的实际操作力获得误差补偿力；根据机器人末端的实际路径和虚拟夹具引导路径获取误差补偿修正力；获取操作者意图参数值；基于所述理想运动方向矩阵、所述禁止运动方向矩阵、所述误差补偿力、所述误差补偿修正力和所述操作者意图参数值获得机器人末端的当前操作力。本发明可约束机器人末端以更加符合实际工况的期望方向运动，保证了虚拟夹具的人性化、智慧化和智能化，提高了人机共融，确保了机器人末端执行过程的精准性。

Description

基于操作者意图的控制方法、装置、计算设备及存储介质
技术领域
本发明属于虚拟夹具控制技术领域，更具体地，涉及一种基于操作者意图的控制方法、装置、计算设备及存储介质。
背景技术
虚拟夹具技术是一种利用软件编程方法实现的生成运动约束算法，广泛应用于工业、医疗等领域。虚拟夹具技术可以在虚拟空间实现机械刚性夹具对禁止运动方向运动的约束，基于机器人自身的高精度以及稳定性特点，它可以辅助机器人精准地沿着期望方向运动，同时由于操作者的手动控制，又为机器人保留了一定程度的灵活性。但是在使用虚拟夹具构造方法过程中可能会出现根据先验知识构造的虚拟夹具无法与理想曲线或曲面完全吻合，或是由于安全问题造成原规划路径需要改变，或是在机器人操作过程中出现扰动、障碍物等突发情况，如在医疗机器人末端引导时，面对手术环境、接受手术者手术状况等的变化，虚拟夹具难以根据主刀医生意图进行自适应调整；或如磨削机器人末端引导时，无法根据复杂加工曲面难以根据操作者意图进行自适应调整。面对此类无法跟随环境和操作者意图而改变虚拟夹具构造的情况，如何融合操作者意图对虚拟夹具控制进行动态调整成为亟待解决的问题。
发明内容
针对现有技术的以上缺陷或改进需求，本发明提供一种基于操作者意图的控制方法、装置、计算设备及存储介质。
其中，所述基于操作者意图的控制方法包括步骤：
S1：基于虚拟夹具引导路径构造理想运动方向矩阵与禁止运动方向矩阵；
S2：基于操作者作用于机器人末端的实际操作力获得误差补偿力；
S3：根据机器人末端的实际路径和虚拟夹具引导路径获取误差补偿修正力；
S4：获取操作者意图参数值，其中，所述意图参数值为所述误差补偿力的力变化率参数值和机器人末端的位置轮廓误差参数值两个参数值中的一个以上参数值；
S5：基于所述理想运动方向矩阵、所述禁止运动方向矩阵、所述误差补偿力、所述误差补偿修正力和所述操作者意图参数值获得机器人末端的当前操作力。
可选地，步骤S5之后还包括步骤：
S6：根据所述当前操作力和导纳控制生成机器人末端的理想位姿。
可选地，步骤S6之后还包括步骤：
S71：获取重复步骤S1至步骤S6得到的机器人末端需要修正的路径点修改后所形成的修正点；
S72：通过局部修正函数修正所述修正点以获得新路径点；
S73：基于所述新路径点获得新的虚拟夹具引导路径；优选地，步骤S73之后还包括步骤：
S74：拟合新的虚拟夹具引导路径和虚拟夹具引导路径形成实际示教路径。
可选地，所述理想运动方向矩阵、所述禁止运动方向矩阵、所述误差补偿力、所述误差补偿修正力和所述操作者意图参数值的函数关系满足公式(1)；
Figure GDA0003062692200000021
其中，M为质量矩阵；D为阻尼矩阵；K为刚度矩阵；Δx为机器人末端的位姿误差，
Figure GDA0003062692200000031
分别为位姿误差的一阶导、二阶导；fe'为fe经修正生成的当前操作力；fe为fh通过力传感器经滤波和重力补偿获得的误差补偿力；fh为操作者作用于机器人末端的实际操作力；Dr为理想运动方向矩阵；Dt为禁止运动方向矩阵；Υ(η)为操作者意图参数值，0≤Υ(η)≤1，操作者意图与Υ(η)呈正比；η为操作者意图表征系数；fc为误差补偿修正力。
可选地，所述操作者意图参数值与虚拟夹具的刚柔性系数的函数关系满足公式(2)；
kt＝1-Υ(η) (2)
其中，kt为虚拟夹具的刚柔性系数，0≤kt≤1。
可选地，步骤S4包括步骤：
S41：获取所述误差补偿力的力变化率；
S42：根据机器人末端的实际路径和虚拟夹具引导路径获取机器人末端的位置轮廓误差；
S43：根据所述力变化率和所述位置轮廓误差得到操作者意图表征系数；
S44：根据操作者意图表征函数和所述操作者意图表征系数得到所述操作者意图参数值，其中，所述操作者意图表征函数为关于所述操作者意图表征系数的函数；优选地，
所述理想运动方向矩阵与所述禁止运动方向矩阵的计算模型满足公式(3)；
Figure GDA0003062692200000032
其中，Dr为理想运动方向矩阵；Dt为禁止运动方向矩阵；Js为虚拟夹具引导路径的运动学模型，是关于曲线参数s的函数；I为单位矩阵；|| ||为求取二范数运算操作；优选地，
所述误差补偿修正力的获取满足公式(4)；
Figure GDA0003062692200000041
其中，K'为弹簧阻尼系统的对角刚度矩阵，B为弹簧阻尼系统的对角刚度矩阵，Xp与
Figure GDA0003062692200000042
分别为虚拟夹具引导路径上轮廓点的位姿与速度，
Figure GDA0003062692200000043
与
Figure GDA0003062692200000044
分别为机器人末端的实际位姿与实际速度。
可选地，所述操作者意图表征函数和所述操作者意图表征系数的函数关系满足公式(5)；
Υ(η)＝η4-2η2+1 (5)
其中，Υ(η)为操作者意图参数值，0≤Υ(η)≤1，操作者意图与Υ(η)呈正比；η为操作者意图表征系数；优选地，
所述力变化率、所述位置轮廓误差和所述操作者意图表征系数的函数关系满足公式(6)；
η＝δα(||f'||)+(1-δ)β(εp) (6)
其中，εp为机器人末端的当前位置轮廓误差；f'为所述误差补偿力的力变化率；α(||f'||)和β(εp)为不同的操作者意图保证系数；δ为线性组合系数，其数值大小根据具体场景选取，其中，α(||f'||)与f'的函数关系满足公式(7)，β(εp)与εp函数关系满足公式(8)；
Figure GDA0003062692200000045
Figure GDA0003062692200000051
其中，Δfmin为最小补偿力值变化率，Δfmax为最大补偿力值变化率，其数值大小根据具体场景选取；dmin为最小位置轮廓误差，dmax最大位置轮廓误差，其数值大小根据具体场景选取。
所述基于操作者意图的虚拟夹具控制装置，包括：
构造模块，用以基于虚拟夹具引导路径构造理想运动方向矩阵与禁止运动方向矩阵；
误差补偿力获取模块，用以基于操作者作用于机器人末端的实际操作力获得误差补偿力；
误差补偿修正力获取模块，用以根据机器人末端的实际路径和虚拟夹具引导路径获取误差补偿修正力；
操作者意图参数值获取模块，用以获取操作者意图参数值，其中，所述意图参数值为所述误差补偿力的力变化率参数值和机器人末端的位置轮廓误差参数值两个参数值中的一个以上参数值；
当前操作力获取模块，用以基于所述理想运动方向矩阵、所述禁止运动方向矩阵、所述误差补偿力、所述误差补偿修正力和所述操作者意图参数值获得机器人末端的当前操作力。
所述终端设备包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的基于操作者意图的虚拟夹具控制程序，所述基于操作者意图的虚拟夹具控制程序配置为实现上述任意一项所述的基于操作者意图的控制方法。
所述计算机可读存储介质上存储有基于操作者意图的虚拟夹具控制程序，所述基于操作者意图的虚拟夹具控制程序被处理器执行时实现上述任意一项所述的基于操作者意图的控制方法的步骤。
总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：
1.本发明中，当当前工况满足先验知识构造的工况时，虚拟夹具可辅助机器人末端基于根据先验知识构造的虚拟夹具精准的沿着期望方向运动(即根据在前虚拟夹具引导路径运动)，当当前工况不满足先验知识构造的工况，出现扰动、障碍物等突发情况时，操作者可根据实际工况来灵活的调整当前引导路径使得实际应用的虚拟夹具能够根据实际工况动态调整，使得虚拟夹具在辅助示教过程中，能够根据操作者意图自适应迭代修正虚拟夹具以更为吻合当前实际操作工况，解决了现有虚拟夹具约束机器人末端要么只能沿期望方向运动，要么不管当前引导路径是否满足实际工况，引导路径完全受被操作者所操控的技术难题。本发明将形成一符合实际工况的实际引导路径，从而约束机器人末端以更加符合实际工况的期望方向运动，保证了虚拟夹具的人性化、智慧化和智能化，提高了人机共融，实现了人机合一，大大提高了机器人末端执行过程的精准性。
2.本发明中，针对符合实际工况的引导路径进行生成，使得符合工况的在前虚拟夹具引导路径依然有效，使得实际引导路径由新的虚拟夹具引导路径和在前虚拟夹具引导路径共同构成，程序生成更为便捷且易于实现，示教过程中，当且仅当操作者有意图改变当前引导路径时机器人末端才会偏离在前虚拟夹具引导路径，否则机器人末端将沿在前虚拟夹具引导路径继续前进，减少了操作者在示教过程中的操作复杂度和繁重的工作量，整个操作过程，操作者仅作为监督指导作用，无需全程参与。
3.本发明中，将操作者意图与虚拟夹具的刚柔性系数进行线性组合，使得本发明的虚拟夹具的刚柔性系数动态可变，可根据操作者意图实时调整，实现了虚拟夹具的实际引导路径可为在前虚拟夹具引导路径，也可为根据操作者意图改变后的新的虚拟夹具引导路径，使得虚拟夹具不再受刚柔性系数为固值的选择制约，大大提高了虚拟夹具辅助精度和效率。
4.本发明中，操作者意图同时受到力变化率和位置轮廓误差约束，通过力变化率和位置轮廓误差的功能互补，提高了操作者的操作舒适度，减轻其操作负担，提高操作者更改虚拟夹具引导路径的稳定性，同时保证机器人响应的及时性。
5.本发明中，四阶多项式实现了操作者意图在较小数值处虚拟夹具便具有较快的响应，且整体为光滑曲线，同时保证了计算效率。
附图说明
图1为本发明的基于操作者意图的控制方法的一种实施例流程图；
图2为本发明的基于操作者意图的控制方法的另一种实施例流程图；
图3为本发明的基于操作者意图的控制方法的另一种实施例流程图；
图4为本发明的位置轮廓误差定义一种实施例示意图；
图5为本发明的SCODEF法约束点变形的一种实施例示意图；
图6为本发明的SCODEF法约束点变形的另一种实施例示意图；
图7为本发明的基于操作者意图的控制方法的结构控制图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
在本发明的一种实施例中，如图1所示，一种基于操作者意图的控制方法，适用于虚拟夹具，包括步骤：
S1：基于虚拟夹具引导路径构造理想运动方向矩阵与禁止运动方向矩阵。
具体而言，所述理想运动方向矩阵与所述禁止运动方向矩阵的计算模型满足公式(3)；
Figure GDA0003062692200000081
其中，Dr为理想运动方向矩阵；Dt为禁止运动方向矩阵；Js为虚拟夹具引导路径的运动学模型，是关于曲线参数s的函数；I为单位矩阵；| |||为求取二范数运算操作。
优选地，步骤S1还包括步骤S101：根据机器人末端的实际位姿对虚拟夹具引导路径的曲线参数进行限位处理，生成边界约束模型。
其中，所述边界约束模型为：
Figure GDA0003062692200000082
其中，smax为由虚拟夹具引导路径所确定的曲线参数的上限位，s′为经边界约束后获得的曲线参数。
S2：基于操作者作用于机器人末端的实际操作力获得误差补偿力。
具体而言，误差补偿力fe由操作者作用于机器人末端的实际操作力fh通过力传感器经滤波和重力补偿获得，通过在机器人末端设置力传感器以及重力补偿与滤波器，其中，力传感器用于检测牵引机器人过程中的实际操作力fh，重力补偿与滤波器接收力传感器发送的信号，并对该信号进行滤波处理和重力补偿处理，以获取误差补偿力fe。优选地，力传感器为六维力传感器。
S3：根据机器人末端的实际路径和虚拟夹具引导路径获取误差补偿修正力。
值得说明的是，当机器人末端偏移虚拟夹具引导路径时，虚拟夹具将引导机器人末端朝向虚拟夹具引导路径牵引，此时将会产生一个牵引力，从而对虚拟夹具辅助操作过程中的路径偏差进行在线补偿，具体包括步骤：获取虚拟夹具引导路径的轮廓点，在机器人末端与轮廓点之间设计弹簧阻尼系统，以生成误差补偿修正力fc。其中，误差补偿修正力的获取满足公式(4)；
Figure GDA0003062692200000091
其中，K'为弹簧阻尼系统的对角刚度矩阵，B为弹簧阻尼系统的对角刚度矩阵，Xp与
Figure GDA0003062692200000092
分别为虚拟夹具引导路径上轮廓点的位姿与速度，
Figure GDA0003062692200000093
与
Figure GDA0003062692200000094
分别为机器人末端的实际位姿与实际速度。
具体地，可通过导纳控制获取机器人末端的实际位姿和实际速度，且导纳控制在本发明中满足公式(10)：
Figure GDA0003062692200000095
其中，M为质量矩阵；D为阻尼矩阵；K为刚度矩阵；Δx为机器人末端的位姿误差，
Figure GDA0003062692200000096
分别为位姿误差的一阶导、二阶导；fe'为当前操作力；fe为fh通过力传感器经滤波和重力补偿获得的误差补偿力；fh为操作者作用于机器人末端的实际操作力；fc为误差补偿修正力；kt为虚拟夹具的刚柔性系数，0≤kt≤1，其取值根据具体场景选取。
具体地，当kt＝0时，表示操作者只能控制机器人沿理想运动(前进或后退)，为硬虚拟夹具；当kt＝1时，表示操作者能控制机器人沿任意运动方向运动，为纯粹的机器人动觉示教；当0＜kt＜1时，表示机器人既可沿着理想运动方向运动，也可沿着禁止运动方向运动，为柔性虚拟夹具。
S4：获取操作者意图参数值，其中，所述意图参数值为所述误差补偿力的力变化率参数值和机器人末端的位置轮廓误差参数值两个参数值中的一个以上参数值。
值得说明的是，由于操作者作用于机器人末端的实际操作力最终会转化成通过力传感器经滤波和重力补偿获得的误差补偿力来参与机器人各个关节的控制，因此，为了便于力变化率的获取，本实施例将力变化率参数值定义为误差补偿力的力变化率参数值。在实际应用中，力变化率参数值也可为实际操作力的力变化率参数值，但其本质与本实施例相同，也应属于本发明的保护范围。本实施例中的力变化率满足公式(7)；
Figure GDA0003062692200000101
其中，Δfmin为最小补偿力值变化率，Δfmax为最大补偿力值变化率，其数值大小根据具体场景选取。
本实施例中的位置轮廓误差定义为在待操作对象的坐标系下机器人末端的实际位置到参考位置轮廓上最近点的向量，即公式(11)：
εp＝Pc-Pa (11)
其中，Pa为机器人末端的实际位置；Pc为参考位置轮廓上距离实际位置最近的位置点，也被称之为足点或轮廓点；εp为机器人末端的轮廓误差；如图4所示，显而易见，机器人末端的轮廓误差εp的大小为εp＝||Pc-Pa||，方向由Pa指向Pc。
可选地，获取机器人位置轮廓误差包括具体步骤：
S301：通过全向查表的方式获得离当前机器人末端实际位置点Pa最近的虚拟夹具引导路径参考点Pl，轮廓点Pc必定位于最近参考点Pl的前后即Pl-1与Pl之间或Pl与Pl+1之间。
S302：利用参考点Pl-1、Pl与Pl+1的二阶多项式曲线近似描述路径的局部几何特征P(s)。其中，s为曲线参数。
S302：求解估计机器人末端的轮廓点Pc'，通过公式(12)计算：
Figure GDA0003062692200000111
其中，β为曲线参数s的取值区间，轮廓点Pc'通过黄金分割法对公式(12)求解获得。
S303：计算估计的位置轮廓误差
Figure GDA0003062692200000112
S5：基于所述理想运动方向矩阵、所述禁止运动方向矩阵、所述误差补偿力、所述误差补偿修正力和所述操作者意图参数值获得机器人末端的当前操作力。
具体而言，所述理想运动方向矩阵、所述禁止运动方向矩阵、所述误差补偿力、所述误差补偿修正力和所述操作者意图参数值的函数关系满足公式(1)；
Figure GDA0003062692200000113
其中，M为质量矩阵；D为阻尼矩阵；K为刚度矩阵；Δx为机器人末端的位姿误差，
Figure GDA0003062692200000114
分别为位姿误差的一阶导、二阶导；fe'为fe经修正生成的当前操作力；fe为fh通过力传感器经滤波和重力补偿获得的误差补偿力；fh为操作者作用于机器人末端的实际操作力；Dr为理想运动方向矩阵；Dt为禁止运动方向矩阵；Υ(η)为操作者意图参数值，0≤Υ(η)≤1，操作者意图与Υ(η)呈正比；η为操作者意图表征系数；fc为误差补偿修正力。
具体地，所述操作者意图参数值与虚拟夹具的刚柔性系数的函数关系满足公式(2)；
kt＝1-Υ(η) (2)
其中，kt为虚拟夹具的刚柔性系数，0≤kt≤1。
优选地，步骤S4包括步骤：
S41：获取所述误差补偿力的力变化率；
S42：根据机器人末端的实际路径和虚拟夹具引导路径获取机器人末端的位置轮廓误差；
S43：根据所述力变化率和所述位置轮廓误差得到操作者意图表征系数；
S44：根据操作者意图表征函数和所述操作者意图表征系数得到所述操作者意图参数值，其中，所述操作者意图表征函数为关于所述操作者意图表征系数的函数。
优选地，所述操作者意图表征函数和所述操作者意图表征系数的函数关系满足公式(5)；
Υ(η)＝η4-2η2+1 (5)
其中，Υ(η)为操作者意图参数值，0≤Υ(η)≤1，操作者意图与Υ(η)呈正比；η为操作者意图表征系数。
优选地，所述力变化率、所述位置轮廓误差和所述操作者意图表征系数的函数关系满足公式(6)；
η＝δα(||f'||)+(1-δ)β(εp) (6)
其中，εp为机器人末端的当前位置轮廓误差；f'为所述误差补偿力的力变化率；α(||f'||)和β(εp)为不同的操作者意图保证系数；δ为线性组合系数，其数值大小根据具体场景选取，其中，α(||f'||)与f'的函数关系满足公式(7)，β(εp)与εp函数关系满足公式(8)；
Figure GDA0003062692200000121
Figure GDA0003062692200000131
其中，Δfmin为最小补偿力值变化率，Δfmax为最大补偿力值变化率，其数值大小根据具体场景选取；dmin为最小位置轮廓误差，dmax最大位置轮廓误差，其数值大小根据具体场景选取。
优选地，步骤S44之前包括步骤：构建所述操作者意图表征函数：将所述操作者意图表征函数定义为四阶多项式，其为公式(13)；
Υ(η)＝aη4+bη3+cη2+dη+e (13)
其中，a、b、c、d和e均为常数。
基于公式(15)约束所述四阶多项式；
根据公式(13)、公式(14)和公式(15)得到所述操作者意图表征函数，其为公式(5)；
Υ'(η)＝4aη3+3bη2+2cη+d (14)
Figure GDA0003062692200000132
Υ(η)＝η4-2η2+1 (5)
其中，公式(14)为公式(13)的导函数。
在实际应用中，当操作者无意图改变虚拟夹具的当前引导路径时(即操作者意图参数值为0)，则虚拟夹具约束机器人末端沿其引导路径所构成的理想运动方向矩阵和禁止运动方向矩阵进行精准运行，操作者只能控制机器人末端沿虚拟夹具引导路径运动，此时的虚拟夹具为硬虚拟夹具；当操作者有意图改变虚拟夹具的当前引导路径时(操作者意图参数值介于0和最大值之间)，则获取操作者意图参数值(即力变化率值(操作者想改变虚拟夹具引导路径必须改变其作用于机器人末端的实际操作力，同时导致误差补偿力改变)和/或机器人末端的轮廓误差参数值(操作者想改变虚拟夹具引导路径，势必使得机器人末端偏移引导路径而使得机器人末端的实际路径和引导路径之间出现误差)，当机器人末端偏移虚拟夹具引导路径时，为了机器人末端能够重新回到虚拟夹具引导路径必定产生误差补偿修正力，因此，结合当前的误差补偿力、误差补偿修正力、以及操作者意图参数值使得虚拟夹具可在当前的理想运动方向矩阵或禁止运动方向矩阵的限制，实时调整虚拟夹具引导路径，此时的虚拟夹具为柔性虚拟夹具；当操作者意图完全控制机器人末端时(即操作者意图参数值为最大值)，则此时操作者可控制机器人末端沿任意运动方向运动，为纯粹的机器人动觉示教。
在本发明的基于操作者意图的控制方法的另一实施例中，如图2所示，在上述实施例的基础上，步骤S5之后还包括步骤：
S6：根据所述当前操作力和导纳控制生成机器人末端的理想位姿；且所述当前操作力和导纳控制满足公式(1)；
Figure GDA0003062692200000141
本实施例中，本发明的机器人控制模型优选为导纳控制，通过导纳控制将当前操作力转化为机器人末端的理想位姿。利用导纳控制更适用于柔性环境交互或者自由环境操作的特性，使得本发明更加符合人机交互的习惯，提高本发明人机交互的操作感和沉浸感。
在本发明的基于操作者意图的控制方法的另一实施例中，如图3所示，在上述任一实施例的基础上，步骤S6之后还包括步骤：S7：生成新的虚拟夹具引导路径。可选地，步骤S7包括步骤：
S71：获取重复步骤S1至步骤S6得到的机器人末端需要修正的路径点修改后所形成的修正点；
S72：通过局部修正函数修正所述修正点以获得新路径点；
S73：基于所述新路径点获得虚拟夹具的新引导路径。
可理解的是，当操作者意图调整虚拟夹具的当前引导路径时，机器人末端将在偏离虚拟夹具引导路径的新路径上运动，为了保证形成新路径上的各个新路径点连接的光滑度，通过局部修正函数对修正点进行修正而获得新路径点，并根据这些新路径点获取新引导路径，从而使得新引导路径与为修正的引导路径的结合而形成一更加符合当前工况的实际引导路径，提高虚拟夹具的辅助精度和效率，提高了人机交互的统一性，达到机随人动，提高机器人的智慧化和智能化程度，实现人机共融。
示例性的，当现有虚拟夹具不满足实际工况时，将虚拟夹具的引导路径上需要修正的路径点称为约束点Xk，修改后的路径点称为修正点X'k，如图5所示，约束点Xk与修正点X'k之间的位移向量通过公式(16)定义；
Figure GDA0003062692200000151
在SCODEF法中约束半径r决定了变形的影响范围。但约束半径r与位移向量
Figure GDA0003062692200000152
显然呈正比，即约束半径r与位移向量d满足公式(17)；
Figure GDA0003062692200000153
其中，g为比例系数，其数值大小的选择受曲线的形状和位置影响，公式(17)中约束半径r的大小能够直观体现虚拟夹具的局部修正。
接着以约束点Xk为中心的局部变形函数F，通过公式(18)进行定义；
Figure GDA0003062692200000154
为了使变形具备连续性，因此局部变形函数F必须为一个连续函数，为了获得虚拟夹具引导路径的Akima样条约束点的平滑变形，这里采用公式(5)中定义的四阶多项式f(x)作为局部变形函数，局部变形函数的系数通过公式(19)进行定义；
Figure GDA0003062692200000155
其中，{si}i-0:N-1为Akima样条曲线的曲线参数集，N为曲线中插值点的个数。在局部变形函数的作用下，将得到一系列修正后的新路径点，经Akima样条插值得到新的符合实际工况的虚拟夹具的引导路径。
具体地，步骤S71具体包括步骤：定义变形约束，选择一个引导点并将其分配到一个新的位置；步骤S72具体包括步骤：定义局部修正函数，以获得一系列修正后的新路径点；步骤S73具体包括步骤：将所有的新路径点经插值后获得新的虚拟夹具引导路径。
上述方法可局部修正路径，但在在本发明中，虚拟夹具辅助机器人示教编程过程中，根据实际需求往往需要修正现有虚拟夹具引导路径的一部分，其中，修正部分的路径点通过基于操作者意图的虚拟夹具控制结构辅助示教获得。因此，本发明辅助示教获得的示教路径始末点并不能总是与原有虚拟夹具引导路径上的插值点相适配，因此，为了将新的示教路径与原有虚拟夹具引导路径的其余部分相结合，这里进一步基于点的变形方法SCODEF法(即简单约束对象变形法)修正原有虚拟夹具引导路径的最近点，以匹配示教路径的始末点。因此，步骤S73之后还包括步骤：
S74：拟合新的虚拟夹具引导路径和虚拟夹具引导路径形成实际示教路径。
具体地，设X1和X2为原有虚拟夹具引导路径(即上述虚拟夹具引导路径)上的插值点，X1'和X'2为新的虚拟夹具引导路径(即修正后的虚拟夹具引导路径)上的始末点，如图6所示，插值点X1和X2分别是在原有虚拟夹具引导路径上距离始末点X1'和X'2最近的插值点，其通过距离查表法依次获得。
Figure GDA0003062692200000161
与
Figure GDA0003062692200000162
分别由X1与X1'和X2与X'2，约束半径r1和r2正比于位移量，且
值得说明的是，机器人末端的位置轮廓误差还可通过其他方式获取，约束半径r1与位移向量
Figure GDA0003062692200000163
约束半径r2与位移向量
Figure GDA0003062692200000164
分别满足公式(20)和公式(21)；
Figure GDA0003062692200000171
Figure GDA0003062692200000172
其中，g1与g2为比例系数，其数值大小的选择受曲线的形状和位置影响，公式(20)和公式(21)中约束半径r1和r2的大小能够直观体现虚拟夹具的局部修正。
之后，插值点X1和X2经局部修正函数F(x)获得新的一系列插值点包括示教路径始末点X1'和X'2。其中，局部修正函数F(x)与上述局部变形函数F的定义相同。接着，将新的插值点存储于X1之前和X2之后，X1和X2之间则由示教路径点(即新的虚拟夹具引导路径)替代。最后，通过Akima样条插值构造虚拟夹具的实际示教路径。上述所有方法同样适用于虚拟夹具引导路径方向修正，由此构造出新的6D虚拟夹具，便于同类型的实际工况的辅助示教编程。
如特征跟踪法、最近点识别小领域、协方差树等；操作者意图参数值与虚拟夹具的刚柔性系数的函数关系可为多项式函数关系，具体可根据实际应用进行设定；操作者意图表征函数可仅为关于力变化率或位置轮廓误差的函数；操作者意图表征函数与操作者意图表征系数的函数关系还可其他函数关系，具体可根据实际应用进行设定，以上但不局限以上方法应均设于本发明的保护范围，且以上步骤除非明确具有先后关系的，步骤之间可同时进行或依次进行。值得说明的是，对虚拟夹具进行局部修正的方法还可为其他的点的变形方法，如自由变形法、直接操纵自由变形法、简单径向变形法等。
在本发明另一种实施例中，一种基于操作者意图的虚拟夹具控制装置，包括构造模块，用以基于虚拟夹具引导路径构造理想运动方向矩阵与禁止运动方向矩阵；误差补偿力获取模块，用以基于操作者作用于机器人末端的实际操作力获得误差补偿力；误差补偿修正力获取模块，用以根据机器人末端的实际路径和虚拟夹具引导路径获取误差补偿修正力；操作者意图参数值获取模块，用以获取操作者意图参数值，其中，所述意图参数值为所述误差补偿力的力变化率参数值和机器人末端的位置轮廓误差参数值两个参数值中的一个以上参数值；当前操作力获取模块，用以基于所述理想运动方向矩阵、所述禁止运动方向矩阵、所述误差补偿力、所述误差补偿修正力和所述操作者意图参数值获得机器人末端的当前操作力。
值得说明的是，构造模块、误差补偿力获取模块、误差补偿修正力获取模块、操作者意图参数值获取模块以及当前操作力获取模块可为可实现上述步骤的一个模块或者一个以上模块。
可选地，还包括导纳控制模块，用以根据所述当前操作力和导纳控制生成机器人末端的当前位姿、当前速度和当前加速度。
可选地，还包括新引导路径生成模块，用以获取重复步骤S1至步骤S6得到的机器人末端需要修正的路径点修改后所形成的修正点；通过局部修正函数修正所述修正点以获得新路径点；并基于所述新路径点获得虚拟夹具的新引导路径。
具体地，误差补偿力获取模块包括力传感器、滤波器以及重力补偿模块，力传感器用于检测牵引机器人过程中的实际操作力fh，重力补偿与滤波器接收力传感器发送的信号，并对该信号进行滤波处理和重力补偿处理，以获取误差补偿力fe。
误差补偿修正力获取模块包括正运动学模型、轮廓误差估计器以及边界约束模型，通过正运动学模型得到虚拟夹具引导路径上轮廓点的位姿Xp与速度
Figure GDA0003062692200000181
通过正运动学模型和轮廓误差估计器得到机器人末端的实际位姿
Figure GDA0003062692200000182
实际速度
Figure GDA0003062692200000183
曲线函数s和轮廓误差εp；通过边界约束模型对曲线参数s进行限位处理形成经边界约束后获得的曲线参数s′；通过构造模块获得理想运动方向矩阵Dr和禁止运动方向矩阵Dt；通过操作者意图参数值获取模块分别获取力变化率和轮廓误差并得到操作者意图参数值，通过当前操作力获取模块基于所述理想运动方向矩阵、所述禁止运动方向矩阵、所述误差补偿力、所述误差补偿修正力和所述操作者意图参数值获得机器人末端的当前操作力fe'，当前操作力fe'依次通过导纳控制模块、逆运动学、PID控制器生成机器人各个关节角。
在本发明的另一实施例中，一种终端设备，所述终端设备包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的基于操作者意图的虚拟夹具控制程序，所述基于操作者意图的虚拟夹具控制程序配置为实现上述任意一项所述的基于操作者意图的控制方法。
在本发明的另一实施例中，一种计算机可读存储介质，所述计算机可读存储介质上存储有基于操作者意图的虚拟夹具控制程序，所述基于操作者意图的虚拟夹具控制程序被处理器执行时实现上述任意一项所述的基于操作者意图的控制方法的步骤。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (12)

1.一种基于操作者意图的控制方法，适用于虚拟夹具，其特征在于，包括步骤：
S1：基于虚拟夹具引导路径构造理想运动方向矩阵与禁止运动方向矩阵；
S2：基于操作者作用于机器人末端的实际操作力获得误差补偿力；
S3：根据机器人末端的实际路径和虚拟夹具引导路径获取误差补偿修正力；
S4：获取操作者意图参数值，其中，所述意图参数值为所述误差补偿力的力变化率参数值和机器人末端的位置轮廓误差参数值两个参数值中的一个以上参数值；
S5：基于所述理想运动方向矩阵、所述禁止运动方向矩阵、所述误差补偿力、所述误差补偿修正力和所述操作者意图参数值获得机器人末端的当前操作力；
所述理想运动方向矩阵、所述禁止运动方向矩阵、所述误差补偿力、所述误差补偿修正力和所述操作者意图参数值的函数关系满足公式(1)；
Figure FDA0003078355490000011
其中，M为质量矩阵；D为阻尼矩阵；K为刚度矩阵；Δx为机器人末端的位姿误差，
Figure FDA0003078355490000012
分别为位姿误差的一阶导、二阶导；f′e为fe经修正生成的当前操作力；fe为fh通过力传感器经滤波和重力补偿获得的误差补偿力；fh为操作者作用于机器人末端的实际操作力；Dr为理想运动方向矩阵；Dt为禁止运动方向矩阵；Υ(η)为操作者意图参数值，0≤Υ(η)≤1，操作者意图与Υ(η)呈正比；η为操作者意图表征系数；fc为误差补偿修正力。
2.如权利要求1所述的基于操作者意图的控制方法，其特征在于，步骤S5之后还包括步骤：
S6：根据所述当前操作力和导纳控制生成机器人末端的理想位姿。
3.如权利要求2所述的基于操作者意图的控制方法，其特征在于，步骤S6之后还包括步骤：
S71：获取重复步骤S1至步骤S6得到的机器人末端需要修正的路径点修改后所形成的修正点；
S72：通过局部修正函数修正所述修正点以获得新路径点；
S73：基于所述新路径点获得新的虚拟夹具引导路径；
步骤S73之后还包括步骤：
S74：拟合新的虚拟夹具引导路径和虚拟夹具引导路径形成实际示教路径。
4.如权利要求1所述的基于操作者意图的控制方法，其特征在于：
所述操作者意图参数值与虚拟夹具的刚柔性系数的函数关系满足公式(2)；
kt＝1-Υ(η) (2)
其中，kt为虚拟夹具的刚柔性系数，0≤kt≤1。
5.如权利要求1所述的基于操作者意图的控制方法，其特征在于，步骤S4包括步骤：
S41：获取所述误差补偿力的力变化率；
S42：根据机器人末端的实际路径和虚拟夹具引导路径获取机器人末端的位置轮廓误差；
S43：根据所述力变化率和所述位置轮廓误差得到操作者意图表征系数；
S44：根据操作者意图表征函数和所述操作者意图表征系数得到所述操作者意图参数值，其中，所述操作者意图表征函数为关于所述操作者意图表征系数的函数。
6.如权利要求1所述的基于操作者意图的控制方法，其特征在于，所述理想运动方向矩阵与所述禁止运动方向矩阵的计算模型满足公式(3)；
Figure FDA0003078355490000031
其中，Dr为理想运动方向矩阵；Dt为禁止运动方向矩阵；Js为虚拟夹具引导路径的运动学模型，是关于曲线参数s的函数；I为单位矩阵；|| ||为求取二范数运算操作。
7.如权利要求1所述的基于操作者意图的控制方法，其特征在于，
所述误差补偿修正力的获取满足公式(4)；
Figure FDA0003078355490000032
其中，K'为弹簧阻尼系统的对角刚度矩阵，B为弹簧阻尼系统的对角刚度矩阵，Xp与
Figure FDA0003078355490000033
分别为虚拟夹具引导路径上轮廓点的位姿与速度，
Figure FDA0003078355490000034
与
Figure FDA0003078355490000035
分别为机器人末端的实际位姿与实际速度。
8.如权利要求5所述的基于操作者意图的控制方法，其特征在于：
所述操作者意图表征函数和所述操作者意图表征系数的函数关系满足公式(5)；
Υ(η)＝η4-2η2+1 (5)
其中，Υ(η)为操作者意图参数值，0≤Υ(η)≤1，操作者意图与Υ(η)呈正比；η为操作者意图表征系数。
9.如权利要求5所述的基于操作者意图的控制方法，其特征在于：
所述力变化率、所述位置轮廓误差和所述操作者意图表征系数的函数关系满足公式(6)；
η＝δα(||f'||)+(1-δ)β(εp) (6)
其中，εp为机器人末端的当前位置轮廓误差；f'为所述误差补偿力的力变化率；α(||f'||)和β(εp)为不同的操作者意图保证系数；δ为线性组合系数，其数值大小根据具体场景选取，其中，α(||f'||)与f'的函数关系满足公式(7)，β(εp)与εp函数关系满足公式(8)；
Figure FDA0003078355490000041
Figure FDA0003078355490000042
其中，Δfmin为最小补偿力值变化率，Δfmax为最大补偿力值变化率，其数值大小根据具体场景选取；dmin为最小位置轮廓误差，dmax最大位置轮廓误差，其数值大小根据具体场景选取。
10.一种基于操作者意图的虚拟夹具控制装置，其特征在于，该装置用于实现权利要求1-9任一项所述的方法，包括：
构造模块，用以基于虚拟夹具引导路径构造理想运动方向矩阵与禁止运动方向矩阵；
误差补偿力获取模块，用以基于操作者作用于机器人末端的实际操作力获得误差补偿力；
误差补偿修正力获取模块，用以根据机器人末端的实际路径和虚拟夹具引导路径获取误差补偿修正力；
操作者意图参数值获取模块，用以获取操作者意图参数值，其中，所述意图参数值为所述误差补偿力的力变化率参数值和机器人末端的位置轮廓误差参数值两个参数值中的一个以上参数值；
当前操作力获取模块，用以基于所述理想运动方向矩阵、所述禁止运动方向矩阵、所述误差补偿力、所述误差补偿修正力和所述操作者意图参数值获得机器人末端的当前操作力。
11.一种终端设备，其特征在于，所述终端设备包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的基于操作者意图的虚拟夹具控制程序，所述基于操作者意图的虚拟夹具控制程序配置为实现如权利要求1-9任意一项所述的基于操作者意图的控制方法。
12.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有基于操作者意图的虚拟夹具控制程序，所述基于操作者意图的虚拟夹具控制程序被处理器执行时实现如权利要求1-9任意一项所述的基于操作者意图的控制方法的步骤。
CN202010458443.0A 2020-05-27 2020-05-27 基于操作者意图的控制方法、装置、计算设备及存储介质 Active CN111702757B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010458443.0A CN111702757B (zh) 2020-05-27 2020-05-27 基于操作者意图的控制方法、装置、计算设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010458443.0A CN111702757B (zh) 2020-05-27 2020-05-27 基于操作者意图的控制方法、装置、计算设备及存储介质

Publications (2)

Publication Number Publication Date
CN111702757A CN111702757A (zh) 2020-09-25
CN111702757B true CN111702757B (zh) 2021-08-17

Family

ID=72538176

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010458443.0A Active CN111702757B (zh) 2020-05-27 2020-05-27 基于操作者意图的控制方法、装置、计算设备及存储介质

Country Status (1)

Country Link
CN (1) CN111702757B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112975966B (zh) * 2021-02-24 2022-07-29 深圳市优必选科技股份有限公司 机器人摆动腿落地检测方法、装置和机器人
CN113305836B (zh) * 2021-05-21 2022-09-27 华中科技大学 一种虚拟引导的局部迭代修正方法及系统
CN113894798B (zh) * 2021-11-26 2023-04-11 遨博(北京)智能科技股份有限公司 一种机器人控制方法和机器人系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR2926240A1 (fr) * 2008-01-15 2009-07-17 Blm Soc Responsabilite Limitee Procede pour l'apprentissage d'un robot ou similaire et dispositif pour la mise en oeuvre de ce procede.
CN103991077A (zh) * 2014-02-19 2014-08-20 吉林大学 一种基于力融合的机器人手控器共享控制方法
CN109822550A (zh) * 2019-02-21 2019-05-31 华中科技大学 一种复杂曲面机器人高效高精度示教方法
CN110992777A (zh) * 2019-11-20 2020-04-10 华中科技大学 多模态融合的示教方法、装置、计算设备及存储介质

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8010180B2 (en) * 2002-03-06 2011-08-30 Mako Surgical Corp. Haptic guidance system and method

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR2926240A1 (fr) * 2008-01-15 2009-07-17 Blm Soc Responsabilite Limitee Procede pour l'apprentissage d'un robot ou similaire et dispositif pour la mise en oeuvre de ce procede.
CN103991077A (zh) * 2014-02-19 2014-08-20 吉林大学 一种基于力融合的机器人手控器共享控制方法
CN109822550A (zh) * 2019-02-21 2019-05-31 华中科技大学 一种复杂曲面机器人高效高精度示教方法
CN110992777A (zh) * 2019-11-20 2020-04-10 华中科技大学 多模态融合的示教方法、装置、计算设备及存储介质

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
Susana S＇anchez Restrepo1 et al.Iterative Virtual Guides Programming for Human-Robot Comanipulation.《2017 IEEE International Conference on Advanced Intelligent Mechatronics》.2017,第219-226页. *
Virtual fixtures with autonomous error compensation for human-robot cooperative tasks;Castillo-Cruces, Raul A et al;《Robotica》;20100331;第28卷;第267-277页 *
基于虚拟夹具的空间机器人遥操作与示教技术研究;甘凯;《中国优秀硕士学位论文全文数据库（信息科技辑）》;20180715(第07期);第1-68页 *
融合视觉的柔性虚拟夹具辅助遥操作研究;蒋再男等;《华中科技大学学报（自然科学版）》;20111231;第39卷(第12期);第68-71页 *

Also Published As

Publication number Publication date
CN111702757A (zh) 2020-09-25

Similar Documents

Publication Publication Date Title
CN111702757B (zh) 基于操作者意图的控制方法、装置、计算设备及存储介质
CN111413966B (zh) 一种递进式模型预测无人驾驶规划跟踪协同控制方法
CN111660306B (zh) 一种基于操作者舒适度的机器人可变导纳控制方法及系统
CN109017984B (zh) 一种无人车的轨迹跟随控制方法、控制系统及相关装置
CN111045432B (zh) 一种欠驱动水面船非线性路径跟踪控制系统及方法
KR20140147267A (ko) 위치 제어 산업 로봇의 제어 방법 및 장치
CN112605996B (zh) 一种面向冗余机械臂的无模型碰撞避免控制方法
CN113110504B (zh) 一种基于强化学习和视线法的无人艇路径跟踪方法
CN110262229B (zh) 基于mpc的车辆自适应路径追踪方法
CN110162046A (zh) 基于事件触发型模型预测控制的无人车路径跟随方法
JP2007076807A (ja) パワーアシスト装置とその適応モデル予測制御方法
CN112578671B (zh) 一种基于u模型优化smc的agv轨迹跟踪控制方法
WO2020179798A1 (ja) 加工プログラム変換装置、数値制御装置、加工プログラム変換方法および機械学習装置
Wang et al. Extended state observer-based fixed-time trajectory tracking control of autonomous surface vessels with uncertainties and output constraints
CN111283683B (zh) 一种机器人视觉特征规划轨迹的伺服跟踪加速收敛方法
CN113101079A (zh) 基于多约束条件的智能轮椅、动态共享控制方法及系统
CN109582026B (zh) 基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法
JP5254737B2 (ja) 車両運動制御装置及びプログラム
Cepeda-Gomez et al. Application of sliding mode control to swarms under conflict
Ni et al. A bioinspired neural model based extended Kalman filter for robot SLAM
CN113305836B (zh) 一种虚拟引导的局部迭代修正方法及系统
CN111830832B (zh) 仿生滑翔机器海豚平面路径跟踪方法及系统
CN114987492A (zh) 一种考虑曲率约束的自动驾驶车辆轨迹平滑优化方法
EP3726301B1 (en) Control device
CN113960998A (zh) 一种无人艇模糊预测控制系统及方法

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