CN113865415A - 一种五轴式弹药装填机器人的控制方法 - Google Patents

Abstract

本发明公开了一种五轴式弹药装填机器人的控制方法。该方法主要实现步骤是：1、确定待机位、取弹位、放弹位和补给位的机器人位置姿态；2、自动装填流程；3、自动补给流程；该方法的执行流程避免了环境干涉，保证执行动作安全可靠；提出的各轴之间的同步控制方法有效提高了任务执行的效率；针对第四运动轴的特殊结构形式，提出的控制指令计算方法，有效解决了如何控制顶升电缸运动来实现第四运动轴的准确旋转。

Description

一种五轴式弹药装填机器人的控制方法

技术领域

本发明涉及一种机器人控制方法，具体涉及一种五轴式弹药装填机器人的控制方法。

背景技术

目前我国车载火炮的装填采用人工方式实现，存在士兵劳动强度大，效率低等问题，因此设计了一款五轴式弹药装填机器人取代人工装填，减少了人员数量和劳动强度。

该机器人的结构示意简图如图1所示，该五轴式弹药装填机器人共有五个运动轴，其中，第一运动轴为水平横移轴，第二运动轴为大臂旋转轴，第三运动轴为小臂旋转轴，第四运动轴为弹托旋转轴，第五运动轴为弹托夹爪控制轴，五个轴依次串行连接；

其中，第一运动轴由电机连接减速机驱动滚珠丝杠沿直线导轨滑块运动，第二运动轴以及第三运动轴均采用电机连接减速机直接驱动旋转，而第四运动轴的旋转动作依靠电机连接减速机驱动电缸作直线运动实现。

由于该机器人需要自动完成从弹药仓中取出弹药并进行装填弹药的任务，以及从外部向弹药筒中补充弹药的任务，并且由于该弹药装填机器人与市场上各类机器人结构形式均不同，所以现有的机器人控制方法无法适用于该机器人上。因此需要一种控制方法来实现对该机器人的控制，完成弹药自动装填和弹药自动补给任务。

发明内容

为了解决背景技术中的问题，本发明提供了一种五轴式弹药装填机器人的控制方法。

为了实现发明目的，本发明的具体技术方案是：

一种五轴式弹药装填机器人的控制方法，其具体实现步骤如下：

步骤1.1：确定待机位：定义机器人整体到达待机位时，各运动轴处于零位，其表达式为P₀(0,0,0,0,0)；

待机位时各轴运动状态如下：

第一运动轴运动到原位传感器所在位置，记此时第一运动轴的位置的表达式为0；

第二运动轴旋转到使机身与横行导轨平行的状态，记此时第二运动轴的位置为零位，其表达式为0；

第三运动轴旋转到使小臂收拢保持与大臂夹紧的状态，记此时第三运动轴的位置为零位，其表达式为0；

使顶升电缸收缩到底，第四运动轴驱使弹托与地面之间保持水平，记此时第四运动轴的位置为零位，其表达式为0；

第五运动轴使夹爪松开，记此时第四运动轴的位置为零位，其表达式为0；

步骤1.2：确定取弹位，定义机器人整体到达取弹位时，各运动轴位置表达式为P₁(H₁,θ₂₁,θ₃₁,θ₄₁,0)。

取弹位时各轴运动状态如下：

第一运动轴运动到H₁位置；H₁位置为水平横移轴上对应于弹药仓的位置；

第二运动轴旋转到使弹托正对弹药仓的状态，记此时第二运动轴的位置的表达式为θ₂₁；

第三运动轴向上旋转，机器人的第四运动轴向下旋转，调整第三运动轴和第四运动轴，使弹托保持垂直于地面且弹嵌入到弹托中，记此时第三运动轴的位置和第四运动轴的位置表达式分别为θ₃₁,θ₄₁；

第五运动轴此时处于零位；

步骤1.3：确定放弹位，定义机器人整体到达放弹位时，各运动轴位置表达式为P₂(H₂,0,θ₃₂,θ₄₂,0)。

放弹位时各轴运动状态如下：

第一运动轴运动到H₂位置；H₂位置为水平横移轴上对应于待装填弹药设备的位置；

第二运动轴运动至零位；

第三运动轴旋转到当第四运动轴处于零位时，弹托与地面之间保持水平状态，记此时第三运动轴的位置表达式为θ₃₂；

