[发明专利]一种三自由度转动手控器及其控制方法

权利要求书

1.一种三自由度转动手控器，其特征在于，包括第一驱动装置、第二驱动装置、第三驱动装置、操作手柄(1)、力传感器(2)、法兰盘(4)和十字铰支架(3)，法兰盘(4)固定在操作台上，力传感器(2)安装在法兰盘(4)上，第一驱动装置下端通过套筒(8)安装在力传感器(2)上，第一驱动装置包括：Z轴电机(7)、套筒(8)、Z轴联轴器(11)和Z轴轴承箱(9)，套筒(8)包围在Z轴电机(7)外，Z轴电机(7)动力输出端与Z轴联轴器(11)的一端连接，Z轴联轴器(11)外套装有Z轴轴承箱(9)，Z轴轴承箱(9)与套筒(8)顶端连接，Z轴联轴器(11)的另一端连接X轴节叉(10)的节叉杆，X轴节叉(10)的一个节叉头套在X轴第二轴承箱(21)上，X轴第二轴承箱(21)安装在十字铰支架(3)的D端，X轴节叉(10)的另一个节叉头套在X轴第一轴承箱(13)上，X轴第一轴承箱(13)安装在十字铰支架(3)的B端；使能按钮(6)安装在操作手柄(1)上，操作手柄(1)通过手柄杆(24)与Y轴节叉(16)固定连接；所述十字铰支架(3)的四端顺时针设置A、B、C、D端，A、B、C、D端均为阶梯轴，十字铰支架(3)A端连接Y轴节叉(16)，十字铰支架(3)B端连接第二驱动装置和X轴节叉(10)；十字铰支架(3)C端连接第三驱动装置和Y轴节叉(16)；十字铰支架(3)D端与X轴节叉(10)连接；第二驱动装置包括X轴电机(12)、X轴第一轴承箱(13)和X轴联轴器(14)，X轴电机(12)的动力输出端通过X轴联轴器(14)与十字铰支架(3)的B端连接，X轴联轴器(14)外套装有X轴第一轴承箱(13)，X轴第一轴承箱(13)外套有X轴节叉(10)的一个叉头；第三驱动装置包括Y轴电机(15)、Y轴第一轴承箱(22)和Y轴联轴器(17)，Y轴电机(15)的动力输出端通过Y轴联轴器(17)与十字铰支架(3)的C端连接，Y轴联轴器(17)外套装有Y轴第一轴承箱(22)，Y轴第一轴承箱(22)外套装有Y轴节叉(16)的一个叉头，Y轴节叉(16)的另一个叉头套在Y轴第二轴承箱(23)上，Y轴第二轴承箱(23)安装在十字铰支架(3)的A端。

2.根据权利要求1所述的三自由度转动手控器，其特征在于，所述力传感器(2)为六位力传感器。

3.根据权利要求1所述的三自由度转动手控器，其特征在于，所述三自由度转动手控器外侧有防护罩(18)。

4.根据权利要求3所述的三自由度转动手控器，其特征在于，所述使能按钮(6)通过螺纹连接安装在操作手柄(1)上，操作手柄(1)轴向开有螺纹盲孔，操作手柄(1)侧面开有安装使能按钮(6)的螺纹孔，操作手柄(1)上的螺纹盲孔和侧面的螺纹孔相连通；所述操作手柄(1)的螺纹盲孔与手柄杆(24)一端的外螺纹配合，操作手柄(1)通过螺纹连接固定在手柄杆(24)的一端，手柄杆(24)的另一端通过螺纹连接固定在Y轴节叉(16)的中部；手柄杆(24)从操作手柄(1)到Y轴节叉(16)的方向，依次在手柄杆(24)外侧圆周上安装有风琴防护罩轴承(20)和防护罩轴承(25)，防护罩轴承(25)将手柄杆(24)和防护罩(18)连接在一起。

