一种平面双五连杆机构的控制方法及控制系统技术方案

本申请提供了一种平面双五连杆机构的控制方法及控制系统，该方法包括：建立平面双五连杆机构质心的逻辑轴到物理轴的力学解耦矩阵；基于旋转角度、电机转轴半径及曲柄长度确定机械臂的当前位置；利用机械臂的当前位置及机械臂与质心之间的物理关系，确定机构质心的当前位置；基于机构质心的当前位置及参考位置，确定机构质心的逻辑轴力；将逻辑轴力以及机械臂的当前位置输入力学解耦矩阵，确定旋转电机的电机转矩，按照电机转矩对旋转电机进行控制，通过旋转电机带动平面双五连杆机构运转。通过采用上述平面双五连杆机构的控制方法及控制系统，解决了现有运动台的控制方法中响应速度慢、控制自由度低及旋转角度行程有限的问题。问题。问题。

【技术实现步骤摘要】
一种平面双五连杆机构的控制方法及控制系统


[0001]本申请涉及自动控制
，具体而言，涉及一种平面双五连杆机构的控制方法及控制系统。

技术介绍

[0002]在工业生产领域，运动台的控制精度直接决定了工业设备的加工精度。运动台在运动过程中对其非运动部分具有很强的冲击作用，严重影响了运动台的反馈控制精度。为了提高运动台控制精度，设置了与运动台连接的平衡机构，通过平衡机构实现对运动台的控制，以消除冲击作用，因此，如何对平衡机构进行有效控制，成了提高运动台控制精度的关键。目前，平衡机构通常为以下两种：一种是将减震器和直线电机驱动作为平衡机构，在运动台与基础框架之间增加减震器，以减小振动。另一种是采用直线电机驱动的平衡机构，需要布置多个直线电机来对平面自由度进行控制，其控制策略相对简单，出力解耦易于实现。
[0003]然而，针对第一种平衡机构的控制方法中，主要通过减震器来消减冲击作用，由于减震器仅能在单一自由度上起作用，导致控制自由度单一的问题，且控制过程中的响应速度较慢。针对第二种平衡机构的控制方法中，由于直线电机的驱动方向较为单一，在进行旋转控制时存在旋转角度行程有限的问题。

技术实现思路