第四运动旋转到使弹托到达与地面垂直的状态，记此时第三运动轴的位置表达式为θ₄₂；

第五运动轴运动至零位；

步骤1.4：确定补弹位，定义机器人整体到达补弹位时，各运动轴位置表达式为P₃(H₃,θ₂₃,0,0,0)；

补弹位时各轴运动状态如下：

第一运动轴运动到H₃位置，H₃位置为水平横移轴上对应于补弹口的位置；

第二运动轴旋转到使弹托正对补弹口，记此时第二运动轴的位置表达式为θ₂₃；

第三运动轴运动至零位；

第四运动轴运动至零位

第五运动轴运动至零位；

步骤2：自动装填流程：

当机器人控制器接收到上位机发送来的自动装填指令后，首先判断机器人是否在待机位，如果不在待机位，则向上位机发送报警信息；

如果在待机位，则开始执行装填动作；

首先同步控制第一运动轴、第二运动轴分别运动到H₁,θ₂₁，同步控制第三运动轴、第四运动轴分别运动到θ₃₁,θ₄₁，控制夹爪夹紧抓取弹；

接着同步控制第三运动轴、第四运动轴分别运动到零位，同步控制第一运动轴运动到H₂位置，第二运动轴运动到零位，再控制第三运动轴运动到θ₃₂；

待接收到上位机的装填指令后，控制夹爪松开，控制第四运动轴运动到θ₄₂，然后向上位机发送装填完成的信号，最后控制第一运动轴运动到零位，同步控制第三运动轴、第四运动轴分别运动到零位；

步骤3：自动补给流程：

当机器人控制器接收到上位机发送来的自动补给指令后，首先判断机器人是否在待机位，如果不在待机位，则向上位机发送报警信息；

如果在待机位，首先首先同步控制第一运动轴、第二运动轴分别运动到H₃,θ₂₃，接收到上位机的放置成功指令后，控制夹爪夹紧弹，然后控制第二运动轴回到零位，再同步控制第第一运动轴、第二运动轴分别运动到H₁,θ₂₁，同步控制第三运动轴、第四运动轴分别运动到θ₃₁,θ₄₁，控制夹爪松开，向上位机发送补给完成信号，最后同步控制第三运动轴、第四运动轴分别运动到零位，同步控制第一运动轴、第二运动轴运动到零位。

进一步地，上述步骤2和步骤3中第一运动轴和第二运动轴的同步控制，以及第三运动轴和第四运动轴的的同步控制具体过程为：

首先，计算同步运行的时间T；

其中：(N₁，N₂)代表两个需要同步控制的运动轴运动到下一位置时所需时间，单位为秒；

N₁＝(同步运动轴1目标位置-同步运动轴1初始位置)×同步运动轴1减速比×同步运动轴1机械传动比÷同步运动轴1电机最大转速×60；

N₂＝(同步运动轴2目标位置-同步运动轴2初始位置)×同步运动轴2减速比×同步运动轴2机械传动比÷同步运动轴2电机最大转速×60；

对于第一运动轴位置单位为毫米，对于第二、三、四运动轴位置的单位为弧度；

然后，根据以下公式计算两个需要同步控制的运动轴的运动轨迹：

其中s_i0为运动初始位置；

接着，将运动轴轨迹转化为各个运动轴对应的电机控制指令。

进一步地，所述第一运动轴、第二运动轴、第三运动轴对应的电机控制指令Q1的计算公式为：

Q1＝s_i(t)÷减速比÷机械传动比×2^{电机编码器位数}+机器人零位编码器值；

进一步地，所述第四运动轴的电机控制指令Q2的计算公式为：

首先，计算第四运动轴旋转角度θ₄与顶升电缸伸缩长度h之间的关系：

其中：

上式中，h₀为顶升电缸收缩到底时，顶升电缸底部和小臂的铰接点D与顶升电缸顶部与弹托的铰接点E之间的长度；

b为顶升电缸顶部与弹托的铰接点E到弹托实际旋转点F之间的长度；

c为弹托实际旋转点F到弹托和小臂之间的连接件中固定点G的长度；

d为顶升电缸顶部与弹托的铰接点E到弹托和小臂之间的连接件固定点G的长度；

α为顶升电缸与小臂之间的夹角；

β为弹托和小臂之间的连接件与小臂之间的夹角；

