Nothing Special   »   [go: up one dir, main page]

CN114454157B - 适用于机器人的局部轨迹调整与人机共享控制方法和系统 - Google Patents

适用于机器人的局部轨迹调整与人机共享控制方法和系统 Download PDF

Info

Publication number
CN114454157B
CN114454157B CN202111575047.7A CN202111575047A CN114454157B CN 114454157 B CN114454157 B CN 114454157B CN 202111575047 A CN202111575047 A CN 202111575047A CN 114454157 B CN114454157 B CN 114454157B
Authority
CN
China
Prior art keywords
robot
track
module
time
local
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
CN202111575047.7A
Other languages
English (en)
Other versions
CN114454157A (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.)
Shenzhen Research Institute Of Shanghai Jiao Tong University
Shanghai Jiaotong University
Original Assignee
Shenzhen Research Institute Of Shanghai Jiao Tong University
Shanghai Jiaotong University
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 Shenzhen Research Institute Of Shanghai Jiao Tong University, Shanghai Jiaotong University filed Critical Shenzhen Research Institute Of Shanghai Jiao Tong University
Priority to CN202111575047.7A priority Critical patent/CN114454157B/zh
Publication of CN114454157A publication Critical patent/CN114454157A/zh
Application granted granted Critical
Publication of CN114454157B publication Critical patent/CN114454157B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/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
    • B25J9/1666—Avoiding collision or forbidden zones
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots
    • A61B34/35—Surgical robots for telesurgery
    • 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/1602—Programme controls characterised by the control system, structure, architecture
    • 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/1674—Programme controls characterised by safety, monitoring, diagnostic
    • B25J9/1676—Avoiding collision or forbidden zones
    • 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
    • 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/1689—Teleoperation
    • 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/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Health & Medical Sciences (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Surgery (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Biomedical Technology (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • Medical Informatics (AREA)
  • Molecular Biology (AREA)
  • Animal Behavior & Ethology (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Automation & Control Theory (AREA)
  • Feedback Control In General (AREA)
  • Manipulator (AREA)

Abstract

本发明提供了一种适用于机器人的局部轨迹调整与人机共享控制方法和系统，以提升手术机器人的自主性，将人与机器人的关系从主从式转变为协同式。当人的指令与机器人参考轨迹相差较大时，机器人会结合人的虚拟交互力对自身参考轨迹进行局部主动调整；当人与机器人意图相差较小时，将综合考虑人机双方指令，并基于系统安全性评估指标动态调节人机混合成本函数，计算最优控制量，实现人机共享控制。本发明还提供相应的计算机程序存储介质与机器人。

Description

适用于机器人的局部轨迹调整与人机共享控制方法和系统
技术领域
本发明涉及遥操作手术机器人技术领域，具体地，涉及适用于机器人的局部轨迹调整与人机共享控制方法和系统。尤其是一种适用于遥操作适用于机器人的局部轨迹调整与人机共享控制方法和系统及其手术机器人。
背景技术
微创手术机器人将先进的智能机器人技术融合于临床外科手术中，充分发挥机器人在手术任务中高稳定性、灵活操作性、运动精确性等优势，大大降低外科医生手术强度，避免因持续高强度工作导致的误操作几率提高的风险。
微创手术机器人一直以来都是世界各国的重点投入方向,相关研究成果不断推陈出新：最著名的是da Vinci手术机器人系统，并经过不间断的优化更新，其在机械臂操作灵巧性、安全交互性等方面的性能更加凸显。中国的手术机器人发展集中于近十年：如上海交通大学研发的“神刀华佗”微创手术机器人；天津大学研发的“妙手”系列机器人；哈尔滨工业大学研发的“华鹊－II”型微创手术机器人等。
在手术机器人的控制方面，目前手术机器人系统多为主从式控制模式，即医生通过操作遥操作杆远程控制从端机械臂的运动，自动化程度相对较低，医生的工作负担仍相对较高，且仍对医生的技术水平有较高的要求。另一方面，由于手术任务的复杂性与多样性，使用完全自动控制的机器人执行手术任务是在短期内是不可能实现的，因此人机共享的理念是更适用于当前手术机器人系统的，这将人与机器人的关系从主从式转变为协同式，机器人的运动由人与机器人共同决定。
现有的人机共享的思想大量被应用于控制方面，即共享控制。但当前系统的一个局限性是，人类不能通过遥操作设备影响机器人原本设定的未来期望轨迹，这也表明了机器人对人的意图的预测不足。此外，现有技术仍不能很好地根据实际情况动态调节人与机器人的控制比例，人机控制尚未以较高的自动化程度进行融合。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种适用于机器人的局部轨迹调整与人机共享控制方法和系统。
根据本发明提供的一种适用于机器人的局部轨迹调整与人机共享控制方法，包括如下任一个或任多个步骤：
步骤S0，基于一次轨迹规划确定初始参考轨迹的步骤：基于期望末端位置和传感器数据中获取的周围环境信息，生成可行轨迹集；从当前可行轨迹集中筛选出一条最优轨迹作为一次规划的参考轨迹；
步骤S1，考虑人的意图的局部轨迹重规划的步骤：人类通过遥操作设备传输给机器人运动指令，通过虚拟交互力判断人意图是否强烈，当人意图强烈时，机器人进行局部参考轨迹调整；
步骤S2，基于系统安全性评估指标的人机控制权重调整的步骤：通过传感器数据评估当前机器人构型的安全性，构建表征系统安全性的评估指标，并基于此评估指标动态调节人机控制权重；
步骤S3，基于人机混合成本函数的模型预测控制步骤：构建基于机器人与人的控制成本的混合成本函数；通过混合成本函数的模型预测控制器，计算得到最优控制指令，实现人机共享控制。
优选地，所述步骤S1包括：
步骤S1.1：建立表征人类交互力的虚拟力模型；
步骤S1.2：通过虚拟交互力判断人意图是否强烈；若是，则机器人不调整参考轨迹；；如否，则机器人进行局部参考轨迹调整；
所述步骤S2包括：
步骤2.1：基于传感器的数据，实时感知环境信息，建立表征系统安全性的指标；其中，所述环境信息包括距离边界的距离、距离障碍物的距离；
步骤S2.2：将系统安全性指标作为调节人机控制权重的依据；
所述步骤S3包括：
步骤S3.1：基于从端系统的动力学模型，建立从端系统状态空间表达式；
步骤S3.2：根据机器人与人的期望指令，分别计算机器人、人的误差向量；
步骤S3.3：在任一时刻k，计算机器人成本函数；
步骤S3.4：在任一时刻k，计算人类成本函数；
步骤S3.5：基于表征系统安全性的指标建立上述时刻k混合成本函数；
步骤S3.6：基于模型预测控制框架，形成控制问题：在任一时刻k，优化时域为t＝[k,k+1],将其划分为等时间步长的P个离散时间段，滚动优化的目标为最小化此时域内的累计混合成本函数；预测模型即为基于从端系统的动力学模型所建立的状态空间表达式；系统安全性约束为当前机器人的位置距离最近障碍物与最近边界的距离；
步骤S3.7：在每个优化时域t＝[k,k+1]内求解上述优化问题，计算k时刻的最优控制量输入。
优选地，在所述步骤S0中：
确定机器人在笛卡尔空间中的目标构型为xfinal，初始位置x0，期望轨迹时长T，目标为规划一条从初始构型到目标构型xfinal最优的轨迹，即为参考轨迹xd(t)，t为时间变量；
通过传感设备，获取体内环境中障碍物位置分布和运动边界使用快速扩展随机树RRT算法生成可行轨迹集通过如下优化目标从可行轨迹集中筛选出一条最优轨迹作为一次规划的参考轨迹xd(t)：
其中，p代表了可行轨迹，优化指标由三部分组成，分别用于表征最短路径、最大限度避免障碍物、最大限度避免边界，αl,αo,αb均为正常数，用于调节上述三部分的比例。
优选地，所述步骤S1包括：
步骤S1.1：建立表征人类交互力的虚拟力模型：
Mm,Dm,Km分别代表了惯性矩阵、阻尼矩阵、刚度矩阵；
代表了当前机器人机械臂末端的位置；
为对应期望值；
Fh表征人类通过遥操作设备向从端机器人施加的虚拟交互力；
步骤S1.2：通过虚拟交互力判断人意图是否强烈，判断方法如下：
设定阈值δi,i＝1,...,m，为对应虚拟交互力Fh第i个分量的下限，实时监测人类通过遥操作设备向从端机器人施加的虚拟交互力Fh的数值；
1)若Fh≤δi，则机器人不调整参考轨迹；
2)若Fh＞δi，则机器人进行局部参考轨迹调整；
在步骤S1.2中，所述局部参考轨迹调整的步骤包括：
步骤S1.2.1：确定待调整的局部轨迹范围t∈[ts,tf]，t为时间变量，ts、tf分别表征局部轨迹的起始时间、终止时间；
步骤S1.2.2：将原轨迹xd(t)离散化为局部离散轨迹
其中，分别是xd(t)的第i个分量，δ为选取的离散点的时间间隔；
步骤S1.2.3：对从当前位置到xd(tf)的局部轨迹进行重规划，生成局部可行轨迹集
步骤S1.2.4：对于局部可行轨迹γd，局部轨迹能量E(γd)为γd调整后轨迹能量：
为原局部轨迹能量；
α为一个正常数；
R为正定对称矩阵；
基于表征调整后轨迹能量指标，从可行轨迹集中选出调整轨迹能量最小的一条可行轨迹作为最优轨迹γd作为调整后参考轨迹；
所述步骤S2包括：
步骤S2.1：基于传感器的数据，实时感知环境信息，建立表征系统安全性的指标；其中，所述环境信息包括距离边界的距离、距离障碍物的距离；
步骤S2.2：将系统安全性指标作为调节人机控制权重的依据；
所述步骤S2.1包括如下步骤：
步骤S2.1.1：基于传感数据得到的体内环境中障碍物位置分布和运动边界下标i表示序号；
步骤S2.1.2：构建当前时刻表征可移动余量向量dres(k)：
γd(k-1)为上一时刻k-1的参考轨迹位置，若在步骤S1.2中判断为人意图较强，则参考轨迹γd为调整后的期望轨迹，否则，参考轨迹γd为一次规划得到的初始参考轨迹；k表示时刻；
步骤S2.1.3：计算实际偏移量d(k)为机器人当前位置构型x(k-1)与对应期望值γd(k-1)的距离：
d(k)＝||x(k-1)-γd(k-1)||
步骤S2.1.4：定义一个饱和函数dsat:[0,dres]→(0,dres)
μ1,μ2,θ2,ξ为Richards曲线的参数；
dmax(k)＝min{d(k),dres(k)}，参数值决定了曲线的形状，是根据实际情况设置的预定值；
步骤S2.1.5：建立表征系统安全性的指标λ(k)：
所述步骤S3包括：
步骤S3.1：基于从端系统的动力学模型，建立从端系统状态空间表达式：
Mx,Cx,Gx分别是从端机器人在笛卡尔空间的惯性矩阵、科里奥利力离心力矩阵、重力矩阵，J为从机器人关节空间到笛卡尔空间的雅可比矩阵；控制输入为从端机器人的关节力矩输入τ；其中，状态变量为系统构型及其导数；
步骤S3.2：根据机器人与人的期望指令，分别计算机器人与人的误差向量，机器人的误差向量为Δxr＝x-γd，人的误差向量为Δxh＝x-xhd；其中，γd是机器人的期望笛卡尔空间中构型位置；xhd是人的期望笛卡尔空间中构型位置，由人通过主端交互装置的力输入转化得到；
步骤S3.3：在任一时刻k，计算机器人成本函数Cr(k)：
Q1r,Q2r,Q3r均为正定矩阵；
步骤S3.4：在任一时刻k，计算人类成本函数：
Q1h,Q2h均为正定矩阵；
步骤S3.5：基于表征系统安全性的指标λ(k)建立k时刻混合成本函数：
C(k)＝λ(k)Ch(k)+(1-λ(k))Cr(k)
步骤S3.6：基于模型预测控制框架，形成如下控制问题：
z(k+1|k)＝A(k)z(k)+B(k)τ(k)+C(k)
该控制问题为：在任一时刻k，优化时域为t＝[k,k+1],将其划分为等时间步长的P个离散时间段，滚动优化的目标为最小化此时域内的累计混合成本函数；预测模型即为基于从端系统的动力学模型所建立的状态空间表达式，A(k),B(k),C(k)为对应的离散系数矩阵；系统安全性约束为，ho与hb分别代表当前机器人的位置距离最近障碍物与最近边界的距离，需保证其不小于对应的阈值δo,δb；
步骤S3.7：在每个优化时域t＝[k,k+1]内求解上述优化问题，计算k时刻的最优控制量输入。
根据本发明提供的一种适用于机器人的局部轨迹调整与人机共享控制系统，包括如下任一个或任多个模块：
模块M0，基于一次轨迹规划确定初始参考轨迹的步骤：基于期望末端位置和传感器数据中获取的周围环境信息，生成可行轨迹集；从当前可行轨迹集中筛选出一条最优轨迹作为一次规划的参考轨迹；
模块M1，考虑人的意图的局部轨迹重规划的步骤：人类通过遥操作设备传输给机器人运动指令，通过虚拟交互力判断人意图是否强烈，当人意图强烈时，机器人进行局部参考轨迹调整；
模块M2，基于系统安全性评估指标的人机控制权重调整的步骤：通过传感器数据评估当前机器人构型的安全性，构建表征系统安全性的评估指标，并基于此评估指标动态调节人机控制权重；
模块M3，基于人机混合成本函数的模型预测控制步骤：构建基于机器人与人的控制成本的混合成本函数；通过混合成本函数的模型预测控制器，计算得到最优控制指令，实现人机共享控制。
优选地，所述模块M1包括：
模块M1.1：建立表征人类交互力的虚拟力模型；
模块M1.2：通过虚拟交互力判断人意图是否强烈；若是，则机器人不调整参考轨迹；；如否，则机器人进行局部参考轨迹调整；
所述模块M2包括：
步骤2.1：基于传感器的数据，实时感知环境信息，建立表征系统安全性的指标；其中，所述环境信息包括距离边界的距离、距离障碍物的距离；
模块M2.2：将系统安全性指标作为调节人机控制权重的依据；
所述模块M3包括：
模块M3.1：基于从端系统的动力学模型，建立从端系统状态空间表达式；
模块M3.2：根据机器人与人的期望指令，分别计算机器人、人的误差向量；
模块M3.3：在任一时刻k，计算机器人成本函数；
模块M3.4：在任一时刻k，计算人类成本函数；
模块M3.5：基于表征系统安全性的指标建立上述时刻k混合成本函数；
模块M3.6：基于模型预测控制框架，形成控制问题：在任一时刻k，优化时域为t＝[k,k+1],将其划分为等时间步长的P个离散时间段，滚动优化的目标为最小化此时域内的累计混合成本函数；预测模型即为基于从端系统的动力学模型所建立的状态空间表达式；系统安全性约束为当前机器人的位置距离最近障碍物与最近边界的距离；
模块M3.7：在每个优化时域t＝[k,k+1]内求解上述优化问题，计算k时刻的最优控制量输入。
优选地，在所述模块M0中：
确定机器人在笛卡尔空间中的目标构型为xfinal，初始位置x0，期望轨迹时长T，目标为规划一条从初始构型到目标构型xfinal最优的轨迹，即为参考轨迹xd(t)，t为时间变量；
通过传感设备，获取体内环境中障碍物位置分布和运动边界使用快速扩展随机树RRT算法生成可行轨迹集通过如下优化目标从可行轨迹集中筛选出一条最优轨迹作为一次规划的参考轨迹xd(t)：
其中，p代表了可行轨迹，优化指标由三部分组成，分别用于表征最短路径、最大限度避免障碍物、最大限度避免边界，αl,αo,αb均为正常数，用于调节上述三部分的比例。
优选地，所述模块M1包括：
模块M1.1：建立表征人类交互力的虚拟力模型：
Mm,Dm,Km分别代表了惯性矩阵、阻尼矩阵、刚度矩阵；
代表了当前机器人机械臂末端的位置；
为对应期望值；
Fh表征人类通过遥操作设备向从端机器人施加的虚拟交互力；
模块M1.2：通过虚拟交互力判断人意图是否强烈，判断系统如下：
设定阈值δi,i＝1,...,m，为对应虚拟交互力Fh第i个分量的下限，实时监测人类通过遥操作设备向从端机器人施加的虚拟交互力Fh的数值；
1)若Fh≤δi，则机器人不调整参考轨迹；
2)若Fh＞δi，则机器人进行局部参考轨迹调整；
在模块M1.2中，所述局部参考轨迹调整的步骤包括：
模块M1.2.1：确定待调整的局部轨迹范围t∈[ts,tf]，t为时间变量，ts、tf分别表征局部轨迹的起始时间、终止时间；
模块M1.2.2：将原轨迹xd(t)离散化为局部离散轨迹
其中，分别是xd(t)的第i个分量，δ为选取的离散点的时间间隔；
模块M1.2.3：对从当前位置到xd(tf)的局部轨迹进行重规划，生成局部可行轨迹集
模块M1.2.4：对于局部可行轨迹γd，局部轨迹能量E(γd)为γd调整后轨迹能量：
为原局部轨迹能量；
α为一个正常数；
R为正定对称矩阵；
基于表征调整后轨迹能量指标，从可行轨迹集中选出调整轨迹能量最小的一条可行轨迹作为最优轨迹γd作为调整后参考轨迹；
所述模块M2包括：
模块M2.1：基于传感器的数据，实时感知环境信息，建立表征系统安全性的指标；其中，所述环境信息包括距离边界的距离、距离障碍物的距离；
模块M2.2：将系统安全性指标作为调节人机控制权重的依据；
所述模块M2.1包括如下步骤：
模块M2.1.1：基于传感数据得到的体内环境中障碍物位置分布和运动边界下标i表示序号；
模块M2.1.2：构建当前时刻表征可移动余量向量dres(k)：
γd(k-1)为上一时刻k-1的参考轨迹位置，若在模块M1.2中判断为人意图较强，则参考轨迹γd为调整后的期望轨迹，否则，参考轨迹γd为一次规划得到的初始参考轨迹；k表示时刻；
模块M2.1.3：计算实际偏移量d(k)为机器人当前位置构型x(k-1)与对应期望值γd(k-1)的距离：
d(k)＝||x(k-1)-γd(k-1)||
模块M2.1.4：定义一个饱和函数dsat:[0,dres]→(0,dres)
μ1,μ2,θ2,ξ为Richards曲线的参数；
dmax(k)＝min{d(k),dres(k)}，参数值决定了曲线的形状，是根据实际情况设置的预定值；
模块M2.1.5：建立表征系统安全性的指标λ(k)：
所述模块M3包括：
模块M3.1：基于从端系统的动力学模型，建立从端系统状态空间表达式：
Mx,Cx,Gx分别是从端机器人在笛卡尔空间的惯性矩阵、科里奥利力离心力矩阵、重力矩阵，J为从机器人关节空间到笛卡尔空间的雅可比矩阵；控制输入为从端机器人的关节力矩输入τ；其中，状态变量为系统构型及其导数；
模块M3.2：根据机器人与人的期望指令，分别计算机器人与人的误差向量，机器人的误差向量为Δxr＝x-γd，人的误差向量为Δxh＝x-xhd；其中，γd是机器人的期望笛卡尔空间中构型位置；xhd是人的期望笛卡尔空间中构型位置，由人通过主端交互装置的力输入转化得到；
模块M3.3：在任一时刻k，计算机器人成本函数Cr(k)：
Q1r,Q2r,Q3r均为正定矩阵；
模块M3.4：在任一时刻k，计算人类成本函数：
Q1h,Q2h均为正定矩阵；
模块M3.5：基于表征系统安全性的指标λ(k)建立k时刻混合成本函数：
C(k)＝λ(k)Ch(k)+(1-λ(k))Cr(k)
模块M3.6：基于模型预测控制框架，形成如下控制问题：
z(k+1|k)＝A(k)z(k)+B(k)τ(k)+C(k)
该控制问题为：在任一时刻k，优化时域为t＝[k,k+1],将其划分为等时间步长的P个离散时间段，滚动优化的目标为最小化此时域内的累计混合成本函数；预测模型即为基于从端系统的动力学模型所建立的状态空间表达式，A(k),B(k),C(k)为对应的离散系数矩阵；系统安全性约束为，ho与hb分别代表当前机器人的位置距离最近障碍物与最近边界的距离，需保证其不小于对应的阈值δo,δb；
模块M3.7：在每个优化时域t＝[k,k+1]内求解上述优化问题，计算k时刻的最优控制量输入。
根据本发明提供的一种存储有计算机程序的计算机可读存储介质，所述计算机程序被处理器执行时实现所述的适用于机器人的局部轨迹调整与人机共享控制方法的步骤。
根据本发明提供的一种机器人，包括所述的适用于机器人的局部轨迹调整与人机共享控制系统，或者包括所述的存储有计算机程序的计算机可读存储介质。
与现有技术相比，本发明具有如下的有益效果：
1、本发明能进一步提升手术机器人的自主性，将人与机器人的关系从主从式转变为协同式。
2、在本发明中，当人通过遥操作装置向从端机器人提供的运动指令与机器人参考轨迹相差较大时，机器人将结合人的虚拟交互力对自身参考轨迹进行局部主动调整，同时避免了以下两种可能情况的发生：一是机器人仍掌握较大的控制权忽略了人的意图；二是人掌握较大的权重，将进行持续的主动操作，增加操作者的工作负担，同时不能很好的保证手术中的安全性；
3、本发明将综合考虑人机双方的指令，实时评估当前从端机械臂的构型的安全程度，构建相应指标量化系统安全性，动态调节人与机器人的控制比例：当安全性较高时，将赋予人更大的权重，反之，机器人的拥有更大权重，将人机控制以较高的自动化程度进行融合。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为本发明的原理示意图。
图2为本发明的整体框图。
图3为Richards曲线。
图4为系统安全性指标λ与dsat的映射关系。
图5为共享控制模块框图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
本发明提供了一种适用于机器人的局部轨迹调整与人机共享控制方法，包括以下步骤：
步骤S0：基于一次轨迹规划确定初始参考轨迹的步骤；
步骤S1：考虑人的意图的局部轨迹重规划的步骤；
步骤S2：基于系统安全性评估指标的人机控制权重调整的步骤；
步骤S3：基于人机混合成本函数的模型预测控制的步骤；
本发明能进一步提升手术机器人的自主性，将人与机器人的关系从主从式转变为协同式。当人的指令与机器人参考轨迹相差较大时，机器人会结合人的虚拟交互力对自身参考轨迹进行局部主动调整；当人与机器人意图相差较小时，将综合考虑人机双方指令，并基于系统安全性评估指标动态调节人机混合成本函数，计算最优控制量，实现人机共享控制。
在本实施例中，本发明提供的一种适用于遥操作适用于机器人的局部轨迹调整与人机共享控制方法，所述方法的原理示意图、整体框图分别如图1、图2所示，包括如下步骤：
基于一次轨迹规划确定初始参考轨迹步骤，记为步骤S0，具体地：根据手术任务需求，确定机器人在笛卡尔空间中的目标构型为xfinal，初始位置x0，期望轨迹时长T，则此步骤的目标为规划一条从初始构型到目标构型xfinal最优的轨迹，即为参考轨迹xd(t)，t为时间变量，单位为秒。首先，通过体外CT、超声探头、内窥镜等传感设备，获取体内环境中障碍物(器官、组织、血块等)位置分布和运动边界(气管、血管等)使用快速扩展随机树(RRT)算法生成可行轨迹集通过如下优化目标从可行轨迹集中筛选出一条最优轨迹作为一次规划的参考轨迹xd(t)：
其中，p代表了可行轨迹，优化指标由三部分组成，分别用于表征最短路径、最大限度避免障碍物、最大限度避免边界，αl,αo,αb均为正常数，用于调节上述三部分的比例。
考虑人的意图的局部轨迹重规划步骤，记为步骤S1，具体的：人类通过操作主端遥操作设备向从端机器人传输虚拟交互力，通过虚拟交互力系统判断当前人的操作意图是否强烈，当虚拟交互力大于一定阈值时，表明人控制意图较强烈，机器人将对原参考轨迹进行局部调整；
基于系统安全性评估指标的人机控制权重调整步骤，记为步骤S2，具体的：通过传感器数据评估当前机器人构型在所处环境中是否安全，基于可移动余量的概念构建系统安全性指标，量化当前系统的安全程度，并基于此指标动态调节人机控制权重；
基于人机混合成本函数的模型预测控制步骤，记为步骤S3，具体的：基于从端系统动力学模型构建离散状态空间表达式；根据人与机器人的期望，分别计算人与机器人的成本，并结合系统安全性指标构建混合成本函数，进而形成模型预测控制问题，通过迭代求解最优控制指令，实现人机共享控制。
步骤S0是通过规划的方法得到了机器人初始参考轨迹，根据实际的任务需求，初始参考轨迹也可直接人为设定。
所述步骤S1包括：
步骤S1.1：建立表征人类交互力的虚拟力模型：
其中，代表了当前机械臂末端的位置，为对应期望值。Fh表征人类通过遥操作设备向从端机器人施加的虚拟交互力。Mm,Dm,Km分别代表了惯性矩阵、阻尼矩阵、刚度矩阵。
步骤S1.2：通过虚拟交互力判断人意图是否强烈，判断方法如下：
设定阈值δi,i＝1,...,m，为对应虚拟交互力Fh第i个分量的下限，实时监测人类通过遥操作设备向从端机器人施加的虚拟交互力Fh的数值；
1)若Fh≤δi，则说明人当前的期望指令与机器人的期望运动偏差较小，机器人不调整参考轨迹；
2)若Fh＞δi，则说明人当前的期望指令与机器人的期望运动偏差较大，机器人进行局部参考轨迹调整。
在步骤S1.2中，所述局部参考轨迹调整的步骤包括：
步骤S1.2.1：确定待调整的局部轨迹范围t∈[ts,tf]，t为时间变量，单位为秒，ts、tf分别表征局部轨迹的起始时间、终止时间；
步骤S1.2.2：将原轨迹xd(t)离散化为局部离散轨迹
其中，xdi,i＝1,…,m分别是xd(t)的第i个分量，δ为选取的离散点的时间间隔；
步骤S1.2.3：对从当前位置到xd(tf)的局部轨迹进行重规划，通过RRT算法搜索生成局部可行轨迹集
步骤S1.2.4：对于局部可行轨迹γd，局部轨迹能量E(γd)为γd调整后轨迹能量：
其中，为原局部轨迹能量，为全为1的列向量，fhi、γdi分别是Fh、γd的第i个分量，α为一个正常数，R为正定对称矩阵。可以观察到调整后轨迹能量由三部分组成，分别为第一项原局部轨迹能量，第二项调整轨迹对人类做的功，第三项关于矩阵R的平方范数；
基于表征调整后轨迹能量指标，从可行轨迹集中选出一条最优轨迹γd作为调整后参考轨迹，筛选标准如下：
可以看出，此筛选标准实际上是选择了调整轨迹能量最小的一条可行轨迹，由于优化目标中含有不变量去除所有不变量包含的项即为上述筛选标准。其中，矩阵R的选取可由实际情况决定，这里，我们给出一种基于Minimum-jerk模型的选取方式：
由于Minimum-jerk模型可以更加精确的表达人体的运动轨迹，因此这种选取方式，保证了调整后的轨迹是更符合人类运动习惯的。
所述步骤S2包括：
步骤S2.1：基于传感器的数据，实时感知环境信息，建立表征系统安全性的指标；其中，所述环境信息包括距离边界的距离、距离障碍物的距离；
步骤S2.2、将系统安全性指标作为调节人机控制权重的依据；
所述步骤S2.1包括如下步骤：
步骤S2.1.1：基于传感数据得到的体内环境中障碍物(器官、组织、血块等)位置分布和运动边界(气管、血管等)
步骤S2.1.2：构建当前时刻表征可移动余量向量dres(k)：
其中，γd(k-1)为上一时刻的参考轨迹位置，若在步骤S1.2中判断为人意图较强，则参考轨迹γd为调整后的期望轨迹，否则，参考轨迹γd为一次规划得到的初始参考轨迹；k表示时刻；
步骤S2.1.3：计算实际偏移量为机器人当前位置构型与对应期望值的距离：
d(k)＝||x(k-1)-γd(k-1)||
步骤S2.1.4：使用如图3所示的Richards曲线定义一个饱和函数dsat:[0,dres]→(0,dres)
其中dmax(k)＝min{d(k),dres(k)}，μ1,μ2,θ2,ξ为Richards曲线的参数，参数值决定了曲线的形状，是根据实际情况设置的预定值。θ2为正实数，决定了饱和函数的上升速率，ξ为正实数，决定了曲线的弯曲程度，μ1是一个接近于1的正常数，决定了曲线的上渐近线的位置，μ2是一个正常数，曲线前端滞后的长度；
步骤S2.1.5：建立表征系统安全性的指标λ(k)：
此指标与dsat的映射关系如图4所示。
所述步骤S3人机共享控制模块结构如图5所示，包括：
步骤S3.1：基于从端系统的动力学模型，建立从端系统状态空间表达式：
其中，状态变量为系统构型及其导数；控制输入为从端机器人的关节力矩输入τ；Mx,Cx,Gx分别是从端机器人在笛卡尔空间的惯性矩阵、科里奥利力离心力矩阵、重力矩阵，J为从机器人关节空间到笛卡尔空间的雅可比矩阵；
步骤S3.2、根据机器人与人的期望指令，分别计算机器人与人的误差向量，机器人的误差向量为Δxr＝x-γd，人的误差向量为Δxh＝x-xhd；其中，γd是机器人的期望笛卡尔空间中构型位置，可由轨迹规划等途径获取；xhd是人的期望笛卡尔空间中构型位置，由人通过主端交互装置的力输入转化得到。
步骤S3.3：在任一时刻k，计算机器人成本函数：
上式由机器人的误差向量及其导数的二次型(前两项)和机器人控制输入的二次型(第三项)两部分组成，其中，Q1r,Q2r,Q3r均为正定矩阵；
步骤S3.4：在任一时刻k，计算人类成本函数：
上式由人的误差向量及其导数的二次型组成，其中，Q1h,Q2h均为正定矩阵；
步骤S3.5：基于表征系统安全性的指标λ(k)建立k时刻混合成本函数：
C(k)＝λ(k)Ch(k)+(1-λ(k))Cr(k)
步骤S3.6：基于模型预测控制框架，形成如下控制问题：
z(k+1|k)＝A(k)z(k)+B(k)τ(k)+C(k)
该控制问题为：在任一时刻k，优化时域为t＝[k,k+1],将其划分为等时间步长的P个离散时间段，滚动优化的目标为最小化此时域内的累计混合成本函数；预测模型即为基于从端系统的动力学模型所建立的状态空间表达式，A(k),B(k),C(k)为对应的离散系数矩阵；系统安全性约束为，ho与hb分别代表当前机器人的位置距离最近障碍物与最近边界的距离，需保证其不小于对应的阈值δo,δb；
步骤S3.7、在每个优化时域t＝[k,k+1]内求解上述优化问题，计算k时刻的最优控制量输入。求解上述二次规划(QP)优化问题可选用的方法有很多，包括如活动集、内点法、一阶优化法等，目前也有许多开源的QP求解库可以直接调用。
本发明还提供一种适用于机器人的局部轨迹调整与人机共享控制系统，本领域技术人员可以通过执行所述适用于机器人的局部轨迹调整与人机共享控制方法的步骤流程实现所述适用于机器人的局部轨迹调整与人机共享控制系统，即可以将所述方法理解为所述适用于机器人的局部轨迹调整与人机共享控制系统的一个优选的实施方式。
根据本发明提供的一种适用于机器人的局部轨迹调整与人机共享控制系统，包括如下任一个或任多个模块：
模块M0，基于一次轨迹规划确定初始参考轨迹的步骤：基于期望末端位置和传感器数据中获取的周围环境信息，生成可行轨迹集；从当前可行轨迹集中筛选出一条最优轨迹作为一次规划的参考轨迹；
模块M1，考虑人的意图的局部轨迹重规划的步骤：人类通过遥操作设备传输给机器人运动指令，通过虚拟交互力判断人意图是否强烈，当人意图强烈时，机器人进行局部参考轨迹调整；
模块M2，基于系统安全性评估指标的人机控制权重调整的步骤：通过传感器数据评估当前机器人构型的安全性，构建表征系统安全性的评估指标，并基于此评估指标动态调节人机控制权重；
模块M3，基于人机混合成本函数的模型预测控制步骤：构建基于机器人与人的控制成本的混合成本函数；通过混合成本函数的模型预测控制器，计算得到最优控制指令，实现人机共享控制。
优选地，所述模块M1包括：
模块M1.1：建立表征人类交互力的虚拟力模型；
模块M1.2：通过虚拟交互力判断人意图是否强烈；若是，则机器人不调整参考轨迹；；如否，则机器人进行局部参考轨迹调整；
所述模块M2包括：
步骤2.1：基于传感器的数据，实时感知环境信息，建立表征系统安全性的指标；其中，所述环境信息包括距离边界的距离、距离障碍物的距离；
模块M2.2：将系统安全性指标作为调节人机控制权重的依据；
所述模块M3包括：
模块M3.1：基于从端系统的动力学模型，建立从端系统状态空间表达式；
模块M3.2：根据机器人与人的期望指令，分别计算机器人、人的误差向量；
模块M3.3：在任一时刻k，计算机器人成本函数；
模块M3.4：在任一时刻k，计算人类成本函数；
模块M3.5：基于表征系统安全性的指标建立上述时刻k混合成本函数；
模块M3.6：基于模型预测控制框架，形成控制问题：在任一时刻k，优化时域为t＝[k,k+1],将其划分为等时间步长的P个离散时间段，滚动优化的目标为最小化此时域内的累计混合成本函数；预测模型即为基于从端系统的动力学模型所建立的状态空间表达式；系统安全性约束为当前机器人的位置距离最近障碍物与最近边界的距离；
模块M3.7：在每个优化时域t＝[k,k+1]内求解上述优化问题，计算k时刻的最优控制量输入。
优选地，在所述模块M0中：
确定机器人在笛卡尔空间中的目标构型为xfinal，初始位置x0，期望轨迹时长T，目标为规划一条从初始构型到目标构型xfinal最优的轨迹，即为参考轨迹xd(t)，t为时间变量；
通过传感设备，获取体内环境中障碍物位置分布和运动边界使用快速扩展随机树RRT算法生成可行轨迹集通过如下优化目标从可行轨迹集中筛选出一条最优轨迹作为一次规划的参考轨迹xd(t)：
其中，p代表了可行轨迹，优化指标由三部分组成，分别用于表征最短路径、最大限度避免障碍物、最大限度避免边界，αl,αo,αb均为正常数，用于调节上述三部分的比例。
优选地，所述模块M1包括：
模块M1.1：建立表征人类交互力的虚拟力模型：
Mm,Dm,Km分别代表了惯性矩阵、阻尼矩阵、刚度矩阵；
代表了当前机器人机械臂末端的位置；
为对应期望值；
Fh表征人类通过遥操作设备向从端机器人施加的虚拟交互力；
模块M1.2：通过虚拟交互力判断人意图是否强烈，判断系统如下：
设定阈值δi,i＝1,…,m，为对应虚拟交互力Fh第i个分量的下限，实时监测人类通过遥操作设备向从端机器人施加的虚拟交互力Fh的数值；
1)若Fh≤δi，则机器人不调整参考轨迹；
2)若Fh＞δi，则机器人进行局部参考轨迹调整；
在模块M1.2中，所述局部参考轨迹调整的步骤包括：
模块M1.2.1：确定待调整的局部轨迹范围t∈[ts,tf]，t为时间变量，ts、tf分别表征局部轨迹的起始时间、终止时间；
模块M1.2.2：将原轨迹xd(t)离散化为局部离散轨迹
其中，xdi,i＝1,…,m，分别是xd(t)的第i个分量，δ为选取的离散点的时间间隔；
模块M1.2.3：对从当前位置到xd(tf)的局部轨迹进行重规划，生成局部可行轨迹集
模块M1.2.4：对于局部可行轨迹γd，局部轨迹能量E(γd)为γd调整后轨迹能量：
为原局部轨迹能量；
α为一个正常数；
R为正定对称矩阵；
基于表征调整后轨迹能量指标，从可行轨迹集中选出调整轨迹能量最小的一条可行轨迹作为最优轨迹γd作为调整后参考轨迹；
所述模块M2包括：
模块M2.1：基于传感器的数据，实时感知环境信息，建立表征系统安全性的指标；其中，所述环境信息包括距离边界的距离、距离障碍物的距离；
模块M2.2：将系统安全性指标作为调节人机控制权重的依据；
所述模块M2.1包括如下步骤：
模块M2.1.1：基于传感数据得到的体内环境中障碍物位置分布和运动边界下标i表示序号；
模块M2.1.2：构建当前时刻表征可移动余量向量dres(k)：
γd(k-1)为上一时刻k-1的参考轨迹位置，若在模块M1.2中判断为人意图较强，则参考轨迹γd为调整后的期望轨迹，否则，参考轨迹γd为一次规划得到的初始参考轨迹；k表示时刻；
模块M2.1.3：计算实际偏移量d(k)为机器人当前位置构型x(k-1)与对应期望值γd(k-1)的距离：
d(k)＝||x(k-1)-γd(k-1)||
模块M2.1.4：定义一个饱和函数dsat:[0,dres]→(0,dres)
μ1,μ2,θ2,ξ为Richards曲线的参数；
dmax(k)＝min{d(k),dres(k)}，参数值决定了曲线的形状，是根据实际情况设置的预定值；
模块M2.1.5：建立表征系统安全性的指标λ(k)：
所述模块M3包括：
模块M3.1：基于从端系统的动力学模型，建立从端系统状态空间表达式：
Mx,Cx,Gx分别是从端机器人在笛卡尔空间的惯性矩阵、科里奥利力离心力矩阵、重力矩阵，J为从机器人关节空间到笛卡尔空间的雅可比矩阵；控制输入为从端机器人的关节力矩输入τ；其中，状态变量为系统构型及其导数；
模块M3.2：根据机器人与人的期望指令，分别计算机器人与人的误差向量，机器人的误差向量为Δxr＝x-γd，人的误差向量为Δxh＝x-xhd；其中，γd是机器人的期望笛卡尔空间中构型位置；xhd是人的期望笛卡尔空间中构型位置，由人通过主端交互装置的力输入转化得到；
模块M3.3：在任一时刻k，计算机器人成本函数Cr(k)：
Q1r,Q2r,Q3r均为正定矩阵；
模块M3.4：在任一时刻k，计算人类成本函数：
Q1h,Q2h均为正定矩阵；
模块M3.5：基于表征系统安全性的指标λ(k)建立k时刻混合成本函数：
C(k)＝λ(k)Ch(k)+(1-λ(k))Cr(k)
模块M3.6：基于模型预测控制框架，形成如下控制问题：
z(k+1|k)＝A(k)z(k)+B(k)τ(k)+C(k)
该控制问题为：在任一时刻k，优化时域为t＝[k,k+1],将其划分为等时间步长的P个离散时间段，滚动优化的目标为最小化此时域内的累计混合成本函数；预测模型即为基于从端系统的动力学模型所建立的状态空间表达式，A(k),B(k),C(k)为对应的离散系数矩阵；系统安全性约束为，ho与hb分别代表当前机器人的位置距离最近障碍物与最近边界的距离，需保证其不小于对应的阈值δo,δb；
模块M3.7：在每个优化时域t＝[k,k+1]内求解上述优化问题，计算k时刻的最优控制量输入。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
根据本发明提供的一种存储有计算机程序的计算机可读存储介质，所述计算机程序被处理器执行时实现所述的适用于机器人的局部轨迹调整与人机共享控制方法的步骤。
根据本发明提供的一种机器人，包括所述的适用于机器人的局部轨迹调整与人机共享控制系统，或者包括所述的存储有计算机程序的计算机可读存储介质。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (4)

