基于运动控制器的大巴车自动换电池方法与流程

本发明属于机械自动化领域，具体涉及一种基于运动控制器的大巴车自动换电池方法。

背景技术：

随着人们对环境保护的意识越来越强，环境污染和资源的问题受到人们的极大关注。汽车行业节能减排形势的日益严峻，发展节能环保型汽车成为汽车产业发展的必然选择。目前，以电能为代表的新能源的节能环保型汽车作为动力与储能电池产业终端应用产品，已成为国家鼓励发展的新兴战略性产业。

大巴车换电系统是由两台运动控制器分别控制大巴车两侧的两个换电机器人，通过和上位系统的实时信息交互、按着预先编制好的工艺流程，将大巴车电量不足的电池分别取出放入充电柜，再将满电电池从充电柜取出依次放入大巴车的过程。在全自动换电过程中，由于每辆车需要完成九块电池的取换，运动控制器和上位系统前后需数十次进行信息交互，因此，研发稳定可靠的全自动换电方法来确保换电机器人能实时、准确的完成电池的取换变得尤其重要。

技术实现要素：

为解决背景技术中问题，本发明提出一种能准确的、快速的、安全的基于运动控制器的大巴车自动换电池方法。

本发明的具体技术方案是：

大巴车的换电分别为大巴车和充电柜直接换电的方式和大巴车通过暂存架和充电柜间接换电的方式。

首先，先给出大巴车和充电柜直接换电的方式的具体步骤：

1)大巴车进入换电区域，上位机、运动控制器以及换电设备开启；

2)上位机向运动控制器发送通讯指令包；通讯指令包分为移动指令包、取电池指令包、拍照准备指令包、消除误差移动指令包和放电池指令包；每个指令包中的参数均包括命令号、目标坐标、设备类型、设备编号以及电池类型；

移动指令包用于充电柜取电池、放电池之前，暂存架取电池、放电池之前以及大巴车取电池、放电池之前，控制换电设备中的底层伺服轴进行移动，完成取电池动作的准备工作；

取电池指令包用于控制换电设备中的底层伺服轴进行移动，完成取电池动作；

拍照准备指令包用于控制换电设备中的底层伺服轴进行移动，完成相机到达最合适的拍照位置；

消除误差移动指令包用于充电柜和暂存架放电池之前，消除换电设备中的底层伺服轴由于电池自重产生的误差，完成放电池动作的准备工作；

放电池指令包用于控制换电设备中的底层伺服轴进行移动，完成放电池动作；

底层伺服轴包括轨道行走轴、货叉轴、升降轴、方位轴、俯仰轴、推拉轴和解锁轴；

3)按照大巴车和充电柜之间直接换电方式进行电池交换；一块电池采用直接换电方式的步骤依次是：大巴车取空电电池、充电柜放空电电池、充电柜取满电电池和大巴车放满电电池；

当进行大巴车取空电电池时：运动控制器依次执行移动指令包、拍照准备指令包、取电池指令包；

当进行充电柜放空电电池时：运动控制器依次执行消除误差移动指令包、放电池指令包；

当进行充电柜取满电电池时：运动控制器依次执行移动指令包、取电池指令包；

当进行大巴车放满电电池时：运动控制器依次执行移动指令包、放电池指令包；

运动控制器重复上面的工作对大巴车上所有的空电电池和充电柜上的满电电池进行交换。

再给出大巴车通过暂存架和充电柜间接换电的方式的具体步骤：包括以下步骤：

1)大巴车进入换电区域，上位机、运动控制器以及换电设备开启；

2)上位机向运动控制器发送通讯指令包；通讯指令包分为移动指令包、取电池指令包、拍照准备指令包、消除误差移动指令包和放电池指令包；每个指令包中的参数均包括命令号、目标坐标、设备类型、设备编号以及电池类型；

移动指令包用于充电柜取电池、放电池之前，暂存架取电池、放电池之前以及大巴车取电池、放电池之前，控制换电设备中的底层伺服轴进行移动，完成取电池动作的准备工作；

