垂直多关节串联结构的机械臂控制方法与流程

本发明涉及工业机器人控制，具体涉及一种垂直多关节串联结构的机械臂控制方法。

背景技术：

1、垂直多关节串联结构机械臂是工业机器人中最常见的构型之一，其通过多个旋转关节串联连接，实现末端执行器在三维空间中的灵活运动。该类机械臂广泛应用于码垛、搬运、装配等工业场景。

2、多轴协同运动的轨迹规划效率低。传统控制方法中，各关节轴的运动规划相互独立，缺乏轴间的协同调度机制。在码垛作业中，机械臂需要在取料位和放料位之间频繁往复运动，现有方法往往采用串行执行方式，即一个轴运动完成后另一个轴才开始运动，导致节拍时间长、生产效率低。部分方案采用五轴联动正反解算法实现多轴协同，但其轨迹规划缺乏灵活的步序调度机制，难以适应复杂多变的码垛需求。

3、现有控制系统对设备运行状态的管理较为粗糙，缺乏精细化的状态定义和状态转换控制。在发生报警、急停等异常情况时，系统往往直接停机，缺乏有序的停止、暂停、恢复等状态转换流程。

技术实现思路

1、为了解决现有技术中的技术问题和缺点，本发明创造提供一种垂直多关节串联结构的机械臂控制方法，实现多轴协同运动的动态步序调度，优化运动轨迹，提高码垛效率。

2、为实现上述目的及其他相关目的，本发明采用如下的技术方案：

3、一种垂直多关节串联结构的机械臂控制方法，其特征在于，包括以下步骤：

4、步骤s1：系统初始化与模式配置，响应于系统上电或复位指令，执行系统初始化任务；

5、步骤s2：建立基于packml标准的设备状态管理模型，定义设备的状态集合和状态转换规则；所述状态集合包括空闲态（idle）、启动中态（starting）、执行态（execute）、完成中态（completing）、完成态（complete）、停止中态（stopping）、停止态（stopped）、中止中态（aborting）、中止态（aborted）、复位中态（resetting）、暂停中态（suspending）、暂停态（suspended）、恢复中态（unsuspending）、保持中态（holding）、保持态（held）、恢复中态（unholding）和清除中态（clearing）；根据当前运行模式和用户指令，控制设备在各状态之间有序转换；

6、步骤s3：多轴运动轨迹的动态步序规划，获取码垛作业的目标参数，所述目标参数至少包括码垛行数、列数、层数以及物料尺寸参数；根据所述目标参数生成运动轨迹的步序序列，每个步序定义各运动轴的目标位置、运动速度、加减速参数和轴间优先级；在步序执行过程中，根据各轴的实时位置状态和外部条件信号的反馈，动态调整当前步序的执行进度和轴间协同关系；包括：

7、为每个步序中的每个运动轴分配执行优先级，优先级高的轴优先启动运动；

8、对于同一优先级的多轴，采用同步启动方式，使多个轴同时开始运动；

9、根据前一轴的运动完成状态，触发下一优先级轴的运动启动；

10、在步序切换时，判断当前步序的所有条件是否满足，若满足则进入下一步序，若不满足则等待条件满足或执行跳步操作。

11、优选的，步骤s3中所述“动态调整当前步序的执行进度”包括步序跳转机制：当检测到当前步序的执行条件不满足且预判该条件在预设时间内无法满足时，跳过当前步序的部分或全部运动指令，直接执行下一步序；所述步序跳转机制至少应用于以下场景：

12、（a）当物料感应信号表明取料位无物料时，跳过取料步序，直接执行待机步序；

13、（b）当放料位被占用时，跳过放料步序，执行等待步序；

14、（c）当托盘已满时，跳过当前码垛步序，执行托盘更换步序。