1.一种适用于机器人的局部轨迹调整与人机共享控制方法，其特征在于，包括如下步骤：
步骤S0，基于一次轨迹规划确定初始参考轨迹的步骤：基于期望末端位置和传感器数据中获取的周围环境信息，生成可行轨迹集；从当前可行轨迹集中筛选出一条最优轨迹作为一次规划的参考轨迹；
步骤S1，考虑人的意图的局部轨迹重规划的步骤：人类通过遥操作设备传输给机器人运动指令，通过虚拟交互力判断人意图是否强烈，当人意图强烈时，机器人进行局部参考轨迹调整；
步骤S2，基于系统安全性评估指标的人机控制权重调整的步骤：通过传感器数据评估当前机器人构型的安全性，构建表征系统安全性的评估指标，并基于此评估指标动态调节人机控制权重；
步骤S3，基于人机混合成本函数的模型预测控制步骤：构建基于机器人与人的控制成本的混合成本函数；通过混合成本函数的模型预测控制器，计算得到最优控制指令，实现人机共享控制；
所述步骤S1包括：
步骤S1.1：建立表征人类交互力的虚拟力模型；
步骤S1.2：通过虚拟交互力判断人意图是否强烈；若是，则机器人不调整参考轨迹；如否，则机器人进行局部参考轨迹调整；
所述步骤S2包括：
步骤2.1：基于传感器的数据，实时感知环境信息，建立表征系统安全性的指标；其中，所述环境信息包括距离边界的距离、距离障碍物的距离；
步骤S2.2：将系统安全性指标作为调节人机控制权重的依据；
所述步骤S3包括：
步骤S3.1：基于从端系统的动力学模型，建立从端系统状态空间表达式；
步骤S3.2：根据机器人与人的期望指令，分别计算机器人、人的误差向量；
步骤S3.3：在任一时刻k，计算机器人成本函数；
步骤S3.4：在任一时刻k，计算人类成本函数；
步骤S3.5：基于表征系统安全性的指标建立上述时刻k混合成本函数；
步骤S3.6：基于模型预测控制框架，形成控制问题：在任一时刻k，优化时域为t＝[k,k+1],将其划分为等时间步长的P个离散时间段，滚动优化的目标为最小化此时域内的累计混合成本函数；预测模型即为基于从端系统的动力学模型所建立的状态空间表达式；系统安全性约束为当前机器人的位置距离最近障碍物与最近边界的距离；
步骤S3.7：在每个优化时域t＝[k,k+1]内求解上述优化问题，计算k时刻的最优控制量输入；
在所述步骤S0中：
确定机器人在笛卡尔空间中的目标构型为xfinal，初始位置x0，期望轨迹时长T，目标为规划一条从初始构型到目标构型xfinal最优的轨迹，即为参考轨迹xd(t)，t为时间变量；
通过传感设备，获取体内环境中障碍物位置分布和运动边界使用快速扩展随机树RRT算法生成可行轨迹集通过如下优化目标从可行轨迹集中筛选出一条最优轨迹作为一次规划的参考轨迹xd(t)：
其中，p代表了可行轨迹，优化指标由三部分组成，分别用于表征最短路径、最大限度避免障碍物、最大限度避免边界，αl,αo,αb均为正常数，用于调节上述三部分的比例；
所述步骤S1包括：
步骤S1.1：建立表征人类交互力的虚拟力模型：
Mm,Dm,Km分别代表了惯性矩阵、阻尼矩阵、刚度矩阵；
代表了当前机器人机械臂末端的位置；
为对应期望值；
Fh表征人类通过遥操作设备向从端机器人施加的虚拟交互力；
步骤S1.2：通过虚拟交互力判断人意图是否强烈，判断方法如下：
设定阈值δi,i＝1,...,m，为对应虚拟交互力Fh第i个分量的下限，实时监测人类通过遥操作设备向从端机器人施加的虚拟交互力Fh的数值；
1)若Fh≤δi，则机器人不调整参考轨迹；
2)若Fh＞δi，则机器人进行局部参考轨迹调整；
在步骤S1.2中，所述局部参考轨迹调整的步骤包括：
步骤S1.2.1：确定待调整的局部轨迹范围t∈[ts,tf]，t为时间变量，ts、tf分别表征局部轨迹的起始时间、终止时间；
步骤S1.2.2：将原轨迹xd(t)离散化为局部离散轨迹
其中，xdi,i＝1,…,m，分别是xd(t)的第i个分量，δ为选取的离散点的时间间隔；
步骤S1.2.3：对从当前位置到xd(tf)的局部轨迹进行重规划，生成局部可行轨迹集
步骤S1.2.4：对于局部可行轨迹γd，局部轨迹能量E(γd)为γd调整后轨迹能量：
为原局部轨迹能量；
α为一个正常数；
R为正定对称矩阵；
基于表征调整后轨迹能量指标，从可行轨迹集中选出调整轨迹能量最小的一条可行轨迹作为最优轨迹γd作为调整后参考轨迹；
所述步骤S2包括：
步骤S2.1：基于传感器的数据，实时感知环境信息，建立表征系统安全性的指标；其中，所述环境信息包括距离边界的距离、距离障碍物的距离；
步骤S2.2：将系统安全性指标作为调节人机控制权重的依据；
所述步骤S2.1包括如下步骤：
步骤S2.1.1：基于传感数据得到的体内环境中障碍物位置分布和运动边界下标i表示序号；
步骤S2.1.2：构建当前时刻表征可移动余量向量dres(k)：
γd(k-1)为上一时刻k-1的参考轨迹位置，若在步骤S1.2中判断为人意图较强，则参考轨迹γd为调整后的期望轨迹，否则，参考轨迹γd为一次规划得到的初始参考轨迹；k表示时刻；
步骤S2.1.3：计算实际偏移量d(k)为机器人当前位置构型x(k-1)与对应期望值γd(k-1)的距离：
d(k)＝||x(k-1)-γd(k-1)||
步骤S2.1.4：定义一个饱和函数dsat:[0,dres]→(0,dres)
μ1,μ2,θ2,ξ为Richards曲线的参数；
dmax(k)＝min{d(k),dres(k)}，参数值决定了曲线的形状，是根据实际情况设置的预定值；
步骤S2.1.5：建立表征系统安全性的指标λ(k)：
所述步骤S3包括：
步骤S3.1：基于从端系统的动力学模型，建立从端系统状态空间表达式：
Mx,Cx,Gx分别是从端机器人在笛卡尔空间的惯性矩阵、科里奥利力离心力矩阵、重力矩阵，J为从机器人关节空间到笛卡尔空间的雅可比矩阵；控制输入为从端机器人的关节力矩输入τ；其中，状态变量为系统构型及其导数；
步骤S3.2：根据机器人与人的期望指令，分别计算机器人与人的误差向量，机器人的误差向量为Δxr＝x-γd，人的误差向量为Δxh＝x-xhd；其中，γd是机器人的期望笛卡尔空间中构型位置；xhd是人的期望笛卡尔空间中构型位置，由人通过主端交互装置的力输入转化得到；
步骤S3.3：在任一时刻k，计算机器人成本函数Cr(k)：
Q1r,Q2r,Q3r均为正定矩阵；
步骤S3.4：在任一时刻k，计算人类成本函数：
Q1h,Q2h均为正定矩阵；
步骤S3.5：基于表征系统安全性的指标λ(k)建立k时刻混合成本函数：
C(k)＝λ(k)Ch(k)+(1-λ(k))Cr(k)
步骤S3.6：基于模型预测控制框架，形成如下控制问题：
z(k+1|k)＝A(k)z(k)+B(k)τ(k)+C(k)
该控制问题为：在任一时刻k，优化时域为t＝[k,k+1],将其划分为等时间步长的P个离散时间段，滚动优化的目标为最小化此时域内的累计混合成本函数；预测模型即为基于从端系统的动力学模型所建立的状态空间表达式，A(k),B(k),C(k)为对应的离散系数矩阵；系统安全性约束为，ho与hb分别代表当前机器人的位置距离最近障碍物与最近边界的距离，需保证其不小于对应的阈值δo,δb；
步骤S3.7：在每个优化时域t＝[k,k+1]内求解上述优化问题，计算k时刻的最优控制量输入。
2.一种适用于机器人的局部轨迹调整与人机共享控制系统，其特征在于，包括如下模块：
模块M0，基于一次轨迹规划确定初始参考轨迹的步骤：基于期望末端位置和传感器数据中获取的周围环境信息，生成可行轨迹集；从当前可行轨迹集中筛选出一条最优轨迹作为一次规划的参考轨迹；
模块M1，考虑人的意图的局部轨迹重规划的步骤：人类通过遥操作设备传输给机器人运动指令，通过虚拟交互力判断人意图是否强烈，当人意图强烈时，机器人进行局部参考轨迹调整；
模块M2，基于系统安全性评估指标的人机控制权重调整的步骤：通过传感器数据评估当前机器人构型的安全性，构建表征系统安全性的评估指标，并基于此评估指标动态调节人机控制权重；
模块M3，基于人机混合成本函数的模型预测控制步骤：构建基于机器人与人的控制成本的混合成本函数；通过混合成本函数的模型预测控制器，计算得到最优控制指令，实现人机共享控制；
所述模块M1包括：
模块M1.1：建立表征人类交互力的虚拟力模型；
模块M1.2：通过虚拟交互力判断人意图是否强烈；若是，则机器人不调整参考轨迹；如否，则机器人进行局部参考轨迹调整；
所述模块M2包括：
步骤2.1：基于传感器的数据，实时感知环境信息，建立表征系统安全性的指标；其中，所述环境信息包括距离边界的距离、距离障碍物的距离；
模块M2.2：将系统安全性指标作为调节人机控制权重的依据；
所述模块M3包括：
模块M3.1：基于从端系统的动力学模型，建立从端系统状态空间表达式；
模块M3.2：根据机器人与人的期望指令，分别计算机器人、人的误差向量；
模块M3.3：在任一时刻k，计算机器人成本函数；
模块M3.4：在任一时刻k，计算人类成本函数；
模块M3.5：基于表征系统安全性的指标建立上述时刻k混合成本函数；
模块M3.6：基于模型预测控制框架，形成控制问题：在任一时刻k，优化时域为t＝[k,k+1],将其划分为等时间步长的P个离散时间段，滚动优化的目标为最小化此时域内的累计混合成本函数；预测模型即为基于从端系统的动力学模型所建立的状态空间表达式；系统安全性约束为当前机器人的位置距离最近障碍物与最近边界的距离；
模块M3.7：在每个优化时域t＝[k,k+1]内求解上述优化问题，计算k时刻的最优控制量输入；
在所述模块M0中：
确定机器人在笛卡尔空间中的目标构型为xfinal，初始位置x0，期望轨迹时长T，目标为规划一条从初始构型到目标构型xfinal最优的轨迹，即为参考轨迹xd(t)，t为时间变量；
通过传感设备，获取体内环境中障碍物位置分布和运动边界使用快速扩展随机树RRT算法生成可行轨迹集通过如下优化目标从可行轨迹集中筛选出一条最优轨迹作为一次规划的参考轨迹xd(t)：
其中，p代表了可行轨迹，优化指标由三部分组成，分别用于表征最短路径、最大限度避免障碍物、最大限度避免边界，αl,αo,αb均为正常数，用于调节上述三部分的比例；
所述模块M1包括：
模块M1.1：建立表征人类交互力的虚拟力模型：
Mm,Dm,Km分别代表了惯性矩阵、阻尼矩阵、刚度矩阵；
代表了当前机器人机械臂末端的位置；
为对应期望值；
Fh表征人类通过遥操作设备向从端机器人施加的虚拟交互力；
模块M1.2：通过虚拟交互力判断人意图是否强烈，判断系统如下：
设定阈值δi,i＝1,...,m，为对应虚拟交互力Fh第i个分量的下限，实时监测人类通过遥操作设备向从端机器人施加的虚拟交互力Fh的数值；
1)若Fh≤δi，则机器人不调整参考轨迹；
2)若Fh＞δi，则机器人进行局部参考轨迹调整；
在模块M1.2中，所述局部参考轨迹调整的步骤包括：
模块M1.2.1：确定待调整的局部轨迹范围t∈[ts,tf]，t为时间变量，ts、tf分别表征局部轨迹的起始时间、终止时间；
模块M1.2.2：将原轨迹xd(t)离散化为局部离散轨迹
其中，xdi,i＝1,...,m，分别是xd(t)的第i个分量，δ为选取的离散点的时间间隔；
模块M1.2.3：对从当前位置到xd(tf)的局部轨迹进行重规划，生成局部可行轨迹集
模块M1.2.4：对于局部可行轨迹γd，局部轨迹能量E(γd)为γd调整后轨迹能量：
为原局部轨迹能量；
α为一个正常数；
R为正定对称矩阵；
基于表征调整后轨迹能量指标，从可行轨迹集中选出调整轨迹能量最小的一条可行轨迹作为最优轨迹γd作为调整后参考轨迹；
所述模块M2包括：
模块M2.1：基于传感器的数据，实时感知环境信息，建立表征系统安全性的指标；其中，所述环境信息包括距离边界的距离、距离障碍物的距离；
模块M2.2：将系统安全性指标作为调节人机控制权重的依据；
所述模块M2.1包括如下步骤：
模块M2.1.1：基于传感数据得到的体内环境中障碍物位置分布和运动边界下标i表示序号；
模块M2.1.2：构建当前时刻表征可移动余量向量dres(k)：
γd(k-1)为上一时刻k-1的参考轨迹位置，若在模块M1.2中判断为人意图较强，则参考轨迹γd为调整后的期望轨迹，否则，参考轨迹γd为一次规划得到的初始参考轨迹；k表示时刻；
模块M2.1.3：计算实际偏移量d(k)为机器人当前位置构型x(k-1)与对应期望值γd(k-1)的距离：
d(k)＝||x(k-1)-γd(k-1)||
模块M2.1.4：定义一个饱和函数dsat:[0,dres]→(0,dres)
μ1,μ2,θ2,ξ为Richards曲线的参数；
dmax(k)＝min{d(k),dres(k)}，参数值决定了曲线的形状，是根据实际情况设置的预定值；
模块M2.1.5：建立表征系统安全性的指标λ(k)：
所述模块M3包括：
模块M3.1：基于从端系统的动力学模型，建立从端系统状态空间表达式：
Mx,Cx,Gx分别是从端机器人在笛卡尔空间的惯性矩阵、科里奥利力离心力矩阵、重力矩阵，J为从机器人关节空间到笛卡尔空间的雅可比矩阵；控制输入为从端机器人的关节力矩输入τ；其中，状态变量为系统构型及其导数；
模块M3.2：根据机器人与人的期望指令，分别计算机器人与人的误差向量，机器人的误差向量为Δxr＝x-γd，人的误差向量为Δxh＝x-xhd；其中，γd是机器人的期望笛卡尔空间中构型位置；xhd是人的期望笛卡尔空间中构型位置，由人通过主端交互装置的力输入转化得到；
模块M3.3：在任一时刻k，计算机器人成本函数Cr(k)：
Q1r,Q2r,Q3r均为正定矩阵；
模块M3.4：在任一时刻k，计算人类成本函数：
Q1h,Q2h均为正定矩阵；
模块M3.5：基于表征系统安全性的指标λ(k)建立k时刻混合成本函数：
C(k)＝λ(k)Ch(k)+(1-λ(k))Cr(k)
模块M3.6：基于模型预测控制框架，形成如下控制问题：
z(k+1|k)＝A(k)z(k)+B(k)τ(k)+C(k)
s.t.
该控制问题为：在任一时刻k，优化时域为t＝[k,k+1],将其划分为等时间步长的P个离散时间段，滚动优化的目标为最小化此时域内的累计混合成本函数；预测模型即为基于从端系统的动力学模型所建立的状态空间表达式，A(k),B(k),C(k)为对应的离散系数矩阵；系统安全性约束为，ho与hb分别代表当前机器人的位置距离最近障碍物与最近边界的距离，需保证其不小于对应的阈值δo,δb；
模块M3.7：在每个优化时域t＝[k,k+1]内求解上述优化问题，计算k时刻的最优控制量输入。
3.一种存储有计算机程序的计算机可读存储介质，其特征在于，所述计算机程序被处理器执行时实现权利要求1所述的适用于机器人的局部轨迹调整与人机共享控制方法的步骤。
4.一种机器人，其特征在于，包括权利要求2所述的适用于机器人的局部轨迹调整与人机共享控制系统，或者包括权利要求3所述的存储有计算机程序的计算机可读存储介质。
CN202111575047.7A 2021-12-21 2021-12-21 适用于机器人的局部轨迹调整与人机共享控制方法和系统 Active CN114454157B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111575047.7A CN114454157B (zh) 2021-12-21 2021-12-21 适用于机器人的局部轨迹调整与人机共享控制方法和系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111575047.7A CN114454157B (zh) 2021-12-21 2021-12-21 适用于机器人的局部轨迹调整与人机共享控制方法和系统

