一种三自由度等速解耦空间机器人主动球型腕与万向柔顺
控制方法
技术领域
[0001] 本
发明属于自动化工程技术领域，涉及一种通过内、外部球笼式
万向节传动机构驱动上、下两个偏置旋转体合成侧摆、
俯仰与输出端悬浮自转运动的三自由度等速解耦空间机器人高集成主动球型
手腕机构及一种通过腕部三个
转动关节的系统
摩擦力前馈补偿途径实现该球型腕在航天
接触作业条件下跟随末端交互式作用力以消除力干涉的万向柔顺控制方法。
背景技术
[0002] 空间机器人在空间实验室或空间站的维护与日常工作中发挥着重要作用，如空间舱段的组装、
航天器的维修、飞船的控制与舱
门的开启，均可代替宇航员完成，空间机器人的显著特征是结构紧凑，高集成度，大功重比和作业空间。
[0003] 应用于危险和极限环境的空间机器人对腕部的性能指标要求苛刻。腕部是连接臂与手的重要
基础部件，其功用是改变机械手的空间
姿态，腕部的姿态范围及性能直接影响空间机器人
末端执行器的
定位精度、灵活性与作业功能。机器人任意
位置及姿态的控制需要六个自由度，手腕姿态调整应有三个自由度，位于前端腕部的重量又构成小臂的附加
载荷而增加能耗。腕部应在狭小空间内实现三轴回转，其机构设计复杂、可变参数多，手腕的性能指标已经成为限制机器人在空间极限环境下应用与功能扩展的主要
瓶颈。
[0004] 目前，机器人腕部尚不能完全满足空间作业要求，其三自由度运动的传递普遍采用
齿轮啮合，结构复杂、集成度低，甚至存在运动耦合，使得末端执行器的灵活性、姿态调整能力和精确定位能力受限，因此，尚不能通过腕部的调整完成一些高精度的复杂作业。
[0005]
机器人手腕基本上可分为以下三类：1）球型手腕，它的三个关节轴线相交于一点,其位置和姿态解耦,逆运动学分析简单，结构紧凑,功重比大，所以空间机器人通常采用球型手腕。但受机械结构限制,球型腕存在作业空间小、姿态定位精度低、运动平稳性差和自转
角度小等
缺陷；2）非球型手腕，它的三个轴线不交于一点，克服了机械结构的局限性,每个关节的转动角度都能达到360°以上，其缺点是结构不紧凑,体积庞大，功重比小，运动不解耦，控制精度低；3）并联结构手腕，此种手腕实际上是一种小型并联机构。
[0006] 1991年Graco Robotics,Inc.研制了双十字万向节紧凑型万向腕，它的
斜齿轮传动链具有防倾斜作用，俯仰和侧摆分别由两个相交的万向节转动轴传动，功重比大，结构强度高，结构紧凑，但结构复杂，加工难度大，双万向节万向
传动比不断变化，姿态定位精度低，并存在跳跃等不平稳现象。
[0007] NASA研制了新一代的腕部模
块，
传动系统采用三个并联的螺杆驱动实现侧摆、俯仰，结构简单，成本低，灵活性好，适于非结构化环境，NASA喷
气动力实验室已成功用于危险品处理。其缺点是需要三个驱动实现侧摆、俯仰两个运动，作业空间范围小。
[0008] 韩国提出了一种球型两自由度主动机器人腕部关节，它的下旋转体运动通过
齿轮传动实现，上半球的转动通过内部十字万向节传递动力，上、下半球的两个偏置斜面的相对转动复合成腕子的侧摆与俯仰两自由度运动，结构紧凑、简单，两个输入运动解耦，可通过
离合器啮合程度控制传递力矩，使腕部在大力矩作用时成为被动腕，起到过载保护和避免机械干涉的作用，保护腕部不受损，但离合器脱离时，腕子输出端的姿态会出现重新标定困难，使腕部不能恢复到原点。它只能实现两自由度运动，其上半球由内十字万向节驱动，传动比不断变化，因此，上半球为非等速转动，影响了上、下半球合成运动末端侧摆与俯仰的定位精度。十字万向节的有效驱动角度一般在15度以内，大于15度时，传动效率会降低很大，甚至卡死，因此传动负载能力会降低，其侧摆与俯仰角度范围被限制为15度左右。
[0009] 哈尔滨工业大学与大连理工大学联合研制了一种解耦型三自由度球型空间机器人手腕(
申请号：CN201110025549.2),具体由上、下旋转体和输出端构成，上、下半球形旋转体均采用回转平面法线与旋
转轴带夹角的偏置方式，下旋转体由齿轮独立等速驱动，上旋转体通过内十字万向节驱动，二者相对运动合成了输出端的侧摆与俯仰运动。自转运动通过
轴承悬浮于上旋转体的输出端实现，并通过独立的外部十字万向节驱动其自转。由于所研制的球型腕采用十字万向节，因此，实现旋转体等速转动困难，上、下旋转体合成输出端的侧摆与俯仰姿态难以控制，影响了定位精度，外十字万向节的自转也存在同样问题。十字万向节最大传动角为15度，因此，最大姿态角度只能达到15度，最大传动角时万向节传动效率显著降低，以此为依据，上、下半球形旋转体回转平面与
旋转轴的偏置角度应为十字万向节最大准许传动角度之半，即7.5度。可是，为了实现大姿态角，其半球形旋转体的偏置角度却设计为15度，十字万向节实际最大传动角度达到了30度，远远超过了万向节15度的最大极限角度，导致传动效率低，输出端负载能力下降，甚至万向节会出现卡死等现象，这是该球型腕的主要缺陷。此外，该腕部将三个绝对码盘安装于上、下半球和自转机构
电机与减速系统中电机尾部测速轴上(一般该位置用来安装相对测速码盘)，由于电机减速系统减速比很大，即使电机与减速系统输出端转动很小角度，三个绝对码盘也要转动多转，导致测量误差加大，
数据处理不便，此外，绝对码盘的转速很高，磨损加剧，不仅影响其寿命，还会进一步导致测量误差加大。由于十字万向节传动比不断变化，绝对码盘只能间接测量万向节输入端角度，不能确定万向节输出端角度，因此，腕部侧摆、俯仰和自转三个姿态角的准确控制难以实现，可见，三个绝对码盘安装于电机尾部测速轴上也是该腕部的一个缺点，将三个绝对码盘以并联方式安装于上、下半球和自转机构电机与减速驱动系统的输出端，才能更准确的控制腕部姿态角，试验中，我们发现输出端的姿态控制误差较大，这是该球型腕的主要缺点。
[0010] 目前，空间
机械臂主要采用球型腕，普遍存在结构复杂、平稳性差、控制精度低、姿态角度范围小等缺点，球型腕通常采用十字万向节，因其变速传动、小角度和低负载等局限性，影响了末端执行器的灵活性、姿态精确定位能力，也影响了定位时的
稳定性。通过其它大关节的配合实现执行器末端姿态调整会带来整个机械臂的振颤而影响定位精度，给空间机器人在外星球取样任务和完成空间装配等任务的顺利完成带来困难与不便。
[0011] 有鉴于此，我们提出一种由相对独立
运动链组成的等速解耦型三自由度高集成球型空间机器人手腕，采用回转平面法线与旋转轴带夹角的偏置方式的上、下半球形旋转体来保证腕部结构的紧凑性与灵活性，并首次采用等速球笼式万向节代替十字万向节，下旋转体由齿轮独立等速驱动，上旋转体通过内球笼式万向节实现换向等速驱动，由于球笼式万向节的等速性，提高了上、下旋转体相对运动合成输出端的侧摆与俯仰角的控制精度。自转运动通过轴承悬浮于上旋转体的输出端实现，并通过独立的外部等速球笼万向节驱动其自转，由于球笼式万向节的等速性，提高了自转角度的控制精度。球笼万向节及外部衍生机构可望突破现有球型腕姿态角度的调整范围，理论上，所提出球型腕的侧摆与俯仰角度可高达42度(十字万向节传动手腕仅为15度)，上、下半球形旋转体回转平面与旋转轴的偏置角度可高达21度（十字万向节传动手腕仅为7.5度），显著提高了姿态角的范围。控制上，得益于球笼万向节的等速性，将三个绝对码盘以并联方式安装于上、下半球和自转机构
驱动电机与减速系统的输出端，绝对码盘转动圈数明显减少，测量误差小，数据处理方便，绝对码盘转速低，避免了绝对码盘系统的磨损，通过闭环便可准确控制腕部姿态角，此外，球笼万向节传输力矩大，使腕部有效载荷能力显著增大，球型腕姿态的等速调整，可减小机械臂在
真空状态下的振动，提高运动的稳定性。该球型腕实现了三自由度等速解耦，集程度高，可望突破球型腕姿态角范围、负载能力，并能提高球型腕的灵活性、稳定性与控制精度等指标，集球型腕与非球型腕的优点于一体。
[0012] 据我们掌握的资料和立项查新显示，目前，国内外还没有采用等速球笼万向节和其衍生机构的等速解耦型三自由度主动球型腕部机构相关资料的报道，因此，本
专利具有较为突出的原创性。
[0013] 可是，机械臂在未建模环境中的抓取、安装、定位等作业依然是极具挑战性的课题，空间机器人开启舱门仍是国际上悬而未决的难题，研究表明机器人腕部不能开启舱门的原因是腕部姿态转角有限，通过其它关节的配合运动实现末端执行器的旋转存在运动耦合问题，甚至发生震颤，还会由于舱门旋转轨迹与机械臂控制轨迹误差而引起的机械干涉而使舱门开启失败，甚至损坏机械臂。采用所提出的三自由度解耦等速球型腕为解决上述问题奠定了基础，解决途径是采用主动形式实现末端执行器精确定位与抓紧，采用万向柔顺随动模式增加腕部的柔顺性，以避免接触运动的机械干涉。可见，使球型腕具备接触作业功能才能有效解决控制轨迹与实际轨迹误差带来的干涉问题，机械臂末端腕部的精确姿态调整与接触作用能力极大减小了机械臂的震颤与干涉，使航天领域舱门开启等诸多接触作业难题迎刃而解。
[0014] 机器人作业可分为非接触与接触两大类：前者为机器人在自由空间中运动，不受外界环境约束，采用单纯位置控制即可，机器人位置控制只适合在结构化可建模环境内作业；后者受外界环境约束，如空间机器人开门、装配，旋转
曲柄和外星球
采样等任务。对于接触作业，只采用位置控制已不能胜任，因为作业环境对机器人有接触力影响，其末端微小位置偏差可导致较大的接触力，极易损坏机械臂或作业目标，可见，未建模环境中目标抓取等作业绝非易事。目前，空间机械臂手腕尚不完全具备非结构未建模环境中接触作业的能力。
[0015] 迄今为止，柔顺控制主要分为两种：主动式和被动式。机器人通过处理
力反馈信息并采用一定控制策略去主动控制作用力的方式称为主动柔顺控制。相反，机器人不通过力反馈而只是凭借机械等柔顺机构，使其自然顺从接触环境的方式称为被动柔顺控制。主动柔顺控制又分为主动阻抗控制和力/位置混合控制。力/位置混合控制方法对外界环境动力学依赖性强，需要详尽的任务描述。被动柔顺控制的研究主要是通过机械装置本身或特殊设计的被动柔顺机构来实现，如机器人手腕的RCC弹性附件,缺点是要增加额外的机械装置，使腕部机构变得复杂笨重，适应能力差。
[0016] 在实现被控物体柔顺性能方面，经典控制并没有探究物体实现柔顺运动的内在本质，只是将物体与环境间的力干涉问题归结为末端的接触力控制并进行控制过程中的干扰的抑制来实现柔顺性。形象的说，物体柔顺与否是其内在使然，表现出的柔顺性只是与外界环境作用的结果。实现被控对象的柔顺性应从其内部考虑，柔顺模式下的机器人手腕如果经过一定控制策略变为柔顺的绳子，其内力便不会阻碍末端干涉运动，也不会产生接触作业的干涉问题，该设想是一种主动控制实现万向柔顺的控制方法。具体实施途径是对腕部模块柔顺运动过程中的上旋转体电机与减速系统、下旋转体电机与减速系统、自转电机与减速系统的内部摩擦阻力进行补偿，建立万向柔顺模式下的腕部控
制模型，使其跟随末端与环境间的交互式力，进而实现万向柔顺模式。由于球型腕的侧摆、俯仰与自转三个运动关节系统的内部摩擦阻力都得到了前馈补偿，消除了输出末端与环境间的交互式力与
扭矩，变成了一个“绳子”，实现了完全随动，可望完全消除干涉力。基于内部阻力补偿的全柔顺概念也为主动柔顺控制。
[0017] 国内外基于
摩擦力补偿的柔顺模式鲜有研究成果面世。加拿大宇航人员近期研究了一种二自由度主被动球形腕，Saleh Ahmad等人将摩擦力补偿应用于模块化可重构机器人被动模式，实现了关节主、被动模式控制。并安装在移动平台上，进行了门开启实验，避免了内部干涉力，取得了较好的研究成果。然而该球形腕只有二自由度，不能实现外部干涉力沿腕部末端
输出轴Z方向转动力矩的柔顺补偿，严格来讲，不是真正意义上的万向柔顺，应用时不能消除Z向转动干涉力矩，换句话讲二自由度球型腕依然要承受输出末端与环境间的交互式扭转，球型腕依然有被损坏的危险，柔顺控制模式下尚不能变为柔顺的绳子，该腕部采用十字万向节传动，属于非等速传动，存在控制精度低、姿态角度小与负载能力差等问题。
[0018] 我们所提出的一种三自由度球笼万向节等速解耦空间机器人主动球型手腕提高了球型腕的主动控制精度、姿态角范围、负载能力和稳定性，也为适应非结构环境内开启舱门等接触作业奠定了基础。研究表明机械臂旋启舱门后的拉开过程中，末端执行器的运动轨迹还会与舱门旋转轨迹发生机械干涉而失败，甚至损坏腕部；机械臂在外星球探险时，从
土壤中拉出石头的过程也同样存在易损坏腕部的问题；抓取卫星作业的“大容错，软捕获”要求也给机械臂提出了新的要求，解决途径是实现球型腕的全柔顺随动工作模式控制。
[0019] 针对上述局限性，在我们所提出一种三自由度球笼万向节等速解耦空间机器人主动球型手腕机构的基础上，基于将球型腕变为柔顺绳子的思想，本专利又提出一种三自由度等速解耦空间机器人球型腕万向柔顺控制方法，首次提出万向柔顺概念，可望解决空间机械臂在非结构环境内接触作业这一难题。
[0020] 具体途径是在球型腕输出末端安装ATI六维力
传感器，建立万向柔顺模式下考虑末端与环境间交互式力及关节内部阻力影响的电机力矩模型，在控制系统中施加前馈力矩补偿阻碍末端运动的系统内部摩擦力，控制实现输出端跟随末端与环境间交互式力的柔顺随动，实现腕部模块主动万向柔顺随动模式控制。
[0021] 当通过侧摆、俯仰实现执行器末端定位后，由于腕部的自转是等速解耦运动，可通过腕部本身的大自转角度旋开舱门；由于拉开舱门时，会出现机器人末端执行器运动轨迹与舱门转动圆轨迹的干涉，进而产生末端执行器与转动舱门之间的交互力，依据球型腕输出末端安装ATI测量的六维力，施加末端与环境间接触力及内部摩擦力的补偿力矩，使腕子变成万向柔顺的“绳子”，并按运动学逆运算控制侧摆与俯仰电机转动关系，使腕子末端执行器追随转动舱门的轨迹，避开干涉，顺利实现舱门的开启；该全柔顺随动控制模式还可以确保球型腕完成空间装配、空间卫星回收抓取和在外星球取样时从土壤中拉出
矿石等非建模复杂环境内的接触作业。
[0022] 据我们掌握的资料和立项查新显示，国内外目前还没有三自由度等速解耦空间机器人球型腕万向柔顺控制模式研究的相关资料的报道，因此，本专利具有较为突出的原创性。
发明内容
[0023] 本发明要解决的技术问题是提供一种通过内部球笼式
等速万向节和外球笼式等速万向节衍生传动机构驱动上、下两个偏置旋转体合成侧摆、俯仰与悬浮自转三自由度等速解耦的空间机器人高集成主动球型手腕机构及一种通过腕部三个转动关节的系统摩擦力前馈补偿途径实现该球型腕在航天接触作业条件下跟随末端交互式作用力以消除力干涉的万向柔顺控制方法。
[0024] 本发明的技术方案是：
[0025] 一种三自由度等速解耦空间机器人主动球型手腕可以实现三个主动输入运动的等速解耦，使三个主动输入运动分别成为独立的等速解耦运动。上、下两个旋转体均采用旋转轴与旋转体截面法线带夹角的偏置方式，因此，两个旋转体的接触平面是偏置的斜面，腕部的侧摆和俯仰是由两个主动驱动的上、下旋转体所合成的运动，当下旋转体自转一定角度时，上旋转体的输出端的转动是以下旋转体斜面法线为转动轴，以两倍于斜面偏置角为圆锥角的圆锥，当下旋转体自转360度时，上旋转体输出端的运动范围是以下旋转体自转轴为轴心，以四倍于斜面偏置角为圆锥角的圆锥，也就是腕子侧摆与俯仰的空间范围，两个偏置斜面相结合的双半球旋转体使腕部的俯仰和侧摆作业空间明显加大。下旋转体的驱动通过齿轮传动实现，上旋转体的驱动通过内部等速球笼万向节传动实现，
空间布局紧凑，结构简单，重量轻，上旋转体的旋转轴与下旋转体的旋转轴的交点为上、下旋转体半球形的球心，上、下半球旋转体内部采用半球形空心结构，内部空心半球形与上、下旋转体半球形具有同一个球心，以便容纳和保证内部球笼等速万向节与旋转体内壁相互运动时不发生任何干涉，内部球笼等速万向节的下部表面有六条弧形凹槽的星形套和上部表面有相应的六条弧形凹槽的球形壳具有同一个中心，也即万向节的回转中心，内万向节的回转中心与上下旋转体球心重合，因此，避免了侧摆和俯仰两个运动的干涉，实现了两个运动的解耦。输出端的自转是通过外部等速球笼万向节驱动末端执行器
接口的等速传动，末端执行器接口通过轴承悬浮于上旋转体凸起输出端，使自转成为独立的等速解耦运动，并可输出旋转扭矩。内部等速球笼万向节、外部等速球笼万向节的转动中心和两个偏置斜面相结合的双半球球心重合，构成球型腕，实现了三主动驱动运动的等速解耦，三个主动驱动运动相互独立，互不影响。由于外部万向节的输出端与上半球轴颈采用轴承连接实现悬浮，使腕部的旋转运动成为独立的等速运动，实现了运动的等速解耦。与外部等速球笼万向节输出端连接的末端执行器接口的姿态由上、下旋转体形成的侧摆与俯仰角度决定，外部等速球笼万向节不旋转时，末端执行器姿态的侧摆与俯仰与上旋转体随动并保持一致，自转角不变，得益于球笼万向节的等速传动特性，无论外部等速球笼万向节输出端的侧摆与俯仰角度如何变化，外部等速球笼万向输出端的自转角始终等于输入端的输入角度，且二者转速相等，机器人末端执行器的自转角调整方便，腕部完成定位后，由于外部球笼万向节是悬浮在上旋转体输出端上的一个独立解耦运动，突破了以往球型腕输出端对自转角的限制，使输出端自转角度显著增加，外部球笼万向节可带动末端执行器实现大角度自转运动，顺利实现一些复杂装配作业。上、下旋转体按照运动学逆运算的转角位置旋转，能实现腕部输出端在过腕部底座轴线的任意平面内的摆动，也可以实现腕部输出端在锥形空间内任意轨迹的控制，为了准确的控制末端执行器的运动轨迹，在下旋转体电机与减速系统、上旋转体电机与减速系统和外部球笼万向节电机与减速系统输出端分别并联三个绝对码盘，分别与上、下旋转体和自转机构的电机与减速装置构成位置闭环控制，并由安装于上、下旋转体和自转机构的电机减速装置电机末端测速轴上的相对码盘构成速度闭环控制，按照运动学逆运算的转角位置控制上、下旋转体和自转机构的电机运动规律，可实现球型腕姿态的准确控制。
[0026] 在万向柔顺控制技术方案上主要是结合三自由度等速机构的的解耦特征，采用几何方法和齐次变换矩阵，建立上、下旋转体和自转机构输入角与球型腕在球
坐标系内的侧摆、俯仰与自转角关系，得到正、逆运动学方程，由于采用等速球笼万向节传动，使正、逆运动学方程的求解变得简便。根据几何结构与运动关系推导球型腕从球坐标系到基坐标系∑0的雅可比矩阵，得出上、下旋转体和自转机构输入
角速度与球型腕在球坐标系内的侧摆、俯仰与自转角速度的关系，以正确控制球型腕末端的运动方向，为实施万向柔顺作业模式提供理论依据。然后，将球型腕末端执行器接口联接ATI六自由度力/力矩传感器，用来检测末端执行器与外界环境间作用力，当末端受到广义外力作用时，六维力传感器内会检测到相应的六维力，力传感器的输出力
信号是基于传感器本身坐标系的等效力向量，还不是接触力本身，通过多次力向量转换，最终将传感器坐标系力向量转换到基坐标系∑0，最后得到末端执行器所受到的与环境的交互式外力经球型腕部结构引起的电机与减速器主动输出轴上的等效作用力，并通过对腕部模块运动过程的分析，建立合适的摩擦力模型，阻碍腕部万向柔顺运动的摩擦力主要产生在动力及传递系统，考虑到在低速运动情况下，恒定摩擦的量级决定了整个摩擦力的量级，因此摩擦力可以用一个前馈力矩来补偿，基于摩擦力补偿的柔顺模式采用比例
控制器，建立腕部与环境之间的交互补偿力矩，由于雅可比矩阵描述了输入力与输出力的方向性，因此，所补偿的力方向已知，以通过奇次变换和雅可比矩阵转换到基坐标系内力的方向为引导，依据三自由度等速解耦球型手腕末端ATI六维力传感器所测量末端执行器与环境间交互式作用力向球型腕关节坐标系内的转换结果建立万向柔顺模式下考虑末端与环境间接触力及关节系统内部摩擦阻力影响的各关节的电机力矩模型，在控制系统中对侧摆、俯仰与自转传动机构进行系统内部摩擦力前馈补偿，实现侧摆、俯仰传动机构和悬浮式自转传动机构与环境间交互式作用力的随动，实现腕部模块万向柔顺模式控制，解除腕部俯仰、侧摆和自转的力约束，变成一个全柔顺球型腕，使腕部末端执行器沿舱门开启轨迹运动时，不受任何与环境间交互式作用力与力矩的作用，避开腕部末端执行器完成外星球采样时拔出石头和航天器舱门拉开等复杂作业发生力干涉，保护腕部不受损。
[0027] 由于悬浮式自转运动与侧摆与俯仰是解耦的，成为一个独立的运动，如果不对自转传动进行摩擦力前馈补偿，就实现了球型腕的半柔顺模式，以通过奇次变换和雅可比矩阵转换到基坐标系内力的方向为引导，依据三自由度等速解耦球型手腕末端ATI六维力传感器所测量末端执行器与环境间交互式作用力向球型腕关节坐标系内的转换结果所建立关节机
电动力学模型，对侧摆与俯仰进行摩擦力前馈补偿，实现侧摆与俯仰传动与环境间交互式作用力的随动，解除腕部俯仰、侧摆的力约束，有效的解除机械干涉，变成一个半柔顺球型腕，实现接触作业的腕部末端执行器在沿着舱门开启轨迹运动时，不受任何与环境间交互式作用力，半柔顺主动控制可驱动悬浮式自转传动机构实现等速解耦大角度自转作业，在避开腕部末端执行器轨迹与作业对象运动轨迹发生干涉情形下，依然能实施如舱门旋钮的旋启及危险环境下关闭
阀门等复杂作业，并保护腕部不受损。
[0028] 本发明的效果和益处是：
[0029] 一种三自由度等速解耦空间机器人手腕的上旋转体和下旋转体的旋转轴的交点分别与上旋转体内部球笼等速万向节的转动中心和外部球笼等速万向节的转动中心重合，构成球型腕，可实现侧摆、俯仰和自转三个主动输入运动的等速解耦，成为独立运动，姿态控制简便易行，球笼万向节的等速特性可避免动载荷作用下十字万向节的跳跃与不平稳等问题，侧摆、俯仰、自转角度控制准确，上、下旋转体的转动和悬浮式自转运动的匀速控制简便易行，显著提高了球型腕的姿态定位精度和运动的平稳性，避免了机械臂的颤动现象。由于球笼万向节的大传动角度特征和大传递扭矩能力，显著提高了球型腕姿态调整角度范围，也增强了球型腕末端在主动模式下的力和力矩的输出。
[0030] 腕部的侧摆和俯仰是由两个主动驱动的上、下旋转体在偏置斜面上合成的运动，两个偏置斜面相结合的上、下旋转体使腕部的俯仰和侧摆作业空间明显加大。结构紧凑、简单，加工容易。
[0031] 与末端执行器相连接的输出端的自转运动是由外部球笼万向节的输出端与上旋转体轴颈采用轴承连接实现悬浮，因此使腕部的自转运动成为独立的运动，实现运动解耦，可以实现等速大角度自转，控制角度准确。
[0032] 末端执行器姿态的侧摆与俯仰和上旋转体输出端随动并保持一致，但转动姿态不变，使末端执行器法线方向调整方便，腕部完成定位后，由于外部球笼万向节所传递的自转是通过轴承悬浮在于旋转体输出端上的一个独立的等速解耦运动，外部球笼万向节可带动球型腕输出端末端执行器实现等速大自转运动，输出力矩平稳，角度控制准确，使空间复杂装配作业能够顺利实现。
[0033] 由于侧摆、俯仰与自转等速驱动关节的内部摩擦阻力都得到了前馈补偿，球型腕完全变成了一个柔顺的“绳子”，消除了球型腕输出末端与环境间的交互干涉力，避免球型腕受损，全柔顺控制模式适于航天非建模复杂环境的接触作业，可确保球型腕完成舱门的开启、空间站装配、空间卫星回收抓取任务和在外星球取样时从土壤中拉出矿石等作业。
[0034] 只对侧摆、俯仰两个驱动关节的内部摩擦阻力进行前馈补偿，消除了球型腕输出末端与环境间的交互力(不补偿Z轴的与环境间的交互力矩)，球型腕变成一个万向节，球型腕输出端的侧摆与俯仰随动于环境间的交互力，避免球型腕受损，半柔顺控制可驱动外部球笼万向节实现等速解耦大角度自转运动，完成舱门旋启及危险环境下阀门关闭等作业。
附图说明
[0035] 附图1是本发明机器人主动手腕结构示意图。
[0036] 附图2是本发明机器人主动手腕上、下旋转体及内、外万向节传动机构截面示意图。
[0037] 附图3(A)是本发明机器人手腕侧摆与俯仰电机传动系统截面示意图。
[0038] 附图3(B)是本发明机器人手腕自转电机传动系统截面示意图。
[0039] 附图3(C)是本发明机器人手腕上、下旋转体电机传动系统绝对码盘截面示意图。
[0040] 附图3(D)是本发明机器人手腕自转电机传动系统绝对码盘截面示意图。
[0041] 附图4(A)为球型腕三主动输入运动传递简图。
[0042] 附图4(B)为球型腕与底架固的定坐标系和与各运动机构固结的动坐标系的建立简图。
[0043] 附图4(C)为球型腕的作业区域范围分析简图。
[0044] 附图4(D)为球坐标系内球型腕侧摆、俯仰角度与驱动输入角度的分析简图。
[0045] 附图4(E)为将传感器坐标系内末端执行器所受到的与环境的交互式外力向量转换到基坐标系∑0的分析简图。
[0046] 附图4(F)为等速解耦球形腕摩擦力补偿实现万向柔顺控制
框图。
[0047] 附图5为三自由度等速解耦球型腕全、半柔顺控制模式实现过程的说明。
[0048] 图中：
[0049] a侧摆与俯仰传动机构；b悬浮式等速自转传动机构；c底座；d末端执行器；e ATI六维力传感器；f三自由度机器人手腕。
[0050] 1下拨叉；2上拨叉；3
钢珠；4钢珠
保持架；5末端执行器接口；6
法兰轴承；7法兰轴承；8球形壳；9上旋转体；10轴承；11下旋转体；12钢珠；13钢珠保持架；14星形套轴承；15星形套；16下旋转体法兰轴承；17法兰轴承；18旋转
支架轴承；19减速器底架；20减速器端盖；21外部球笼万向节转盘；
[0051] 22上旋转体一级齿轮；23上旋转体二级齿轮；24上旋转体电机与减速系统；25相对转角传感器；26下旋转体电机与减速系统；27下旋转体一级齿轮；28下旋转体二级齿轮；29自转电机与减速系统；30自转一级齿轮；31自转齿轮组；32
外齿圈；33上旋转体绝对转角码盘传动齿轮；34上旋转体绝对转角码盘；35下旋转体绝对转角码盘传动齿轮；36下旋转体绝对转角码盘；37自转绝对转角码盘传动齿轮；38自转绝对转角码盘；
[0052] J1为下旋转体电机与减速系统26驱动的主动关节，其转角为θ1；J3为上旋转体电机与减速系统24驱动内球笼等速万向节的主动关节，其转角为θ3；J4为自转电机与减速系统29驱动外部球笼等速万向节的主动关节，其转角为θ4；J2为上旋转体9与下旋转体11相对转动关节，其转角为θ2，θ2为上、下旋转体转动时在旋转体倾角为 (21度)的斜面上的角度差，基于球笼万向节的等速性，有θ2=θ3-θ1，L1代表
前杆系，L2代表后杆系。
[0053] ∑0为建立在球心并与减速器底架19固结的基坐标系；∑ 1为建立在球心并与下旋转体轴线方向固结的坐标系，其转角为θ1；∑ 2为建立在球心的内部万向节上与下旋转体斜面法线方向固结的坐标系，其转角为θ2；∑ 3为建立在球心的内部万向节输出端上与上旋转体轴线方向固结的坐标系，其转角为θ3；∑ 4为建立在球心的外部万向节上与末端执行器接口5固结的坐标系，其转角为θ4，即外部万向节输出端转角；O为球心参考点，S为手腕末端输出参考点，L为参考点S距球心距离，α和β分别为球型腕末端输出端在基坐标系中用球坐标描述的纬度角和经度角，γ为俯仰角，δ为
偏航角，θ6为外部万向节输入端转角。
[0054] Fsx，Fsy，Fsz，τsx，τsy，τsz分别为在六维ATI传感器坐标系内检测到的末端执行器与环境交互式外力与力矩在x,y,z方向的分量。
[0055] Fs为六维力传感器所检测的环境交互力，τsi为电机转动广义转动坐标系内腕关节与环境相互作用力矩，τsid为理想力矩， 为摩擦力前馈补偿力矩， 为实际干扰摩擦力。
具体实施方式
[0056] 以下结合技术方案和附图详细叙述本发明的具体
实施例。
[0057] 结合附图2、图3(A)和3(C)说明三自由度等速解耦球型手腕侧摆和俯仰运动的实现,三自由度等速解耦型球型机器人主动手腕包括内部球笼等速万向节，它包括球形壳8，六个钢珠12，钢珠保持架13和星形套15，动力由星形套15经钢珠12传到球形壳8输出，球形壳8与星形套15具有同一个球心，即为内球笼等速万向节的转动中心。
[0058] 三自由度等速解耦球型机器人主动手腕的侧摆和俯仰运动机构由上旋转体9和下旋转体11通过轴承10连接而成，上旋转体9和下旋转体11的半球截面的法线均采用与其旋转轴线带一定倾斜角的偏置方式(倾斜角为球笼万向节最大传动角度之半21度)，下旋转体11下旋转体法兰轴承16与减速器端盖20相连接，下旋转体11的转动由直接与下旋转体电机与减速系统26相连的下旋转体一级齿轮27与下旋转体二级齿轮28的啮合传动来实现；上旋转体(9)转动的运动传动链是：上旋转体电机减速系统(24)的末端与上旋转体一级齿轮(22)相连，上旋转体二级齿轮(23)与内部球笼等速万向节的星形套(15)轴端相固结，上旋转体一级齿轮(22)与上旋转体二级齿轮(23)啮合经由内部球笼等速万向节的星形套(15)经钢珠(12)传到球形壳(8)输出动力带动上旋转体(9)转动，空间布局紧凑；上旋转体9的旋转轴与下旋转体11的旋转轴的交点与内部球笼等速万向节的转动中心重合，上旋转体9与下旋转体11内部采用半球形空心结构，该空心半球形、上旋转体9和下旋转体11三者球心重合，以便容纳和保证内部球笼等速万向节与旋转体内壁相互运动时不发生任何干涉，内部球笼万向节的星形套15和球形壳8的中心也与球心重合，因此，避免了侧摆和俯仰两个运动的干涉，实现了两个运动的解耦。
[0059] 当下旋转体11转动一定角度时，上旋转体9的输出端(凸起处)的转动是以下旋转体11斜面的法线为轴线，以两倍于斜面倾斜角为圆锥角的圆锥，当下旋转体自转360度时，上旋转体输出端的运动范围是以下旋转体自转轴为轴心，以四倍于斜面倾斜角为圆锥角的圆锥，也就是腕子侧摆与俯仰的空间范围，两个偏置斜面相结合的上、下旋转体使腕部的俯仰和侧摆的作业空间明显加大，上旋转体凸起处为腕部输出端，可以输出旋转扭矩和作用力，腕部输出端姿态的侧摆与俯仰角取决于下旋转体11的转角位置和上旋转体9在斜面处相对于下旋转体11的转角所决定。
[0060] 按照逆运动学变换关系可以计算出实现腕部输出端在通过减速器底架19轴心线的任意平面内或者空间任意曲线摆动时，上旋转体9和下旋转体11的绝对转角位置关系；上旋转体电机与减速系统24输出端与上旋转体一级齿轮22相连，上旋转体绝对转角码盘传动齿轮33与上旋转体绝对转角码盘34相连，上旋转体9转动角度位置的检测可以通过上旋转体一级齿轮22和上旋转体绝对转角码盘传动齿轮33的啮合带动旋转的上旋转体绝对转角码盘34的反馈角度的闭环控制来实现；下旋转体电机与减速系统26的输出端与下旋转体一级齿轮27相连，下旋转体绝对转角码盘传动齿轮35与上旋转体绝对转角码盘36相连，下旋转体11转动角度位置的检测可以通过下旋转体一级齿轮27和下旋转体绝对转角码盘传动齿轮35带动的啮合带动上旋转体绝对转角码盘36的反馈角度的闭环控制来实现；上旋转体电机与减速系统24、下旋转体电机与减速系统26和自转电机与减速系统29的电机测速轴分别连接相对转角传感器25用于实现其转速的闭环控制；上旋转体9和下旋转体11按照雅可矩阵确定的传动规律转动，可求出实现腕部输出端在通过减速器底架19轴心线的任意平面内摆动时，上旋转体电机与减速系统24和下旋转体电机与减速系统26的转向关系。
[0061] 按照逆运动学变换关系计算的角度采用位置与速度双闭环控制上旋转体电机与减速系统24和下旋转体电机与减速系统26就可以准确的实现上旋转体输出端在所期待任意平面内的摆动，也可以实现腕部输出端在四倍于斜面偏置角度作业空间内任意轨迹的运动。
[0062] 结合附图2、图3(B)和3(D)说明三自由度等速解耦球型手腕自转运动的实现。
[0063] 三自由度等速解耦型球型机器人主动手腕包括外部球笼等速万向节，其外部万向节实际上是球笼等速万向节的衍生机构，由六条带滚珠弧形凹槽的下拨叉1(相当于星形套)、六条带滚珠弧形凹槽的上拨叉2(相当于球形壳)、六个钢珠3和钢珠保持架4组成，六个钢球3分别装在由六组内外
滚道所对出的空间里，并被保持架4限定在同一个平面内。六条带滚珠弧形凹槽的下拨叉1(相当于星形套)固定在外部球笼万向节转盘21上，并由旋转支架轴承18实现与减速器端盖20实现转动连接。动力由拨叉1经钢球3传到上拨叉
2输出，六条圆弧形下拨叉1沿外部球笼万向节转盘21轴线的圆周方向以60度均匀布置，六条圆弧形上拨叉2沿末端执行器接口5轴线的圆周方向以60度均匀布置，并保证六条圆弧形下拨叉1的球心与六条圆弧形上拨叉2的球心重合，这个球心就是万向节的旋转中心。
从结构上保证万向节在工作过程中，其传力点永远位于两轴交点的平面上，工作时无论传动方向，六个钢球全部传力，因此，结构紧凑，承载能力强，拆装方便，显著优点是两轴最大交角为42度(十字万向节仅为15度)。
[0064] 末端执行器接口5的自转运动传动链是：自转电机与减速系统29带动与其相连的自转一级齿轮30与自转齿轮组31啮合，外部球笼万向节转盘21与外齿圈32相连接，自转齿轮组31再与外齿圈32啮合带动外部球笼万向节转盘21旋转，外部球笼万向节转盘21再带动外部球笼等速万向节的六个下拨叉1经钢球3传到六个上拨叉2输出端，外部球笼等速万向节的六个上拨叉2作为输出端与末端执行器接口5固结，六个上拨叉2再带动末端执行器接口5实现手腕自转，末端执行器接口5通过法兰轴承6、7与上旋转体9的轴颈(凸起处)实现悬浮连接，因此使腕部末端执行器接口5的旋转运动成为独立运动，使自转运动实现等速解耦，末端执行器接口5的姿态始终由上、下旋转体的侧摆与俯仰决定，当外部万向节不旋转时，末端执行器接口5的侧摆与俯仰与上旋转体随动并保持一致，自转角不变，这样就可以方便的调整机器人末端执行器接口5的法线方向，在腕部完成定位后，由于自转运动是悬浮在上旋转体9上的一个解耦的独立运动，突破了球形腕对输出端自转角的限制，使输出端自转角度大于360°，使旋启阀门等复杂作业能够顺利实现。
[0065] 末端执行器接口5的转动角度位置的检测过程是：由自转电机与减速系统29的输出端与自转一级齿轮30相连，自转绝对转角码盘传动齿轮37与自转绝对转角码盘38连接，自转一级齿轮30与自转绝对转角码盘传动齿轮37相啮合带动自转绝对转角码盘38转动；自转电机与减速系统29的电机测速轴连接相对转角传感器25，自转转速由相对转角传感器25的闭环控制实现。无论末端执行器接口5的侧摆与俯仰角度多大，外部球笼万向节可带动末端执行器接口5实现大于360度以上的等速自转运动，可以准确的控制自转角度。
[0066] 由于三个主动驱动运动相互独立，互不影响，内部万向节、外部万向节的转动中心和两个偏置斜面相结合的上旋转体9和下旋转体11的球心重合，构成球型腕，实现了三个主动驱动运动的解耦。一种三自由度等速解耦球型机器人手腕可以实现三个主动输入运动的等速解耦，使三个主动输入运动分别成为独立的等速运动。
[0067] 下面结合附图1说明实现三自由度等速解耦球型机器人手腕输出端姿态调整控制与大角度旋转作业的具体实施方式与操作过程。
[0068] 三自由度等速解耦主动手腕在作业之前进行姿态零点标定的过程是：驱动上旋转体电机与减速系统24使上旋转体9的轴颈(凸起处)与底座c的轴线重合，此时上旋转体9与下旋转体11的相对转角为零,对三自由度解耦手腕的侧摆与俯仰没有影响，悬浮式自转传动机构b的自转角度对侧摆与俯仰也没有影响，可处于任意位置作零点标定，此时分别给上旋转体绝对转角码盘34、下旋转体绝对转角码盘36相并联和自转绝对转角码盘38上电，它们便存储并记忆三自由度解耦手腕的零点位置；当三自由度解耦手腕定位时，使三自由度等速解耦手腕处于主动驱动模式，上旋转体电机与减速系统24与上旋转体绝对转角码盘34、下旋转体电机与减速系统26与下旋转体绝对转角码盘36、自转电机与减速系统
29与自转绝对转角码盘38分别构成位置闭环控制系统,上旋转体电机与减速系统24与相对转角传感器25、下旋转体电机与减速系统26与相对转角传感器25、自转电机与减速系统29与相对转角传感器25分别构成速度闭环控制系统,首先根据机器人手腕末端的侧摆、俯仰与自转的姿态需求，对姿态调整进行轨迹规划，按照逆运动学变换关系计算的角度采用位置与速度双闭环控制上旋转体电机与减速系统24和下旋转体电机与减速系统26和自转电机与减速系统29就可以准确的控制机器人手腕末端到达所需的侧摆、俯仰与自转姿态位置。实现上旋转体9转角、下旋转体11转角和外部球笼万向节输入
传动轴的转角位置的控制，得益于球笼万向节的等速功能，进而可以记忆并确定三自由度解耦手腕的侧摆、俯仰和自转角度位置。
[0069] 三自由度等速解耦球型手腕输出端姿态包括相对于底座c的俯仰、侧摆与自转角度，首先驱动侧摆与俯仰传动机构a和悬浮式自转传动机构b，调整手腕输出端的法线方向与作业目标中心线与方位一致，完成对作业目标的夹紧；由于悬浮式自转传动机构b的运动与侧摆与俯仰是解耦的，成为一个独立的运动，并能实现大角度自转，因此可以驱动悬浮式等速自转传动机构b完成大角度旋启等复杂作业。
[0070] 结合附图2、图4(A)、4(B)、4(C)说明三自由度等速解耦球型机器人主动手腕的空间作业范围。
[0071] 当下旋转体11转动角度固定时，上旋转体9的输出端(凸起处)的转动是以下旋转体11斜面的法线为轴线(当固定θ1，改变θ2时)，以两倍于斜面倾斜角为圆锥角的小圆锥(小圆锥角为 (42度))；当下旋转体自转360度时，上旋转体输出端的运动范围是以下旋转体自转轴为轴心(θ1和θ2同时任意改变)，以四倍于斜面倾斜角为圆锥角的圆锥(大圆锥角为 (84度))，也就是腕子侧摆与俯仰的作业范围，两个偏置斜面相结合的上、下旋转体使腕部的俯仰和侧摆作业范围明显加大，上旋转体凸起处为腕部输出端，可以输出旋转扭矩和作用力，腕部的侧摆与俯仰角度取决于下旋转体11的转角位置和上旋转体9在斜面处相对于下旋转体11的转角。
[0072] 结合附图2、图4(A)、4(B)、4(D)说明三自由度等速解耦球型机器人手腕运动学与逆运动学的推导过程，得出球坐标系内球型腕侧摆、俯仰与自转姿态角度与上旋转体、下旋转体和自转机构输入角度的关系，进而依据逆运动学，完成轨迹规划，得出按一定轨迹运动时，腕部各电机的角度、角速度。
[0073] 手腕末端输出的
位姿由手腕的三个自由度决定，应该首先得到各坐标系的变换关系，由∑0到∑ 4的系统传递矩阵为：
[0074]
[0075] 描述了∑j相对于∑ i的位置和方位。
[0076]
[0077] cn＝cos(θn)；sn＝sin(θn)；n=1，2，3，4。
[0078] 易得到坐标系∑0到∑ 3的变换矩阵为：
[0079]
[0080]
[0081]
[0082] 沿着末端输出轴线自转的第三个自由度相对于其它两自由度独立，可单独考虑手腕的侧摆、俯仰两自由度。在球心用RPY角描述坐标系∑3相对于∑ 0的位姿有：
[0083]
[0084] 则有：
[0085]
[0086] 令式（4）两端元素（3,3）对应相等解得：
[0087]
[0088] 采用逆变换 左乘（4）方程式两端，有
[0089]
[0090] 令方程式（6）两端（2,3）元素对应相等，则有
[0091] s1cδsγ+c1sδ＝cφsφc2-sφcφ （7）
[0092] 令d2＝cφsφc2-sφcφ，解得：
[0093]
[0094]
[0095] 最后在方程（6）两端乘以逆矩阵 得
[0096]
[0097] 令式（9）两端元素（1,1）和（1,2）对应相等，解得
[0098] θ3＝arctan2(sγsδ(c1c2-s1s2cφ)+
[0099] cδ(s1c2+c1s2cφ)-cγsδsφs2, （10）
[0100] cγ(c1c2-s1s2cφ)+sφs2sγ)
[0101] 以上通过矩阵变换建立了下旋转体与上旋转体驱动角度θ1和θ3与侧摆、俯仰角度之间的关系。第三个自由度的自转运动为解耦运动，外部球笼万向节为等速传动，输入角速度与输出角速度的关系为：
[0102]
[0103] 下面通过建立球坐标求得俯仰、侧摆角度。设S点坐标为（xs,ys,zs），用球面坐标表示为：
[0104]
[0105] 由几何关系可得俯仰、侧摆角度γ，δ与S点坐标的关系为
[0106]
[0107] 进一步可得球坐标系中，纬度角α和经度角β与俯仰、侧摆角度的关系：
[0108]
[0109] 按照逆运动学变换关系计算的角度采用位置与速度双闭环控制上旋转体电机与减速系统24和下旋转体电机与减速系统26就可以准确的实现上旋转体输出端在所需的任意平面内摆动，也可以实现腕部输出端在四倍于斜面偏置角度空间内任意轨迹的运动。
[0110] 结合附图2、图4(A)、4(B)、4(D)说明三自由度等速解耦球型腕从三个主动驱动角度到球坐标系的雅可比矩的推导过程，得出上旋转体、下旋转体和自转机构输入角速度与球型腕在球坐标系内的侧摆、俯仰与自转角速度与转向的关系。
[0111] 球型腕关节的雅可比矩阵为关节空间向操作空间运动速度的传动比，也是关节空间向操作空间作用力的传动比，采用几何方法构建腕关节雅可比矩阵。
[0112]
[0113] 其中：
[0114] 输出广义速度与输入角速度的关系式为
[0115]
[0116] 从球坐标到基坐标系∑0的雅可比矩阵为：
[0117]
[0118] 式(17)可以得出上旋转体、下旋转体和自转机构输入角速度与球型腕在球坐标系内的侧摆、俯仰与自转角速度与转向的关系。
[0119] 结合附图2、图4(A)、4(B)、4(D)、4(E)说明三自由度等速解耦球型腕末端ATI六维力传感器所测量末端执行器与环境间交互力向球型腕关节坐标系内的转换过程和建立关节机电动力学模型的过程。
[0120] 将ATI的六自由度力/力矩传感器与三自由度手腕的末端执行器接口联接，用以检测末端执行器与外界环境间作用力的大小。当末端受到广义外力F作用时，六维力传感器内会检测到相应的力Fs。力传感器的输出力信号是基于传感器本身坐标系的等效力向量，非接触力本身。因此，需要一定的转换策略将传感器的力向量转换成所需要坐标系内的力向量。首先将传感器坐标系力向量转换成坐标系∑3内的向量Fc。设传感器坐标系检测到的力向量为： 坐标系∑3中的等效力向量为
[0121] 由 我们即可得到Fc；
[0122] 这里
[0123] 腕部模块的第三个自转自由度相对其他两个侧摆与俯仰自由度独立，在不考虑自转自由度时有：
[0124]
[0125] 其 中：Jv代表球坐标系与坐标系∑ 3力矩变换的雅可比矩阵，矩阵
[0126] 矩阵I为6×6单位矩阵，矩阵U为6×6选择矩阵，其目的是将球型腕自转Z方向的力与力矩消除。
[0127] 为了简便起见，定义矩阵 可由公式 计算得到ταβ。
[0128] 进一步将球坐标系内的力转换到基坐标系∑0内的电机转动广义转动坐标系[0129]
[0130] J2为从球坐标到基坐标系∑ 0的雅可比矩阵。
[0131] 此时，自转方向Z的力没有转换，只实现了侧摆、俯仰二个方向力的转化，该方程用于实现球型腕的半柔顺模式控制。
[0132] 为了不失一般性，推导三自由度的力转换过程，若考虑加入自转时，可得：
[0133]
[0134]
[0135] 进一步将球坐标系内的力转换到基坐标系∑0，即电机转动广义转动坐标系T
[0136] τs(136)＝J ·ταβ4 (20)
[0137] J为从球坐标到基坐标系∑0的雅可比矩阵。
[0138] 这样就可得到末端执行器所受到的外力经过腕部模块结构引起的主动输出末端轴上的等效作用力。由于雅可比矩阵描述了输入与输出量的方向性，因此，所补偿的力方向已知。
[0139] 全柔顺模式下每个关节的动力学模型可以描述为：
[0140]
[0141] 式中，i为1、3、6，代表三个主动关节，Imi为电机转轴惯性矩，γi代表传动比，τsi为由腕关节与环境相互作用力引起的力矩，τi为主动电机输出力矩。 代表关节摩擦力。
[0142] 结合附图4(F)说明等速解耦球形腕三个关节系统摩擦力矩前馈补偿实施万向柔顺控制的具体过程。
[0143] 图4(F)中，理想力矩τsid通过与腕关节与环境相互作用力矩τsi构成
负反馈并通过摩擦力前馈补偿力矩 来克服实际干扰摩擦力 最终驱动球型腕实现万向柔顺控制模式。
[0144] 万向柔顺模式下的摩擦力矩补偿不同于精确位置定位及位置
跟踪控制下的摩擦力矩补偿，关节摩擦力矩的摩擦数量级补偿到远小于外力矩即可关节就可以实现与外力的随动。选择适合描述腕部模块运动模式的系统摩擦力模型：
[0145]
[0146] 其中，fci为库伦摩擦参数，fsi为静态摩擦参数，fτi为相应于Stribeck影响的正参数，bi为粘性
摩擦系数， 为取决于位置的摩擦和未建模摩擦误差， 为一符号函数。
[0147] 低速下常量摩擦力的量级决定了整个摩擦力的量级，设
[0148]
[0149] 可将Fi分为常量与变量之和，即Fi＝FiC+Fiv，FiC和Fiv代表摩擦的常量部分和变量部分。阻碍腕部万向柔顺运动的摩擦力主要产生在动力及传递系统。考虑到在低速运动情况下，恒定摩擦的量级决定了整个摩擦力的量级。因此摩擦力可以用一个前馈力矩来补偿，即
[0150]
[0151] 和 代表常量摩擦值的估计参数。
[0152] 基于摩擦力补偿的被动柔顺模式采用比例控制器，腕部与环境之间的交互补偿力矩可以表示为如下：
[0153]
[0154] 这里： 为标称摩擦力， 为τsi标称值，τsid为理想力矩，kpi为比例控制器的增益。
[0155] 通过上述公式(21)、(24)可得到：
[0156]
[0157] 这里：
[0158] 这样就建立了一个较完整的动力学方程。为了使柔顺性控制效果良好，选取恰当控制参数，使上式可满足：
[0159]
[0160] 可见，按照上述具体实施方案，如果全柔顺模式的前馈摩擦力补偿效果好，便可完全实现球型腕的全柔顺控制。
[0161] 下面结合附图5说明实现三自由度等速解耦球型腕全柔顺与半柔顺模式控制的具体实施方式。
[0162] 三自由度机器人手腕f的侧摆与俯仰传动机构a的上旋转体9与下旋转体11按逆运动学计算出的角度规律驱动，可以实现腕部末端执行器d在过底座c轴线的任意平面内的摆动，也可以实现锥形作业空间内的任意摆动；三自由度等速解耦球型手腕输出端姿态包括相对于底座c的侧摆、俯仰与自转角度，首先驱动侧摆与俯仰传动机构a的上旋转体9与下旋转体11和悬浮式自转传动机构b按逆运动学计算出的角度规律驱动，调整手腕输出端的法线方向与作业目标中心线与方位一致，完成对作业目标的夹紧；考虑到拉开舱门等接触作业时，能避开腕部末端执行器d轨迹与舱门运动轨迹发生干涉，在机器人腕部末端执行器d上安装一个ATI六维力传感器e，上旋转体电机与减速系统24、下旋转体电机与减速系统26和悬浮式自转传动机构b分别与ATI六维力传感器e检测到的力构成位置、速度和力的闭环控制，依据三自由度等速解耦球型腕末端ATI六维力传感器所测量末端执行器与环境间交互式作用力向球型腕关节坐标系内的转换过程和建立关节机电动力学模型，对上旋转体电机与减速系统24、下旋转体电机与减速系统26和悬浮式自转传动机构b三个关节系统进行摩擦力前馈补偿，上旋转体电机与减速系统24、下旋转体电机与减速系统
26驱动侧摆与俯仰传动机构a不断调整摆动以实现追随力F消除球型腕末端执行器所受到的与环境间交互式作用力；悬浮式自转传动机构b不断的调整自转以实现追随力矩M消除球型腕末端执行器所受到的与环境间交互式作用力矩，实现电机减速装置和运动传递机构的摩擦力完全补偿，变成一个全柔顺球型腕，提高腕部的灵活性，实现接触作业的腕部末端执行器d在沿着舱门开启轨迹运动时，不受任何与环境间交互式作用力的作用，保护腕部拔出石头和拉开舱门时不受损坏。全柔顺作业模式可以解除腕部俯仰、侧摆和自转的力约束，有效的解除机械干涉，满足空间极限环境对机器人腕部的特殊需求，完成外星球采样时拔出石头和航天器舱门拉开等复杂作业。
[0163] 当力F的方向改变时，按照雅可比矩阵改变侧摆与俯仰传动机构a的上旋转体电机与减速系统24、下旋转体电机与减速系统26和悬浮式自转传动机构b的转动方向，可不断实现不同方向的摩擦力补偿。三自由度机器人手腕f采用全柔顺模式作业时，侧摆、俯仰与自转的位置依然可以由上旋转体绝对转角码盘34、下旋转体绝对转角码盘36和自转绝对转角码盘38存储记忆，三自由度机器人手腕f作业完成恢复到主动腕时，可以恢复到零点标定位置。
[0164] 由于悬浮式自转传动机构b的自转运动是独立的等速解耦运动，作为特例，如果不对悬浮式自转传动机构b进行摩擦力前馈补偿，就实现了球型腕的半柔顺模式，末端执行器姿态的侧摆与俯仰与上旋转体一致，末端执行器法线方向调整方便，完成对作业目标的定位后，上旋转体电机与减速系统24、下旋转体电机与减速系统26分别与ATI六维力传感器e检测到的力F构成前馈力矩闭环控制，依据三自由度等速解耦球型手腕末端ATI六维力传感器所测量末端执行器与环境间交互式作用力向球型腕关节坐标系内的转换过程和建立关节机电动力学模型，对上旋转体电机与减速系统24、下旋转体电机与减速系统26驱动的关节系统进行摩擦力前馈补偿，上旋转体电机与减速系统24、下旋转体电机与减速系统26驱动侧摆与俯仰传动机构a不断调整摆动以实现追随力F消除球型腕末端执行器所受到的与环境间交互式作用力；实现侧摆与俯仰电机减速装置的摩擦力补偿，球型腕变成一个半柔顺控制模式，球型腕的侧摆、俯仰两个驱动关节的内部摩擦阻力补偿后，消除了球型腕输出末端与环境间的交互力，变成一个万向节，球型腕的输出端跟与环境间的交互力随动，消除了干涉力，避免球型腕受损，半柔顺主动控制可驱动悬浮式自转传动机构b实现等速解耦大角度自转运动，完成舱门旋启及危险环境下阀门关闭等作业。