γ为顶升电缸与弹托之间的夹角；

然后，计算第四运动轴控制指令：

按照控制指令的发送周期提取运动轴轨迹的插补点，根据第四运动轴旋转角度θ₄与顶升电缸伸缩长度h之间的关系，计算每个插补时刻对应的电缸收缩行程，第四运动轴控制指令Q2＝电缸收缩行程÷电缸驱动电机减速比÷电缸机械传动比×2^{电机编码器位数}+机器人零位编码器值。

本发明的有益效果是：

本发明针对五轴式弹药装填机器人的结构特点和机器人所处环境的特点，设计了完整的弹药自动装填和自动补给方案；其执行流程避免了环境干涉，保证执行动作安全可靠；提出的同步控制方法有效提高了任务执行的效率；针对第四运动轴的特殊结构形式，提出的控制指令计算方法，有效解决了如何控制顶升电缸运动来实现第四运动轴的准确旋转。

附图说明

图1为五轴式弹药装填机器人的结构原理图。

图2为自动装填过程的控制流程图。

图3为自动补弹过程的控制流程图。

图4为机器人的运动坐标系示意图。

具体实施方式

下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本发明的描述中，需要说明的是，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。

一种五轴式弹药装填机器人的控制方法，该机器人包括五个运动轴，如图1所示：其中，第一运动轴为水平横移轴，第二运动轴为大臂旋转轴，第三运动轴为小臂旋转轴，第四运动轴为弹托旋转轴，第五运动轴为弹托夹爪控制轴，五个轴依次串行连接；

其中，第一运动轴由电机连接减速机驱动滚珠丝杠沿直线导轨滑块运动，第二运动轴以及第三运动轴均采用电机连接减速机直接驱动旋转，而第四运动轴的旋转动作依靠电机连接减速机驱动电缸运动实现。

其具体控制方法如下：

步骤1：确定待机位、取弹位、放弹位和补给位的机器人位置姿态；

步骤1.1：确定待机位，定义机器人整体到达待机位时，各运动轴处于零位，各运动轴位置表达式为P₀(0,0,0,0,0)

具体确定过程如下：

开机上电后，操作示教器上的一轴横移按钮使机器人的第一运动轴运动到原位传感器所在位置；操作示教器上的二轴旋转按钮使机器人的第二运动轴旋转到当小臂在水平状态时与横行导轨平行的状态；操作示教器上的三轴旋转按钮使机器人的第三运动轴旋转到使小臂收拢保持与地面平行的状态；操作示教器上的四轴旋转按钮使机器人的第四运动轴旋转到使机器人的弹托收拢保持与小臂夹紧的状态；操作示教器上的夹爪松开按钮，控制第五运动轴使夹爪松开；操作示教器使控制器将当前各轴的电机绝对值编码器位置记录下来作为机器人零位编码器值，并定义机器人处于待机位P₀(0,0,0,0,0)；

步骤1.2：确定取弹位，定义机器人整体到达取弹位时，各运动轴位置表达式为P₁(H₁,θ₂₁,θ₃₁,θ₄₁,0)

以待机位作为起始位置，确定取弹位，其具体操作过程如下：

操作示教器上的一轴横移按钮使机器人的第一运动轴运动到H₁位置；操作示教器上的二轴旋转按钮使机器人的第二运动轴逆时针向左旋转到弹托正对弹仓；操作示教器上的三轴旋转按钮使机器人的第三运动轴向上旋转，操作示教器上的四轴旋转按钮使机器人的第四运动轴向下旋转，反复操作调整第三运动轴和第四运动轴旋转，使弹托保持垂直于地面且弹嵌入到弹托中，此时夹爪处于张开状态；操作示教器使控制器将当前各轴的位置记录下来并定义为取弹位P₁(H₁,θ₂₁,θ₃₁,θ₄₁,0)。

步骤1.3：确定放弹位，定义机器人整体到达放弹位时，各运动轴位置表达式为P₂(H₂,0,θ₃₂,θ₄₂,0)；

以取弹位作为起始位置，确定放弹位，首先，操作示教器上的三轴旋转按钮使机器人的第三运动轴向下旋转，操作示教器上的四轴点动旋转按钮使机器人的第四运动轴向上旋转，反复操作调整第三运动轴和第四运动轴，使小臂和弹托回到零位；操作示教器上的二轴旋转按钮使机器人的第二运动轴旋转回到零位；操作示教器上的一轴横移按钮使机器人的第一运动轴运动到H₂位置；