15、通过增加了步序跳转机制，使系统具备自适应调节能力。当外部条件发生变化（如缺料、堵料、托盘已满）时，系统可跳过不必要的运动步序，避免无效运动。这一机制与动态步序调度相结合，使机械臂能够根据实时工况灵活调整作业流程，在非理想工况下仍能保持较高的运行效率。

16、优选的，步骤s2中所述状态转换规则具体包括：

17、（a）空闲态（idle）接收到启动指令后，转换至启动中态（starting）；启动中态完成初始化准备工作后，自动转换至执行态（execute）；

18、（b）执行态（execute）接收到停止指令后，转换至停止中态（stopping）；停止中态完成轴停止和状态保存后，自动转换至停止态（stopped）；

19、（c）执行态（execute）接收到暂停指令后，转换至暂停中态（suspending）；暂停中态完成运动暂停和状态保存后，自动转换至暂停态（suspended）；暂停态接收到恢复指令后，转换至恢复中态（unsuspending），然后自动恢复至执行态（execute）；

20、（d）执行态（execute）接收到中止指令或检测到严重报警时，转换至中止中态（aborting）；中止中态完成紧急停止和故障记录后，自动转换至中止态（aborted）；中止态接收到复位指令后，转换至复位中态（resetting），复位完成后自动转换至空闲态（idle）；

21、（e）任何状态检测到急停信号时，立即转换至中止中态（aborting）。

22、优选的，还包过步骤s4：在执行码垛作业过程中，并行执行多层级安全保护任务，包括：

23、软限位保护层：获取各运动轴的实时位置，将所述实时位置与预设的正向软限位和反向软限位进行比较，若超出限位范围则触发报警并停止该轴运动；

24、扭矩碰撞检测层：获取各运动轴的实时扭矩值，将所述实时扭矩值与预设的空载扭矩阈值和满载扭矩阈值进行比较，若超出阈值则触发碰撞报警并停止相应轴的运动；

25、急停联动层：实时监测急停信号和防撞感应信号的状态，若检测到急停信号或防撞感应信号触发，则立即停止所有运动轴和关联设备的运行；

26、气缸状态监测层：实时监测各气缸的原位信号和动位信号，若在预设时间内未检测到目标位置的到位信号，则触发气缸到位超时报警。

27、优选的，步骤s4中所述软限位保护层还包括动态软限位调整机制：

28、根据机械臂的当前运行模式自动调整软限位范围：在手动模式下，采用第一软限位范围，该范围小于机械臂的硬限位行程；在生产模式下，采用第二软限位范围，该范围大于第一软限位范围但小于硬限位行程；

29、当检测到轴位置接近软限位边界时，自动降低该轴的运动速度；当轴位置超出软限位范围时，触发报警并停止该轴运动。

30、本方案中对状态转换规则进行了具体化限定，特别是定义了暂停/恢复机制（规则c）和分级停止机制（规则b与规则d的区分）。暂停机制允许设备在不停机的情况下临时中断作业并快速恢复，而中止机制用于处理严重故障。这一分级处理与步骤s4形成联动：轻微异常触发暂停，严重故障触发中止。通过这种分级响应，既保证了安全性，又最大限度减少了异常情况下的生产损失。不同模式下采用不同的限位范围——手动模式（调试场景）限位更严格以保护设备和人员，生产模式（正常运行场景）限位适当放宽以充分利用工作空间。这一机制与步骤s1和扭矩碰撞检测形成多层级防护：软限位作为第一道防线预防超程，扭矩检测作为第二道防线检测碰撞。

31、优选的，步骤s4中所述扭矩碰撞检测层还包括空载扭矩自学习机制：在机械臂首次上电或定期维护时，执行空载扭矩自学习步骤：控制各运动轴以预设速度在预设行程范围内往复运动，记录各轴在不同位置的扭矩值，建立空载扭矩基准曲线；在实际运行过程中，将实时扭矩值与所述空载扭矩基准曲线进行比较，若实时扭矩值超出空载扭矩基准值预设倍率，则判定为发生碰撞，触发碰撞报警；所述预设倍率根据轴的类型分别设置，z轴的预设倍率小于x轴和y轴的预设倍率。