Publications (2)

Publication Number Publication Date
CN114454157A CN114454157A (zh) 2022-05-10
CN114454157B true CN114454157B (zh) 2024-05-14

Family

ID=81405596

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111575047.7A Active CN114454157B (zh) 2021-12-21 2021-12-21 适用于机器人的局部轨迹调整与人机共享控制方法和系统

Country Status (1)

Country Link
CN (1) CN114454157B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115877760B (zh) * 2022-12-05 2024-09-06 西北工业大学 一种操作场景相关的机器人操作交互过程共享自主控制方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108388270A (zh) * 2018-03-21 2018-08-10 天津大学 面向安全域的集群无人机轨迹姿态协同控制方法
CN110996825A (zh) * 2017-07-13 2020-04-10 直观外科手术操作公司 用于在多个器械臂之间切换控制权的系统和方法
CN111805546A (zh) * 2020-07-20 2020-10-23 中国人民解放军国防科技大学 一种基于脑机接口的人-多机器人共享控制的方法及系统
WO2021077725A1 (zh) * 2019-10-21 2021-04-29 南京航空航天大学 一种基于驾驶意图的周围车辆运动状态预测系统及方法
CN113370210A (zh) * 2021-06-23 2021-09-10 华北科技学院(中国煤矿安全技术培训中心) 一种机器人主动避撞系统及方法
CN214758698U (zh) * 2020-09-23 2021-11-19 谭军 手机5g网络远程共享控制钓真鱼机器人

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9447532B2 (en) * 2014-01-23 2016-09-20 Nike, Inc. Automated assembly and stitching of shoe parts

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110996825A (zh) * 2017-07-13 2020-04-10 直观外科手术操作公司 用于在多个器械臂之间切换控制权的系统和方法
CN108388270A (zh) * 2018-03-21 2018-08-10 天津大学 面向安全域的集群无人机轨迹姿态协同控制方法
WO2021077725A1 (zh) * 2019-10-21 2021-04-29 南京航空航天大学 一种基于驾驶意图的周围车辆运动状态预测系统及方法
CN111805546A (zh) * 2020-07-20 2020-10-23 中国人民解放军国防科技大学 一种基于脑机接口的人-多机器人共享控制的方法及系统
CN214758698U (zh) * 2020-09-23 2021-11-19 谭军 手机5g网络远程共享控制钓真鱼机器人
CN113370210A (zh) * 2021-06-23 2021-09-10 华北科技学院(中国煤矿安全技术培训中心) 一种机器人主动避撞系统及方法

