主操作手及其驱动力获取方法、装置及存储介质与流程

1.本技术属于主操作手领域，尤其涉及主操作手及其驱动力获取方法、装置及存储介质。


背景技术：

2.主操作手是一种可通过主操作手来控制从端设备操作的机器人。由于主操作手与从端设备分离，主操作手被广泛应用于危险空间探索、大众娱乐、工业生产、医疗服务等领域。在主操作手系统中，主操作手作为操作者与从端设备之间的交互设备，向从端设备传递操作者给出的位姿、速度等信息，也可向操作者传递从端设备受到的力或力矩等环境信息，以使操作者具有操作临场感，便于及时对从端设备的运动做有效的控制和干预。
3.为了使得主操作手实现6个运动自由度，目前的主操作手通常采用位置和姿态部分解耦后串联的方式，位置部分由3自由度delta并联机构或串联机构实现3个方向的移动，姿态部分由三轴或四轴交汇的串联机构实现3个方向的转动。然而现有的这种delta并联主操作手在进行静力平衡计算时，计算过程较为复杂，不利于降低操作时延。


技术实现要素：

4.有鉴于此，本技术实施例提供了一种主操作手及其驱动力获取方法、装置及存储介质，以解决相关技术中的delta并联主操作手在进行静力平衡计算时，计算过程较为复杂，不利于降低操作时延的问题。
5.第一方面，本技术实施例提供了一种主操作手的驱动力获取方法，所述主操作手包括多个模块，所述多个模块中至少包括静平台、动平台，且所述动平台受用户控制进行运动；其中，所述静平台包括第一静平台和第二静平台，第一静平台上包括n个第一静位置点，第二静平台上包括与第一静位置点一一对应的n个第二静位置点，每个第一静位置点和第二静位置点均设置有对应的主动驱动装置，所述动平台上包括与所述n个第二静位置点一一对应的n个第二动位置点，所述第一静平台上还包括与所述n个第一静位置点一一对应的n个第一动位置点；
6.所述方法包括：
7.确定第二静位置点的驱动输入量关于动平台运动位姿的第二驱动表示公式；其中，所述运动位姿用于指示所述动平台相对于所述第一静平台或第二静平台的位姿；所述第二静位置点的驱动输入量用于指示所述第二动位置点在所述主动驱动装置的控制下，产生的相对于对应的第二静位置点的位置变化；
8.基于所述第二驱动表示公式，确定第二静位置点的被动旋转量关于动平台运动位姿的被动表示公式，并基于所述第二驱动表示公式，确定第一静位置点的驱动输入量关于动平台运动位姿的第一驱动表示公式；所述被动旋转量用于指示所述动平台进行运动后所述动位置点产生的相对于第二静位置点的位置变化，所述第一静位置点的驱动输入量用于指示所述第一动位置点在所述主动驱动装置的控制下，产生的相对于对应的第一静位置点
的位置变化；
9.基于所述被动表示公式，确定所述被动旋转量与预设运动参数间的第一关系表示公式，以及，基于所述第一驱动表示公式和第二驱动表示公式，确定驱动输入量与所述预设运动参数间的第二关系表示公式；所述第二关系表示公式中包括逆雅克比矩阵，所述逆雅克比矩阵用于表示所述驱动输入量与所述主操作手的输出间的映射关系；
10.根据预先测量获得的主操作手中的各个模块的质心点初始状态下在静平台坐标系中的坐标、所述第一关系表示公式和所述第二关系表示公式，确定所述各个模块与所述预设运动参数间的映射关系；所述静平台坐标系基于所述第一静平台建立，所述映射关系中包括雅克比矩阵，所述雅克比矩阵用于表示所述各个模块与所述预设运动参数间的映射关系；
11.根据第二静位置点的驱动输入量和被动旋转量，确定动平台运动位姿的位姿数据；所述驱动输入量由所述第一静位置点和/或第二静位置点对应的主动驱动装置控制而产生；
12.根据所述主操作手接收到的受力、所述各个模块的重力、所述各个模块的雅克比矩阵、所述逆雅克比矩阵以及所述动平台运动位姿的位姿数据，确定所述主操作手输出的驱动力；
13.根据所述驱动力，对所述主操作手进行力反馈以及重力平衡。
14.本技术实施例与现有技术相比存在的有益效果是：本技术通过确定第二静位置点以及第一静位置点的驱动输入量关于动平台运动位姿的驱动表示公式，以及第二静位置点的被动旋转量的被动表示公式，并通过所述驱动表示公式和被动表示公式确定用于表示所述驱动输入量及被动旋转量与预设运动参数间的关系表示公式，基于所测量的各模块的质心点在初始状态下在静平台坐标系中的坐标，结合关系表示公式确定模块与预设运动参数间的映射关系，根据第二静位置点的驱动输入量与被动旋转量，确定主操作手的动平台的位姿数据，基于主操作手接收到的受力、所述各个模块的重力、所述各个模块的映射关系以及动平台运动位姿的位姿数据，确定所述主操作手输出的驱动力，从而使得计算过程不需要计算每个部件的受力平衡，有利于简化计算过程，有利于提高主操作手的响应速度。
附图说明
15.为了更清楚地说明本技术实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
16.图1是本技术实施例提供的一种驱动力获取方法的应用场景示意图；
17.图2-图2e是本技术实施例提供的一种主操作手的结构示意图；
18.图3是本技术实施例提供的一种主操作手的结构示意图；
19.图4是本技术实施例提供的一种主操作手模块的支架的动力连接示意图；
20.图5是本技术实施例提供的一种第二动位置点点的坐标系变化示意图；
21.图6是本技术实施例提供的一种第一静位置点的坐标系变化示意图；
22.图7是本技术实施例提供的一种第二静位置点的坐标系变化示意图；
23.图8是本技术实施例提供的一种驱动力获取方法的实现流程示意图；
24.图9是本技术实施例提供的一种求解驱动参数示意图；
25.图10是本技术实施例提供的主操作手模块的分解示意图；
26.图11是本技术实施例提供的一种主操作手的控制装置的示意图；
27.图12是本技术实施例提供的主操作手的示意图。
具体实施方式
28.以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本技术实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本技术。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本技术的描述。
29.为了说明本技术所述的技术方案，下面通过具体实施例来进行说明。
30.参见图1，本技术实施例提供的驱动力获取方法的应用场景示意图。如图1所示，所述从端机器人是可以根据所述主操作手的指令做出对应行为的设备，所述主操作手用于操作者与所述从端机器人之间进行交互，向从端机器人传递操作者给出的位姿、速度等信息，同时也可向操作者传递从端系统受到的力/力矩等环境信息。
31.具体的，所述主操作手通过确定第二静位置点以及第一静位置点的驱动输入量关于动平台运动位姿的驱动表示公式，以及第二静位置点的被动旋转量的被动表示公式，并通过所述驱动表示公式和被动表示公式确定用于表示所述驱动输入量及被动旋转量与预设运动参数间的关系表示公式，基于所测量的各模块的质心点在初始状态下在静平台坐标系中的坐标，结合关系表示公确定模块与预设运动参数间的映射关系，根据第二静位置点的驱动输入量与被动旋转量，确定主操作手的动平台的位姿数据，基于主操作手接收到的受力、所述各个模块的重力、所述各个模块的映射关系以及动平台运动位姿的位姿数据，确定所述主操作手输出的驱动力，从而使得计算过程不需要计算每个部件的受力平衡。
32.所述从端机器人将在作业环境中的受力信息发送至所述主操作手，接收所述主操作手发送的操作指令并根据所述操作指令进行作业。
33.如图2为本技术实施例提供的一种主操作手的结构示意图，如图2所示，该主操作手包括间隔布置的动平台10和静平台，所述静平台包括第二静平台620和第一静平台(底座)627、以及连接所述动平台10与所述静平台的n个支架30，其中n为支架的数量，n≥2。优选地，n＝3。还优选地，n个支架30沿周向均匀间隔分布。更优地，n个支架30相对静平台20的中心轴线朝向动平台10倾斜聚拢，以减小操作组件100整体的体积，使得医生的手臂可以在较小的空间范围内适应动平台10的运动情况，进而更方便地调节手术器械的位置。所述动平台10上可以固定连接手柄。
34.参考图2-图2e，本实施例中，支架30包括上支撑件31以及连接在上支撑件31上的第一滑杆33。优选地，所述上支撑件31呈板状。所述第一滑杆33上套设有第一直线电机34。所述第一滑杆33优选为丝杆。所述第一直线电机34用于驱动所述第一滑杆33相对第一直线电机34沿直线移动。所述上支撑件31可有效防止第一滑杆33移动至使得第一直线电机34脱离第一滑杆33。
35.优选地，所述支架30还包括固定连接在所述上支撑件31上的侧面支撑件35，用于
(例如通过粘接)固定第一光栅刻度尺36。所述侧面支撑件35与所述第一滑杆33相对。所述第一直线电机34还固定连接有一与所述第一光栅刻度尺36相对的第一读数头37。由此，当第一滑杆33相对第一直线电机34直线运动时，与所述第一滑杆33连接的所述上支撑件31、与所述上支撑件31固定连接的侧面支撑件35、以及与所述侧面支撑件35固定连接的第一光栅刻度尺36也将跟随第一滑杆33一起相对第一直线电机34运动。由于第一读数头37与所述第一直线电机34固定连接，因此，第一光栅刻度尺36与第一读数头37产生相对运动，使得第一读数头37能够记录第一光栅刻度尺36的移动距离，也即第一滑杆33相对第一直线电机34的移动距离。
36.还优选地，所述第三连接件38还包括与所述第一支撑板381垂直连接的第三支撑板384，用于固定连接一个或多个第一滑块385。所述第一侧板351上固定连接有第一滑轨355，所述第一滑轨355的长度方向与所述第一侧板351的长度方向一致。所述一个或多个第一滑块385与所述第一滑轨355滑动连接。
37.由此，当第一直线电机34驱动第一滑杆33相对第一直线电机34直线运动时，与所述第一滑杆33连接的所述上支撑件31、与所述上支撑件31固定连接的侧面支撑件35、以及与所述侧面支撑件35固定连接的第一光栅刻度尺36和第一滑轨355也将跟随第一滑杆33一起相对第一直线电机34运动。由于第一读数头37、第一直线电机34、以及第一滑块385通过第三连接件538固定连接，因此，第一滑轨355将相对第一滑块385滑动。
38.本实施例的第二静平台620大致呈六边形，包括三个彼此间隔的第一侧边、以及三个彼此间隔的第二侧边，其中三个第一侧边分别具有一突出的支撑轴。支撑轴上转动连接有一支撑柱。所述支撑柱包括可转动地套设在所述支撑轴上的第一连接区段、以及与第一连接区段连接的并用于与第三连接件连接的第二连接区段。
39.所述第二连接区段优选呈半圆状，并在其中部具有一第五通孔。所述第三连接件的第二连接柱插入所述第五通孔，并与所述第二连接区段转动连接。优选地，所述第二连接柱与所述第二连接区段之间设有轴承组件。所述第二连接柱具有一自其自由端凹陷的第五接收孔。螺钉或销等第二连接元件插入第二连接柱的第五接收孔，并优选通过第二垫片与所述轴承组件抵顶。
40.所述第一连接区段呈中空的立方体状，并优选突出于所述第二连接区段，用于(例如通过螺钉)固定连接编码器。编码器与第一直线电机34电连接。当通过第三连接件与支撑柱转动连接的支架30在第二直线电机660的驱动下相对第二静平台620发生转动时，编码器将记录第三连接件相对第二静平台620的转动角度，因此也将记录第一滑杆相对第二静平台620的转动角度。
41.所述第二静平台620的三个彼此间隔的第二侧边分别通过相应的支撑架626固定(例如通过螺钉)至第一静平台(底座)627，第一静平台(底座)627可放置在地面。
42.本实施例中，所述第一静平台(底座)627大致呈三角形，其三个角与所述支撑架626(例如通过螺钉)固定连接，且每个角设有一左支撑件641和右支撑件642。本实施例中，所述左支撑件641和右支撑件642呈板状，并分别垂直于所述底座627的相邻两侧面。所述第一静平台(底座)627的每一侧面间隔设有一连接在左支撑件641和右支撑件642之间的第二滑杆643。
43.同时参考图2，每一所述第二滑杆643上套设有一第二直线电机660。所述第二滑杆
643优选为丝杆，使得所述第二直线电机660能够沿所述第二滑杆643作直线运动，其中，所述左支撑件641和所述右支撑件642可有效防止所述第二直线电机660移动至脱离第二滑杆643。
44.本实施例中，所述第一静平台(底座)627的每一侧面上还固定连接(例如粘接)有一第二光栅刻度尺628。所述第二直线电机660固定连接有一与第二光栅刻度尺628相对的第二读数头661。由此，当第二直线电机660沿着第二滑杆643直线运动时，与所述第二直线电机660固定连接的第二读数头661也将跟随第二直线电机660一起运动。由于第二光栅刻度尺628与所述底座627固定连接，因此，第二读数头661与第二光栅刻度尺628产生相对运动，使得第二读数头661能够记录其自身的移动距离，进而记录第二直线电机660相对第二滑杆643的移动距离。
45.优选地，所述第二直线电机660与所述第二读数头661通过第四连接件670固定连接。具体地，所述第四连接件670包括第四支撑板671。所述第二直线电机660与所述第二读数头661(例如通过螺钉)固定连接在第四支撑板的底面。优选地，如图2a和图2b所示，所述第四连接件670还包括固定连接在所述第四支撑板的一侧的第五支撑板672，用于固定连接一个或多个第二滑块662。本实施例中，所述第二直线电机660、第二读数头661、以及第二滑块662均固定在第四支撑件670的底面。所述底座627上固定连接有第二滑轨629，所述第二滑轨629的长度方向与所述底座627的每一侧面的长度方向一致。所述一个或多个第二滑块662与所述第二滑轨629滑动连接。
46.由此，当第二直线电机660相对第二滑杆643直线运动时，通过所述第四连接件670与所述第二直线电机660固定连接的所述第二读数头661、以及第二滑块662也将跟随第二直线电机660一起相对第二滑杆643运动。由于第二滑轨629与底座627固定连接，因此，第二滑块662将相对第二滑轨629滑动。通过第二滑块662和第二滑轨629的配合，有效提高了第二直线电机660相对第二滑杆643运动的稳定性。
47.本实施例中，所述第五支撑板672的远离所述第二滑块662的顶面固定连接有一纵长的第三滑轨680。所述第三滑轨680的长度方向垂直于相应的所述第二滑轨629的长度方向。优选地，所述第三滑轨680通过一支撑条(例如通过螺钉)固定连接在第五支撑板672上。优选地，三条第三滑轨680距离底座627的高度各不相同，这可以通过在第五支撑板上设置垫块来实现，其中垫块与支撑条固定连接。换言之，三条第三滑轨680对应的垫块的高度各不相同，从而使得三条第三滑轨680距离底座627的高度各不相同。优选地，垫块与第四支撑板之间通过第二加强肋连接，以提高第四连接件670的强度。更优地，第四连接件670为一一体件，例如可通过注塑成型形成。
48.参见图2-图2e，所述第三滑轨680上滑动连接有一或多个第三滑块682。所述一或多个第三滑块682上固定连接有一轴承座683，所述轴承座683内设有一可转动的球轴承684，所述球轴承684与一连杆685连接。所述连杆685与所述第一滑杆33滑动连接以保证第一滑杆33能够被第一直线电机34驱动以沿直线移动，优选套设在第一滑杆33外。
49.由此，当第二直线电机660相对第二滑杆643直线运动时，与第二直线电机660固定连接的第四连接件670、与第四连接件670固定连接的第三滑轨680也将跟随第二直线电机660一起相对第二滑杆643移动，在此过程中，连杆685将被第二直线电机660驱动以沿着第二滑轨629移动，同时自动沿着第三滑轨680移动，进而带动支架30相对第二静平台620转
动。
50.本实施例的操作组件600的工作原理简述如下：医生通过手指控制手柄上的按钮，以将指令通过控制器传递给手术机械臂，使得手术器械动作；手术机械臂的传感器将手术器械的动作数据通过控制器传递给第一直线电机和第二直线电机660，并借助于编码器控制支架30(因此也是控制第一直线电机)相对于第二静平台620的转动角度，借助于第一读数头控制被第一直线电机驱动的第一滑杆的移动距离，借助于第二读数头661控制第二直线电机660的移动距离，进而使得动平台10以及与之固定连接的手柄模拟手术器械动作，从而实现医生在手术操作过程中与手术器械的力学信息的交互，即通过操作组件600实现了医生对手术器械的真实手术操作的模拟。值得一提的是，手术器械的动作优选在手柄处等比例放大，虽然也可以不放大。
51.如图3、图4所示的主操作手示意图中，本实施例的操作组件600共有6个驱动量，驱动力大小为fi，单位n，分别为用于驱动三条连杆685相对静平台620转动的三个第二直线电机660、以及用于驱动三条连杆685移动的三个第一直线电机，且第一直线电机与静平台转动连接，三条连杆685的一端也分别与动平台10转动连接。
52.因此，本实施例的操作组件能够实现6自由度，并能同时实现力反馈和姿态控制，且布线简单。进一步地，利用滑杆传动可有效避免力反馈出错。
53.图3为本技术实施例的主操作手的主要结构的配合关系示意图。如图3所示，所述主操作手包括静平台、动平台、静铰接点(静位置点)s1，s2，s3，s4，s5，s6、动铰接点(动位置点)m1，m2，m3，m4，m5，m6和驱动l1，l2，l3，l4，l5，l6。
54.其中，驱动li(i＝1,2,3)的轴线可以根据安装在第一静平台上的驱动装置的运动轨迹确定，例如第二直线电机660沿第二滑杆643运动，则运动轨迹为沿第二滑杆643。由于运动轨迹为直线，且互相不平行，由此可以确定三个li轴线的内切圆，称为第一静铰接圆(如图3所示，圆心为os，半径可以通过测量确定，记为rs)。同时，将该三个轴线(即沿第二滑杆643)与内切圆的切点分别确定为静铰接点(第一静位置点)si(i＝1,2,3)。
55.根据连杆685与第一静平台的交点，可以确定动铰接点(第一动位置点)mi(i＝1,2,3)。由于第一静位置点si(i＝1,2,3)位于第一静平台所形成的平面中，且mi为连杆685与第一静平台的交点，因此第一静位置点si(i＝1,2,3)与第一动位置点mi(i＝1,2,3)在同一平面内，即第一静铰接圆所在的平面。同时，第一动位置点mi(i＝1,2,3)随连杆685的运动而运动，根据连杆685的运动轨迹，第一动位置点mi(i＝1,2,3)可以在驱动li的(i＝1,2,3)轴线方向、以及轴线的垂直方向上运动。
56.连杆685相对于第二静平台转动连接，根据转动连接的连接点，可以确定静铰接点(第二静位置点)si(i＝4,5,6)。举例来说，连杆685与第二静平台通过预设连接件连接，将第二静平台上与该连接件的连接点作为第二静位置点si(i＝4,5,6)。同时，基于该三个第二静位置点，可以确定第二静铰接圆(如图3所示，圆心为os’，半径可以通过测量确定，记为r
s_new
)。
57.连杆685相对于动平台转动连接，根据转动连接的连接点，可以确定动铰接点(第二动位置点)mi(i＝4,5,6)。举例来说，连杆685与动平台通过预设连接件(例如可以包括球铰)连接，将动平台上与该连接件的连接点作为第二动位置点mi(i＝4,5,6)。可以理解的是，该三个第二动位置点随动平台的运动而运动。同时，基于该三个第二动位置点，可以确
定第二动铰接圆(如图3所示，圆心为om，半径可以通过测量确定，记为rm)。
58.驱动li(i＝4,5,6)可以由安装在第二静平台处的驱动装置(例如上文所述第一直线电机34)所驱动的第一滑杆33的运动控制(即第一滑杆33沿连杆685的滑动)，该3个直线驱动li(i＝4,5,6)的轴线分别与具有对应关系的第二动位置点和第二静位置点simi(i＝4,5,6)连线平行。在一个实施例中，可以将li(i＝4,5,6)确定为对应的第二动位置点与第二静位置点之间的距离。
59.为描述方便，第一静平台的静铰接点可以称为第一静铰接点或第一静位置点，第一静铰接点对应的动铰接点可以称为第一动铰接点或第一动位置点。第一静平台的静铰接点可以称为第二静铰接点或第二静位置点，第二静位置点对应的动铰接点可以称为第二动铰接点或第二动位置点。
60.其中，第一静平台为627，动平台为10，第二静平台620位于第一静平台与动平台之间。第一静位置点(第一静铰接点)包括si(i＝1,2,3)；第二静位置点(第二静铰接点)包括si(i＝4,5,6)；第一动位置点(第一动铰接点)包括mi(i＝1,2,3)；第二动位置点(第二动铰接点)包括mi(i＝4,5,6)。第一静位置点的驱动输入量，用于指示具有对应关系的第一静位置点si(i＝1,2,3)和第一动位置点mi(i＝1,2,3)在第二滑块运动方向上(即沿第二滑杆643方向)的距离为li(i＝1,2,3)；第二静位置点的驱动输入量，用于指示第二静位置点和第二动位置点在第一滑杆33运动方向上(即沿连杆685方向)的距离为li(i＝4,5,6)。
61.在一个实施例中，可以基于静平台、动平台、各个静位置点、以及各个动位置点建立坐标系。举例来说，主操作手中可以建立有以下坐标系：第一动位置点第一坐标系、第一动位置点第二坐标系、第二动位置点第一坐标系以及第二动位置点第二坐标系；其中，所述第一动位置点第一坐标系基于所述第一动位置点建立，所述第一动位置点第二坐标系基于所述第一动位置点、所述第二静位置点、以及所述第二动位置点建立；所述第二动位置点第一坐标系基于所述第二动位置点和所述动平台建立，所述第二动位置点第二坐标系基于所述第二动位置点和所述静位置点第二坐标系建立。其中，所述第二动位置点转动角度包括：所述第二动位置点第二坐标系的第一方向坐标轴相对于第二动位置点第一坐标系旋转的第一角度α
mi
，以及，所述第二动位置点第二坐标系的第二方向坐标轴相对于第二动位置点第一坐标系旋转的第二角度β
mi
。
62.为便于描述，可以先确定一个初始状态(或称为默认状态)。在初始状态下，动平台、第一静平台、第二静平台平行，且各个第一动位置点与对应的第一静位置点之间的距离相等，各个第二动位置点与对应的第二动位置点之间的距离相等且固定。下文所涉及的初始状态均可以为这种状态。
63.下面结合图4示意性示出的主操作手的动力结构示意图，对各个坐标系的建立进行具体介绍。
64.基于第一静铰接圆，可以建立静平台坐标系o
s-x
syszs
：
65.将圆心os确定为静平台坐标系的坐标原点os；
66.将从静平台坐标系的坐标原点os指向任意一个第一静位置点的方向确定为静平台坐标系的第一方向坐标轴。例如，将os指向s1的方向确定为静平台坐标系的x轴的方向，记为xs轴；
67.将第三方向坐标轴确定为垂直于第一静铰接圆所在平面，且指向动铰接圆所在平
面。例如，将垂直于第一静铰接圆所在平面，由os指向动铰接圆所在平面的方向确定为静平台坐标系z轴的方向，记为zs轴；
68.在通过上述方法确定静平台坐标系的第一方向坐标轴和第三方向坐标轴后，即可根据坐标系的基本原理确定第二方向坐标轴。例如，可以通过右手定则，确定静平台坐标系y轴的方向，记为ys轴。
69.类似地，基于动铰接圆的圆心om，可以建立动平台坐标系o
m-xmymzm。将圆心om作为动平台坐标系的原点om；将从om指向任意一个动位置点的方向确定为动平台坐标系的第一方向坐标轴，例如向om指向m1的方向确定为x轴的方向，记为xm轴；将动平台坐标系的第三方向坐标轴确定为垂直于动铰接圆所在平面，且背离静铰接圆所在平面，可以记为zm轴。在第一方向坐标轴和第三方向坐标轴的基础上，可以确定第二方向坐标轴，记为ym轴。可以理解的是，在初始状态时，动平台坐标系的各坐标轴与静平台坐标系的对应坐标轴平行。后续，随着动平台的运动，动平台坐标系可以随着动平台的位姿变化而变化。
70.当然，不必局限于此，由于第一静平台和第二静平台不随用户控制而运动，且互相平行，因此也可以基于第二静平台对应的第二静铰接圆来建立静平台坐标系，本实施例不进行限定。
71.在一个实施例中，还可以基于每个第二动位置点mi(i＝4,5,6)分别建立第二动位置点第一坐标系o
mi-x
miymizmi
(i＝4,5,6)和第二动位置点第二坐标系o
mi-new-x
mi-newymi-newzmi-new
(i＝4,5,6)。下面以具有对应关系的s4和m4为例，并结合图5进行说明。
72.下面介绍建立第二动位置点第一坐标系o
mi-x
miymizmi
(i＝4,5,6)的方法：
73.将第二动位置点m4确定为第二动位置点第一坐标系的坐标原点o
m4
；
74.将从动铰接圆的圆心om指向m4的方向确定为第二动位置点第一坐标系的第一方向坐标轴(例如x轴，记为x
m4
)的方向；
75.将动平台坐标系的第三方向坐标轴线(例如zs)的方向确定为第二动位置点第一坐标系的第三方向坐标轴线(例如z轴，记为z
m4
)的方向；
76.将垂直于第二动位置点第一坐标系的第一方向坐标轴(例如x
m4
)和第三坐标轴线(例如z
m4
)且符合右手定则的方向确定为第二动位置点第一坐标系的第二方向坐标轴(例如y轴，记为y
m4
)的方向。
77.下面介绍建立第二动位置点第二坐标系o
mi-new-x
mi-newymi-newzmi-new
(i＝4,5,6)的方法：
78.将第二动位置点m4确定为第二动位置点第二坐标系的坐标原点o
m4-new
；
79.在初始状态下，将第二动位置点第一坐标系的第二方向坐标轴(例如y
m4
)的方向确定为第二动位置点第二坐标系的第二方向坐标轴(例如y轴，记为y
m4-new
)；
80.在初始状态下，将从第二静位置点s4指向第二动位置点m4的方向确定为动位置点第二坐标系的第三方向坐标轴(例如z轴，记为z
m1-new
)的方向；
81.将相对于上述第二方向坐标轴y
m1-new
和第三方向坐标轴z
m1-new
，满足右手定则的方向确定为第一方向坐标轴(例如x轴，记为x
m1-new
)的方向。
82.可以理解的是，第二动位置点第二坐标系可由动位置点第一坐标系通过姿态变换得到，变换过程为由动位置点第一坐标系先绕自身y轴转动，再绕自身x轴转动，记绕自身y轴转动角度为β
mi
，绕自身x轴转动角度为α
mi
。
83.在一个实施例中，还可以基于每个第二静位置点si(i＝4,5,6)分别建立第二静位置点第一坐标系o
si-x
siysizsi
(i＝4,5,6)和第二静位置点第二坐标系o
si-new-x
si-newysi-newzsi-new
(i＝4,5,6)。下面以具有对应关系的s4和m4为例，并结合图6进行说明。
84.下面介绍建立第二静位置点第一坐标系o
si-x
siysizsi
(i＝4,5,6)的方法：
85.将第二静位置点s4确定为第二静位置点第一坐标系的坐标原点o
s4
；
86.将从第二静铰接圆的圆心o
s’指向第二静位置点s4的方向确定为第二静位置点第一坐标系的第一方向坐标轴(例如x轴，记为x
s4
)的方向；
87.将静平台坐标系的第三方向坐标轴线(例如zs轴)的方向确定为第二静位置点第一坐标系的第三方向坐标轴线(例如z轴，记为z
s4
)的方向；
88.将垂直于第二静位置点第一坐标系的第一方向坐标轴(例如x
s41
轴)和第三坐标轴线(例如z
s4
轴)且符合右手定则的方向确定为静位置点第一坐标系的第二方向坐标轴(例如y轴，记为y
s4
轴)的方向。
89.下面介绍建立第二静位置点第二坐标系o
si-new-x
si-newysi-newzsi-new
(i＝4,5,6)的方法：
90.将第二静位置点s4确定为第二静位置点第二坐标系的坐标原点o
s4-new
；
91.将从s4指向与第二动位置点m4的方向确定为第二静位置点第二坐标系的第三方向坐标轴(例如z轴，记为z
s4-new
)的方向；
92.将在初始状态下第二静位置点第一坐标系的第二坐标轴线(例如y轴y
s1
)的方向，确定为初始状态下第二静位置点第二坐标轴的第二方向坐标轴(例如y轴，记为y
s1-new
)的方向；
93.将相对于上述第二方向坐标轴y
s4-new
和第三方向坐标轴z
s4-new
，满足右手定则的方向确定为第一方向坐标轴(例如x轴，记为x
s4-new
)的方向。
94.可以理解的是，第二静位置点第二坐标系可由静位置点第一坐标系通过姿态变换得到，变换过程为由静位置点第一坐标系先绕自身x轴转动，再绕自身y轴转动，记绕x
si
轴转角为θ
si
，绕y
si
轴转角为
95.需要说明的是，上述只是以s4和m4为例进行示例性说明。在实际应用中，还可以其他具有对应关系的第二静位置点和第二动位置点建立第二静位置点第一坐标系、第二静位置点第二坐标系、第二动位置点第一坐标系和第二动位置点第二坐标系，例如以s5和m5建立静位置点第一坐标系o
s5-x
s5ys5zs5
、以及静位置点第二坐标系o
s5-new-x
s5-newys5-newzs5-new
等。这里不再赘述。
96.在一个实施例中，还可以基于每个第一动位置点mi(i＝1,2,3)分别建立第一动位置点第一坐标系o
mi-x
miymizmi
(i＝1,2,3)、和第一动位置点第二坐标系o
mi-new-x
mi-newymi-newzmi-new
(i＝1,2,3)；以及基于每个第一静位置点si(i＝1,2,3)分别建立第一静位置点第一坐标系o
si-x
siysizsi
(i＝1,2,3)，下面以具有对应关系的s1和m1为例，并结合图7进行说明。
97.下面介绍建立第一静位置点第一坐标系o
si-x
siysizsi
(i＝1,2,3)的方法：
98.将第一静位置点s1确定为第一静位置点第一坐标系的坐标原点o
s1
；
99.将从静平台坐标系的坐标原点os指向第一静位置点s1的方向确定为第一静位置点第一坐标系的第一方向坐标轴(例如x轴，记为x
s1
)的方向；
100.将静平台坐标系的第三方向坐标轴线(例如zs轴)的方向确定为第一静位置点第一坐标系的第三方向坐标轴线(例如z轴，记为z
s1
)的方向；
101.将垂直于第一静位置点第一坐标系的第一方向坐标轴(例如x
s1
轴)和第三坐标轴线(例如z
s1
轴)且符合右手定则的方向确定为第一静位置点第一坐标系的第二方向坐标轴(例如y轴，记为y
s1
轴)的方向。
102.下面介绍建立第一动位置点第一坐标系o
mi-x
miymizmi
(i＝1,2,3)的方法：
103.将第一动位置点m1确定为第一动位置点第一坐标系的坐标原点o
m1
；
104.第一动位置点第一坐标系各轴线分别与第一静位置点第一坐标系o
si-x
siysizsi
(i＝1,2,3)的各轴线平行，例如第一方向坐标轴x
m1
与x
s1
平行，第二方向坐标轴y
m1
与y
s1
平行，第三方向坐标轴z
m1
与z
s1
平行。
105.下面介绍建立第一动位置点第二坐标系o
mi-new-x
mi-newymi-newzmi-new
(i＝1,2,3)的方法：
106.将第一动位置点m1确定为第一动位置点第二坐标系的坐标原点o
m1-new
；
107.第一动位置点第坐标系各轴线分别与第二静位置点第二坐标系o
si-new-x
si-newysi-newzsi-new
(i＝4,5,6)的各轴线平行，例如第一方向坐标轴x
m1-new
与x
s4-new
平行，第二方向坐标轴y
m1-new
与y
s4-new
平行，第三方向坐标轴z
m1-new
与z
s4-new
平行。
108.需要说明的是，上述只是以s1和m1为例进行示例性说明。在实际应用中，还可以其他具有对应关系的第一静位置点和第一动位置点建立第一静位置点第一坐标系、第一动位置点第一坐标系和第一动位置点第二坐标系，例如以s1和m1建立静位置点第一坐标系o
s1-x
s1ys1zs1
等。这里不再赘述。
109.图8为本技术实施例提供的一种主操作手结构的主操作手的驱动力获取方法的实现流程示意图，所述主操作手包括多个模块，所述多个模块中至少包括静平台、动平台，且所述动平台受用户控制进行运动；其中，所述静平台包括第一静平台和第二静平台，第一静平台上包括n个第一静位置点，第二静平台上包括与第一静位置点一一对应的n个第二静位置点，每个第一静位置点和第二静位置点均设置有对应的主动驱动装置，所述动平台上包括与所述n个第二静位置点一一对应的n个第二动位置点，所述第一静平台上还包括与所述n个第一静位置点一一对应的n个第一动位置点。
110.其中，主动驱动装置可以与第一静位置点或第二静位置点对应，并驱动相对应的组件沿指定方向运动。例如图2所示的主操作手结构图，与第一静位置点si(i＝1,2,3)对应的主动驱动装置为第二直线电机660，可以驱动与第一静位置点对应的第二滑块沿第二滑杆643滑动；与第二静位置点si(i＝4,5,6)对应的主动驱动装置为第一直线电机，可以驱动与第二静位置点对应的第一滑杆沿连杆685滑动。
111.如图8所示为本技术实施例提供的一种主操作手的驱动力获取方法的实现流程示意图。
112.本技术可以通过运动学逆解的方式，确定驱动表示公式和被动表示公式。
113.在一个实施例中，运动学逆解可以为根据已知的动平台的位姿求解驱动量l
1-l6和被动旋转量其中，动平台的位姿可以通过动平台坐标系相对其初始状态的姿态欧拉转角θ
x
、θy、θz，以及动平台坐标系原点在静平台坐标系中的位置坐标m0＝[m
x
,my,mz]来表示。当然，动平台位姿也可以通过其他方式来表示，本实施例不进行限定。至于如何确定动平台
的位姿，具体可以参见下文运动学正解的方法，这里暂不赘述。
[0114]
在一个实施例中，本技术所述的多个模块中至少还包括多个第一连接模块、第二连接模块，所述第一连接模块用于连接具有对应关系的第一静位置点和第一动位置点；所述第二连接模块用于连接具有对应关系的第二静位置点和第二静位置点。
[0115]
其中，所述驱动输入量包括：具有对应关系的第一静位置点和第一动位置点在第一连接模块的调节方向上的第一驱动距离li(i＝1,2,3)，以及具有对应关系的第二静位置点和第二动位置点在第二连接模块的调节方向上的第二驱动距离li(i＝4,5,6)，所述驱动表示公式包括：所述第一驱动距离和所述第二驱动距离关于所述动平台运动位姿的距离表示公式f
li
。
[0116]
其中，所述被动旋转量包括：每个第二动位置点相对于对应的第二静位置点沿第一轴心旋转的第一旋转变化量θ
si
(i＝4,5,6)；每个第二动位置点相对于对应的第二静位置点沿第二轴心旋转的第二旋转变化量所述被动表示公式包括所述第二旋转变化量关于所述动平台运动位姿的被动表示公式以及所述第一旋转变化量θ
si
关于所述动平台运动位姿的旋转表示公式f
θsi
。
[0117]
根据前文所述的坐标系建立方法，这里的第一轴心可以与第二静位置点第一方向坐标轴(例如y
si
轴，i＝4,5,6)重合，第二轴心可以与第二静位置点第二方向坐标轴(例如x
si
轴，i＝4,5,6)重合。
[0118]
可以理解的是，驱动量和被动旋转量的具体数值是可以通过测量等方法确定的，而本步骤并不是为了通过动平台位姿来确定其具体数值，而是确定驱动输入量和被动旋转量关于动平台位姿的表示公式。
[0119]
下面结合步骤s801和步骤s802介绍通过运动学逆解来确定驱动输入量和被动旋转量关于动平台位姿的表示公式的方法。
[0120]
在s801中，确定第二静位置点的驱动输入量关于动平台运动位姿的第二驱动表示公式。
[0121]
其中，所述运动位姿用于指示所述动平台相对于所述第一静平台或第二静平台的位姿；所述第二静位置点的驱动输入量用于指示所述第二动位置点在所述主动驱动装置的控制下，产生的相对于对应的第二静位置点的位置变化。
[0122]
下面结合图4所示的主操作手示意图介绍确定第二驱动表示公式的过程，包括：
[0123]
a1，确定所述第二静位置点在所述静平台坐标系下的坐标si(i＝4,5,6)以及确定所述第二动位置点在所述动平台坐标系的坐标mi(i＝4,5,6)。
[0124]
根据平面坐标旋转定理，任意坐标点(x，y)绕原点旋转θ角度后的坐标可表示为
[0125]
x'＝x cosθ-y sinθ,y'＝x sinθ+y cosθ
[0126]
基于此，根据图4所示的主操作手示意图确定的坐标系，第一静铰接圆的半径为rs，则第一静位置点s1在静平台坐标系中的坐标为(rs，0)。以第一静位置点s1为基准，其他静位置点可以视为由该第一静位置点绕静平台坐标系原点(即第一静铰接圆圆心)旋转得到。由此，可以分别确定各个第一静位置点si(i＝1,2,3)的坐标，其齐次坐标si可表示为：
[0127]
si＝(r
s cosθi,r
s sinθi,0,1)
t
[0128]
其中，每个第一静位置点对应的平面旋转角度θ不同，具体可以根据各个第一静位置点在第一静铰接圆上的分布确定。以图4为例，第一静平台上包括三个第一静位置点，且
该三个静位置点构成等边三角形，则各第一静位置点对应的平面旋转角度θ如下表的表1所示
[0129] s1s2s3角度值θ
i0°
120
°
240
°
[0130]
表1
[0131]
基于类似的方法，可以确定各个第二静位置点si(i＝4,5,6)的坐标。例如可以以与s1对应的第二静位置点s4为基准，分别确定各个第二静位置点si(i＝4,5,6)在静平台坐标系的第一方向坐标轴和第二方向坐标轴上坐标。同时，可以根据第一静平台和第二静平台之间的距离来确定各个第二静位置点si(i＝4,5,6)的第三方向坐标轴上的坐标，例如，可以将通过两个静铰接圆圆心在静平台坐标系第三方向坐标轴上的距离来表示，记为hs。
[0132]
基于同样的方法，还可以以与s1对应的第二动位置点m4为基准，分别确定各个第二动位置点mi(i＝4,5,6)在动平台坐标系下的坐标。
[0133]
a2，根据所述静平台坐标系与所述动平台坐标系的第一转换矩阵、以及所述动位置点在所述动平台坐标系的坐标mi(i＝4,5,6)，确定所述第二动位置点在所述静平台坐标系的位置坐标
[0134]
所述静平台坐标系与所述动平台坐标系的第一转换矩阵，即动静平台坐标系间转换矩阵由欧拉转角θ
x
、θy、θz，和动平台坐标系原点在静平台坐标系下的坐标m0＝[m
x
,my,mz]来表示。具体可以表示为：
[0135][0136]
根据该第一转换矩阵可以确定第二动位置点mi(i＝4,5,6)在静平台坐标系下的位置坐标，记为
[0137][0138]
其中，mi可以根据上文所述的平面旋转定理确定。
[0139]
a3，基于所述第二动位置点在所述静平台坐标系的位置坐标以及所述第二静位置点在所述静平台坐标系的坐标si，确定所述第二驱动表示公式。
[0140]
第二静位置点的驱动输入量li(i＝4,5,6)，可以为动静位置点的间距，记为||simi||(i＝4,5,6)。已知第二动位置点和第二静位置点在所述静平台坐标系下的坐标，即可根据两点间距离公式求得。
[0141]
在s802中，基于所述第二驱动表示公式，确定第二静位置点的被动旋转量关于动平台运动位姿的被动表示公式，并基于所述第二驱动表示公式，确定第一静位置点的驱动输入量关于动平台运动位姿的第一驱动表示公式。
[0142]
其中，所述被动旋转量用于指示所述动平台进行运动后所述动位置点产生的相对于第二静位置点的位置变化，所述第一静位置点的驱动输入量用于指示所述第一动位置点在所述主动驱动装置的控制下，产生的相对于对应的第一静位置点的位置变化。
[0143]
其中，基于所述第二驱动表示公式，确定第二静位置点的被动旋转量关于动平台运动位姿的被动表示公式，包括：
[0144]
b1，基于所述静平台坐标系与第二静位置点第一坐标系的第二转换矩阵所述第二静位置点第一坐标系与第二静位置点第二坐标系的第三转换矩阵确定所述静平台坐标系与第二静位置点第二坐标系的第四转换矩阵
[0145]
根据几何关系可知：静平台坐标系到静位置点si(i＝4,5,6)对应的第二静位置点第一坐标系的第二转换矩阵以及第二静位置点第一坐标系到第二静位置点第二坐标系的第三转换矩阵如下：
[0146][0147][0148]
其中，为第二静位置点si(i＝4,5,6)在静平台坐标系下位置坐标。θi为平面旋转角度，例如可以如表1所示；θ
si
为第二静位置点si(i＝4,5,6)第二坐标系绕自身x轴转动的角度，为第二静位置点si(i＝4,5,6)第二坐标系绕自身y轴转动角度，θ
si
和随动平台运动而变化。
[0149]
从而可得静平台坐标系到第二静位置点si(i＝4,5,6)第二坐标系的第四转换矩阵
[0150]
b2，基于所述第四转换矩阵、所述第二动位置点在所述静平台坐标系的位置坐标以及所述第二静位置点在所述静平台坐标系的坐标si，确定所述被动表示公式。
[0151]
将矩阵中第三列的前三个元素可构成单位向量，记为该单位向量用于表示在静平台坐标系下，第二静位置点si(i＝4,5,6)第二坐标系z轴的单位方向向量。
[0152]
根据几何关系，可以确定该单位向量与具有对应关系的第二动位置点mi(i＝4,5,6)和第二静位置点si(i＝4,5,6)的连线平行，由此可以确定：
[0153][0154]
由于与和θ
si
相关，且可以根据静平台坐标系下第二静位置点和第二动位置点的坐标确定，由上式可获得关于θ
si
和的两个或三个方程，联立方程即求得φsi和θsi关于动平台的运动位姿的表达式。
[0155]
基于此，在通过运动学正解的方法确定动平台的运动位姿的具体数值后，基于该具体数值，即求得φsi和θsi以及第四转换矩阵运动学正解的方法将在下文介绍，这里暂不赘述。
[0156]
下面介绍确定第一驱动表示公式的方法。在一个实施例中，确定第一静位置点的驱动输入量关于动平台运动位姿的第一驱动表示公式，可以包括：
[0157]
c1，基于指定位置点a
si
在第二静位置点第二坐标系下的坐标，确定所述指定位置点在静平台坐标系下的坐标。
[0158]
为进一步解算第一动位置点mi(i＝1,2,3)的位置坐标，从而得到驱动l
1-l3，现定义一指定位置点，或者称为过渡点a
si
，下面结合图9进行介绍。
[0159]
如图9所示，驱动装置li(i＝4,5,6)实际驱动轴线与具有对应关系的第二动位置点和第二静位置点的连线simi(i＝4,5,6)始终平行，且两直线间距为记常值da，因此两条直线的单位向量相同。将过第二静位置点si(i＝4,5,6)的两直线公垂线与li(i＝4,5,6)实际驱动轴线的交点确定为过渡点a
si
，根据第二静位置点第二坐标系的定义规则可知，该过渡点a
si
位于第二静位置点第二坐标系的第一方向坐标轴上。因此，可以确定该过渡点a
si
在第二静位置点第二坐标系下齐次坐标为
[0160]
根据第二静位置点第二坐标系与静平台坐标系的第四转换矩阵，可以将该过渡点a
si
转换到静平台坐标系下，即为：
[0161][0162]
c2，基于所述指定位置点在静平台坐标系下的坐标、第一动位置点在静平台坐标系的位置坐标，确定所述第一驱动表示公式。
[0163]
记第一动位置点mi(i＝1,2,3)在静平台坐标系下的齐次坐标为：
[0164][0165]
在一个实施例中，根据mi(i＝1,2,3)第一动位置点位于静平台平面上，即其在静平台坐标系的xy平面(以x为第一方向坐标轴，y为第二方向坐标轴为例)上可知，其第三方向坐标轴上的坐标为0。
[0166]
在一个实施例中，根据几何关系，可以确定上述指定位置点a
si
(i＝4,5,6)与第一
动位置点mi(i＝1,2,3)在每个方向轴上的差值，与向量在该方向轴上的比值，并且，每个方向轴上计算得到的比值相等。可以确定第一动位置点mi(i＝1,2,3)在静平台坐标系的第一方向坐标轴和第二方向坐标轴上的位置坐标，即：
[0167][0168]
进一步地，将第一动位置点mi在静平台坐标系下的坐标，转换到第一静位置点第一坐标下，得到第一动位置点在第一静位置点第一坐标系下的坐标
[0169][0170]
由此，可以确定驱动li(i＝1,2,3)为第一动位置点在第一静位置点第一坐标系的第二方向坐标轴上的坐标，以第二方向坐标轴为y轴为例，可记为：
[0171][0172]
s803，基于所述被动表示公式，确定所述被动旋转量与预设运动参数间的第一关系表示公式，以及，基于所述第一驱动表示公式和第二驱动表示公式，确定驱动输入量与所述预设运动参数间的第二关系表示公式。
[0173]
其中，所述第二关系表示公式中包括逆雅克比矩阵，所述逆雅克比矩阵用于表示所述驱动输入量与所述主操作手的输出间的映射关系。
[0174]
在一个实施例中，预设运动参数可以包括动平台的运动参数，例如动平台的运动速度，线速度v和角速度ω。
[0175]
根据上述步骤s801-802，可以确定直线驱动li(i＝1,2,3,4,5,6)、被动旋转量和是关于动平台坐标系位姿m0＝[m
x
,my,mz]，θ
x
、θy、θz的函数式，为便于描述，将其表示为
[0176][0177][0178][0179]
将上述表示公式对时间t求导，可得
[0180][0181]
[0182][0183]
为便于描述，可以将其转换为矩阵形式，即被动旋转量和与主手输出间的关系为
[0184][0185][0186]
同样地，驱动输入量li与主操作手输出间的第二关系表示公式可以为：
[0187]
[l
′
1 l
′
2 l
′
3 l
′
4 l
′
5 l
′6]
t
＝j
haptic
[v
x v
y v
z ω
x ω
y ωz]
t
[0188]
s804，根据预先测量获得的主操作手中的各个模块的质心点初始状态下在静平台坐标系中的坐标、所述第一关系表示公式和所述第二关系表示公式，确定所述各个模块与所述预设运动参数间的映射关系。
[0189]
其中，所述静平台坐标系基于所述第一静平台建立，所述映射关系中包括雅克比矩阵，所述雅克比矩阵用于表示所述各个模块与所述预设运动参数间的映射关系。
[0190]
在确定各个模块与预设运动参数间的映射关系时，可以包括：
[0191]
d1，根据所述第一关系表示公式以及所述第二关系表示公式，确定所述第二动位置点转动角度与预设运动参数间的第三关系表示公式。
[0192]
其中，所述第二动位置点转动角度用于指示所述第二动位置点相对于第一静位置点的转动角度。
[0193]
在一个实施例中，第二动位置点转动角度可以包括第二动位置点第一坐标系转换到第二坐标系时，绕自身第一方向坐标轴(例如x轴)转动的第一角度α
mi
和绕自身第二方向坐标轴(例如y轴)转动的第二角度β
mi
。也就是说，第二动位置点第二坐标系是由第一坐标系先绕y
mi
轴转动β
mi
，再绕转动后的x
mi
轴转动α
mi
后得到的。
[0194]
第二动位置点第二坐标系可利用动平台坐标系位姿参数：
[0195]
根据所述第二动位置点第二坐标系与所述动平台坐标系的关系可获得如下关系表示式：
[0196][0197]
其中，为静平台坐标系到第二静位置点第一坐标系转换矩阵，为第二静位置点第一坐标系到第二静位置点第二坐标系转换矩阵，为静平台坐标系到动平台坐标系的转换矩阵，t
γ
为第二静位置点si(i＝4,5,6)的第二静位置点第二坐标系绕自身z轴转动角度γ，并位移至第二动位置点mi(i＝4,5,6)的转换矩阵；为第二动位置点第一坐标系到第二动位置点第二坐标系的转换矩阵，可知其中t
β
为第二动位置点mi(i＝4，5，6)的动位置点第二坐标系绕自身y轴转动角度β的转换矩阵，t
α
为绕自身x轴转动角度α的转换矩阵。为动平台坐标系到第二动位置点第一坐标系的转换矩阵。
[0198]
基于此，可以确定t
γ
：
[0199][0200]
根据t
γ
的定义可知t
γ
(3,1)＝t
γ
(3,2)＝0，则根据上式可得两个及以上关于转动角度α
mi
、β
mi
的方程，解方程组即可得到关于转动角度α
mi
、β
mi
。为便于描述，可以将其表示为如下形式：
[0201][0202][0203]
对α
mi
、β
mi
对时间t求导，可得：
[0204][0205][0206]
可以将其转换为矩阵形式，即α
mi
和β
mi
与主手输出间的关系可表示为：
[0207][0208][0209]
可以理解的是，上述第三关系表示公式可以在后续步骤中作为中间量，而不需要求得具体的数值。
[0210]
d2，将所述初始状态下各个模块的质心点的初始坐标转换到与该模块对应的坐标系中。
[0211]
为建立运动过程中主操作手完整的雅克比矩阵，可以将主操作手结构分解为多个大模块，比如图10所示的10个模块，各模块质心在对应的坐标系下位置坐标保持不变或某一坐标值随某参数发生线性变化。
[0212]
在一个实施例中，主操作手中可以包括动平台、静平台、第一连接模块和第二连接模块。其中，第一连接模块的数量为n，每个所述第一连接模块包括球铰模块sph_joint和第一滑块模块block_s123；所述第二连接模块的数量为n，每个所述第二连接模块包括固定板模块board、第二滑块模块block_s456、动平台连接轴shaft_m、动平台连接关节模块link_m和导向筒模块guide_cylinder和静平台连接关节模块link_s。
[0213]
所述第二动位置点通过动平台连接关节模块和动平台连接轴模块与所述固定板模块连接，所述固定板模块与所述第二滑块连接，所述第二滑块通过所述静平台连接关节模块与第二静位置点连接，并且所述第二静位置点对应的主动驱动装置控制固定板模块在所述滑块模块上滑动控制所述第二静位置点和第二动位置点之间的第二驱动距离。
[0214]
所述第一滑块模块通过所述球铰模块与所述固定板模块连接；且所述第一静位置点对应的主动驱动装置控制第一滑块模块在所述静平台上滑动控制所述第一静位置点和第一动位置点之间的距离。
[0215]
所述动平台对应的坐标系为动平台坐标系，且所述动平台的质心坐标在所述动平台坐标系下不变。
[0216]
所述第一滑块模块对应的坐标系为第一静位置点第一坐标系，且所述第一滑块模块的质心坐标在所述第一静位置点第一坐标系的第一方向坐标轴和第三方向坐标轴上不变，并在第二方向坐标轴上随第一驱动距离线性变化。
[0217]
所述球铰模块对应的坐标系为第一动位置点第一坐标系，且所述球铰模块质心坐标在所述第一动位置点第一坐标系下不变。
[0218]
所述导向筒模块对应的坐标系为第一动位置点第二坐标系，且所述导向筒模块质心坐标在所述第一动位置点第二坐标系下不变。
[0219]
所述固定板模块对应的坐标系为第二静位置点第二坐标系，且所述固定板模块的质心坐标在所述第二静位置点第二坐标系的第一方向坐标轴和第二方向坐标轴上不变，并在第三方向坐标轴上随所述第二静位置点和第二动位置点之间的距离线性变化。
[0220]
所述第二滑块模块对应的坐标系为第二静位置点第二坐标系，且所述滑块模块的质心坐标在所述第二静位置点第二坐标系下不变。
[0221]
所述静平台连接关节模块对应的坐标系为第二静位置点第一坐标系绕第一方向坐标轴转动所述第一旋转变化量后的坐标系，且所述静平台连接模块的质心坐标在所述对应的坐标系下不变。
[0222]
所述动平台连接轴模块对应的坐标系为第二动位置点第二坐标系，且所述动平台连接轴模块的质心在所述第二动位置点第二坐标系下不变。
[0223]
所述动平台连接关节模块对应的坐标系为第二动位置点第一坐标系绕第二方向坐标轴转动所述第二转动角度后的坐标系，且所述连接关节模块的质心坐标在所述对应的坐标系下不变。
[0224]
如图10所示，该主操作手中包括：静平台plat_s模块、驱动l
1-l3所在滑块block_s123模块(即第一滑块模块)、球铰sph_joint模块、导向筒guide_cylinder模块、支链光栅固定板等board模块(即固定板模块)、驱动l
4-l6所在滑块block_s456模块(即第二滑块模块)、block_s456模块与静平台连接link_s模块(即静平台连接关节模块)、board模块与动铰接副连接shaft_m模块(即动平台连接轴模块)、shaft_m模块与动平台的连接link_m模块(即动平台连接关节模块)、以及动平台plat_m模块。下面以第一方向坐标轴为x轴，第二方向坐标轴为y轴，第三方向坐标轴为z轴为例，对每个模块的模块质心的变化情况进行说明。
[0225]
plat_s模块被固定在基座上，质心在运动过程中不发生任何位置变化，所以该部分质量不影响驱动力/力矩大小。
[0226]
block_s123模块质心在第一静位置点si(i＝1,2,3)第一坐标系下，位置坐标
沿x，z轴坐标不变，沿y轴坐标随第一驱动li(i＝1,2,3)变化而线性变化；sph_joint模块质心在第一动位置点mi(i＝1,2,3)第一坐标系下位置坐标不变；guide_cylinder模块质心在第一动位置点mi(i＝1,2,3)第二坐标系下位置坐标不变；board模块质心在第二静位置点si(i＝4,5,6)第二坐标系下，位置坐标沿x，y轴坐标不变，沿z轴坐标随第二驱动li(i＝4,5,6)变化而线性变化；block_s456模块质心在第二静位置点si(i＝4,5,6)第二坐标系下，位置坐标不变；link_s模块质心在第二静位置点si(i＝4,5,6)第二坐标系绕自身x轴转动θ
si
后的坐标系下，位置坐标不变；shaft_m模块质心在第二动位置点mi(i＝4,5,6)第二坐标系下，位置坐标不变；link_m模块质心在第二动位置点mi(i＝4,5,6)第二坐标系绕自身y轴转动β
mi
角度后的坐标系下，位置坐标不变；plat_m模块质心在动平台坐标系下，位置坐标c
m_re
不变。
[0227]
初始状态，各模块质心在静平台坐标系下位置坐标可以通过测量可得，以block_s123模块为例，将其质心坐标记为然后，可以将初始状态模块质心在静平台坐标系下的位置坐标，转换到与该模块对应的坐标下。
[0228]
举例来说，初始状态，block_s123模块的模块质心在对应的第一静位置点第一坐标系下位置坐标位置坐标可通过如下公式确定：其中i＝1,2,3。
[0229]
对于其他模块的模块质心，也可以采用类似的方法来确定初始状态各个模块质心在对应坐标系下的位置坐标。具体方法与block_s123模块的模块质心类似，这里不再赘述。
[0230]
d3，基于初始状态下各个模块的质心点在所述对应坐标系中的初始坐标、以及所述静平台坐标系与所述对应坐标系的转换矩阵，确定各个模块的质心点在静平台坐标系中的实时坐标。
[0231]
其中，所述实时坐标与所述驱动输入量、所述被动旋转量和所述位姿数据中的至少一个相关。
[0232]
在根据d2所述的方法确定上述初始状态各个模块质心在对应坐标系下的位置坐标后，则可以根据运动状态下模块质心在对应坐标系的位置坐标变化规律，确定运动状态下各个模块质心在对应坐标系下的位置坐标；然后，再根据转换矩阵，将位置坐标转换到静平台坐标系下，从而可以得到运动过程中各个模块质心在静平台坐标系下的位置坐标。
[0233]
下面仍以block_s123模块的模块质心为例进行说明。运动状态下，该模块质心在对应的坐标系下的位置坐标可以表达为：
[0234]
[0235]
其中，为block_s123模块的模块质心在第一静位置点第一坐标系下的实时坐标，可以根据其初始坐标来确定。具体来说，block_s123模块的模块质心在第一方向坐标轴和第三方向坐标轴上不变，在第三方向坐标轴上随驱动li(i＝1,2,3)线性变化，由此，可以根据初始状态的坐标和驱动li来确定实时坐标即可以确定与li相关。
[0236]
采用类似的方法，也可以其他模块质心在对应坐标系下的变化规律来确定模块质心在对应坐标系下的实时坐标，然后再转换到静平台坐标系下。这里不再进行详细描述。
[0237]
根据对各个模块模块质心的分析，可以确定：与li相关，与m
x
、my、mz、θ
x
、θy、θz相关，与θ
si
、ψ
si
、m
x
、my、mz、θ
x
、θy、θz相关，其中，i＝1,2,3；与li、θ
si
、ψ
si
相关，与θ
si
、ψ
si
相关，与θ
si
相关，与α
mi
、β
mi
、θ
si
、ψ
si
、m
x
、my、mz、θ
x
、θy、θz相关，与β
mi
、θ
si
、ψ
si
、m
x
、my、mz、θ
x
、θy、θz相关，与m
x
、my、mz、θ
x
、θy、θz相关，其中，i＝4,5,6。
[0238]
d4，根据所述各个模块的质心点在所述静平台坐标系的实时坐标、所述第一关系表示公式、所述第二关系表示公式以及所述第三关系表示公式，确定所述各个模块与所述预设运动参数间的映射关系。
[0239]
针对d3所述方法中得到的各个模块质心在静平台坐标系下的实时坐标，可以对时间t进行求导，从而确定所述各个模块与所述预设运动参数间的映射关系。
[0240]
仍以block_s123模块为例，其实时坐标可以记为：
[0241][0242]
对时间t进行求导，可以得到：
[0243][0244]
其中，i＝1,2,3。为block_s123模块的雅可比矩阵，具体为：
[0245][0246]
采用类似的方法，还可以得到其他各个模块的雅克比矩阵，例如sph_joint模块的
雅克比矩阵guide_cylinder模块的雅克比矩阵以及，board模块的雅克比矩阵block_s456模块的雅克比矩阵link_s模块的雅克比矩阵shaft_m模块的雅克比矩阵link_m模块的雅克比矩阵plat_m模块的雅克比矩阵jm，i＝4,5,6。
[0247]
可以理解的是，上述方法中只是以block_s123模块为例，对于其他的模块，还可以根据被动旋转量与预设运动参数间的第一关系表示公式、驱动输入量li与主操作手输出间的第二关系表示公式、以及第二动位置点转动角度与预设运动参数间的第三关系表示公式来得到对应的雅克比矩阵，而本领域技术人员可以在参考block_s123模块的基础上得到，这里不再赘述。
[0248]
s805，根据第二静位置点的驱动输入量和被动旋转量，确定动平台运动位姿的位姿数据。
[0249]
其中，所述位姿数据包括所述动平台坐标系原点在静平台坐标系中的位置坐标m0，m0＝[mx,my,mz]；以及，所述动平台坐标系相对于初始状态动平台坐标系的旋转量：θ
x
、θy、θz。
[0250]
在一个实施例中，可以直接利用测量得到的各驱动参数li和被动旋转量ψ
si
进行计算。这里不进行具体限定。
[0251]
在确定主操作手的动平台的位姿数据时，可以包括：
[0252]
e1，确定各个第二静位置点的驱动输入量和被动旋转量。
[0253]
其中，驱动输入量包括主操作手手各驱动参数li，被动旋转量包括被动旋转量ψ
si
，确定各个第二静位置点的驱动输入量和被动旋转量可以包括：
[0254]
测量得到各个第一静位置点和第一动位置点在第一连接模块的调节方向上的第一驱动距离li、各个第二静位置点和第二动位置点在第二连接模块的调节方向上的第二驱动距离li、以及每个第二动位置点相对于对应的第二静位置点沿第二轴心旋转的第二旋转变化量
[0255]
测量得到各个第二静位置点到所述第一静平台所在平面的距离hsi。
[0256]
基于各个第二静位置点的距离hsi、以及与该第二静位置点对应的第一静位置点的第一驱动距离，确定各个第二静位置点的第一旋转变化量θ
si
。
[0257]
基于所述第二驱动距离、所述第一旋转变化量和所述第二旋转变化量，确定各个第二静位置点的驱动输入量和被动旋转量。
[0258]
e2，基于所述驱动输入量和被动旋转量，确定所述各第二动位置点在所述静平台坐标系下的坐标，并基于该坐标确定动平台坐标系原点在所述静平台坐标系下的位置坐标。
[0259]
根据坐标系的建立规则，可以确定第二动位置点mi(i＝4,5,6)在第二静位置点si(i＝4,5,6)对应的第二静位置点第二坐标系下的坐标，其齐次坐标可表示为根据第二静位置点第二坐标系与静平台坐标系的转换关系，可以将该坐标转换到静平台坐标系下，得到各个第二动位置点在静平台坐标系下的位置坐标坐标
[0260]
其中，可选地，第二静位置点第二坐标系与静平台坐标系的转换关系可以根据第二静位置点第二坐标系与第二静位置点第一坐标系的转换关系以及第二静位置点第一坐标系与静平台坐标系的转换关系确定其中，已知，而与第一旋转变化量θ
si
和第二旋转变化量相关。根据图2所示的主操作手的结构，第二旋转变化量的数值可以根据角度测量装置(例如编码器)测量得到的转动角度确定。这里，i＝4,5,6。
[0261]
下面介绍确定第一旋转变化量θ
si
(i＝4,5,6)的方法：
[0262]
以θ
s4
为例，第二动位置点s4与第一静位置点s1和第一动位置点m1对应。根据几何关系求得θ
s4
[0263][0264]
其中，hs为第二静位置点s4到所述第一静平台所在平面的距离，也即s4与点p之间的距离，可以通过测量得到。
[0265]
采用同样的方法，可以得到其他第二静位置点对应的第一旋转变化量θ
s5
、θ
s6
，这里不再赘述。
[0266]
由此，根据各个第二静位置点si(i＝4,5,6)处的第一旋转变化量θ
si
和第二旋转变化量可以确定各第二静位置点第二坐标系与静平台坐标系的转换矩阵，从而得到各个第二动位置点在静平台坐标系下的坐标值。
[0267]
基于各个第二动位置点在静平台坐标系下的坐标值，可以确定动平台坐标系原点的位置，即该原点在静平台坐标系下的坐标m0。
[0268]
以3个第二动位置点构成等边三角形为例，则等边三角形外接圆圆心坐标，即动平台坐标系原点，位置坐标为：
[0269][0270]
当然，若各个第二动位置点之间的距离不相等，也可以根据几何关系确定其外接圆圆心，并将该圆心作为动平台坐标系原点位置，这里不再赘述。
[0271]
e3，根据所述动平台坐标系原点在所述静平台坐标系中的位置坐标、以及所述各第二动位置点在所述静平台坐标系下的坐标，确定所述动平台坐标系相对于所述静平台的旋转量。其中，所述第二动位置点包括一个位于所述动平台坐标系的第一方向坐标轴上的指定位置点。
[0272]
在一个实施例中，可以基于上述原点及任意一个第二动位置点来确定第一方向坐标轴。也就是说，所述第二动位置点中包括一个位于所述动平台坐标系的第一方向坐标轴(以x轴为例)的指定位置点。
[0273]
在确定动平台坐标系相对于静平台坐标系的旋转量时，可以包括：
[0274]
基于所述动平台坐标系原点在所述静平台坐标系中的位置坐标和所述指定位置点的坐标，确定所述动平台坐标系的第一方向坐标轴；
[0275]
基于所述动平台坐标系原点在所述静平台坐标系中的位置坐标和任意两个动位
置点的坐标，确定所述动平台坐标系的第三方向坐标轴；所述任意两个动位置点位于第一方向坐标轴和第二方向坐标轴所构成的平面上，所述第一方向坐标轴、第二方向坐标轴和第三方向坐标轴构成空间直角坐标系；
[0276]
基于所述第一方向坐标轴和第三方向坐标轴，确定所述动平台坐标系的第二方向坐标轴；
[0277]
基于所述动平台坐标系的第一方向坐标轴、第二方向坐标轴、第三方向坐标轴、以及所述动平台坐标系原点在所述静平台坐标系中的位置坐标，确定所述静平台坐标系和动平台坐标系的第一转换矩阵；
[0278]
基于所述静平台坐标系和动平台坐标系的第一转换矩阵，确定所述动平台坐标系相对于初始状态下该动平台坐标系的旋转量。
[0279]
以第一方向坐标轴为x轴，第二方向坐标轴为y轴，第三方向坐标轴为z轴为例，若动平台坐标系原点与第二动位置点m4构成x轴，则动平台坐标系原点与动位置点m4构成的方向向量即为动平台坐标系x轴方向，可以通过如下公式确定：
[0280][0281]
在动平台坐标系中，可以动平台所在平面作为xoy面，因此，可以将动平台所在平面的垂线且背离静平台所在平面的方向作为z轴方向。由此，可以从动平台平面上任意选取两个向量来确定z轴方向。例如，可以取从原点指向任意两个第二动位置点的向量，例如和则该叉积即为z轴方向，可以通过如下公式确定：
[0282][0283]
基于已确定的x轴方向和z轴方向，动平台坐标系y轴单位方向向量遵循右手螺旋法则，可以通过如下公式确定
[0284][0285]
在确定动平台坐标系各个方向坐标轴和原点坐标后，即可以确定动平台坐标系和静平台坐标系之间的第一转换矩阵t_s_m：
[0286][0287]
同时，根据步骤s801中确定的第一转换矩阵关于动平台位姿的表示，则可以确定动平台坐标系位姿数据的参数θ
x
、θy、θz：
[0288][0289]
[0290][0291]
其中，(1，3)表示第1行第3列的元素，(1，2)表示第1行第2列的元素，(2，3)表示第2行第3列的元素。
[0292]
e4，将所述动平台坐标系原点在所述静平台坐标系中的位置坐标、以及所述动平台坐标系相对于所述静平台坐标系的旋转量，确定为所述动平台运动位姿的位姿数据。
[0293]
根据步骤e2中确定的动平台坐标系原点在静平台坐标系下的坐标，以及步骤e3中确定的动平台坐标系相对于静平台的旋转量，则可以确定动平台的运动位姿。
[0294]
可选的，将所述位姿数据发送至对应的从端机器人，可以使所述从端机器人根据所述位姿数据进行相应的运动。
[0295]
在实际的操作环境中，医生在操纵主操作手的手柄时，主操作手可以实时发送位姿数据给从端机器人，使所述从端机器人根据所述位姿数据进行相应的运动，从而实现实时对从端机器人进行远端操纵。
[0296]
s806，根据所述主操作手接收到的受力、所述各个模块的重力、所述各个模块的雅克比矩阵、所述逆雅克比矩阵以及所述动平台运动位姿的位姿数据，确定所述主操作手输出的驱动力。
[0297]
具体的，确定所述主操作手输出的驱动力的求解可以包括：
[0298]
f1，根据虚功原理，确定所述主操作手的静力平衡方程。
[0299]
其中，所述静力平衡方程用于表示所述主操作手输出的驱动力、各个模块的重力以及所述主操作手接收到的受力做的虚功的和为零。
[0300]
以图2所示的主操作手为例，对应的静力平衡方程可以为：
[0301][0302]
其中，为各个驱动输入量li的驱动力大小，符号δ表示虚拟，δli为驱动输入量li沿驱动方向的虚位移，δxm为动平台坐标系原点在静平台坐标系下三个方向坐标轴的虚位移和在动平台坐标系下三个方向坐标轴的虚转角，δx
模块名
为各模块质心点沿静平台坐标系三个轴方向上的虚位移，f
haptic
为主操作手接收到的受力反馈力在静平台坐标系下的映射。
[0303]
其中，f
haptic
可以先通过力传感器和力矩传感器测量得到主操作手接收到的受力反馈力在动平台坐标系下的映射，然后再将其转换到静平台坐标系下。
[0304]
例如，该反馈力在动平台坐标系原点的映射可以为：其中，其中，分别为主操作手动平台坐标系下，反馈力在x轴方向的分力，反馈力在y轴方向的分力，反馈力在z轴方向的分力，反馈力在x轴方向产生的扭矩，反馈力在y轴方向产生的扭矩，反馈力在z轴方向产生的扭矩。
[0305]
通过动平台坐标系与静平台坐标系间的旋转矩阵可以将该反馈力转换为静平台坐标系下的映射f
haptic
：
[0306][0307][0308]
其中，f
x
，fy，fz，τ
x
，τy，τz分别为主操作手静平台坐标系下，反馈力在x轴方向的分力，反馈力在y轴方向的分力，反馈力在z轴方向的分力，反馈力在x轴方向产生的扭矩，反馈力在y轴方向产生的扭矩，反馈力在z轴方向产生的扭矩。
[0309]
其中，可以根据动平台与静平台之间的转换矩阵确定，具体来说，
[0310]
f2，基于所述位姿数据，确定所述雅克比矩阵的取值和所述逆雅克比矩阵的取值。
[0311]
根据步骤s805中确定的动平台的位姿数据，可以确定各个雅克比矩阵和逆雅克比的值。
[0312]
f3，基于各个模块相对于静平台的位姿，确定各个模块的重力在静平台坐标系下的重力分量。
[0313]
在主操作手的不同安装姿态下，各个模块在静平台坐标系下的重力分量是不同。在一个实施例中，可以先确定主操作手在典型安装姿态下的各个模块的重力，然后再根据典型安装姿态与实际安装姿态的转换矩阵来确定各个模块的重力在静平台坐标系下的分量。
[0314]
其中，可选地，典型安装姿态可以为水平安装姿态(重力沿静平台坐标系x轴方向)或垂直安装姿态(重力沿静平台坐标系z轴方向)等。
[0315]
以安装姿态为以重力沿静平台坐标系x轴负方向为例，记重力加速度为g，则可以表示出各模块的重力。以第一滑块模块block_s123为例，重力可表示为：
[0316][0317]
其中，m
模块名
为模块质量大小，单位kg。
[0318]
f4，根据所述主操作手接收的受力在所述静平台坐标系或动平台坐标系的映射、所述各个模块的重力分量、所述各个模块的雅克比矩阵的取值、以及所述逆雅克比矩阵的取值，对所述静力平衡方程进行运算，获得所述驱动力。
[0319]
针对步骤f1中的静力平衡方程，将该静力学平衡方程的等式两侧同时除以δt，并根据所述各个模块的雅克比矩阵对该公式进行简化，可以得到：
[0320][0321]
将其中的重力部分用fg表示，即：
[0322][0323]
由此，可以确定驱动力为：
[0324][0325]
至此，重力与负载力作用下的驱动力求解完成。
[0326]
s807，根据所述驱动力，对所述主操作手进行力反馈以及重力平衡。
[0327]
所述驱动力通过所述主操作手的电机进行输出，作用于所述主操作手的操作手柄部分，使所述操作手柄部分达到重力平衡以及将所述主操作手接收到的受力反馈给操作所述主操作手的操作者。
[0328]
其中，所述主操作手的手柄部分包括手柄以及和所述手柄相连接的动平台。
[0329]
在一个实施例中，当反馈力为0，即f
haptic
＝[0 0 0 0 0 0]
t
，则主操作手只需平衡重力，则得到的驱动力用于实现重力平衡，达到使主操作手操作端“悬浮”在空中的效果。
[0330]
本发明实施例中，在获取到所述驱动力后，可以通过所述主操作的电机进行输出，作用于所述主操手的支链，通过所述支链将所述驱动力作用于与所述支链转动连接的动平台，从而使包括所述动平台的手柄部分达到重力平衡以及将所述驱动力反馈给操作所述主操手的操作者，比如将器械末端与人体组织的作用力/力矩反馈给医生。
[0331]
可见，通过确定第二静位置点以及第一静位置点的驱动输入量关于动平台运动位姿的驱动表示公式，以及第二静位置点的被动旋转量的被动表示公式，并通过所述驱动表示公式和被动表示公式确定用于表示所述驱动输入量及被动旋转量与预设运动参数间的关系表示公式，基于所测量的各模块的质心点在初始状态下在静平台坐标系中的坐标，结合关系表示公确定模块与预设运动参数间的映射关系，根据第二静位置点的驱动输入量与被动旋转量，确定主操作手的动平台的位姿数据，基于主操作手接收到的受力、所述各个模块的重力、所述各个模块的映射关系以及动平台运动位姿的位姿数据，确定所述主操作手输出的驱动力；根据所述驱动力，对所述主操作手进行力反馈以及重力平衡，使得主操作手的重力达到平衡，让主操作手实现“悬浮”的效果，使操作者在操作的时候不需要人为去克服重力而长时间提着主操作手，提高了主操作手的可操作性以及操作便利性，同时，通过确定各个模块的雅克比矩阵，通过一系列的矩阵运算获得驱动力，不需要根据传统的牛顿-欧拉法来列出每个部件的受力平衡方程，通过复杂的运算来计算驱动力，简化了计算和/或运
算的量，能够较快计算出驱动力，减少了操作时延、提高操作速度，进而提高工作效率。
[0332]
参见图11，本技术一实施例提供的驱动力获取装置的结构示意图。为了便于说明，仅示出了与本技术实施例相关的部分。该装置可设置于上述主操作手终端中。该装置包括：
[0333]
第二驱动表示公式确定单元1201，用于确定第二静位置点的驱动输入量关于动平台运动位姿的第二驱动表示公式；其中，所述运动位姿用于指示所述动平台相对于所述第一静平台或第二静平台的位姿；所述第二静位置点的驱动输入量用于指示所述第二动位置点在所述主动驱动装置的控制下，产生的相对于对应的第二静位置点的位置变化；
[0334]
第一驱动表示公式确定单元1202，用于基于所述第二驱动表示公式，确定第二静位置点的被动旋转量关于动平台运动位姿的被动表示公式，并基于所述第二驱动表示公式，确定第一静位置点的驱动输入量关于动平台运动位姿的第一驱动表示公式；所述被动旋转量用于指示所述动平台进行运动后所述动位置点产生的相对于第二静位置点的位置变化，所述第一静位置点的驱动输入量用于指示所述第一动位置点在所述主动驱动装置的控制下，产生的相对于对应的第一静位置点的位置变化；
[0335]
第二关系表示公式确定单元1203，用于基于所述被动表示公式，确定所述被动旋转量与预设运动参数间的第一关系表示公式，以及，基于所述第一驱动表示公式和第二驱动表示公式，确定驱动输入量与所述预设运动参数间的第二关系表示公式；所述第二关系表示公式中包括逆雅克比矩阵，所述逆雅克比矩阵用于表示所述驱动输入量与所述主操作手的输出间的映射关系；
[0336]
映射关系确定单元1204，用于根据预先测量获得的主操作手中的各个模块的质心点初始状态下在静平台坐标系中的坐标、所述第一关系表示公式和所述第二关系表示公式，确定所述各个模块与所述预设运动参数间的映射关系；所述静平台坐标系基于所述第一静平台建立，所述映射关系中包括雅克比矩阵，所述雅克比矩阵用于表示所述各个模块与所述预设运动参数间的映射关系；
[0337]
位姿数据确定单元1205，用于根据第二静位置点的驱动输入量和被动旋转量，确定动平台运动位姿的位姿数据；所述驱动输入量由所述第一静位置点和/或第二静位置点对应的主动驱动装置控制而产生；
[0338]
驱动力确定单元1206，用于根据所述主操作手接收到的受力、所述各个模块的重力、所述各个模块的雅克比矩阵、所述逆雅克比矩阵以及所述动平台运动位姿的位姿数据，确定所述主操作手输出的驱动力；
[0339]
力反馈单元1207，用于根据所述驱动力，对所述主操作手进行力反馈以及重力平衡。
[0340]
图11所述主操作手的驱动力获取装置，与图8所述的主操作手的驱动力获取方法对应。
[0341]
图12是本技术一实施例提供的主操作手的示意图。如图12所示，该实施例的主操作手13包括：处理器130、存储器131以及存储在所述存储器131中并可在所述处理器130上运行的计算机程序132，例如主操作手的控制程序。所述处理器130执行所述计算机程序132时实现上述各个主操作手的控制方法实施例中的步骤。或者，所述处理器130执行所述计算机程序132时实现上述各装置实施例中各模块/单元的功能。
[0342]
示例性的，所述计算机程序132可以被分割成一个或多个模块/单元，所述一个或
者多个模块/单元被存储在所述存储器131中，并由所述处理器130执行，以完成本技术。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序132在所述主操作手13中的执行过程。
[0343]
所述主操作手可包括，但不仅限于，处理器130、存储器131。本领域技术人员可以理解，图12仅仅是主操作手13的示例，并不构成对主操作手13的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述主操作手还可以包括输入输出设备、网络接入设备、总线等。
[0344]
所称处理器130可以是中央处理单元(central processing unit，cpu)，还可以是其他通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电路(application specific integrated circuit，asic)、现场可编程门阵列(field-programmable gate array，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
[0345]
所述存储器131可以是所述主操作手13的内部存储单元，例如主操作手13的硬盘或内存。所述存储器131也可以是所述主操作手13的外部存储设备，例如所述主操作手13上配备的插接式硬盘，智能存储卡(smart media card,smc)，安全数字(secure digital,sd)卡，闪存卡(flash card)等。进一步地，所述存储器131还可以既包括所述主操作手13的内部存储单元也包括外部存储设备。所述存储器131用于存储所述计算机程序以及所述主操作手所需的其他程序和数据。所述存储器131还可以用于暂时地存储已经输出或者将要输出的数据。
[0346]
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本技术的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
[0347]
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
[0348]
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本技术的范围。
[0349]
在本技术所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另
一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
[0350]
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
[0351]
另外，在本技术各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
[0352]
所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本技术实现上述实施例方法中的全部或部分流程，也可以通过计算机程序指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(rom，read-only memory)、随机存取存储器(ram，random access memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括是电载波信号和电信信号。
[0353]
以上所述实施例仅用以说明本技术的技术方案，而非对其限制；尽管参照前述实施例对本技术进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本技术各实施例技术方案的精神和范围，均应包含在本技术的保护范围之内。

技术特征：
1.一种主操作手的驱动力获取方法，其特征在于，所述主操作手包括多个模块，所述多个模块中至少包括静平台、动平台，且所述动平台受用户控制进行运动；其中，所述静平台包括第一静平台和第二静平台，第一静平台上包括n个第一静位置点，第二静平台上包括与第一静位置点一一对应的n个第二静位置点，每个第一静位置点和第二静位置点均设置有对应的主动驱动装置，所述动平台上包括与所述n个第二静位置点一一对应的n个第二动位置点，所述第一静平台上还包括与所述n个第一静位置点一一对应的n个第一动位置点；所述方法包括：确定第二静位置点的驱动输入量关于动平台运动位姿的第二驱动表示公式；其中，所述运动位姿用于指示所述动平台相对于所述第一静平台或第二静平台的位姿；所述第二静位置点的驱动输入量用于指示所述第二动位置点在所述主动驱动装置的控制下，产生的相对于对应的第二静位置点的位置变化；基于所述第二驱动表示公式，确定第二静位置点的被动旋转量关于动平台运动位姿的被动表示公式，并基于所述第二驱动表示公式，确定第一静位置点的驱动输入量关于动平台运动位姿的第一驱动表示公式；所述被动旋转量用于指示所述动平台进行运动后所述动位置点产生的相对于第二静位置点的位置变化，所述第一静位置点的驱动输入量用于指示所述第一动位置点在所述主动驱动装置的控制下，产生的相对于对应的第一静位置点的位置变化；基于所述被动表示公式，确定所述被动旋转量与预设运动参数间的第一关系表示公式，以及，基于所述第一驱动表示公式和第二驱动表示公式，确定驱动输入量与所述预设运动参数间的第二关系表示公式；所述第二关系表示公式中包括逆雅克比矩阵，所述逆雅克比矩阵用于表示所述驱动输入量与所述主操作手的输出间的映射关系；根据预先测量获得的主操作手中的各个模块的质心点初始状态下在静平台坐标系中的坐标、所述第一关系表示公式和所述第二关系表示公式，确定所述各个模块与所述预设运动参数间的映射关系；所述静平台坐标系基于所述第一静平台建立，所述映射关系中包括雅克比矩阵，所述雅克比矩阵用于表示所述各个模块与所述预设运动参数间的映射关系；根据第二静位置点的驱动输入量和被动旋转量，确定动平台运动位姿的位姿数据；所述驱动输入量由所述第一静位置点和/或第二静位置点对应的主动驱动装置控制而产生；根据所述主操作手接收到的受力、所述各个模块的重力、所述各个模块的雅克比矩阵、所述逆雅克比矩阵以及所述动平台运动位姿的位姿数据，确定所述主操作手输出的驱动力；根据所述驱动力，对所述主操作手进行力反馈以及重力平衡。2.根据权利要求1所述的方法，其特征在于，根据所述主操作手接收到的受力、所述各个模块的重力、所述各个模块的雅克比矩阵、所述逆雅克比矩阵以及所述动平台运动位姿的位姿数据，确定所述主操作手输出的驱动力，包括：根据虚功原理，确定所述主操作手的静力平衡方程，其中，所述静力平衡方程用于表示所述主操作手输出的驱动力、各个模块的重力以及所述主操作手接收到的受力做的虚功的和为零；基于所述位姿数据，确定所述雅克比矩阵的取值和所述逆雅克比矩阵的取值；
基于各个模块相对于静平台的位姿，确定各个模块的重力在静平台坐标系下的重力分量；根据所述主操作手接收的受力在所述静平台坐标系或动平台坐标系的映射、所述各个模块的重力分量、所述各个模块的雅克比矩阵的取值、以及所述逆雅克比矩阵的取值，对所述静力平衡方程进行运算，获得所述驱动力，所述动平台坐标系基于所述动平台建立。3.根据权利要求1所述的方法，其特征在于，所述主操作手中还基于第二静位置点建立第二静位置点第一坐标系和第二静位置点第二坐标系，其中，所述第二静位置点第一坐标系基于第二静位置点的初始位置建立，所述第二静位置点第二坐标系基于具有对应关系的第二静位置点和第二动位置点建立；所述确定第二静位置点的驱动输入量关于动平台运动位姿的第二驱动表示公式，包括：确定所述第二静位置点在所述静平台坐标系下的坐标以及确定所述第二动位置点在所述动平台坐标系的坐标；根据所述静平台坐标系与所述动平台坐标系的第一转换矩阵、以及所述动位置点在所述动平台坐标系的坐标，确定所述第二动位置点在所述静平台坐标系的位置坐标；基于所述第二动位置点在所述静平台坐标系的位置坐标、以及所述第二静位置点在所述静平台坐标系的坐标，确定所述第二驱动表示公式。4.根据权利要求3所述的方法，其特征在于，所述基于所述第二驱动表示公式，确定第二静位置点的被动旋转量关于动平台运动位姿的被动表示公式，包括：基于所述静平台坐标系与第二静位置点第一坐标系的第二转换矩阵、所述第二静位置点第一坐标系与第二静位置点第二坐标系的第三转换矩阵，确定所述静平台坐标系与第二静位置点第二坐标系的第四转换矩阵；基于所述第四转换矩阵、所述第二动位置点在所述静平台坐标系的位置坐标、以及所述第二静位置点在所述静平台坐标系的坐标，确定所述被动表示公式。5.根据权利要求3所述的方法，其特征在于，所述基于所述第二驱动表示公式，确定第一静位置点的驱动输入量关于动平台运动位姿的第一驱动表示公式，包括：基于指定位置点在第二静位置点第二坐标系下的坐标，确定所述指定位置点在静平台坐标系下的坐标；基于所述指定位置点在静平台坐标系下的坐标、第一动位置点在静平台坐标系的位置坐标，确定所述第一驱动表示公式。6.根据权利要求3所述的方法，所述多个模块中至少还包括多个第一连接模块、第二连接模块，所述第一连接模块用于连接具有对应关系的第一静位置点和第一动位置点；所述第二连接模块用于连接具有对应关系的第二静位置点和第二动位置点；所述驱动输入量包括：具有对应关系的第一静位置点和第一动位置点在第一连接模块的调节方向上的第一驱动距离，以及具有对应关系的第二静位置点和第二动位置点在第二连接模块的调节方向上的第二驱动距离，所述驱动表示公式包括：所述第一驱动距离和所述第二驱动距离关于所述动平台运动位姿的距离表示公式；所述被动旋转量包括：每个第二动位置点相对于对应的第二静位置点沿第一轴心旋转的第一旋转变化量；每个第二动位置点相对于对应的第二静位置点沿第二轴心旋转的第二
旋转变化量；所述被动表示公式包括所述第二旋转变化量关于所述动平台运动位姿的被动表示公式、以及所述第一旋转变化量关于所述动平台运动位姿的旋转表示公式。7.根据权利要求6所述的方法，其特征在于，所述根据预先测量获得的主操作手中的各个模块的质心点初始状态下在所述静平台坐标系中的坐标、所述第一关系表示公式和所述第二关系表示公式，确定所述各个模块与所述预设运动参数间的映射关系，包括：根据所述第一关系表示公式以及所述第二关系表示公式，确定所述第二动位置点转动角度与预设运动参数间的第三关系表示公式，其中，所述第二动位置点转动角度用于指示所述第二动位置点相对于第一静位置点的转动角度；将所述初始状态下各个模块的质心点的初始坐标转换到与该模块对应的坐标系中；基于初始状态下各个模块的质心点在所述对应坐标系中的初始坐标、以及所述静平台坐标系与所述对应坐标系的转换矩阵，确定各个模块的质心点在静平台坐标系中的实时坐标；所述实时坐标与所述驱动输入量、所述被动旋转量和所述位姿数据中的至少一个相关；根据所述各个模块的质心点在所述静平台坐标系的实时坐标、所述第一关系表示公式、所述第二关系表示公式以及所述第三关系表示公式，确定所述各个模块与所述预设运动参数间的映射关系。8.根据权利7所述的方法，其特征在于，所述主操作手中还建立有以下坐标系：第一动位置点第一坐标系、第一动位置点第二坐标系、第二动位置点第一坐标系以及第二动位置点第二坐标系；其中，所述第一动位置点第一坐标系基于所述第一动位置点建立，所述第一动位置点第二坐标系基于所述第一动位置点、所述第二静位置点、以及所述第二动位置点建立；所述第二动位置点第一坐标系基于所述第二动位置点和所述动平台建立，所述第二动位置点第二坐标系基于所述第二动位置点和所述静位置点第二坐标系建立；所述第二动位置点转动角度包括：所述第二动位置点第二坐标系的第一方向坐标轴相对于第二动位置点第一坐标系旋转的第一角度，以及，所述第二动位置点第二坐标系的第二方向坐标轴相对于第二动位置点第一坐标系旋转的第二角度；所述第一连接模块的数量为n，每个所述第一连接模块包括球铰模块和第一滑块模块；所述第二连接模块的数量为n，每个所述第二连接模块包括固定板模块、第二滑块模块、动平台连接轴、动平台连接关节模块和导向筒模块和静平台连接关节模块；所述第二动位置点通过动平台连接关节模块和动平台连接轴模块与所述固定板模块连接，所述固定板模块与所述第二滑块连接，所述第二滑块通过所述静平台连接关节模块与第二静位置点连接，并且所述第二静位置点对应的主动驱动装置控制固定板模块在所述滑块模块上滑动控制所述第二静位置点和第二动位置点之间的第二驱动距离；所述第一滑块模块通过所述球铰模块与所述固定板模块连接；且所述第一静位置点对应的主动驱动装置控制第一滑块模块在所述静平台上滑动控制所述第一静位置点和第一动位置点之间的距离；所述动平台对应的坐标系为动平台坐标系，且所述动平台的质心坐标在所述动平台坐标系下不变；所述第一滑块模块对应的坐标系为第一静位置点第一坐标系，且所述第一滑块模块的质心坐标在所述第一静位置点第一坐标系的第一方向坐标轴和第三方向坐标轴上不变，并在第二方向坐标轴上随第一驱动距离线性变化；
所述球铰模块对应的坐标系为第一动位置点第一坐标系，且所述球铰模块质心坐标在所述第一动位置点第一坐标系下不变；所述导向筒模块对应的坐标系为第一动位置点第二坐标系，且所述导向筒模块质心坐标在所述第一动位置点第二坐标系下不变；所述固定板模块对应的坐标系为第二静位置点第二坐标系，且所述固定板模块的质心坐标在所述第二静位置点第二坐标系的第一方向坐标轴和第二方向坐标轴上不变，并在第三方向坐标轴上随所述第二静位置点和第二动位置点之间的距离线性变化；所述第二滑块模块对应的坐标系为第二静位置点第二坐标系，且所述滑块模块的质心坐标在所述第二静位置点第二坐标系下不变；所述静平台连接关节模块对应的坐标系为第二静位置点第一坐标系绕第一方向坐标轴转动所述第一旋转变化量后的坐标系，且所述静平台连接模块的质心坐标在所述对应的坐标系下不变；所述动平台连接轴模块对应的坐标系为第二动位置点第二坐标系，且所述动平台连接轴模块的质心在所述第二动位置点第二坐标系下不变；所述动平台连接关节模块对应的坐标系为第二动位置点第一坐标系绕第二方向坐标轴转动所述第二角度后的坐标系，且所述连接关节模块的质心坐标在所述对应的坐标系下不变。9.根据权利要求6所述的方法，其特征在于，所述根据第二静位置点的驱动输入量和被动旋转量，确定动平台运动位姿的位姿数据包括：确定各个第二静位置点的驱动输入量和被动旋转量；基于所述驱动输入量和被动旋转量，确定所述各第二动位置点在所述静平台坐标系下的坐标，并基于该坐标确定动平台坐标系原点在所述静平台坐标系下的位置坐标；根据所述动平台坐标系原点在所述静平台坐标系中的位置坐标、以及所述各第二动位置点在所述静平台坐标系下的坐标，确定所述动平台坐标系相对于所述静平台坐标系的旋转量；将所述动平台坐标系原点在所述静平台坐标系中的位置坐标、以及所述动平台坐标系相对于所述静平台坐标系的旋转量，确定为所述动平台运动位姿的位姿数据。10.根据权利要求9所述的方法，其特征在于，所述确定各个第二静位置点的驱动输入量和被动旋转量，包括：测量得到各个第一静位置点和第一动位置点在第一连接模块的调节方向上的第一驱动距离、各个第二静位置点和第二动位置点在第二连接模块的调节方向上的第二驱动距离、以及每个第二动位置点相对于对应的第二静位置点沿第二轴心旋转的第二旋转变化量；测量得到各个第二静位置点到所述第一静平台所在平面的距离；基于各个第二静位置点的距离、以及与该第二静位置点对应的第一静位置点的第一驱动距离，确定各个第二静位置点的第一旋转变化量；基于所述第二驱动距离、所述第一旋转变化量和所述第二旋转变化量，确定各个第二静位置点的驱动输入量和被动旋转量。11.根据权利要求9所述的方法，其特征在于，所述动位置点中包括一个位于所述动平
台坐标系的第一方向坐标轴上的指定位置点；所述根据所述动平台坐标系原点在所述静平台坐标系中的位置坐标、以及所述各第二动位置点在所述静平台坐标系下的坐标，确定所述动平台坐标系相对于所述静平台的旋转量包括：基于所述动平台坐标系原点在所述静平台坐标系中的位置坐标和所述指定位置点的坐标，确定所述动平台坐标系的第一方向坐标轴；基于所述动平台坐标系原点在所述静平台坐标系中的位置坐标和任意两个动位置点的坐标，确定所述动平台坐标系的第三方向坐标轴；所述任意两个动位置点位于第一方向坐标轴和第二方向坐标轴所构成的平面上，所述第一方向坐标轴、第二方向坐标轴和第三方向坐标轴构成空间直角坐标系；基于所述第一方向坐标轴和第三方向坐标轴，确定所述动平台坐标系的第二方向坐标轴；基于所述动平台坐标系的第一方向坐标轴、第二方向坐标轴、第三方向坐标轴、以及所述动平台坐标系原点在所述静平台坐标系中的位置坐标，确定所述静平台坐标系和动平台坐标系的第一转换矩阵；基于所述静平台坐标系和动平台坐标系的第一转换矩阵，确定所述动平台坐标系相对于所述静平台坐标系的旋转量。12.一种主操作手的驱动力获取装置，其特征在于，所述主操作手包括多个模块，所述多个模块中至少包括静平台、动平台，且所述动平台受用户控制进行运动；其中，所述静平台包括第一静平台和第二静平台，第一静平台上包括n个第一静位置点，第二静平台上包括与第一静位置点一一对应的n个第二静位置点，每个第一静位置点和第二静位置点均设置有对应的主动驱动装置，所述动平台上包括与所述n个第二静位置点一一对应的n个第二动位置点，所述第一静平台上还包括与所述n个第一静位置点一一对应的n个第一动位置点；所述装置包括：第二驱动表示公式确定单元，用于确定第二静位置点的驱动输入量关于动平台运动位姿的第二驱动表示公式；其中，所述运动位姿用于指示所述动平台相对于所述第一静平台或第二静平台的位姿；所述第二静位置点的驱动输入量用于指示所述第二动位置点在所述主动驱动装置的控制下，产生的相对于对应的第二静位置点的位置变化；第一驱动表示公式确定单元，用于基于所述第二驱动表示公式，确定第二静位置点的被动旋转量关于动平台运动位姿的被动表示公式，并基于所述第二驱动表示公式，确定第一静位置点的驱动输入量关于动平台运动位姿的第一驱动表示公式；所述被动旋转量用于指示所述动平台进行运动后所述动位置点产生的相对于第二静位置点的位置变化，所述第一静位置点的驱动输入量用于指示所述第一动位置点在所述主动驱动装置的控制下，产生的相对于对应的第一静位置点的位置变化；第二关系表示公式确定单元，用于基于所述被动表示公式，确定所述被动旋转量与预设运动参数间的第一关系表示公式，以及，基于所述第一驱动表示公式和第二驱动表示公式，确定驱动输入量与所述预设运动参数间的第二关系表示公式；所述第二关系表示公式中包括逆雅克比矩阵，所述逆雅克比矩阵用于表示所述驱动输入量与所述主操作手的输出间的映射关系；
映射关系确定单元，用于根据预先测量获得的主操作手中的各个模块的质心点初始状态下在静平台坐标系中的坐标、所述第一关系表示公式和所述第二关系表示公式，确定所述各个模块与所述预设运动参数间的映射关系；所述静平台坐标系基于所述第一静平台建立，所述映射关系中包括雅克比矩阵，所述雅克比矩阵用于表示所述各个模块与所述预设运动参数间的映射关系；位姿数据确定单元，用于根据第二静位置点的驱动输入量和被动旋转量，确定动平台运动位姿的位姿数据；所述驱动输入量由所述第一静位置点和/或第二静位置点对应的主动驱动装置控制而产生；驱动力确定单元，用于根据所述主操作手接收到的受力、所述各个模块的重力、所述各个模块的雅克比矩阵、所述逆雅克比矩阵以及所述动平台运动位姿的位姿数据，确定所述主操作手输出的驱动力；力反馈单元，用于根据所述驱动力，对所述主操作手进行力反馈以及重力平衡。13.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求12所述方法的步骤。

技术总结
本申请属于机器人领域，提出了一种主操作手的驱动力获取方法，包括：通过确定动平台运动位姿的驱动表示公式、所述被动旋转量与预设运动参数间的关系表示公式，驱动输入量、被动旋转量与所述预设运动参数间的关系表示公式，以及各个模块与所述预设运动参数间的映射关系，根据所述主操作手接收到的受力、所述各个模块的重力、所述各个模块的雅克比矩阵、所述逆雅克比矩阵以及所述动平台运动位姿的位姿数据，确定所述主操作手输出的驱动力，本申请不需要计算每个部件的受力平衡即可快速的确定所述主操作手输出的驱动力，有利于简化计算过程，有利于提高主操作手的响应速度。有利于提高主操作手的响应速度。有利于提高主操作手的响应速度。


技术研发人员：陈晓红 黄善灯 柏龙 潘鲁锋
受保护的技术使用者：诺创智能医疗科技（杭州）有限公司
技术研发日：2022.04.26
技术公布日：2023/9/20