[0004]有鉴于此，本申请的目的在于提供一种平面双五连杆机构的控制方法及控制系统，以解决现有运动台的控制方法中响应速度慢、控制自由度低及旋转角度行程有限的问题。
[0005]第一方面，本申请实施例提供了一种平面双五连杆机构的控制方法，平面双五连杆机构包括旋转电机、机械臂及曲柄，控制方法包括：/>[0006]基于平面双五连杆机构的尺寸参数、机械臂与曲柄之间的平面力学关系，建立平面双五连杆机构质心的逻辑轴到物理轴的力学解耦矩阵；
[0007]获取旋转电机的旋转角度，基于旋转角度、电机转轴半径及曲柄长度确定机械臂的当前位置；
[0008]利用机械臂的当前位置及机械臂与质心之间的物理关系，确定平面双五连杆机构质心的当前位置；
[0009]基于平面双五连杆机构质心的当前位置以及参考位置，确定位置误差及位置误差对应的平面双五连杆机构质心的逻辑轴力；
[0010]将逻辑轴力以及机械臂的当前位置输入力学解耦矩阵，确定旋转电机的电机转矩，并按照电机转矩对旋转电机进行控制，通过旋转电机带动平面双五连杆机构运转。
[0011]可选地，平面双五连杆机构包括第一五连杆机构及第二五连杆机构，第一五连杆机构包括第一旋转电机、第二旋转电机、第一机械臂、第一曲柄、第二曲柄、第一连杆及第二
连杆，第二五连杆机构包括第三旋转电机、第四旋转电机、第二机械臂、第三曲柄、第四曲柄、第三连杆及第四连杆；基于平面双五连杆机构的尺寸参数、机械臂与曲柄之间的平面力学关系，建立平面双五连杆机构质心的逻辑轴到物理轴的力学解耦矩阵，包括：根据平面双五连杆机构的尺寸参数建立第一映射矩阵，第一映射矩阵是指平面双五连杆机构质心与机械臂之间的力学映射矩阵；根据机械臂与曲柄之间的平面力学关系，建立第二映射矩阵以及角度解算关系，第二映射矩阵是指机械臂与连杆之间的力学映射矩阵，角度结算关系是指连杆角度与旋转电机的旋转角度之间的解算关系；对曲柄末端进行径向及切向上的力学分析，确定电机转矩计算公式；基于第一映射矩阵、第二映射矩阵、角度解算关系及电机转矩计算公式，建立力学解耦矩阵。
[0012]可选地，根据平面双五连杆机构的尺寸参数建立第一映射矩阵，包括：基于第一机械臂及第二机械臂在X方向的受力之和与平面双五连杆机构质心在X方向的受力之间的相等关系，建立第一质心受力等式；基于第一机械臂及第二机械臂在Y方向的受力之和与平面双五连杆机构质心在Y方向的受力之间的相等关系，建立第二质心受力等式；将第二机械臂与第一机械臂在Y方向的受力之差与第一质心距离的乘积作为第一质心扭矩，第一质心距离是指X方向上第一机械臂原点或者第二机械臂原点与平面双五连杆机构质心原点之间的距离；将第二机械臂与第一机械臂在X方向的受力之差与第二质心距离的乘积作为第二质心扭矩，第二质心距离是指Y方向上第一机械臂原点或者第二机械臂原点与平面双五连杆机构质心原点之间的距离；基于第一质心扭矩与第二质心扭矩之和与平面双五连杆机构的质心扭矩之间的相等关系，构建质心扭矩等式；对第一质心受力等式、第二质心受力等式及质心扭矩等式进行矩阵变换生成第一映射矩阵。
[0013]可选地，根据机械臂与曲柄之间的平面力学关系，建立第二映射矩阵以及角度解算关系，包括：将第一连杆角度的余弦值与第一连杆对第一机械臂的作用力的乘积作为第一机械臂X方向的第一分力，将第二连杆角度的余弦值与第二连杆对第一机械臂的作用力的乘积作为第一机械臂X方向的第二分力，第一连杆角度是指第一连杆与X轴正方向之间逆时针方向的夹角，第二连杆角度是指第二连杆与X轴正方向之间逆时针方向的夹角；基于第一机械臂X方向的第一分力与第一机械臂X方向的第二分力之和与第一机械臂在X方向的受力之间的相等关系，建立第一机械臂的第一受力等式；将第一连杆角度的正弦值与第一连杆对第一机械臂的作用力的乘积作为第一机械臂Y方向的第一分力，将第二连杆角度的正弦值与第二连杆对第一机械臂的作用力的乘积作为第一机械臂Y方向的第二分力；基于第一机械臂Y方向的第一分力与第一机械臂Y方向的第二分力之和与第一机械臂在Y方向的受力之间的相等关系，建立第一机械臂的第二受力等式；将第三连杆角度的余弦值与第三连杆对第二机械臂的作用力的乘积作为第二机械臂X方向的第一分力，将第四连杆角度的余弦值与第四连杆对第二机械臂的作用力的乘积作为第二机械臂X方向的第二分力，第三连杆角度是指第三连杆与X轴正方向之间逆时针方向的夹角，第四连杆角度是指第四连杆与X轴正方向之间逆时针方向的夹角；基于第二机械臂X方向的第一分力与第二机械臂X方向的第二分力之和与第二机械臂在X方向的受力之间的相等关系，建立第二机械臂的第一受力等式；将第三连杆角度的正弦值与第三连杆对第二机械臂的作用力的乘积作为第二机械臂Y方向的第一分力，将第四连杆角度的正弦值与第四连杆对第二机械臂的作用力的乘积作为第二机械臂Y方向的第二分力；基于第二机械臂Y方向的第一分力与第二机械臂Y方向的
第二分力之和与第二机械臂在Y方向的受力之间的相等关系，建立第二机械臂的第二受力等式；对第一机械臂的第一受力等式、第一机械臂的第二受力等式、第二机械臂的第一受力等式以及第二机械臂的第二受力等式进行矩阵转换生成第二映射矩阵。
[0014]可选地，第一机械臂的当前位置包括第一机械臂X轴坐标、第一机械臂Y轴坐标，第二机械臂的当前位置包括第二机械臂X轴坐标、第二机械臂Y轴坐标；根据机械臂与曲柄之间的平面力学关系，建立第二映射矩阵以及角度解算关系，还包括：将第一机械臂Y轴坐标作为被减数，将第一转轴半径与第一旋转角度的正弦值的乘积和Y方向上第一旋转电机与第一机械臂原点之间的距离之和作为减数，将被减数与减数的差值作为第一纵向差值，第一转轴半径为第一旋转电机的转轴半径，第一旋转角度为第一旋转电机的旋转角度；将第一纵向差值与第一连杆长度的比值作为第一纵向比值，基于第一纵本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种平面双五连杆机构的控制方法，其特征在于，所述平面双五连杆机构包括旋转电机、机械臂及曲柄，所述控制方法包括；基于所述平面双五连杆机构的尺寸参数、机械臂与曲柄之间的平面力学关系，建立平面双五连杆机构质心的逻辑轴到物理轴的力学解耦矩阵；获取所述旋转电机的旋转角度，基于所述旋转角度、电机转轴半径及曲柄长度确定机械臂的当前位置；利用所述机械臂的当前位置及机械臂与质心之间的物理关系，确定平面双五连杆机构质心的当前位置；基于所述平面双五连杆机构质心的当前位置以及参考位置，确定位置误差及所述位置误差对应的平面双五连杆机构质心的逻辑轴力；将所述逻辑轴力以及机械臂的当前位置输入所述力学解耦矩阵，确定所述旋转电机的电机转矩，并按照所述电机转矩对所述旋转电机进行控制，通过所述旋转电机带动平面双五连杆机构运转。2.根据权利要求1所述的控制方法，其特征在于，所述平面双五连杆机构包括第一五连杆机构及第二五连杆机构，所述第一五连杆机构包括第一旋转电机、第二旋转电机、第一机械臂、第一曲柄、第二曲柄、第一连杆及第二连杆，所述第二五连杆机构包括第三旋转电机、第四旋转电机、第二机械臂、第三曲柄、第四曲柄、第三连杆及第四连杆；所述基于所述平面双五连杆机构的尺寸参数、机械臂与曲柄之间的平面力学关系，建立平面双五连杆机构质心的逻辑轴到物理轴的力学解耦矩阵，包括：根据所述平面双五连杆机构的尺寸参数建立第一映射矩阵，所述第一映射矩阵是指平面双五连杆机构质心与机械臂之间的力学映射矩阵；根据机械臂与曲柄之间的平面力学关系，建立第二映射矩阵以及角度解算关系，所述第二映射矩阵是指机械臂与连杆之间的力学映射矩阵，所述角度结算关系是指连杆角度与旋转电机的旋转角度之间的解算关系；对曲柄末端进行径向及切向上的力学分析，确定电机转矩计算公式；基于所述第一映射矩阵、所述第二映射矩阵、所述角度解算关系及所述电机转矩计算公式，建立力学解耦矩阵。3.根据权利要求2所述的控制方法，其特征在于，所述根据所述平面双五连杆机构的尺寸参数建立第一映射矩阵，包括：基于所述第一机械臂及所述第二机械臂在X方向的受力之和与所述平面双五连杆机构质心在X方向的受力之间的相等关系，建立第一质心受力等式；基于所述第一机械臂及所述第二机械臂在Y方向的受力之和与所述平面双五连杆机构质心在Y方向的受力之间的相等关系，建立第二质心受力等式；将所述第二机械臂与所述第一机械臂在Y方向的受力之差与第一质心距离的乘积作为第一质心扭矩，所述第一质心距离是指X方向上第一机械臂原点或者第二机械臂原点与平面双五连杆机构质心原点之间的距离；将所述第二机械臂与所述第一机械臂在X方向的受力之差与第二质心距离的乘积作为第二质心扭矩，所述第二质心距离是指Y方向上第一机械臂原点或者第二机械臂原点与平面双五连杆机构质心原点之间的距离；
基于所述第一质心扭矩与所述第二质心扭矩之和与平面双五连杆机构的质心扭矩之间的相等关系，构建质心扭矩等式；对所述第一质心受力等式、所述第二质心受力等式及所述质心扭矩等式进行矩阵变换生成第一映射矩阵。4.根据权利要求2所述的控制方法，其特征在于，所述根据机械臂与曲柄之间的平面力学关系，建立第二映射矩阵以及角度解算关系，包括：将第一连杆角度的余弦值与第一连杆对第一机械臂的作用力的乘积作为第一机械臂X方向的第一分力，将第二连杆角度的余弦值与第二连杆对第一机械臂的作用力的乘积作为第一机械臂X方向的第二分力，所述第一连杆角度是指第一连杆与X轴正方向之间逆时针方向的夹角，所述第二连杆角度是指第二连杆与X轴正方向之间逆时针方向的夹角；基于所述第一机械臂X方向的第一分力与所述第一机械臂X方向的第二分力之和与所述第一机械臂在X方向的受力之间的相等关系，建立第一机械臂的第一受力等式；将第一连杆角度的正弦值与第一连杆对第一机械臂的作用力的乘积作为第一机械臂Y方向的第一分力，将第二连杆角度的正弦值与第二连杆对第一机械臂的作用力的乘积作为第一机械臂Y方向的第二分力；基于所述第一机械臂Y方向的第一分力与所述第一机械臂Y方向的第二分力之和与所述第一机械臂在Y方向的受力之间的相等关系，建立第一机械臂的第二受力等式；将第三连杆角度的余弦值与第三连杆对第二机械臂的作用力的乘积作为第二机械臂X方向的第一分力，将第四连杆角度的余弦值与第四连杆对第二机械臂的作用力的乘积作为第二机械臂X方向的第二分力，所述第三连杆角度是指第三连杆与X轴正方向之间逆时针方向的夹角，所述第四连杆角度是指第四连杆与X轴正方向之间逆时针方向的夹角；基于所述第二机械臂X方向的第一分力与所述第二机械臂X方向的第二分力之和与所述第二机械臂在X方向的受力之间的相等关系，建立第二机械臂的第一受力等式；将第三连杆角度的正弦值与第三连杆对第二机械臂的作用力的乘积作为第二机械臂Y方向的第一分力，将第四连杆角度的正弦值与第四连杆对第二机械臂的作用力的乘积作为第二机械臂Y方向的第二分力；基于所述第二机械臂Y方向的第一分力与所述第二机械臂Y方向的第二分力之和与所述第二机械臂在Y方向的受力之间的相等关...

【专利技术属性】
技术研发人员：李红钢，杨际腾，陈国兴，贾闯闯，
申请(专利权)人：北京半导体专用设备研究所中国电子科技集团公司第四十五研究所，
类型：发明
国别省市：