取电池指令包用于控制换电设备中的底层伺服轴进行移动，完成取电池动作；

拍照准备指令包用于控制换电设备中的底层伺服轴进行移动，完成相机到达最合适的拍照位置；

消除误差移动指令包用于充电柜和暂存架放电池之前，消除换电设备中的底层伺服轴由于电池自重产生的误差，完成放电池动作的准备工作；

放电池指令包用于控制换电设备中的底层伺服轴进行移动，完成放电池动作；

底层伺服轴包括轨道行走轴、货叉轴、升降轴、方位轴、俯仰轴、推拉轴和解锁轴；

3)按照大巴车和充电柜之间设置有暂存架的结构采用间接换电方式进行电池交换；一块电池采用间接换电方式的步骤是：充电柜取满电电池、暂存架放满电电池、大巴车取空电电池、暂存架放空电电池、暂存架取满电电池、大巴车放满电电池、暂存架取空电电池和充电柜放空电电池；

当进行充电柜取满电电池时：运动控制器依次执行移动指令包、取电池指令包；

当进行暂存架放满电电池时：运动控制器依次执行消除误差移动指令包、放电池指令包；

当进行大巴车取空电电池时：运动控制器依次执行移动指令包、拍照准备指令包、取电池指令包；

当进行暂存架放空电电池时：运动控制器依次执行消除误差移动指令包、放电池指令包；

当进行暂存架取满电电池时：运动控制器依次执行移动指令包、取电池指令包；

当进行大巴车放满电电池时：运动控制器依次执行移动指令包、放电池指令包；

当进行暂存架取空电电池时：运动控制器依次执行移动指令包、取电池指令包；

当进行充电柜放空电电池时：运动控制器依次执行消除误差移动指令包、放电池指令包；

运动控制器重复上面的工作对大巴车上所有的空电电池和充电柜上的满电电池进行交换。

需要说明的是：所述运动控制器执行移动指令包进行控制的具体步骤是：

a1)运动控制器拆解移动指令包，根据设备和电池类型判断移动指令包类型，并根据移动指令包类型执行不同的移动指令；

a2)若对充电柜或大巴车执行移动指令，则分别以上位机的通讯值更新升降轴、行走轴、方位轴和俯仰轴的目标位置；若对暂存架执行移动指令，根据暂存架层号和电池大小类型分别以运动控制器的内存设定值更新升降轴、方位轴和俯仰轴的目标位置；目标速度和操作模式设定均来自于运动控制器内部初始设定，同时启动各伺服轴的运行，运行步变量Step更新；

a3)判断轨道行走轴、升降轴、方位轴和俯仰轴动作是否完成，如未完成继续执行步骤a2)；如果完成，将轨道行走轴、升降轴、方位轴和俯仰轴启动标清零，运行步变量Step更新；

a4)若对充电柜或大巴车执行移动指令，则分别以上位机的通讯值更新货叉轴的目标位置；若对暂存架执行移动指令，根据暂存架层号和电池大小类型分别以运动控制器的内存设定值更新货叉轴的目标位置；目标速度和操作模式设定均来自于运动控制器内部初始设定，同时执行货叉轴启动，运行步变量Step更新；

a5)判断货叉轴动作是否完成，如未完成继续执行步骤a4)；如果完成，将货叉轴启动标清零，运行步变量Step更新；

a6)底层伺服轴启动标清零、命令完成标置1，运行步变量Step清零，初始状态标清零，移动指令包结束。

需要说明的是：运动控制器执行取电池指令包进行控制的具体步骤是：

b1)运动控制器拆解取电池指令包，根据设备和电池类型判断取电池指令包类型，并根据取电池指令包类型执行不同的取电池指令；

b2)准备执行取电池动作，首先判别解锁轴角度是否在电池锁正常进入范围，正常范围为：解锁位±0.3°，若在正常范围，则执行步骤b3)，否则，执行解锁轴移动到解锁位，运行步变量Step更新；

b3)若对充电柜或大巴车执行取电池，则根据电池大小类型以运动控制器的内存设定值更新推拉轴的目标位置、目标速度和操作模式；