然后操作示教器上的三轴旋转按钮使机器人的第三运动轴向上旋转，使弹托到达水平状态；操作示教器上的四轴旋转按钮使机器人的第四运动轴向下旋转，使弹托到达与地面垂直的状态；操作示教器使控制器将当前各轴的位置下来并定义为放弹位P₂(H₂,0,θ₃₂,θ₄₂,0)；

步骤1.4：确定补弹位，定义机器人整体到达补弹位时，各运动轴位置表达式为P₃(H₃,θ₂₃,0,0,0)；

以放弹位作为起始位置，确定补弹位，操作示教器上的一轴横移按钮使机器人的第一运动轴运动到H₃位置；操作示教器上的四轴旋转按钮使机器人的第四运动轴旋转到零位状态；操作示教器上的三轴旋转按钮使机器人的第三运动轴向下旋转到零位状态；操作示教器上的二轴旋转按钮使机器人的第二运动轴旋转到弹托正对补弹口；操作示教器使控制器将当前各轴的位置记录下来并定义为补弹位P₃(H₃,θ₂₃,0,0,0)。

步骤2：自动装填流程，如图2所示：

当机器人控制器接收到上位机发送来的自动装填指令后，首先判断机器人是否在待机位，如果不在待机位，则向上位机发送报警信息；

如果在待机位，则开始执行装填动作；

首先同步控制第一运动轴、第二运动轴分别运动到H₁,θ₂₁，同步控制第三运动轴、第四运动轴分别运动到θ₃₁,θ₄₁，控制夹爪夹紧抓取弹；

接着同步控制第三运动轴、第四运动轴分别运动到零位，同步控制第一运动轴运动到H₂位置，第二运动轴运动到零位，再控制第三运动轴运动到θ₃₂；

待接收到上位机的装填指令后，控制夹爪松开，控制第四运动轴运动到θ₄₂，然后向上位机发送装填完成的信号，最后控制第一运动轴运动到零位，同步控制第三运动轴、第四运动轴分别运动到零位；

步骤3：自动补给流程，如图3所示：

当机器人控制器接收到上位机发送来的自动补给指令后，首先判断机器人是否在待机位，如果不在待机位，则向上位机发送报警信息；

如果在待机位，首先首先同步控制第一运动轴、第二运动轴分别运动到H₃,θ₂₃，接收到上位机的放置成功指令后，控制夹爪夹紧弹，然后控制第二运动轴回到零位，再同步控制第第一运动轴、第二运动轴分别运动到H₁,θ₂₁，同步控制第三运动轴、第四运动轴分别运动到θ₃₁,θ₄₁，控制夹爪松开，向上位机发送补给完成信号，最后同步控制第三运动轴、第四运动轴分别运动到零位，同步控制第一运动轴、第二运动轴运动到零位。

需要说明是：步骤2和步骤3中第一运动轴和第二运动轴的同步控制，以及第三运动轴和第四运动轴的的同步控制具体过程为：

首先，计算同步运行的时间T

其中：(N₁，N₂)代表两个需要同步控制的运动轴运动到下一位置时所需时间，单位为秒；

N₁＝(同步运动轴1目标位置-同步运动轴1初始位置)×同步运动轴1减速比×同步运动轴1机械传动比÷同步运动轴1电机最大转速×60；

N₂＝(同步运动轴2目标位置-同步运动轴2初始位置)×同步运动轴2减速比×同步运动轴2机械传动比÷同步运动轴2电机最大转速×60；

对于第一运动轴位置单位为毫米，对于第二、三、四运动轴位置的单位为弧度；

然后，根据以下公式计算两个需要同步控制的运动轴的运动轨迹：

其中s_i0为运动初始位置；

接着，将运动轴轨迹转化为各个运动轴对应的电机控制指令。

由于第一运动轴、第二运动轴、第三运动轴均是通过电机的旋转，经减速机直接带动机器人各轴的旋转，因此第一运动轴、第二运动轴、第三运动轴对应的电机控制指令Q1的计算公式为：

Q1＝s_i(t)÷减速比÷机械传动比×2^{电机编码器位数}+机器人零位编码器值；

