本发明涉及自动化与人工智能领域，具体涉及一种融合位置-速度-力的人机安全增强共享控制方法和装置。
背景技术：
1、随着自动化与人工智能的迅速发展，机器人已广泛渗透到人类的日常生活和工作。人与机器人的合作方式也由传统的共存不交互，转变为如今远程操控、人机协同、人机共享合作的控制模式。当前，面对非结构化的场景或者复杂的任务，机器人很难完全自主执行任务。人机交互的方式可以结合双方的优势：人具有善于推理和解决问题的能力，且面对复杂的环境能灵活应变；而机器人在执行方面具有抗疲劳、高精准的优势，特殊的材料结构特性也能胜任恶劣的工作环境。
2、现有的人机共享控制研究主要围绕提升任务执行的表现与效率、机器人自主智能纠偏人的行为、以及机器人主动适应人的行为等方面，相关应用包括自动驾驶下保留驾驶员的控制能力、虚拟夹具提供引导与碰撞规避、强化学习识别并适应人的行为、自主颤动补偿提升手术质量、生产协作中角色智能切换来提高生产效率等。
3、例如，《a confidence-based shared control strategy for the smart tissueautonomous robot (star)》，其提出一种基于置信度的控制权分配函数，能够在自主控制和手动控制之间进行自动切换，从而提高机器人辅助手术系统整体任务的性能。然而，该方案虽然考虑了机器人自主控制与人类控制的结合，但过于强调自主智能的利用，而忽视了人类在手术中的主导地位。在自主智能作为领导者过程中，当外科医生意识到即将发生的危险时，操作人员在返回控制回路时缺乏态势感知，从自主控制突然切换到手动控制会导致瞬态误差增加，如果处理不当则会增加系统的风险。
4、再如，《adaptive impedance controller for human-robot arbitration basedon cooperative differential game theory》，其提出了一种基于合作微分博弈的人机交互控制权重分配方法，用于解决物理人机交互过程中人与机器的角色切换问题。然而，该方案通过力传感器虽然可以采集主手端人施加的力，达到估计人的控制意图的目的，但考虑到手术过程中手感对于医生操控而言十分重要，在机器人自主操控策略和医生手术指引意图的高频交互中，缺乏符合医生操控直觉的力反馈，很难保证手术执行过程的安全可控性。
技术实现思路
1、（一）解决的技术问题
2、针对现有技术的不足，本发明提供了一种融合位置-速度-力的人机安全增强共享控制方法和装置，解决了人与机器人的角色完全转移存在医生不可掌控的隐患以及力学感知信息的缺失无法有效保障机器人手术安全的技术问题。
3、（二）技术方案
4、为实现以上目的，本发明通过以下技术方案予以实现：
5、一种融合位置-速度-力的人机安全增强共享控制方法，包括：
6、将机器人系统简化为一个弹簧-质量-阻尼系统，并建立状态方程；
7、根据人机共享控制的目标需求，分别设定主手位置跟踪、主手速度跟踪和机器人规划轨迹跟踪三种目标控制器；
8、建立相应的运动预测控制模型，以获取各个目标控制器的预测控制状态；
9、根据各个预测控制状态，引入惩罚机制设置各个目标控制器的评价函数；
10、根据各个评价函数，对应计算各个控制目标的预测评价梯度下降值，以获取各个目标控制器在不同场景下的权限分配向量；
11、将权限分配向量作为状态方程的输入，以输出融合后的系统运动状态和运动速度；
12、根据融合后的系统运动状态与主手机器人的实时位置，基于力反馈机制生成作用于主手机器人的触觉力；
13、根据融合后的运动速度以及姿态变化的速度，生成从手机器人的关节角的转动速度。
14、优选的，所述状态方程为：
15、
16、其中，其中， x为系统运动状态，为系统运动速度，为系统的加速度， u为系统总的控制输入， y为系统总的控制输出， m、 b和 k分别为弹簧-质量-阻尼系统的惯性参数、阻尼参数和刚度参数。
17、优选的，所述根据人机共享控制的目标需求，分别设定主手位置跟踪、主手速度跟踪和机器人规划轨迹跟踪三种目标控制器；包括：
18、设置主手位置跟踪控制器：
19、
20、其中， uhx为主手位置跟踪控制器的输入， kpx、 kdx为主手位置跟踪控制器的比例系数和微分系数， xhd为主手机器人的实时位置， ehx为主手机器人的实时位置与系统运动状态的偏差，为误差的变化速度；
21、设置主手速度跟踪控制器：
22、
23、其中， uhv为主手速度跟踪控制器的输入， kpv为主手速度跟踪控制器的比例系数， vhd为主手机器人的实时速度， ehv为主手机器人的实时速度与系统运动速度的偏差；
24、设置规划轨迹跟踪控制器：
25、
26、其中， urx为规划轨迹跟踪控制器的输入， kpr、 kdr为规划轨迹跟踪控制器的比例系数和微分系数， xrd为规划轨迹的实时位置， erx为规划轨迹的实时位置与系统运动状态的偏差，为误差的变化速度。
27、优选的，所述建立相应的运动预测控制模型，以获取各个目标控制器的预测控制状态；包括：
28、
29、其中，为预测区间内控制器 i的预测运动状态，为预测区间内控制器 i的预测运动速度，为预测区间内控制器 i的预测加速度，为预测区间内控制目标 i的预测控制输出；
30、 i=1时，对应的主手位置跟踪控制器的预测控制状态为；
31、 i=2时，对应的主手速度跟踪控制器的预测控制状态为；
32、 i=3时，对应的机器人规划轨迹跟踪控制器的预测控制状态为。
33、优选的，所述根据各个预测控制状态，引入惩罚机制设置各个目标控制器的评价函数；包括：
34、针对主手位置跟踪控制器建立目标评价函数：
35、
36、其中， jhx为主手位置跟踪控制器在预测区间 t0到 t0 +t时间段内的目标函数累计值， ghx为主手位置跟踪控制器在预测区间内单时刻点的目标评价函数值， x0为 t0时刻系统的运动状态；为主手位置跟踪控制目标的预测控制位置，按当前状态向前滚动平均计算得到； phx为惩罚函数， v0为触发惩罚的系统控制速度阈值，用于时减小 jhx；
37、针对主手速度跟踪控制器建立目标评价函数：
38、
39、其中， jhv为主手速度跟踪控制器在预测区间 t0到 t0 +t时间段内的目标函数累计值， ghv为主手速度跟踪控制器在预测区间内单时刻点的目标评价函数值；为主手速度跟踪控制目标的预测控制速度，按当前状态向前滚动平均计算得到；为主手速度跟踪控制器的预测控制状态速度；
40、针对机器人规划轨迹跟踪控制器建立目标评价函数：
41、
42、其中， jrx为规划轨迹跟踪控制器在预测区间 t0到 t0 +t时间段内的目标函数累计值， gx为规划轨迹跟踪控制器在预测区间内单时刻点的目标评价函数值；为规划轨迹跟踪控制目标的预测控制位置，按当前状态向前滚动平均计算得到； d0为触发惩罚的距离阈值， prx为位置距离惩罚函数，用于时减小 jrx； prv为速度距离惩罚函数，用于时减小 jrx。
43、优选的，所述根据各个评价函数，对应计算各个控制目标的预测评价梯度下降值，以获取各个目标控制器在不同场景下的权限分配向量；包括：
44、针对主手位置跟踪、主手速度跟踪和机器人规划轨迹跟踪三个控制目标分别计算预测评价梯度下降值：
45、
46、其中，为主手位置跟踪控制目标的预测评价梯度下降值，为主手速度跟踪控制目标的预测评价梯度下降值，为机器人规划轨迹跟踪控制目标的预测评价梯度下降值；
47、根据场景控制需求融合到全局的系统输入中，以获取各个目标控制器在不同场景下的权限分配向量：
48、
49、其中， ko为比例系数矩阵， λ为梯度调整的步长， ∇j为总的预测评价梯度下降值， φα为饱和抑制函数， ε为一个很小的常数， α为阈值常数。
50、优选的，所述根据融合后的系统运动状态与主手机器人的实时位置，基于力反馈机制生成作用于主手机器人的触觉力；包括：
51、
52、其中， x*为融合后的系统运动状态， f为反馈到主手机器人的触觉力，分别为融合后的系统运动状态与主手机器人的实时位置之间的误差、误差变化速度及误差变化加速度， kf 、bf和 mf分别表示力反馈系统的惯性、阻尼和刚度参数。
53、优选的，所述根据融合后的运动速度以及姿态变化的速度，生成从手机器人的关节角的转动速度；包括：
54、
55、其中，为融合后的运动速度， xm表示固定的远端运动中心， xt表示姿态矩阵的x轴向量， yt表示姿态矩阵的y轴向量， zt表示姿态矩阵的z轴向量， qt为机器人运动的姿态，函数表示将姿态旋转矩阵转换为笛卡坐标系下的欧拉角，为笛卡坐标系下的欧拉角，为欧拉角变化的速度，为从手机器人的关节角的转动速度； j-1为雅可比矩阵。
56、一种融合位置-速度-力的人机安全增强共享控制装置，包括：
57、建立模块，用于将机器人系统简化为一个弹簧-质量-阻尼系统，并建立状态方程；
58、设定模块，用于根据人机共享控制的目标需求，分别设定主手位置跟踪、主手速度跟踪和机器人规划轨迹跟踪三种目标控制器；
59、预测模块，用于建立相应的运动预测控制模型，以获取各个目标控制器的预测控制状态；
60、设置模块，用于根据各个预测控制状态，引入惩罚机制设置各个目标控制器的评价函数；
61、计算模块，用于根据各个评价函数，对应计算各个控制目标的预测评价梯度下降值，以获取各个目标控制器在不同场景下的权限分配向量；
62、输出模块，用于将权限分配向量作为状态方程的输入，以输出融合后的系统运动状态和运动速度；
63、反馈模块，用于根据融合后的系统运动状态与主手机器人的实时位置，基于力反馈机制生成作用于主手机器人的触觉力；
64、生成模块，用于根据融合后的运动速度以及姿态变化的速度，生成从手机器人的关节角的转动速度。
65、（三）有益效果
66、本发明提供了一种融合位置-速度-力的人机安全增强共享控制方法和装置。与现有技术相比，具备以下有益效果：
67、本发明设定主手位置跟踪、主手速度跟踪和机器人规划轨迹跟踪三种控制器，实现位置-速度的融合，提高了人机共享控制中人的掌控能力；此外，将三种控制器融合结果与力反馈机制关联，实现位置-速度-力的融合，增强了系统人机交互的安全性。
1.一种融合位置-速度-力的人机安全增强共享控制方法，其特征在于，包括：
2.如权利要求1所述的人机安全增强共享控制方法，其特征在于，所述状态方程为：
3.如权利要求2所述的人机安全增强共享控制方法，其特征在于，所述根据人机共享控制的目标需求，分别设定主手位置跟踪、主手速度跟踪和机器人规划轨迹跟踪三种目标控制器；包括：
4.如权利要求3所述的人机安全增强共享控制方法，其特征在于，所述建立相应的运动预测控制模型，以获取各个目标控制器的预测控制状态；包括：
5.如权利要求4述的人机安全增强共享控制方法，其特征在于，所述根据各个预测控制状态，引入惩罚机制设置各个目标控制器的评价函数；包括：
6.如权利要求5述的人机安全增强共享控制方法，其特征在于，所述根据各个评价函数，对应计算各个控制目标的预测评价梯度下降值，以获取各个目标控制器在不同场景下的权限分配向量；包括：
7.如权利要求6述的人机安全增强共享控制方法，其特征在于，所述根据融合后的系统运动状态与主手机器人的实时位置，基于力反馈机制生成作用于主手机器人的触觉力；包括：
8.如权利要求6述的人机安全增强共享控制方法，其特征在于，所述根据融合后的运动速度以及姿态变化的速度，生成从手机器人的关节角的转动速度；包括：
9.一种融合位置-速度-力的人机安全增强共享控制装置，其特征在于，包括：