若对暂存架执行取电池，则根据暂存架层号和电池大小类型分别以运动控制器的内存设定值更新推拉轴的目标位置、目标速度和操作模式；

执行推拉轴启动到推拉前进位，运行步变量Step更新；

b4)判断推拉轴动作是否完成，如未完成继续执行，如果完成，将推拉轴启动标清零，运行步变量Step更新；

b5)更新解锁轴目标位置、目标速度和操作模式，执行解锁轴到锁止位；

b6)判断解锁轴动作是否完成，如未完成继续执行，如果完成，将解锁轴启动标清零，运行步变量Step更新，并执行下一步；

b7)若对充电柜或大巴车执行取电池，则根据电池大小类型以内存设定值分别更新推拉轴和升降轴的目标位置、目标速度和操作模式；设定的目标位置和目标速度必须确保推拉轴和升降轴协同作业，使电池匀速稳定的取出；

若对暂存架执行取电池，则根据暂存架层号和电池大小类型以内存设定值更新推拉轴和升降轴的目标位置、目标速度和操作模式，设定的目标位置和目标速度必须确保推拉轴和升降轴协同作业，使电池匀速稳定的取出；

执行推拉轴伺服回到推拉原位，同时启动升降轴上升，运行步变量Step更新；

b8)判断升降轴动作是否完成，如未完成继续执行，如果完成，将升降轴启动标清零，运行步变量Step更新；

b9)判断推拉轴动作是否完成，如未完成继续执行，如果完成，将推拉轴启动标清零，运行步变量Step更新；

b10)判断有无“取电池失败”，如有,报“取电池失败”错误，如没有，各轴启动标清零、命令完成标置1，运行步变量Step清零，初始状态标清零，至此，取电池指令包结束。

需要说明的是：运动控制器执行消除误差移动指令包进行控制的具体步骤是：

c1)运动控制器拆解消除误差移动指令包，根据设备和电池类型判断消除误差移动指令包类型，并根据消除误差移动指令包类型执行不同的消除误差移动指令；

c2)若对暂存架执行消除误差移动指令，则根据暂存架层号和电池大小类型在内存设定目标高度的基础上增加一个向上的偏移量来更新升降轴的目标位置、目标速度和操作模式，以运动控制器的内存设定值分别更新方位轴和俯仰轴的目标位置、目标速度和操作模式；

若对充电柜执行消除误差移动指令，则根据电池大小类型在升降轴通讯值的基础上增加一个向上的偏移量来更新升降轴的目标位置、目标速度和操作模式，以上位机的通讯值分别更新行走轴、方位轴和俯仰轴的目标位置、目标速度和操作模式；

c3)判断轨道行走轴、升降轴、方位轴和俯仰轴动作是否完成，如未完成继续执行步骤c2)；如果完成，将轨道行走轴、升降轴、方位轴和俯仰轴启动标清零，运行步变量Step更新，执行下一步；

c4)若对充电柜执行消除误差移动指令，则分别以通讯值更新货叉轴的目标位置，若对暂存架执行消除误差移动指令，根据暂存架层号和电池大小类型分别以内存设定值更新货叉轴的目标位置；目标速度和操作模式设定均来自于运动控制器内部初始设定，货叉轴执行动作到前进位；

c5)判断货叉轴动作是否完成，如未完成继续执行步骤c4)；如果完成，将货叉轴启动标清零，运行步变量Step更新；

c6)底层伺服轴启动标清零、命令完成标置1，运行步变量Step清零，初始状态标清零，消除误差移动指令包结束。

需要说明的是：运动控制器执行放电池指令包进行控制的具体步骤是：

d1)运动控制器拆解放电池指令包，根据设备和电池类型判断放电池指令包类型，并根据放电池指令包类型执行不同的放电池指令；

d2)若对充电柜或大巴车执行放电池，则根据电池大小类型以运动控制器的内存设定值更新推拉轴和升降轴的目标位置、目标速度和操作模式，设定的目标位置和目标速度必须确保推拉轴和升降轴协同作业，使电池匀速稳定放入；

