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

权利要求书

1.一种四自由度机械手臂的控制方法，其特征在于，该控制方法基于控制系统实现；控制系统采用三级控制系统，包括上位机、运动控制器、底层控制器的控制架构；所述上位机给运动控制器发送位姿信息及运动模式命令后，运动控制器自行完成机械臂的路径规划及运动学解算得到规划结果，将规划结果发送至底层控制器，控制伺服电机运动；所述底层控制器包括运动单元与驱动放大器；该控制方法包括以下步骤：

第一步，上位机通过调用上位机接口程序，将操作模式、位姿信息以及运动命令发送给运动控制器；

所述的上位机接口程序主要负责PC-运动控制器间的通讯，使用Modbus TCP/IP通信协议进行两者之间的数据传输，使用c++完成接口程序的实现，接口程序能在Windows及Linux系统下应用；上位机在使用机械臂时只需调用c++接口程序即可将上位机控制命令发送至运动控制器；

第二步，运动控制器开启Modbus通信服务器，随后使EtherCAT通信网络使之与底层控制层建立通信连接；

第三步，运动控制器中的PLC数据收发程序完成对上位机发送的数据及命令的接收；接收到新命令或数据后，将其传递给PLC命令执行程序进行运动控制；

所述PLC数据收发程序与运动控制程序呈类似多线程工作的方式实现整体机械臂的控制；PLC数据收发程序通过定义各个P变量来读取寄存器中所收到的数据，实现上位机发送的机械臂位姿信息及控制命令的接收，将要实时反馈的机械臂运动过程中各关节位置、速度信息送到相应的寄存器当中，以供PC读取；

第四步，PLC命令执行程序根据相应的命令调用不同的子运动模块进行运动执行；在执行子运动模块前，PLC命令执行程序首先读取PLC数据收发程序所接受到的位姿信息，然后根据上位机指定的路径规划方式和电机个数调用运动学解算算法得到规划结果，并将规划结果直接发送给子运动模块；

所述运动学解算算法具有控制通用性，可实现基于矢量积法的速度雅克比矩阵及其逆解求解运算，通过迭代实现速度雅克比矩阵的SVD分解，并求得速度雅克比矩阵的逆解，解决直角坐标系下的速度与关节坐标系下的速度之间的转换；

所述的控制通用性，是基于改进的DH参数法所产生的DH矩阵来描述所控制的机械臂，每个机械臂连杆由以下矩阵描述：式中为坐标系{i}相对于坐标系{i-1}的变换矩阵，θ_i表示连杆i的关节角度，β_i连杆i的关节扭角，a_i表示连杆i的连杆长度，d_i表示连杆偏距，其中sθ_i＝sin(θ_i)，cθ_i＝cos(θ_i)，sβ_i＝sin(β_i)，cβ_i＝cos(β_i)；当上位机依次设置好每个机械臂连杆的DH参数后，运动控制器自动检测所控制的机械臂的自由度；假设所控制的机械臂具有n个自由度，则工具坐标系相对于基坐标系的齐次变换矩阵为调用矩阵运算子函数即可得到齐次变换矩阵，即完成正向运动学求解；在求解逆向运动学时，运动控制器调用迭代求解子函数完成逆向运动学的求解；

所述的速度雅克比矩阵，是基于矢量积法建立速度模型，对于移动关节：对于转动关节：其中，⁰v为基坐标系下的线速度，⁰ω为基坐标系下的角速度，为关节i的速度矢量，ⁱJ为关节i的单位关节速度引起的线速度和角速度，ⁱz为各关节坐标系z轴在基坐标系的表示，为机械臂末端坐标系原点相对于第i个关节的位置在基坐标系中的表示，为第i关节坐标系相对于基坐标系的变换关系，ⁱp_n为机械臂末端坐标系原点相对于第i个关节的位置；在求解速度雅克比矩阵及逆向运动学过程中运动控制器还具有容错/保护机制；

所述的容错/保护机制：在速度雅克比矩阵求逆问题中，完成雅克比矩阵求逆问题后首先对求解结果进行验算，如果求解错误，则各关节采用默认速度进行动作；由于机械臂使用的工作环境各不相同，导致其工作空间受到限制，当工作角度或工作空间即将超限时，机械臂会自动停止工作并向上位机反馈错误，避免机械臂的机械结构及电控系统的损坏；

第五步，子运动模块得到规划结果后，直接将数值发送给底层控制器，底层控制器会控制电机达到期望位姿，进而完成机械手臂的具体运动。

2.根据权利要求1所述的一种四自由度机械手臂的控制方法，其特征在于，第一步所述的操作模式，按坐标系形式可分直角坐标系模式与关节坐标系模式，按输入特点可分为增量式和绝对式，按轨迹规划方式有最优时间轨迹、最优能量轨迹、最短路径轨迹和用户自定义规划方式。