Also Published As

Publication number Publication date
CN114454157A (zh) 2022-05-10

Similar Documents

Publication Publication Date Title
Su et al. Fuzzy approximation-based task-space control of robot manipulators with remote center of motion constraint
Selvaggio et al. Passive virtual fixtures adaptation in minimally invasive robotic surgery
Haidegger et al. Cascade control for telerobotic systems serving space medicine
Haidegger et al. Controller design solutions for long distance telesurgical applications
KR102603224B1 (ko) 로봇 수술을 위한 시스템 및 방법
Kermorgant et al. Dealing with constraints in sensor-based robot control
JP5750657B2 (ja) 強化学習装置、制御装置、および強化学習方法
CN112428278A (zh) 机械臂的控制方法、装置及人机协同模型的训练方法
JP6321905B2 (ja) 関節システムの制御方法、記憶媒体、制御システム
Bauzano et al. Collaborative human–robot system for hals suture procedures
Kim et al. Real-time adaptive kinematic model estimation of concentric tube robots
Deng et al. A review of bilateral teleoperation control strategies with soft environment
CN114454157B (zh) 适用于机器人的局部轨迹调整与人机共享控制方法和系统
CN115356991B (zh) 基于运动状态优化的多目标运动融合控制方法和系统
Feng Online Hierarchical Optimization for Humanoid Control.
Motaharifar et al. Nonlinear adaptive output-feedback controller design for guidance of flexible needles
Weede et al. Towards cognitive medical robotics in minimal invasive surgery
Cheng et al. A multilateral impedance-controlled system for haptics-enabled surgical training and cooperation in beating-heart surgery
Fracczak et al. Share control of surgery robot master manipulator guiding tool along the standard path
Cursi et al. Task accuracy enhancement for a surgical macro-micro manipulator with probabilistic neural networks and uncertainty minimization
Bauzano et al. A minimally invasive surgery robotic assistant for HALS–SILS techniques
Jiang et al. Lyapunov-based nonlinear model predictive control for the path following of bevel-tip flexible needles in 3D environment
Han et al. Online viewpoint optimization method for high-accuracy intraoperative optical measurement in orthopedic surgeries: An active navigation approach
Dominici et al. Heart motion compensation for robotic-assisted surgery predictive approach vs. active observer
Bauzano et al. Auto-guided movements on minimally invasive surgery for surgeon assistance

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