若对暂存架执行放电池，则根据暂存架层号和电池大小类型分别以运动控制器的内存设定值更新推拉轴和升降轴的目标位置、目标速度和操作模式，设定的目标位置和目标速度必须确保推拉轴和升降轴协同作业，使电池匀速稳定的放入；

执行推拉轴伺服到推拉前进位，同时启动升降轴下降，运行步变量Step更新；

d3)判断升降轴动作是否完成，如未完成继续执行，如果完成，将升降轴启动标清零，运行步变量Step更新；

d4)判断推拉轴动作是否完成，如未完成继续执行，如果完成，将推拉轴启动标清零，运行步变量Step更新；

d5)更新解锁轴目标位置、目标速度和操作模式，执行伺服启动到解锁位；

d6)判断解锁轴动作是否完成，如未完成继续执行，如果完成，将解锁轴启动标清零，运行步变量Step更新；

d7)更新推拉轴目标位置、目标速度和操作模式，执行推拉轴伺服到推拉安全位；

d8)判断推拉轴动作是否完成，如未完成继续执行，如果完成，将推拉轴启动标清零，运行步变量Step更新；

d9)判断有无“放电池失败”，如有,报“放电池失败”错误，如没有，各轴启动标清零、命令完成标置1，运行步变量Step清零，初始状态标清零，放电池指令包结束。

需要说明的是：拍照准备指令包以运动控制器的内存设定值使推拉轴执行一次设定的移动，使相机到达最合适的拍照位置。

本发明的有益效果是：

1、由于运动控制器把大量动作经过分析、压缩、整理形成指令包和上位机进行交互，而换电工艺是通过指令包的柔性组合生成的，故使工艺编辑思路清晰、简洁和灵活；

2、本发明每次交互分批分段，只有当前动作完成且下一组数据全部更新完成，才会启动新一轮通讯指令包的执行，可以确保通讯数据的实时性和准确性。

3、本发明采用了直接换电方式和间接换电方式两种，能够适用于换电大巴车车辆密集的情况以及换电车辆不是很密集的情况，具有更高的换电效率。

附图说明

图1为本发明换电系统通讯拓扑图；

图2为直换工艺的换电流程图；

图3为间接工艺中换电准备的流程图；

图4为间接工艺中换电过程的流程图；

图5为间接工艺中电池充电过程的流程图。

具体实施方式

根据大巴车数量的多少和充电柜电池使用情况，本发明提供两种工艺方案供用户选择；

其一：大巴车电池和充电柜电池直接进行交换，该方案要求充电柜至少有一列空闲位置，且对于换电大巴车车辆密集的情况较适用(简称直换工艺)；其二：大巴车电池通过暂存架和充电柜电池进行交换，该方案对充电柜不做任何要求，同时换电车辆不是很密集的情况有更高的换电效率(简称间接换电工艺)。

上位系统在首次换电命令启动或完成一组通讯命令后，将按照换电工艺步序要求分别给1号和2号运动控制器发送通讯指令包，每块电池的取换都要经过“拍照准备”、“移动”、“取电池”、“消除误差移动”和“放电池”“五个通讯指令包，每个指令包都包含命令号、目标坐标、设备类型、设备编号和电池类型等交互信息。

此处需要说明的是：其中，消除误差移动指令包目的是：由于在换电设备空电行走和满电行走时，由于电池自重会造成坐标值的误差，为了消除该误差，在放电池指令之前都会先执行消除误差移动指令。

而运动控制器根据通讯命令、设备类型等交互信息对通讯指令包进行拆包、分类并分解至底层伺服轴控制指令(即拆包指令)，控制换电机器人一步步完成换电流程。底层伺服轴包括轨道行走轴(X)、货叉轴(Y)、升降轴(Z)、方位轴(B)、俯仰轴(C)、推拉轴(T)和解锁轴(S)。