而第四运动轴的旋转是通过电缸的直线运动转换为第四运动轴的旋转运动，因此第四运动轴的电机控制指令Q2的计算过程比较特殊，具体过程如下：

首先，计算第四运动轴旋转角度θ₄与顶升电缸伸缩长度h之间的关系：

其中：

上式中，h₀为顶升电缸收缩到底时，顶升电缸底部和小臂的铰接点D与顶升电缸顶部与弹托的铰接点E之间的长度；

b为顶升电缸顶部与弹托的铰接点E到弹托实际旋转点F之间的长度；

c为弹托实际旋转点F到弹托和小臂之间的连接件中固定点G的长度；

d为顶升电缸顶部与弹托的铰接点E到弹托和小臂之间的连接件固定点G的长度；

α为顶升电缸与小臂之间的夹角；

β为弹托和小臂之间的连接件与小臂之间的夹角；

γ为顶升电缸与弹托之间的夹角；

然后，计算第四运动轴控制指令：

按照控制指令的发送周期提取运动轴轨迹的插补点，根据第四运动轴旋转角度θ₄与顶升电缸伸缩长度h之间的关系，计算每个插补时刻对应的电缸收缩行程，第四运动轴控制指令Q2＝电缸收缩行程÷电缸驱动电机减速比÷电缸机械传动比×2^{电机编码器位数}+机器人零位编码器值。

在机器人单机调试阶段，上位机可直接向控制器发送想要机器人弹托到达的空间位姿，控制器根据转换关系得到要到达指定的空间位姿，应该控制机器人各运动轴如何运动；在机器人正式运行执行任务阶段，根据该转换关系控制器根据每个时刻机器人各运动轴状态计算机器人空间位姿状态，并发送给上位机实时显示到人机交互界面上，转换关系建立过程如下：

A：定义装填机器人的运动坐标系；

如图4所示，定义基坐标系

横行导轨上安装的原位传感器中心为基坐标系

的原点，第一运动轴向右运动的方向为X₀轴正方向，垂直于第一运动轴向上的方向定为Z₀轴正方向，右手定则确定Y₀轴；

定义坐标系

各轴与基坐标系

的方向一致，原点为第一运动轴与横行导轨的连接点；

定义坐标系

各轴与基坐标系

的方向一致，原点为第二运动轴中心点；

定义坐标系

原点为第三运动轴旋转轴中心点，Z₃轴为转动轴，逆时针旋转为正方向，X₃轴与X₂轴同向，Y₃轴垂直向上为正；

定义坐标系

各轴与坐标系

的方向一致，原点为弹托与第四运动轴之间的连接点。

B：建立弹托中心点在空间中的位姿(X,Y,Z,A,B,C)与各个轴运动状态之间的关系：

其中X、Y、Z为弹托在基坐标系

中的位置，A为弹托绕Z₀轴旋转的角度，B为弹托绕Y₀轴旋转的角度，C为弹托绕X₀轴旋转的角度。

其中，θ₂、θ₃、θ₄分别为第二、三、四运动轴旋转的角度，a₀为第一运动轴沿横行导轨向右移动的距离，d₁为坐标系

原点与基坐标系

原点沿Z₀轴正方向的距离，d₂为坐标系

原点与坐标系

原点沿Z₀轴正方向的距离，a₂和d₃分别为坐标系

原点与坐标系

原点沿X₀轴和Z₀轴正方向的距离，a₃和d₄分别为坐标系

原点与坐标系

原点沿X₀轴和Z₀轴正方向的距离。

Claims (4)

1.一种五轴式弹药装填机器人的控制方法，其特征在于，其具体实现步骤如下：

步骤1：确定待机位、取弹位、放弹位和补给位的机器人位置姿态；

步骤1.1：确定待机位，定义机器人整体到达待机位时，各运动轴处于零位，各运动轴位置表达式为P₀(0,0,0,0,0)；

步骤1.2：确定取弹位，定义机器人整体到达取弹位时，各运动轴位置表达式为P₁(H₁,θ₂₁,θ₃₁,θ₄₁,0)；

步骤1.3：确定放弹位，定义机器人整体到达放弹位时，各运动轴位置表达式为P₂(H₂,0,θ₃₂,θ₄₂,0)；

步骤1.4：确定补弹位，定义机器人整体到达补弹位时，各运动轴位置表达式为P₃(H₃,θ₂₃,0,0,0)；