5.根据权利要求4所述的三自由度转动手控器，其特征在于，塔型风琴防护罩(19)小口端与风琴防护罩轴承(20)连接，塔型风琴防护罩(19)大口端与防护罩(18)通过螺钉连接。

6.根据权利要求1所述的三自由度转动手控器，其特征在于，所述与十字铰支架(3)A端连接的Y轴节叉(16)的节叉头上安装有配重装置(5)，配重装置(5)的重量与第二驱动装置的重量相等。

7.根据权利要求1所述的三自由度转动手控器的控制方法，其特征在于，包括以下步骤：

步骤一，接通电源将三自由度转动手控器打开，将当前位置作为操作手柄(1)的初始位置，从而完成操作手柄(1)的初始化；

步骤二，三自由度转动手控器底部的力传感器(2)读取到X，Y，Z三个方向上的扭力τ_x、τ_y、τ_z，将读取到的力数值通过滤波算法进行平滑处理，并通过力控算法计算得到电机目标位置，在X轴上的坐标tarposition_x、在Y轴上的坐标tarposition_y、在Z轴上的坐标tarposition_z，下发给对应的X轴电机(12)、Y轴电机(15)和Z轴电机(7),使电机产生与使用者施加给操作手柄(1)力方向相反的力，从而达到力反馈的效果，即

tarposition_x＝K_px·τ_x

tarposition_y＝K_py·τ_y

tarposition_z＝K_pz·τ_z

其中比例系数K_px、K_py、K_pz为手柄转动到限位时期望力矩与对应电机脉冲的比值；

步骤三，对目标位置进行平滑处理以防止手柄出现抖动现象，以10ms为一个周期，Δ_px.py.pz表示两个周期之间可以接受的最大电机位置差，位置差数值在Δ_px.py.pz内电机运动较为顺滑，位置差数值在Δ_px.py.pz外电机运动顿挫感强抖动明显，若上个计算周期的目标位置与本计算周期的目标位置的差小于最大值Δ_px.py.pz，即

|tarposition_x,y,z-tarposition′_x,y,z|Δ_px.py.pz

则本计算周期的目标位置保持不变，其中tarposition_x,y,z为当前目标位置，tarposition′_x,y,z为上个计算周期目标位置；

若上个计算周期的目标位置与本计算周期的目标位置的差大于最大值Δ_px.py.pz，即

|tarposition_x,y,z-tarposition′_x,y,z|Δ_px.py.pz

则令本计算周期的目标位置等于上个计算周期的目标位置加Δ_px.py.pz，即

tarposition_x.y.z＝tarposition′_x,y,z+Δ_px.py.pz；

步骤四，对手柄控制机器人下达速度指令，对机器人的笛卡尔空间姿态进行加减速规划，Δ_vx.vy.vz表示两个周期之间可以接受的最大速度指令差，速度指令差在Δ_vx.vy.vz范围内，被控机器人运动较为平滑，速度指令差在Δ_vx.vy.vz范围外，被控机器人可能在运动时会出现抖动的情况，若本周期的速度指令与上周期的速度指令的差值小于Δ_vx.vy.vz，即

|V_x,y,z-V′_x,y,z|Δ_vx.vy.vz

则本周期速度指令保持不变，其中V_x,y,z为当前速度指令速度，V′_x,y,z为上个计算周期速度指令速度；

若本周期的速度指令与上个计算周期的速度指令的差值大于Δ_vx.vy.vz，即

|V_x,y,z-V′_x,y,z|Δ_vx.vy.vz

则令本周期速度指令等于上个计算周期速度指令加Δ_vx.vy.vz,即

V_x.y.z＝V′_x,y,z+Δ_vx.vy.vz；

步骤五，对实际速度指令进行限速处理，当实际速度指令大于最大速度，令实际速度指令等于最大速度；

步骤六，将最终计算出的实际速度指令下发给目标装置进行控制。