每个通讯指令包执行开始，上位机给运动控制器开辟的连续存储区写通讯数据，包括命令号、目标坐标、设备类型、设备编号和电池类型等，运动控制器会另行保存当前命令号，同时将All_Done(命令完成标)清零，当在通讯指令包启动固定时间间隔内检测到上位计算机数据刷新完成信号后，才会正式启动通讯指令包的执行，确保每组数据都是更新过的最新数据。当每个通讯指令包执行完毕，都会将All_Done(命令完成标)置1，而上位机检测到All_Done的跳变同时将当前动作与运动控制器保存的命令号对应的动作比较，如果一致，就会启动新一轮通讯指令包的执行，每个通讯指令包从开始到结束再到下一个通讯指令包开始都是如此，直至工艺动作全部完成。

以下通过对大巴车一块电池进行取换的两种换电池工艺分别进行描述，从而阐明由通讯指令包生成换电工艺的过程，最后再对每个通讯指令包内部拆包、分类及分解的过程进行详述。

1、大巴车电池和充电柜电池直接进行交换(直换工艺)

如图2所示，每块电池的取换要经过“拍照准备”、“移动”、“取电池指令”、“消除误差移动”和“放电池”五个通讯指令包，取换工艺就是这五个指令包的有序组合生成的。通常在大巴车两侧，一侧安装五块电池、另一侧安装4块电池。

更换一块电池的步骤依次为：大巴车取空电电池、充电柜放空电电池、充电柜取满电电池和大巴车放满电电池；

当进行大巴车取空电电池时：运动控制器依次执行移动指令包、拍照准备指令包、取电池指令包；

当进行充电柜放空电电池时：运动控制器依次执行消除误差移动指令包、放电池指令包；

当进行充电柜取满电电池时：运动控制器依次执行移动指令包、取电池指令包；

当进行大巴车放满电电池时：运动控制器依次执行移动指令包、放电池指令包；

运动控制器重复上面的工作对大巴车上所有的空电电池和充电柜上的满电电池进行交换。

2、大巴车电池通过暂存架和充电柜电池进行交换(间接换电工艺)

大巴车电池通过暂存架和充电柜电池进行交换，根据暂存架存放电池的状态可执行三种换电工艺，即换电准备工艺、换电工艺和电池充电工艺，下面分别进行描述。

1)换电准备工艺

换电准备工艺，即在暂存架无电池状态，将充电柜满电电池按一定顺序依次放入暂存架的过程，为执行和大巴车的电池互换做准备。换电准备工艺一块电池的取换流程如图3所示：

换电准备工艺一块电池的取换包括“移动”、“取电池”、“消除误差移动”和“放电池”四个通讯指令包，换电准备工艺就是这四个指令包的有序组合生成的。

2)换电工艺

换电工艺即将暂存架满电电池和大巴车空电电池按一定顺序依次进行交换，完成大巴车换电流程。换电工艺一块电池的取换流程如图4所示：

换电工艺一块电池的取换包括“拍照准备”、“移动”、“取电池”、“消除误差移动”和“放电池”五个通讯指令包，换电工艺就是这五个指令包的有序组合生成的。

3)电池充电工艺

电池充电工艺，即将暂存架空电电池和充电柜满电电池按一定顺序依次进行交换。电池充电工艺一块电池的取换流程如图5所示：

电池充电工艺一块电池的取换包括“移动”、“取电池”、“消除误差移动”和“放电池”四个通讯指令包，电池充电工艺就是这四个指令包的有序组合生成的。

三、通讯指令包

从上面描述可以看到，不论哪种换电工艺，都包括“移动”、“取电池”、“消除误差移动”和“放电池”四个通讯指令包，只是和大巴车电池交换时多了一个“拍照准备”指令包。

每个轴的启动都包括对目标位置、目标速度和操作模式的更新和轴启动标的触发四个元素，其中目标位置或者来自于上位机的通讯值，或者来自于运动控制器的内存设定值，目标速度是按电机额定速度的百分比进行设定，来自于运动控制器的内存设定值，操作模式包括伺服绝对位置、条码绝对位置、伺服增量位置三种控制模式，在自动运行中，行走轴和升降轴选择条码绝对位置模式，货叉轴、方位轴、俯仰轴、推拉轴和解锁轴均选择伺服绝对位置模式。