32、本方案通过建立空载扭矩基准曲线而非使用固定阈值，系统能够自适应不同机械状态（如润滑条件变化、机械磨损等）下的正常扭矩波动，减少误报警。同时，z轴采用更小的倍率阈值（因为z轴负责升降，碰撞后果更严重），体现了差异化的安全策略。这一机制与步骤s4相结合，提高了碰撞检测的准确性和可靠性。

33、优选的，步骤s1行系统初始化任务，包括：输入任务获取外部输入信号状态、通讯任务建立与上位机和从站设备的通信连接、系统诊断任务检测各硬件模块的健康状态；配置系统运行模式，所述运行模式至少包括生产模式、手动模式和初始化模式；其中，所述初始化模式用于系统首次上电或复位后的轴回原和参数初始化，所述手动模式用于人工调试和点位调试，所述生产模式用于执行自动码垛作业。

34、优选的，所述方法还包括事件管理与报警分级处理机制：

35、定义事件优先级等级，所述优先级等级至少包括紧急事件、警告事件和信息事件；

36、将各类异常情况映射至对应的事件优先级等级：

37、急停触发、伺服故障、碰撞检测触发归类为紧急事件，触发后立即执行中止流程；

38、物料缺料超时、气缸到位超时、输送带运行超时归类为警告事件，触发后记录报警信息并保持当前状态等待人工确认；

39、设备状态变更、步序完成等归类为信息事件，仅用于状态显示和日志记录；

40、当多个事件同时发生时，按照优先级从高到低的顺序依次处理；警告事件在触发后，若在预设时间内未被确认且条件已恢复，则自动清除。

41、本方案中紧急事件触发状态机的中止流程，警告事件触发暂停流程，信息事件仅作记录。这种分级处理与状态转换规则相呼应，共同构成了从事件检测到状态响应再到动作执行的完整闭环。通过优先级排序和自动清除机制，避免了报警信息的堆积和误报对生产的影响。

42、优选的，步骤s1中所述系统初始化任务还包括轴使能管理和参数初始化：在系统初始化阶段，向各运动轴的伺服驱动器发送使能指令，使各轴进入可操作状态；为各运动轴配置运动参数，所述运动参数至少包括加速度、减速度和点动速度；根据运行模式设置速度倍率：在生产模式下，根据预设的自动速度倍率调整各轴的运动速度；在手动模式下，根据预设的点动速度倍率调整各轴的点动速度；所述速度倍率的取值范围为0至1之间，当速度倍率超出该范围时，自动修正为默认值。

43、本方案中通过将速度倍率限制在0至1之间并进行自动修正，避免了因参数设置不当导致的异常运动。这一机制与模式配置（步骤s1）和步序规划（步骤s3）相结合：不同模式下应用不同的速度倍率，倍率值又直接影响步序中各轴的运动速度参数，实现了从模式选择到参数配置再到运动执行的完整参数传递链。

44、与现有技术相比，本发明的有益效果如下：

45、1.本发明采用基于packml标准的状态管理模型，将设备控制划分为输入任务、通讯任务、系统诊断、接口任务、输出映射等多个标准化模块，各模块功能清晰、耦合度低，便于系统的维护、扩展和跨平台移植。

46、2.本发明通过动态步序调度机制，实现了多轴的分优先级协同运动。与传统串行控制方式相比，x轴和y轴可同步运动，z轴根据优先级策略提前或延后运动，大幅减少了码垛循环时间。

47、3.本发明定义了17种标准状态及完整的状态转换规则，实现了从正常生产到异常处理再到恢复的全流程状态管理。当发生报警或急停时，系统按照预定义的状态转换路径有序停止，避免了无序停机带来的设备损伤和数据丢失。

48、本技术其他附加的优点和有益效果将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本技术的实践了解到。