一种四自由度机械手臂的控制方法技术

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

A control method of four degree of freedom manipulator

The invention belongs to the technical field of manipulator control, and relates to a control method of a four degree of freedom manipulator. The control method is based on the control system. The control system adopts three-level control system, which is based on the control architecture of upper computer, motion controller and bottom controller. After the upper computer sends the position and attitude information and the motion mode command to the motion controller, the motion controller completes the path planning and kinematic solution of the manipulator by itself to get the planning result, and sends the planning result to the lower controller to control the servo motor movement. It can feedback command status, motor status, driver status, position, speed and other information in real time. And the system has control versatility, which can be applied to different manipulator configurations. It has speed forward-looking function, fault tolerance / protection mechanism and hardware protection function, which makes the operation process of the manipulator more stable, process and safety. The invention has high integration degree, greatly reduces the development cost of users, and is easy to promote and popularize robot technology.

【技术实现步骤摘要】
一种四自由度机械手臂的控制方法
本专利技术涉及一种四自由度机械手臂的控制方法，属于机械臂控制领域。
技术介绍
随着服务机器人的广泛应用，对机械臂的重量有了更高的要求，因此轻量型机械臂得到迅速发展。目前现有的控制方案，大多数采用上位机解算运动学及动力学，导致机械臂控制系统的实时性差，且控制方案的非通用性导致机器人控制系统开发难度的增加，使得多数机械臂研发工作在重复性造轮子。
技术实现思路
为了解决上述问题，本专利技术提供了一种四自由度机械手臂的控制方法，使得控制方案对于多数机械臂具有通用性，避免了机械臂在研发过程中重复造轮子的现象。为了达到上述目的，本专利技术的技术方案为：一种四自由度机械手臂的控制方法，该控制方法基于控制系统实现。所述的控制系统采用三级控制系统，基于上位机、运动控制器、底层控制器的控制架构实现，其中运动控制器是所述控制系统的核心。所述上位机给运动控制器发送位姿信息及运动模式命令后，运动控制器自行完成机械臂的路径规划及运动学解算得到规划结果，将规划结果发送至底层控制器，控制伺服电机运动。所述运动控制器中集成了运动学解算算法，具有运动通用性。所述底层控制器由运动单元与驱动放大器构成，运动单元是采用集成了抱闸和编码器的伺服电机。该控制方法具体包括以下步骤：第一步，上位机通过调用上位机接口程序，按照一定的数据格式将操作模式、位姿信息以及运动命令发送给运动控制器。所述的上位机接口程序主要负责PC-运动控制器间的通讯，使用ModbusTCP/IP通信协议进行两者之间的数据传输，使用c++完成接口程序的实现，接口程序能在Windows及Linux系统下应用。上位机在使用机械臂时只需调用c++接口程序即可将上位机控制命令发送至运动控制器。所述的操作模式，按坐标系形式可分直角坐标系模式与关节坐标系模式，按输入特点可分为增量式和绝对式，按轨迹规划方式有最优时间轨迹、最优能量轨迹、最短路径轨迹和用户自定义规划方式。各模式均可由用户通过代码编号进行选择使用，控制灵活性高。第二步，运动控制器开启Modbus通信服务器，随后使能EtherCAT通信网络使之与底层控制层建立通信连接。第三步，运动控制器中的PLC数据收发程序完成对上位机发送的数据及命令的接收。接收到新命令或数据后，将其传递给PLC命令执行程序进行运动控制。所述PLC数据收发程序与运动控制程序呈类似多线程工作的方式实现整体机械臂的控制。PLC数据收发程序通过定义各个P变量来读取寄存器中所收到的数据，实现上位机发送的机械臂位姿信息及控制命令的接收，将要实时反馈的机械臂运动过程中各关节位置、速度等信息送到相应的ModbusServerBuffer寄存器当中，以供PC读取。第四步，PLC命令执行程序根据相应的命令调用不同的子运动模块进行运动执行。在执行子运动模块前，PLC命令执行程序首先读取PLC数据收发程序所接受到的位姿信息，然后根据上位机指定的路径规划方式和电机个数调用运动学解算算法得到规划结果，并将规划结果直接发送给子运动模块。所述运动学解算算法具有控制通用性，可实现基于矢量积法的速度雅克比矩阵及其逆解求解运算，通过迭代实现速度雅克比矩阵的SVD分解，进而由SVD(Jacobi)＝USV(U、V为正交矩阵)以及矩阵特性(正交矩阵的逆等于其本身的转置；对角矩阵的逆等于其本身非零元素求倒)求得速度雅克比矩阵的逆解，解决了直角坐标系下的速度与关节坐标系下的速度之间的转换。所述的控制通用性，是基于改进的DH参数法所产生的DH矩阵来描述所控制的机械臂，每个机械臂连杆由以下矩阵描述：式中为坐标系{i}相对于坐标系{i-1}的变换矩阵，θi表示连杆i的关节角度，βi连杆i的关节扭角，ai表示连杆i的连杆长度，di表示连杆偏距，其中sθi＝sin(θi)，cθi＝cos(θi)，sβi＝sin(βi)，cβi＝cos(βi)。当上位机依次设置好每个机械臂连杆的DH参数后，运动控制器自动检测所控制的机械臂的自由度。假设所控制的机械臂具有n个自由度，则工具坐标系相对于基坐标系的齐次变换矩阵为调用矩阵运算子函数即可得到齐次变换矩阵，即完成正向运动学求解；在求解逆向运动学时，运动控制器调用迭代求解子函数完成逆向运动学的求解。所述的速度雅克比矩阵，是基于矢量积法建立速度模型，通常的情况下，对于移动关节：对于转到关节：其中，0v为基坐标系下的线速度，0ω为基坐标系下的角速度，为关节i的速度矢量，iJ为关节i的单位关节速度引起的线速度和角速度，iz为各关节坐标系z轴在基坐标系的表示，为机械臂末端坐标系原点相对于第i个关节的位置在基坐标系中的表示，为第i关节坐标系相对于基坐标系的变换关系，ipn为机械臂末端坐标系原点相对于第i个关节的位置。因此具有n自由度的机械臂通过矢量积法得到的雅克比矩阵第i列表示为：则雅克比矩阵的形式为：在求解速度雅克比矩阵及逆向运动学过程中运动控制器还具有容错/保护机制。所述的容错/保护机制，包括软件防护和硬件防护。对于软件防护功能，在速度雅克比矩阵求逆问题中，完成雅克比矩阵求逆问题后首先对求解结果进行验算，如果求解错误，则各关节采用默认速度进行动作；对于硬件防护功能，由于机械臂使用的工作环境各不相同，导致其工作空间受到限制。当工作角度或工作空间即将超限时，机械臂会自动停止工作并向上位机反馈错误，避免了机械臂的机械结构及电控系统的损坏。所述PLC命令执行程序主要负责执行上位机给运动控制器传达的控制命令，所述的子运动模块负责将规划后的结果发送至底层控制器，完成各电机的运动控制。第五步，子运动模块得到规划结果后，直接将数值发送给底层控制器，底层控制器会控制电机达到期望位姿，进而完成机械手臂的具体运动。底层控制器通过PVT模式来实现速度前瞻功能，能够使轨迹规划点更流畅的运行且能保证机械臂运行过程中的平稳性。本专利技术的有益效果为：控制系统层次分明，控制模式多样化，充分利用了运动控制器本身插补功能强的特点，使得轨迹规划问题得到有效的解决。且由于具有控制通用性，控制实时性好，使得机械臂开发成本得到大大降低，更易普及和推广。附图说明图1为控制系统结构示意图；图2为控制系统通信连接程序流程图。具体实施方式一种四自由度机械手臂的控制方法，具体实施步骤如下：第一步，上位机通过调用上位机接口程序，按照一定的数据格式将操作模式、位姿信息以及运动命令发送给运动控制器。如图1所示，所述的上位机接口程序主要负责PC-运动控制器间的通讯，使用ModbusTCP/IP通信协议进行两者之间的数据传输，使用c++完成接口程序的实现，接口程序能在Windows及Linux系统下应用。上位机在使用机械臂时只需调用c++接口程序即可将上位机控制命令发送至运动控制器。运动控制器与上位机的通信，本着数据优先级高于命令优先级的原则，其ModbusTCP/IP通信协议数据内容格式如下：位姿速度本文档来自技高网...

【技术保护点】
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矩阵来描述所控制的机械臂，每个机械臂连杆由以下矩阵描述：...

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

【专利技术属性】
技术研发人员：杜宇，邓蕊，
申请(专利权)人：大连大华中天科技有限公司，
类型：发明
国别省市：辽宁;21