下面将对“移动”、“取电池”、消除误差移动和“放电池”四个指令包内部如何拆包、分类及分解的过程进行详述。

(一)移动指令包

当命令代码为1时，执行移动指令包功能。

a1)运动控制器拆解移动指令包，根据设备类型判断移动指令包类型，并根据移动指令包类型执行不同的移动指令；具体的判断办法是：如果设备号为1，即对充电柜执行移动指令，如果设备号为2即对暂存架执行移动指令，如果设备号为3即对大巴车执行移动指令；

a2)若对充电柜或大巴车执行移动指令，则分别以上位机的通讯值更新升降轴、行走轴、方位轴和俯仰轴的目标位置；若对暂存架执行移动指令，根据暂存架层号和电池大小类型分别以运动控制器的内存设定值更新升降轴、方位轴和俯仰轴的目标位置；目标速度和操作模式设定均来自于运动控制器内部初始设定，同时启动各伺服轴的运行，运行步变量Step更新；

a3)判断轨道行走轴、升降轴、方位轴和俯仰轴动作是否完成，如未完成继续执行步骤a2)；如果完成，将轨道行走轴、升降轴、方位轴和俯仰轴启动标清零，运行步变量Step更新；

a4)若对充电柜或大巴车执行移动指令，则分别以上位机的通讯值更新货叉轴的目标位置；若对暂存架执行移动指令，根据暂存架层号和电池大小类型分别以运动控制器的内存设定值更新货叉轴的目标位置；目标速度和操作模式设定均来自于运动控制器内部初始设定，同时执行货叉轴启动，运行步变量Step更新；

a5)判断货叉轴动作是否完成，如未完成继续执行步骤a4)；如果完成，将货叉轴启动标清零，运行步变量Step更新；

a6)底层伺服轴启动标清零、命令完成标置1，运行步变量Step清零，初始状态标清零，移动指令包结束。

(二)取电池指令包

取电池指令包是在移动指令包的基础上，即完成移动指令包的全部步骤后，继续执行后续动作。

b1)运动控制器拆解取电池指令包，根据设备和电池类型判断取电池指令包类型，并根据取电池指令包类型执行不同的取电池指令；

b2)准备执行取电池动作，首先判别解锁轴角度是否在电池锁正常进入范围，正常范围为：解锁位±0.3°，若在正常范围，则执行步骤b3)，否则，执行解锁轴移动到解锁位，运行步变量Step更新；

b3)若对充电柜或大巴车执行取电池，则根据电池大小类型以运动控制器的内存设定值更新推拉轴的目标位置、目标速度和操作模式；

若对暂存架执行取电池，则根据暂存架层号和电池大小类型分别以运动控制器的内存设定值更新推拉轴的目标位置、目标速度和操作模式；

执行推拉轴启动到推拉前进位，运行步变量Step更新；

b4)判断推拉轴动作是否完成，如未完成继续执行，如果完成，将推拉轴启动标清零，运行步变量Step更新；

b5)更新解锁轴目标位置、目标速度和操作模式，执行解锁轴到锁止位；

b6)判断解锁轴动作是否完成，如未完成继续执行，如果完成，将解锁轴启动标清零，运行步变量Step更新，并执行下一步；

b7)若对充电柜或大巴车执行取电池，则根据电池大小类型以内存设定值分别更新推拉轴和升降轴的目标位置、目标速度和操作模式；设定的目标位置和目标速度必须确保推拉轴和升降轴协同作业，使电池匀速稳定的取出；

若对暂存架执行取电池，则根据暂存架层号和电池大小类型以内存设定值更新推拉轴和升降轴的目标位置、目标速度和操作模式，设定的目标位置和目标速度必须确保推拉轴和升降轴协同作业，使电池匀速稳定的取出；

执行推拉轴伺服回到推拉原位，同时启动升降轴上升，运行步变量Step更新；