步骤2：自动装填流程：

当机器人控制器接收到上位机发送来的自动装填指令后，首先判断机器人是否在待机位，如果不在待机位，则向上位机发送报警信息；

如果在待机位，则开始执行装填动作；

首先同步控制第一运动轴、第二运动轴分别运动到H₁,θ₂₁，同步控制第三运动轴、第四运动轴分别运动到θ₃₁,θ₄₁，控制夹爪夹紧抓取弹；

接着同步控制第三运动轴、第四运动轴分别运动到零位，同步控制第一运动轴运动到H₂位置，第二运动轴运动到零位，再控制第三运动轴运动到θ₃₂；

待接收到上位机的装填指令后，控制夹爪松开，控制第四运动轴运动到θ₄₂，然后向上位机发送装填完成的信号，最后控制第一运动轴运动到零位，同步控制第三运动轴、第四运动轴分别运动到零位；

步骤3：自动补给流程：

当机器人控制器接收到上位机发送来的自动补给指令后，首先判断机器人是否在待机位，如果不在待机位，则向上位机发送报警信息；

如果在待机位，首先首先同步控制第一运动轴、第二运动轴分别运动到H₃,θ₂₃，接收到上位机的放置成功指令后，控制夹爪夹紧弹，然后控制第二运动轴回到零位，再同步控制第一运动轴、第二运动轴分别运动到H₁,θ₂₁，同步控制第三运动轴、第四运动轴分别运动到θ₃₁,θ₄₁，控制夹爪松开，向上位机发送补给完成信号，最后同步控制第三运动轴、第四运动轴分别运动到零位，同步控制第一运动轴、第二运动轴运动到零位。

2.根据权利要求1所述的五轴式弹药装填机器人的控制方法，其特征在于：所述步骤2和步骤3中第一运动轴和第二运动轴的同步控制，以及第三运动轴和第四运动轴的同步控制具体过程为：

首先，计算同步运行的时间T：

其中：(N₁，N₂)代表两个需要同步控制的运动轴运动到下一位置时所需时间，单位为秒；

N₁＝(同步运动轴1目标位置-同步运动轴1初始位置)×同步运动轴1减速比×同步运动轴1机械传动比÷同步运动轴1电机最大转速×60；

N₂＝(同步运动轴2目标位置-同步运动轴2初始位置)×同步运动轴2减速比×同步运动轴2机械传动比÷同步运动轴2电机最大转速×60；

然后，根据以下公式计算两个需要同步控制的运动轴的运动轨迹：

其中s_i0为运动初始位置；

接着，将运动轴轨迹转化为各个运动轴对应的电机控制指令。

3.根据权利要求2所述的五轴式弹药装填机器人的控制方法，其特征在于：所述第一运动轴、第二运动轴、第三运动轴对应的电机控制指令Q1的计算公式为：

Q1＝s_i(t)÷减速比÷机械传动比×2^{电机编码器位数}+机器人零位编码器值。

4.根据权利要求2所述的五轴式弹药装填机器人的控制方法，其特征在于：所述第四运动轴的电机控制指令Q2的计算公式为：

首先，计算第四运动轴旋转角度θ₄与顶升电缸伸缩长度h之间的关系：

其中：

上式中，h₀为顶升电缸收缩到底时，顶升电缸底部和小臂的铰接点D与顶升电缸顶部与弹托的铰接点E之间的长度；

b为顶升电缸顶部与弹托的铰接点E到弹托实际旋转点F之间的长度；

c为弹托实际旋转点F到弹托和小臂之间的连接件中固定点G的长度；

d为顶升电缸顶部与弹托的铰接点E到弹托和小臂之间的连接件固定点G的长度；

α为顶升电缸与小臂之间的夹角；

β为弹托和小臂之间的连接件与小臂之间的夹角；

γ为顶升电缸与弹托之间的夹角；

然后，计算第四运动轴控制指令：

按照控制指令的发送周期提取运动轴轨迹的插补点，根据第四运动轴旋转角度θ₄与顶升电缸伸缩长度h之间的关系，计算每个插补时刻对应的电缸收缩行程，第四运动轴控制指令Q2＝电缸收缩行程÷电缸驱动电机减速比÷电缸机械传动比×2^{电机编码器位数}+机器人零位编码器值。

Images