一种七自由度主从同构式遥操作主手
阅读说明：本技术 一种七自由度主从同构式遥操作主手 (Seven-degree-of-freedom master-slave isomorphic teleoperation master hand ) 是由 朱跃 仲军 龚锦 韩震峰 于振中 于 2021-09-13 设计创作，主要内容包括：本发明公开七自由度主从同构式遥操作主手,由基座组件、底座转动关节组件、大臂转动关节组件、二臂转动关节组件、三臂转动关节组件、摆动架转动关节组件、摆动架摆动关节组件、末端回转关节组件依次首尾转动固定,形成七个关节,每个关节均采用电机驱动；电机集驱动器、编码器为一体；并针对该主手提供逆运动学求解方法,本方法根据主手构型和参数建立主手的D-H坐标系以及运动学矩阵方程,通过代数法求解不构成冗余的关节角度,并根据冗余关节绘制连杆平面图,最后采用固定一个关节的方法,通过代数法和几何法相结合,求解其他冗余关节角度的运动学逆解。本发明极大地较少了7自由度冗余主手逆运动学解的个数,有效地提高了求解速度。(The invention discloses a master-slave isomorphic teleoperation master hand with seven degrees of freedom, which is characterized in that a base component, a base rotating joint component, a large-arm rotating joint component, a two-arm rotating joint component, a three-arm rotating joint component, a swing frame swing joint component and a tail end rotating joint component are sequentially fixed in an end-to-end rotating mode to form seven joints, and each joint is driven by a motor; the motor integrates a driver and an encoder into a whole; the method comprises the steps of establishing a D-H coordinate system and a kinematics matrix equation of a main hand according to the configuration and parameters of the main hand, solving angles of joints which do not form redundancy through an algebraic method, drawing a connecting rod plane diagram according to the redundant joints, and finally solving kinematics inverse solutions of other redundant joint angles by combining the algebraic method and a geometric method through a method of fixing one joint. The method greatly reduces the number of 7-degree-of-freedom redundant main inverse kinematics solutions and effectively improves the solving speed.)
技术领域
本发明涉及遥操作机器人技术领域，具体来说是一种七自由度主从同构式遥操作主手。
背景技术
主从遥操作机器人系统能够帮助操作人员远程操作主手进行救援操作，能够在远处实时控制主手，并且能够根据从端主手周围环境的变化即时作出反应，极大地提高了救援效率，并且保障了救援人员的生命安全。
现如今主从遥操作主手的研究仍处于初级阶段，浙江大学搭建了一款以力反馈手柄PHATOM Omni为主端设备，以液压挖掘机为从端设备的力反馈遥操作系统，该系统具有力反馈功能，能较好地控制从端主手的运动。
现有技术暂时还没有一款用于震后救援的主从同构型遥操作机器人系统，且现在国内大多数的遥操作主手都采用已有的主端设备研究主从异构型遥操作机器人系统，增大了系统延迟，降低主从遥操作的透明性。
相较于6自由度主手，7自由度主手的逆运动学求解较为困难，由于多了1个冗余自由度的存在，使得主手的理论运动学逆解有无数个，计算极为复杂，极大地增加了计算时间。
申请号为202011075243.3公开的一种基于位置级逆运动学的七自由度主手限位优化方法，它涉及一种七自由度主手限位优化方法。该发明为了解决现有的数值解无法得到封闭解，存在终态自运动；解析解无法针对偏置构型，存在无法实现对运动优化的问题。该发明首先基于固定某一关节角的参数化求解方法，得到7自由度主手逆运动学的解析解；然后将该固定的关节角度参数作为输入，关节限位作为优化指标，建立最优控制问题；再基于拉格朗日乘子法，将有约束问题转化为无约束问题；最后基于牛顿迭代法实现对最优的关节角参数的求解，通过给定初始构型、期望末端位姿和笛卡尔的路径规划，得到考虑关节限位优化的7个关节空间轨迹。虽然该发明采用固定其中一个关节实现最优控制，但提供的具体算法并不适用本发明所提供的7自由度主手构型。
发明内容
本发明所要解决的技术问题在于如何提供一种主从同构的7自由度主手。
本发明通过以下技术手段实现解决上述技术问题的：
七自由度主从同构式遥操作主手，包括了基座组件(1)、底座转动关节组件(2)、大臂转动关节组件(3)、二臂转动关节组件(8)、三臂转动关节组件(4)、摆动架转动关节组件(5)、摆动架摆动关节组件(6)、末端回转关节组件(7)；
所述基座组件(1)、底座转动关节组件(2)、大臂转动关节组件(3)、二臂转动关节组件(8)、三臂转动关节组件(4)、摆动架转动关节组件(5)、摆动架摆动关节组件(6)、末端回转关节组件(7)依次首尾转动固定，形成七个关节，每个关节均采用电机驱动；所述电机集驱动器、编码器为一体；
7个关节的关系为：第1关节坐标系x1z1y1与基坐标系x0z0y0重合，第2关节坐标系x2z2y2由第一关节坐标系绕x1轴旋转90°，再绕z1转动90°，并沿x1轴移动a1，第3关节坐标系x3z3y3由第2关节坐标系绕z2轴旋转90°，并沿x2轴移动a2，第4关节坐标系x4z4y4由第3关节沿x3轴移动a3，第5关节坐标系x5z5y5由第4关节沿x4轴移动a4，第6关节坐标系由第5关节坐标系沿x轴转90°，再沿z轴转90°，并沿x5移动a5，沿y5移动-d6，第7关节坐标系由第5关节坐标系绕x轴转动90°；所有关节都以各自的z轴为旋转中心转动；
遥操作主手的逆运动学求解方法包括以下步骤：
S1、根据主手构型和参数建立D-H坐标系及运动学矩阵；
S2、采用代数法求解主手不构成冗余的关节角度；
S3、根据主手的冗余关节绘制连杆平面图；
S4、固定一个冗余关节，令其角度为已知量；
S5、采用几何法和代数法相关结合的方法求解冗余关节的运动学逆解。
本发明提供的主手采用电机驱动各个关节，减小主手结构的复杂性，且在电机中集成有编码器，可实时获取关节角度，为控制提供数据基础。本方法首先根据主手构型和参数建立主手的D-H坐标系以及运动学矩阵方程，随后通过代数法求解不构成冗余的关节角度，并根据冗余关节绘制连杆平面图，最后采用固定一个关节的方法，通过代数法和几何法相结合，求解其他冗余关节角度的运动学逆解。本发明极大地较少了7自由度冗余主手逆运动学解的个数，有效地提高了求解速度，降低了求解难度。
进一步的，所述基座组件基座组件(1)包括底座(9)、底座回转电机(10)、底座端盖(11)；所述底座(9)内设有容纳底座回转电机(10)的容纳腔；所述底座端盖(11)固定在底座(9)顶部；所述回转电机(10)的输出轴向上伸出底座(9)、底座端盖(11)；
所述底座转动关节组件(2)包括底座转动主轴(12)、底座回转架；所述底座回转架开设有第一转轴孔；所述主轴(12)竖向与底座第一回转电机(10)的输出轴固定连接，所述第一转轴孔为水平设置。
进一步的，所述大臂转动关节组件(3)包括大臂关节电机(14)、大臂连杆；所述大臂连杆的一端开设有第二转轴孔，另一端开设有第三转轴孔；所述大臂关节电机(14)固定在底座回转架上，大臂关节电机(14)输出轴穿过第一转轴孔、第二转轴孔将大臂连杆与底座转动关节组件(2)转动固定；所述第三转轴孔为水平设置；
所述二臂转动关节组件(8)包括二臂关节电机(21)、二臂连杆；所述二臂连杆的两端分别开设有第四转轴孔、第五转轴孔；所述二臂关节电机(21)固定在大臂连杆上，二臂关节电机(21)输出轴穿过第三转轴孔、第四转轴孔将二臂连杆与大臂连杆转动固定；所述第五转轴孔为水平设置。
进一步的，所述三臂转动关节组件(4)包括三臂关节电机(31)、三臂连杆；所述三臂连杆的两端分别开设有第六转轴孔、第七转轴孔；所述三臂关节电机(31)固定在二臂连杆上，三臂关节电机(31)输出轴穿过第五转轴孔、第六转轴孔将三臂连杆与二臂连杆转动固定；所述第七转轴孔为水平设置；
所述摆动架转动关节组件(5)包括转动关节电机(44)、第一摆动架；所述第一摆动架两端分别开设有第八转轴孔、第九转轴孔；所述转动关节电机(44)固定在三臂连杆上，转动关节电机(44)输出轴穿过第七转轴孔、第八转轴孔将第一摆动架与三臂连杆转动固定；所述第九转轴孔为水平设置且与第八转轴孔垂直。
进一步的，所述摆动架摆动关节组件(6)包括摆动关节电机(53)、第二摆动架；所述第二摆动架上开设有第十转轴孔、第十一转轴孔；所述摆动关节电机(53)固定在第一摆动架上，摆动关节电机(53)的输出轴穿过第九转轴孔、第十转轴孔，将第二摆动架与第一摆动架转动固定；所述第十一转轴孔与第十转轴孔垂直；
所述末端回转关节组件(7)包括第二回转电机(57)、回转架；所述回转架上开设有第十二转轴孔；所述第二回转电机(57)固定在第二摆动架上，第二回转电机(57)的输出轴穿过第十一转轴孔、第十二转轴孔，将回转架与第二摆动架转动固定。
进一步的，所述大臂连杆、二臂连杆、三臂连杆均为镂空架体。
进一步的，所述S1的求解方法包括以下步骤：
S11、根据连杆坐标系之间的其次变换方程可以得出末端坐标系相对于基坐标系的齐次坐标变换矩阵满足公式(1)：
其中，表示相邻两连杆坐标系的其次坐标变换矩阵；所述连杆为相邻两个关节之间的连接件；其中，n，o，a代表的是末端关节坐标系相对于极坐标系的姿态，p代表的是末端关节坐标系相对于极坐标系的位置。。
进一步的，所述S2的求解方法包括以下步骤：
S21、首先在等式(1)两端同时左乘得：
根据等式(2)两端矩阵第2行第4列元素相等可建立方程：
-s1px+c1py＝0(3)
根据等式(3)可以得到第1个关节角θ1有两个解，分别为：Atan2(py,px)和Atan2(-py,-px)；
根据等式(2)两端矩阵第3行第2列元素相等可建立方程：
c6＝-s1ax+c1ay＝k1(4)
由式(4)可得第6个关节角θ6的值由两个解，分别为由于θ1有两个解，可知θ6的值有4个解；
随后在等式(2)两端同时右端乘可得：
根据等式(5)两端矩阵第1行第1列元素以及第3行第3列元素相等，可得：
(nx+c1c6oz)c7+(c1c6nz-ox)s7＝-s1s6(6)
令nx+c1c6oz＝k2，c1c6nz-ox＝k3，则可得第7个关节角θ7的解为
进一步的，所述S3的求解方法包括以下步骤：
S31、根据连杆平面图可以得到lOA＝a2，lAB＝a3，lBC＝a4，D点坐标为
10、根据权利要求1所述的七自由度主从同构式遥操作主手，其特征在于，本发明固定第5关节的关节角；所述S5的求解方法包括以下步骤：
S51、在等式(1)两端同时右乘可得：
令B点在基坐标系中的坐标为(4px，4py，4pz)，可得到B点的坐标为由此可得则可得到第3个关节角θ3的解为
S52、根据等式(7)两端矩阵第1行第3列元素以及第2行第3列元素相等，可得：
4px＝a1c1+a2c1c2+a3c1c23(8)
令k5＝a2c1+a3c1c3,k6＝-a3c1s3,k7＝4px-a1c1，可得：
k7＝k5c2+k6s2(9)
由式(9)可得到第2个关节角
S53、根据等式(7)矩阵第1行第3列元素以及第3行第3列元素相等，可得：
s234＝-(c5c6c7-s5s7)nz+(c5c6s7+s5c7)oz+(c5s6)az(10)
c234＝(s5c6c7+c5s7)nz-(-s5c6s7+s5c7)oz-(s5s6)az(11)
θ234＝atan2(s234,c234)(12)
由式(12)可得到第4个关节角θ4＝θ234-θ2-θ3。
本发明的优点在于：
1、本发明提供的主手采用电机驱动各个关节，减小主手结构的复杂性，且在电机中集成有编码器，可实时获取关节角度，为控制提供数据基础。本方法首先根据主手构型和参数建立主手的D-H坐标系以及运动学矩阵方程，随后通过代数法求解不构成冗余的关节角度，并根据冗余关节绘制连杆平面图，最后采用固定一个关节的方法，通过代数法和几何法相结合，求解其他冗余关节角度的运动学逆解。本发明极大地较少了7自由度冗余主手逆运动学解的个数，有效地提高了求解速度，降低了求解难度。
本发明通过固定第5个关节的原理在于尽量选择对其他关节影响较小的关节进行固定。采用代数法和几何法，能得到每个位置的解析解。
2、各个关节采用转轴孔对接，结构设计简单易组装，简单的结构尽可能的避免运动时的互相干扰，可最大程度满足动作的连贯性和实时性。
3、采用镂空设计的连杆，可进一步降低主手的重量，减少动力输出。
附图说明
图1为本发明实施例中主从同构遥操作主手外观效果图；
图2为本发明实施例中主从同构遥操作主手基座效果图；
图3为本发明实施例中主从同构遥操作主手底座转动关节效果图；
图4为本发明实施例中主从同构遥操作主手二臂转动关节效果图；
图5为本发明实施例中主从同构遥操作主手三臂转动关节效果图；
图6为本发明实施例中主从同构遥操作主手摆动架转动关节效果图；
图7为本发明实施例中主从同构遥操作主手摆动架摆动关节及末端回转关节效果图；
图8为本发明实施例中主从同构遥操作主手末端回转关节效果图；
图9为本发明实施例中主从同构遥操作主手的各个关节之间的关系结构示意图；
图10为本发明实施例中主从同构遥操作主手的坐标图；
图11为本发明实施例中主从同构遥操作主手运动学逆解的流程图；
图12为本发明实施例中主从同构遥操作主手冗余关节连杆平面图。
图中：1、基座组件；2、底座转动关节组件；3、二臂转动关节组件；4、三臂转动关节组件；5、摆动架转动关节组件；6、摆动架摆动关节组件；7、末端回转关节组件；8、大臂转动关节组件；9、底座；10、底座回转电机；11、底座固定盖；12、底座转动主轴；13、后支撑关节板；14、大臂关节电机；15、左侧回转关节板；16、大臂轴承；17、右侧回转关节板；18、左侧大臂关节板；19、大臂上支撑板；20、右侧大臂关节板；21、二臂关节电机；22、二臂轴承；23、二臂轴承端盖；24、二臂轴承支座；25、大臂下支撑板；26、大臂二号连接板；27、大臂右轴；28、大臂一号连接板；29、三臂轴承；30、左侧二臂关节板；31、三臂关节电机；32、二臂上支撑板；33、二臂下关节板；34、二臂二号连接板；35、二臂三号连接板；36、二臂右轴；37、二臂下支撑板；38、右侧二臂关节板；39、三臂一号连接板；40、三臂左轴；41、三臂二号连接板；42、左侧三臂关节板；43、三臂上支撑板；44、摆动架转动关节电机；45、摆动臂轴承；46、摆动臂轴承支座；47、三臂外轴承端盖；48、三臂轴端固定；49、三臂下支撑板；50、右侧三臂关节板；51左侧摆动架关节板；52摆动架顶板；53、摆动架摆动关节电机；54、右侧摆动架关节板；55、摆动臂左侧连接轴；56、摆动架转动轴；57、末端关节电机7；58、摆动关节；59、回转关节端盖。
a1、沿x1轴，从z1移动到z2的距离；a2、沿x2轴，从z2移动到z3的距离；a3、沿x3轴，从z3移动到z4的距离；a4、沿x4轴，从x4移动到z5的距离；a5、沿x5轴，从z5移动到z6的距离；d6、沿z6轴，从x5移动到x6的距离；θ2、绕z2轴，从x1旋转到x2的角度；θ3、绕z3轴，从x2旋转到x3的角度；θ4、绕z4轴，从x3旋转到x4的角度；θ5、绕z5轴，从x4旋转到x5的角度。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例1
如图1所示，公开了一种七自由度主从同构式遥操作主手，结构比例与从手一致。由基座1、底座转动关节组件2、大臂转动关节组件8、二臂转动关节组件3、三臂转动关节组件4、摆动架转动关节组件5、摆动架摆动关节组件6和末端回转关节组件7首尾转动固定组成，形成7个关节，每个关节均采用电机驱动；本实施例中的电机集驱动器、编码器为一体。；
如图2所示，基座1包括底座9、第一回转电机、底座端盖11。底座9具有容纳底座会展电机10的容纳腔，第一回转电机安装在底座9中，底座端盖11通过螺栓连接分别将第一回转电机和底座9固定。第一回转电机的输出轴向上穿出底座端盖11，实现带动上部部件周向转动。
如图3所示，底座转动关节组件2包括底座转动主轴12、底座回转架；底座回转架开设有第一转轴孔；主轴12竖向与底座第一回转电机10的输出轴固定连接，第一转轴孔为水平设置。
底座回转架包括左侧回转关节板15、右侧回转关节板17、后支撑关节板13，左侧回转关节板15和右侧回转关节板17分别通过螺栓连接固定在底座转动主轴12的两侧，后支撑关节板13的两端分别与左侧回转关节板15和右侧回转关节板17通过螺栓连接；大臂关节电机14通过螺栓连接固定在左侧回转关节板15上，大臂轴承16安装在右侧回转关节板17的第一转轴孔内。
如图4所示，大臂转动关节组件3包括大臂关节电机14、大臂连杆；大臂连杆的一端开设有第二转轴孔，另一端开设有第三转轴孔；大臂关节电机14固定在底座回转架上，大臂关节电机14输出轴穿过第一转轴孔、第二转轴孔将大臂连杆与底座转动关节组件2转动固定；第三转轴孔为水平设置。
大臂连杆包括右侧大臂关节板20、左侧大臂关节板18、大臂上支撑板19、大臂下支撑板25。左侧大臂关节板18与二臂关节电机21通过螺栓连接，右侧大臂关节板20与左侧大臂关节板18分别与大臂上支撑板19和大臂下支撑板25通过螺栓连接固定，大臂二号连接板26与右侧大臂关节版20通过螺栓连接，大臂一号连接板28与大臂二号连接板26通过螺栓连接，大臂右轴27与大臂一号连接板28通过螺栓连接；二臂轴承支座24通过螺栓连接固定在右侧大臂关节板20上第三转轴孔内，二臂轴承22安装在二臂轴承支座24内，二臂轴承端盖23通过螺栓连接固定在二臂轴承支座24上，用以固定二臂轴承22，防止脱落。
如图5所示，二臂转动关节组件8包括二臂关节电机21、二臂连杆；二臂连杆的两端分别开设有第四转轴孔、第五转轴孔；二臂关节电机21固定在大臂连杆上，二臂关节电机21输出轴穿过第三转轴孔、第四转轴孔将二臂连杆与大臂连杆转动固定；第五转轴孔为水平设置。
二臂连杆包括左侧二臂关节板30、右侧二臂关节板38、二臂上支撑板32、二臂下关节板33。三臂关节电机31和左侧二臂关节板30通过螺栓固定，左侧二臂关节板30与右侧二臂关节板38分别与二臂上支撑板32和二臂下关节板33通过螺栓连接，三臂轴承29安装在右侧二臂关节板38的第五转轴孔内，二臂二号连接板34与左侧二臂关节板30通过螺栓连接，二臂一号连接板35与二臂二号连接板34通过螺栓连接，二臂右轴36与二臂一号连接板35通过螺栓连接。
如图6所示，三臂转动关节组件4包括三臂关节电机31、三臂连杆；三臂连杆的两端分别开设有第六转轴孔、第七转轴孔；三臂关节电机31固定在二臂连杆上，三臂关节电机31输出轴穿过第五转轴孔、第六转轴孔将三臂连杆与二臂连杆转动固定；第七转轴孔为水平设置。
三臂连杆包括三臂二号连接板41、左侧三臂关节板42、右侧三臂关节板50、三臂上支撑板43。摆动架转动关节电机44与左侧三臂关节板42通过螺栓连接，左侧三臂关节板42与右侧三臂关节板50分别与三臂上支撑板43和二臂下关节板49通过螺栓连接，三臂二号连接板41与左侧三臂关节板42通过螺栓连接，三臂一号连接板39与二臂二号连接板41通过螺栓连接，三臂左轴40与三臂一号连接板39通过螺栓连接，摆动臂轴承45安装在摆动臂轴承支座46的轴承孔内，摆动臂轴承支座46与三臂外轴承端盖47分别安装在右侧三臂关节板50两侧，并通过螺栓固定。
如图7所示，摆动架转动关节组件5包括转动关节电机44、第一摆动架；第一摆动架两端分别开设有第八转轴孔、第九转轴孔；转动关节电机44固定在三臂连杆上，转动关节电机44输出轴穿过第七转轴孔、第八转轴孔将第一摆动架与三臂连杆转动固定；第九转轴孔为水平设置且与第八转轴孔垂直。
第一摆动架包括左侧摆动架关节板51、右侧摆动架关节板54、摆动架顶板52。左侧摆动架关节板51与右侧摆动架关节板54分别通过螺栓固定在摆动架顶板52的两侧，摆动架摆动关节电机53通过螺栓连接固定在摆动架顶板52中，摆动架转动轴56通过螺栓连接固定在左侧摆动架关节板51上，摆动臂左侧连接轴55通过螺栓连接固定在右侧摆动架关节板54上。
如图7所示，摆动架摆动关节组件6包括摆动关节电机53、第二摆动架；第二摆动架上开设有第十转轴孔、第十一转轴孔；摆动关节电机53固定在第一摆动架上，摆动关节电机53的输出轴穿过第九转轴孔、第十转轴孔，将第二摆动架与第一摆动架转动固定；第十一转轴孔与第十转轴孔垂直。
如图8所示，末端回转关节组件7包括第二回转电机57、回转架58；回转架上开设有第十二转轴孔；第二回转电机57固定在第二摆动架上，第二回转电机57的输出轴穿过第十一转轴孔、第十二转轴孔，将回转架58与第二摆动架转动固定。
末端关节电机57通过螺栓连接固定在回转架58中，回转关节端盖59通过螺栓连接固定在末端关节电机57的转动轴上。本实施例中，回转架58为L形。
本实施例中，大臂连杆、二臂连杆、三臂连杆均为镂空架体，以减轻自重，提高稳定性。
如图1～图8所示，底座转动主轴12通过螺栓连接固定在第一回转电机的转动轴上；左侧大臂关节板18的B处通过螺栓固定在大臂关节电机14的转动轴上，大臂右轴27安装在底座转动关节组件2的大臂轴承16的A孔内；右侧二臂关节板38的D处与二臂关节电机通过螺栓连接，二臂右轴36安装在二臂轴承22的C孔中；右侧三臂关节板50的F处通过螺栓固定在三臂关节电机31的转动轴上，三臂左轴40安装在三臂轴承29的E孔中；摆动架转动轴56通过螺栓连接固定在摆动架转动关节电机44的转动轴上，摆动臂左侧连接轴55安装在摆动臂轴承45的G孔中；摆动关节58通过螺栓固定在摆动架摆动关节电机53的转动轴上。
工作过程：电机连接电源后，操作员用手拖动主手末端，主手电机中的绝对值编码器分别检测各个关节转动的角度，然后通过角度一一对应关系控制从端主手运动，同时从端主手将运动过程中各个关节的力矩变化通过等比例缩放反馈给主手电机中的编码器，并实时改变电机的输出力矩。
实施例2
以实施例1的主手为对象，本实施例提供一种逆运动学求解方法。如图9、图10所示，主手7个关节的关系为：第1关节坐标系x1z1y1与基坐标系x0z0y0重合，第2关节坐标系x2z2y2由第一关节坐标系绕x1轴旋转90°，再绕z1转动90°，并沿x1轴移动a1，第3关节坐标系x3z3y3由第2关节坐标系绕z2轴旋转90°，并沿x2轴移动a2，第4关节坐标系x4z4y4由第3关节沿x3轴移动a3，第5关节坐标系x5z5y5由第4关节沿x4轴移动a4，第6关节坐标系由第5关节坐标系沿x轴转90°，再沿z轴转90°，并沿x5移动a5，沿y5移动-d6，第7关节坐标系由第5关节坐标系绕x轴转动90°。该主手主要参数如表1所示。
表1 7自由度冗余主手具体参数
如图11所示，该7自由度冗余主手逆运动学求解方法，包括以下步骤：
S1、根据主手构型和参数建立D-H坐标系及运动学矩阵；
S2、采用代数法求解主手不构成冗余的关节角度；
S3、根据主手的冗余关节绘制连杆平面图；
S4、固定一个冗余关节，令其角度为已知量；
S5、采用几何法和代数法相关结合的方法求解冗余关节的运动学逆解。
如图12所示，该7自由度冗余主手冗余关节连杆平面图的建立方法为，将大臂关节、二臂关节、三臂关节、摆动臂转动关节简化为一条直线，并以坐标系X2O2Y2为原点，其中lOA＝a2，lAB＝a3，lBC＝a4，D点坐标为
工作过程：根据连杆坐标系之间的其次变换方程可以得出末端坐标系相对于基坐标系的齐次坐标变换矩阵满足公式(1)：
其中，表示相邻两连杆坐标系的其次坐标变换矩阵，其中，n，o，a代表的是末端关节坐标系相对于极坐标系的姿态，p代表的是末端关节坐标系相对于极坐标系的位置。
首先在等式(1)两端同时左乘得：
根据等式(2)两端矩阵第2行第4列元素相等可建立方程：
-s1px+c1py＝0(3)
根据等式(3)可以得到第1个关节角θ1有两个解，分别为：Atan2(py,px)和Atan2(-py,-px)；s表示sin，c表示cos。
根据等式(2)两端矩阵第3行第2列元素相等可建立方程：
c6＝-s1ax+c1ay＝k1(4)
由式(4)可得第6个关节角θ6的值由两个解，分别为由于θ1有两个解，可知θ6的值有4个解。
随后在等式(2)两端同时右端乘可得：
根据等式(5)两端矩阵第1行第1列元素以及第3行第3列元素相等，可得：
(nx+c1c6oz)c7+(c1c6nz-ox)s7＝-s1s6(6)
令nx+c1c6oz＝k2，c1c6nz-ox＝k3，则可得第7个关节角θ7的解为
选择固定第5关节的关节角，并设θ5＝90°。
在等式(1)两端同时右乘可得：
令B点在基坐标系中的坐标为(4px，4py，4pz)，可得到B点的坐标为由此可得则可得到第3个关节角θ3的解为
令等式(7)两端矩阵第1行第3列元素以及第2行第3列元素相等，可得：
4px＝a1c1+a2c1c2+a3c1c23(8)
令k5＝a2c1+a3c1c3,k6＝-a3c1s3,k7＝4px-a1c1，可得：
k7＝k5c2+k6s2(9)
由式(9)可得到第2个关节角
令等式(7)矩阵第1行第3列元素以及第3行第3列元素相等，可得：
s234＝-(c5c6c7-s5s7)nz+(c5c6s7+s5c7)oz+(c5s6)az(10)
c234＝(s5c6c7+c5s7)nz-(-s5c6s7+s5c7)oz-(s5s6)az(11)
θ234＝atan2(s234,c234)(12)
由式(12)可得到第4个关节角θ4＝θ234-θ2-θ3。
与上述方法对应的，本实施例还提供一种7自由度冗余主手逆运动学求解系统，包括以下步骤：
D-H坐标系和运动学矩阵建立模块，用以根据主手构型和参数建立D-H坐标系及运动学矩阵；
关节角度求解模块，用以采用代数法求解主手不构成冗余的关节角度；
连杆平面图绘制模块，用以根据主手的冗余关节绘制连杆平面图；
冗余关节固定模块，用以固定一个冗余关节，令其角度为已知量；
计算模块，用以采用几何法和代数法相关结合的方法求解冗余关节的运动学逆解。
如图12所示，该7自由度冗余主手冗余关节连杆平面图的建立方法为，将大臂关节、二臂关节、三臂关节、摆动臂转动关节简化为一条直线，并以坐标系X2O2Y2为原点，其中lOA＝a2，lAB＝a3，lBC＝a4，D点坐标为
工作过程：根据连杆坐标系之间的其次变换方程可以得出末端坐标系相对于基坐标系的齐次坐标变换矩阵满足公式(1)：
其中，表示相邻两连杆坐标系的其次坐标变换矩阵。
首先在等式(1)两端同时左乘得：
根据等式(2)两端矩阵第2行第4列元素相等可建立方程：
-s1px+c1py＝0(3)
根据等式(3)可以得到第1个关节角θ1有两个解，分别为：Atan2(py,px)和Atan2(-py,-px)；
根据等式(2)两端矩阵第3行第2列元素相等可建立方程：
c6＝-s1ax+c1ay＝k1(4)
由式(4)可得第6个关节角θ6的值由两个解，分别为由于θ1有两个解，可知θ6的值有4个解。
随后在等式(2)两端同时右端乘可得：
根据等式(5)两端矩阵第1行第1列元素以及第3行第3列元素相等，可得：
(nx+c1c6oz)c7+(c1c6nz-ox)s7＝-s1s6(6)
令nx+c1c6oz＝k2，c1c6nz-ox＝k3，则可得第7个关节角θ7的解为
选择固定第5关节的关节角，并设θ5＝90°。
在等式(1)两端同时右乘可得：
令B点在基坐标系中的坐标为(4px，4py，4pz)，可得到B点的坐标为由此可得则可得到第3个关节角θ3的解为
令等式(7)两端矩阵第1行第3列元素以及第2行第3列元素相等，可得：
4px＝a1c1+a2c1c2+a3c1c23(8)
令k5＝a2c1+a3c1c3,k6＝-a3c1s3,k7＝4px-a1c1，可得：
k7＝k5c2+k6s2(9)
由式(9)可得到第2个关节角
令等式(7)矩阵第1行第3列元素以及第3行第3列元素相等，可得：
s234＝-(c5c6c7-s5s7)nz+(c5c6s7+s5c7)oz+(c5s6)az(10)
c234＝(s5c6c7+c5s7)nz-(-s5c6s7+s5c7)oz-(s5s6)az(11)
θ234＝atan2(s234,c234)(12)
由式(12)可得到第4个关节角θ4＝θ234-θ2-θ3。
以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。