b8)判断升降轴动作是否完成，如未完成继续执行，如果完成，将升降轴启动标清零，运行步变量Step更新；

b9)判断推拉轴动作是否完成，如未完成继续执行，如果完成，将推拉轴启动标清零，运行步变量Step更新；

b10)判断有无“取电池失败”，如有,报“取电池失败”错误，如没有，各轴启动标清零、命令完成标置1，运行步变量Step清零，初始状态标清零，至此，取电池指令包结束。

(三)消除误差移动指令包

c1)运动控制器拆解消除误差移动指令包，根据设备和电池类型判断消除误差移动指令包类型，并根据消除误差移动指令包类型执行不同的消

除误差移动指令；

c2)若对暂存架执行消除误差移动指令，则根据暂存架层号和电池大小类型在内存设定目标高度的基础上增加一个向上的偏移量来更新升降轴的目标位置、目标速度和操作模式，以运动控制器的内存设定值分别更新方位轴和俯仰轴的目标位置、目标速度和操作模式；

若对充电柜执行消除误差移动指令，则根据电池大小类型在升降轴通讯值的基础上增加一个向上的偏移量来更新升降轴的目标位置、目标速度和操作模式，以上位机的通讯值分别更新行走轴、方位轴和俯仰轴的目标位置、目标速度和操作模式；

c3)判断轨道行走轴、升降轴、方位轴和俯仰轴动作是否完成，如未完成继续执行步骤c2)；如果完成，将轨道行走轴、升降轴、方位轴和俯仰轴启动标清零，运行步变量Step更新，执行下一步；

c4)若对充电柜执行消除误差移动指令，则分别以通讯值更新货叉轴的目标位置，若对暂存架执行消除误差移动指令，根据暂存架层号和电池大小类型分别以内存设定值更新货叉轴的目标位置；目标速度和操作模式设定均来自于运动控制器内部初始设定，货叉轴执行动作到前进位；

c5)判断货叉轴动作是否完成，如未完成继续执行步骤c4)；如果完成，将货叉轴启动标清零，运行步变量Step更新；

c6)底层伺服轴启动标清零、命令完成标置1，运行步变量Step清零，初始状态标清零，消除误差移动指令包结束。

(四)放电池指令包

d1)运动控制器拆解放电池指令包，根据设备和电池类型判断放电池指令包类型，并根据放电池指令包类型执行不同的放电池指令；

d2)若对充电柜或大巴车执行放电池，则根据电池大小类型以运动控制器的内存设定值更新推拉轴和升降轴的目标位置、目标速度和操作模式，设定的目标位置和目标速度必须确保推拉轴和升降轴协同作业，使电池匀速稳定放入；

若对暂存架执行放电池，则根据暂存架层号和电池大小类型分别以运动控制器的内存设定值更新推拉轴和升降轴的目标位置、目标速度和操作模式，设定的目标位置和目标速度必须确保两个轴协同作业，使电池匀速稳定的放入；

执行推拉轴伺服到推拉前进位，同时启动升降轴下降，运行步变量Step更新；

d3)判断升降轴动作是否完成，如未完成继续执行，如果完成，将升降轴启动标清零，运行步变量Step更新；

d4)判断推拉轴动作是否完成，如未完成继续执行，如果完成，将推拉轴启动标清零，运行步变量Step更新；

d5)更新解锁轴目标位置、目标速度和操作模式，执行伺服启动到解锁位；

d6)判断解锁轴动作是否完成，如未完成继续执行，如果完成，将解锁轴启动标清零，运行步变量Step更新；

d7)更新推拉轴目标位置、目标速度和操作模式，执行推拉轴伺服到推拉安全位；

d8)判断推拉轴动作是否完成，如未完成继续执行，如果完成，将推拉轴启动标清零，运行步变量Step更新；

d9)判断有无“放电池失败”，如有,报“放电池失败”错误，如没有，各轴启动标清零、命令完成标置1，运行步变量Step清零，初始状态标清零，放电池指令包结束。