[发明专利]一种四自由度机械手臂的控制方法

摘要

本发明属于机械手臂控制技术领域，涉及一种四自由度机械手臂的控制方法。控制方法基于控制系统实现，控制系统采用三级控制系统，基于上位机、运动控制器、底层控制器的控制架构实现。所述上位机给运动控制器发送位姿信息及运动模式命令后，运动控制器自行完成机械臂的路径规划及运动学解算得到规划结果，将规划结果发送至底层控制器，控制伺服电机运动。能实时的反馈命令状态、电机状态、驱动器状态及位置、速度等信息。且系统具有控制通用性，能适用于不同构型的机械臂构型。具有速度前瞻功能和容错/保护机制及硬件保护功能，使机械臂运行过程更平稳、流程、安全。本发明集成度高，极大程度的降低用户的开发成本，易于机器人技术的推广和普及。

主权项

1.一种四自由度机械手臂的控制方法，其特征在于，该控制方法基于控制系统实现；控制系统采用三级控制系统，包括上位机、运动控制器、底层控制器的控制架构；所述上位机给运动控制器发送位姿信息及运动模式命令后，运动控制器自行完成机械臂的路径规划及运动学解算得到规划结果，将规划结果发送至底层控制器，控制伺服电机运动；所述底层控制器包括运动单元与驱动放大器；该控制方法包括以下步骤：/n第一步，上位机通过调用上位机接口程序，将操作模式、位姿信息以及运动命令发送给运动控制器；/n所述的上位机接口程序主要负责PC-运动控制器间的通讯，使用Modbus TCP/IP通信协议进行两者之间的数据传输，使用c++完成接口程序的实现，接口程序能在Windows及Linux系统下应用；上位机在使用机械臂时只需调用c++接口程序即可将上位机控制命令发送至运动控制器；/n第二步，运动控制器开启Modbus通信服务器，随后使EtherCAT通信网络使之与底层控制层建立通信连接；/n第三步，运动控制器中的PLC数据收发程序完成对上位机发送的数据及命令的接收；接收到新命令或数据后，将其传递给PLC命令执行程序进行运动控制；/n所述PLC数据收发程序与运动控制程序呈类似多线程工作的方式实现整体机械臂的控制；PLC数据收发程序通过定义各个P变量来读取寄存器中所收到的数据，实现上位机发送的机械臂位姿信息及控制命令的接收，将要实时反馈的机械臂运动过程中各关节位置、速度信息送到相应的寄存器当中，以供PC读取；/n第四步，PLC命令执行程序根据相应的命令调用不同的子运动模块进行运动执行；在执行子运动模块前，PLC命令执行程序首先读取PLC数据收发程序所接受到的位姿信息，然后根据上位机指定的路径规划方式和电机个数调用运动学解算算法得到规划结果，并将规划结果直接发送给子运动模块；/n所述运动学解算算法具有控制通用性，可实现基于矢量积法的速度雅克比矩阵及其逆解求解运算，通过迭代实现速度雅克比矩阵的SVD分解，并求得速度雅克比矩阵的逆解，解决直角坐标系下的速度与关节坐标系下的速度之间的转换；/n所述的控制通用性，是基于改进的DH参数法所产生的DH矩阵来描述所控制的机械臂，每个机械臂连杆由以下矩阵描述：