CN110039548B - 一种拼装机控制方法、装置及设备 - Google Patents

Abstract

本申请公开了一种拼装机控制方法，包括对拼装机的连杆机构进行运动学参数标定，获得第一运动学参数；对所述拼装机的并联微调机构进行运动学参数标定，获得第二运动学参数；将所述第一运动学参数和所述第二运动学参数代入预设串联机构运动学模型，对所述拼装机的串联机构进行运动学参数标定，获得第三运动学参数；当接收到控制指令时，结合所述第一运动学参数、第二运动学参数、第三运动学参数控制所述拼装机运行；该方法能够对拼装机整体结构的运动学参数进行完整标定，实现对拼装机更为精准的有效控制，提高拼装机的作业效率。本申请还公开了一种拼装机控制装置及设备，均具有上述有益效果。

Description

一种拼装机控制方法、装置及设备

技术领域

本申请涉及机器人运动学领域，特别涉及一种拼装机控制方法，还涉及一种拼装机控制装置及设备。

背景技术

在盾构工程施工过程中，管片拼装机是盾构机的重要部件，其主要功能是将预制管片衬砌到隧洞内表面，防止隧洞坍塌。拼装机的智能化研究可提高管片拼装工作的效率及安全性，对拼装机进行运动学建模是实现智能化的有力手段。

运动学建模时一般假定被建模对象有着精准的运动学参数，但由于拼装机为大型重载设备，其在机械加工以及设备安装过程中必然会产生误差，使得理想的运动学参数与实际参数存在偏差。进一步，因为拼装机体积庞大，各关节臂杆很长，使得运动学参数误差对最终运动位姿的影响会被放大，从而难以保证运动的高精度。因此，需要对拼装机的运动学参数进行标定，即在机器安装完成后识别出实际的运动学参数，并将实际参数替换原先的理论参数进行运动学计算，从而让拼装机的运动精度得到大幅度的提高。

然而，目前的运动学标定方法只能实现对应特定结构的运动学参数标定，而拼装机为串并联混合的复杂结构，需要标定的运动学参数较多，故无法采用某种既定方法对拼装机的整体结构进行完整标定，给拼装机运动学参数的标定工作带来了较大困难，导致拼装机的运动精度难以保证，从而降低了拼装机的作业效率，也给施工人员带来了安全隐患。

因此，如何提供一种能够对拼装机整体结构的运动学参数进行完整标定的方法，实现对拼装机更为精准的有效控制，进一步提高拼装机的作业效率是本领域技术人员亟待解决的问题。

发明内容

本申请的目的是提供一种拼装机控制方法，该拼装机控制方法能够对拼装机整体结构的运动学参数进行完整标定，实现对拼装机更为精准的有效控制，进一步提高了拼装机的作业效率；本申请的另一目的是提供一种拼装机控制装置及设备，也具有上述有益效果。

为解决上述技术问题，本申请提供了一种拼装机控制方法，所述拼装机控制方法包括：

对拼装机的连杆机构进行运动学参数标定，获得第一运动学参数；

对所述拼装机的并联微调机构进行运动学参数标定，获得第二运动学参数；

将所述第一运动学参数和所述第二运动学参数代入预设串联机构运动学模型，对所述拼装机的串联机构进行运动学参数标定，获得第三运动学参数；

当接收到控制指令时，结合所述第一运动学参数、第二运动学参数、第三运动学参数控制所述拼装机运行。

其中，所述对拼装机的连杆机构进行运动学参数标定，获得第一运动学参数，包括：

获取所述连杆机构中的各关节单独动作时，所述连杆机构末端的空间位置信息；

对各所述空间位置信息进行数据拟合，获得对应的关节轴线；

根据各所述关节轴线的位置关系计算获得所述第一运动学参数。

其中，所述对所述拼装机的并联微调机构进行运动学参数标定，获得第二运动学参数，包括：

获取第一预设数量组所述并联微调机构的实际位姿，以及与各所述实际位姿对应的并联微调机构中俯仰油缸和偏转油缸的行程数据信息；

通过预设并联微调机构运动学模型对所述行程数据信息和当前第二运动学参数进行计算，获得所述第一预设数量组所述并联微调机构的理论位姿，并结合所述第一预设数量组所述并联微调机构的实际位姿计算获得所述并联微调机构的总位姿误差；

判断所述并联微调机构的总位姿误差是否满足第二预设迭代精度；

若不满足，则根据所述预设并联微调机构运动学模型和所述当前第二运动学参数构建所述并联微调机构的误差系数矩阵；

根据所述误差系数矩阵和所述并联微调机构的总位姿误差计算获得所述第二运动学参数的微分变量；

将所述第二运动学参数的微分变量叠加至所述当前第二运动学参数，进行迭代处理，直至所述并联微调机构的总位姿误差满足所述第二预设迭代精度，获得所述第二运动学参数。

其中，所述将所述第一运动学参数和所述第二运动学参数代入预设串联机构运动学模型，对所述拼装机的串联机构进行运动学参数标定，获得第三运动学参数，包括：

获取第二预设数量组所述串联机构的实际位姿，以及与各所述实际位姿对应的串联机构中各关节传感器的数据信息；

将所述第一运动学参数和所述第二运动学参数代入预设串联机构运动学模型；

通过所述预设串联机构运动学模型对所述各关节传感器的数据信息和当前第三运动学参数进行计算，获得所述第二预设数量组所述串联机构的理论位姿，并结合所述第二预设数量组所述串联机构的实际位姿计算获得所述串联机构的总位姿误差；

判断所述串联机构的总位姿误差是否满足第三预设迭代精度；

若不满足，则通过拼装机运动学参数误差模型对所述串联机构的总位姿误差进行处理，获得所述第三运动学参数的微分变量；

将所述第三运动学参数的微分变量叠加至所述当前第三运动学参数，进行迭代处理，直至所述串联机构的总位姿误差满足所述第三预设迭代精度，获得所述第三运动学参数。

为解决上述技术问题，本申请还提供了一种拼装机控制装置，所述拼装机控制装置包括：

第一运动学参数标定模块，用于对拼装机的连杆机构进行运动学参数标定，获得第一运动学参数；

第二运动学参数标定模块，用于对所述拼装机的并联微调机构进行运动学参数标定，获得第二运动学参数；

第三运动学参数标定模块，用于将所述第一运动学参数和所述第二运动学参数代入预设串联机构运动学模型，对所述拼装机的串联机构进行运动学参数标定，获得第三运动学参数；

拼装机控制模块，用于当接收到控制指令时，结合所述第一运动学参数、第二运动学参数、第三运动学参数控制所述拼装机运行。

其中，所述第一运动学参数标定模块具体用于获取所述连杆机构中的各关节单独动作时，所述连杆机构末端的空间位置信息；对各所述空间位置信息进行数据拟合，获得对应的关节轴线；根据各所述关节轴线的位置关系计算获得所述第一运动学参数。

其中，所述第二运动学参数标定模块具体用于获取第一预设数量组所述并联微调机构的实际位姿，以及与各所述实际位姿对应的并联微调机构中俯仰油缸和偏转油缸的行程数据信息；通过预设并联微调机构运动学模型对所述行程数据信息和当前第二运动学参数进行计算，获得所述第一预设数量组所述并联微调机构的理论位姿，并结合所述第一预设数量组所述并联微调机构的实际位姿计算获得所述并联微调机构的总位姿误差；判断所述并联微调机构的总位姿误差是否满足第二预设迭代精度；若不满足，则根据所述预设并联微调机构运动学模型和所述当前第二运动学参数构建所述并联微调机构的误差系数矩阵；根据所述误差系数矩阵和所述并联微调机构的总位姿误差计算获得所述第二运动学参数的微分变量；将所述第二运动学参数的微分变量叠加至所述当前第二运动学参数，进行迭代处理，直至所述并联微调机构的总位姿误差满足所述第二预设迭代精度，获得所述第二运动学参数。

其中，所述第三运动学参数标定模块具体用于获取第二预设数量组所述串联机构的实际位姿，以及与各所述实际位姿对应的串联机构中各关节传感器的数据信息；将所述第一运动学参数和所述第二运动学参数代入预设串联机构运动学模型；通过所述预设串联机构运动学模型对所述各关节传感器的数据信息和当前第三运动学参数进行计算，获得所述第二预设数量组所述串联机构的理论位姿，并结合所述第二预设数量组所述串联机构的实际位姿计算获得所述串联机构的总位姿误差；判断所述串联机构的总位姿误差是否满足第三预设迭代精度；若不满足，则通过拼装机运动学参数误差模型对所述串联机构的总位姿误差进行处理，获得所述第三运动学参数的微分变量；将所述第三运动学参数的微分变量叠加至所述当前第三运动学参数，进行迭代处理，直至所述串联机构的总位姿误差满足所述第三预设迭代精度，获得所述第三运动学参数。

为解决上述技术问题，本申请还提供了一种拼装机控制设备，所述拼装机控制设备包括拼装机本体，以及如上任意一种拼装机控制装置。

本申请所提供的一种拼装机控制方法，包括对拼装机的连杆机构进行运动学参数标定，获得第一运动学参数；对所述拼装机的并联微调机构进行运动学参数标定，获得第二运动学参数；将所述第一运动学参数和所述第二运动学参数代入预设串联机构运动学模型，对所述拼装机的串联机构进行运动学参数标定，获得第三运动学参数；当接收到控制指令时，结合所述第一运动学参数、第二运动学参数、第三运动学参数控制所述拼装机运行。

可见，本申请所提供的拼装机控制方法，针对拼装机串并联混合的复杂结构，根据其结构特点对拼装机进行结构划分，并对划分后的各部分结构采用各自对应的特定方法进行运动学参数的标定，首先，分别对拼装机连杆机构的运动学参数和并联微调机构的运动学参数进行标定，然后结合连杆机构的运动学参数和并联微调机构的运动学参数标定获得拼装机串联结构的运动学参数，由此，通过阶段性划分实现了对拼装机整体结构中所有运动学参数的标定，有效提高了拼装机的运动精度，进一步提高了拼装机的作业效率，并降低了对施工人员带来的安全隐患。

本申请所提供的一种拼装机控制装置及设备，均具有上述有益效果，在此不再赘述。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本申请所提供的一种拼装机控制方法的流程示意图；

图2为本申请所提供的一种连杆机构运动学参数标定方法的流程示意图；

图3为本申请所提供的一种并联微调机构运动学参数标定方法的流程示意图；

图4为本申请所提供的一种串联机构运动学参数标定方法的流程示意图；

图5为本申请所提供的一种连杆机构的运动学模型；

图6为本申请所提供的一种并联微调机构的运动学模型；

图7为本申请所提供的一种串联机构的运动学模型；

图8为本申请所提供的一种拼装机控制装置的结构示意图。

具体实施方式

本申请的核心是提供一种拼装机控制方法，该拼装机控制方法能够对拼装机整体结构的运动学参数进行完整标定，实现对拼装机更为精准的有效控制，进一步提高了拼装机的作业效率；本申请的另一核心是提供一种拼装机控制装置及设备，也具有上述有益效果。

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

请参考图1，图1为本申请所提供的一种拼装机控制方法的流程示意图，该拼装机控制方法可以包括：

S100：对拼装机的连杆机构进行运动学参数标定，获得第一运动学参数；

本步骤旨在实现对拼装机连杆机构的运动学参数标定，以获得连杆机构的运动学参数，即上述第一运动学参数。具体而言，拼装机的连杆机构是由提升油缸及扼架构成，因此，对于连杆机构的运动学参数标定，可结合其结构特点采用适合的标定方法实现，以达到较高的精度。

作为一种优选实施例，请参考图2，图2为本申请所提供的一种连杆机构运动学参数标定方法的流程示意图，上述对拼装机的连杆机构进行运动学参数标定，获得第一运动学参数，可以包括：

S110：获取连杆机构中的各关节单独动作时，连杆机构末端的空间位置信息；

S120：对各空间位置信息进行数据拟合，获得对应的关节轴线；

S130：根据各关节轴线的位置关系计算获得第一运动学参数。

本申请实施例提供了一种较为具体的连杆机构的运动学参数标定方法，即基于轴线测量法实现。具体的，首先对连杆结构的末端位置进行测量，该测量过程主要在于，通过单独动作待测量关节获得对应的空间位置，即上述空间位置信息，由此，即可采用该方法实现获取连杆机构中各关节单独动作状态下对应的空间位置信息；其中，上述测量过程可基于激光跟踪仪实现，再将测量得到的数据发送至处理器进行后续处理；进一步，通过数据拟合的方法，根据各空间位置信息获得各关节对应的关节轴线，此时，通过对各关节轴线的位置关系进行计算，即可获得上述第一运动学参数。

S200：对拼装机的并联微调机构进行运动学参数标定，获得第二运动学参数；

本步骤旨在实现对拼装机并联微调机构的运动学参数标定，以获得并联微调机构的运动学参数，即上述第二运动学参数。具体而言，拼装机的并联微调机构实质上为通过并联的俯仰油缸和偏转油缸驱动的球形关节，因此，对于并联微调机构的运动学参数标定，可根据该结构特点采用适合的标定方法实现，以达到较高的精度。

作为一种优选实施例，请参考图3，图3为本申请所提供的一种并联微调机构运动学参数标定方法的流程示意图，上述对拼装机的并联微调机构进行运动学参数标定，获得第二运动学参数，可以包括：

S210：获取第一预设数量组并联微调机构的实际位姿，以及与各实际位姿对应的并联微调机构中俯仰油缸和偏转油缸的行程数据信息；

S220：通过预设并联微调机构运动学模型对行程数据信息和当前第二运动学参数进行计算，获得第一预设数量组并联微调机构的理论位姿，并结合第一预设数量组并联微调机构的实际位姿计算获得并联微调机构的总位姿误差；

S230：判断并联微调机构的总位姿误差是否满足第二预设迭代精度；若不满足，则执行S240；若满足，则执行S270；

S240：根据预设并联微调机构运动学模型和当前第二运动学参数构建并联微调机构的误差系数矩阵；

S250：根据误差系数矩阵和并联微调机构的总位姿误差计算获得第二运动学参数的微分变量；

S260：将第二运动学参数的微分变量叠加至当前第二运动学参数，返回S220进行迭代处理；

S270：获得第二运动学参数。

本申请实施例提供了一种较为具体的并联微调机构的运动学参数标定方法，即通过基于位姿误差的标定方法实现，该过程需要预先构建并联微调机构运动学模型，在实际标定过程中，直接调用即可。具体而言，首先使并联微调机构在其工作空间内随意动作，测量获得多组实际位姿，也即上述第一预设数量组实际位姿；同时，根据获取的并联微调机构中俯仰油缸和偏转油缸的行程数据信息结合当前第二运动学参数通过预设并联微调机构运动学模型计算获得其理论位姿；进一步将实际位姿与理论位姿相结合，计算获得并联微调机构的总位姿误差，此时判断总位姿误差是否满足第二预设迭代精度，若不满足，即可通过上述预设并联微调机构运动学模型对该并联微调机构的总位姿误差进行迭代处理，其迭代过程如下：根据预设并联微调机构运动学模型构建并联微调机构的误差系数矩阵，并结合上述总位姿误差计算获得第二运动学参数的微分变量，将该第二运动学参数的微分变量与当前的第二运动学参数叠加，得到新的第二运动学参数；进一步，根据更新后的第二运动学参数再次求得新的总位姿误差，并基于新的总位姿误差判断是否进入下次迭代处理。由此，通过迭代计算，便可获得并联微调机构满足一定精度条件，即上述第二预设迭代精度的第二运动学参数。另外，上述第一预设数量以及第二预设迭代精度的具体取值均可以由施工人员根据实际需求预先进行设定，其具体取值并不影响本技术方案的实施。

需要说明的是，对于上述第一运动学参数的获取过程与第二运动学参数的获取过程，其执行顺序并不唯一，可同时进行，也可先后进行，对于该先后顺序，本申请同样不做限定，图1中所示的执行顺序仅为本申请所提供的一种实施方式。

S300：将第一运动学参数和第二运动学参数代入预设串联机构运动学模型，对拼装机的串联机构进行运动学参数标定，获得第三运动学参数；

该步骤旨在实现对拼装机整体的运动学参数标定，可等效为对拼装机串联机构的运动学参数标定，从而获得第三运动学参数。具体而言，第三运动学参数通过预设串联机构运动学模型结合上述第一运动学参数和第二运动学参数进行标定计算即可获得，当然，该预设串联机构运动学模型同样为预先构建，在实际标定过程中，直接调用即可。

作为一种优选实施例，请参考图4，图4为本申请所提供的一种串联机构运动学参数标定方法的流程示意图，上述通过拼装机运动学参数误差模型对位姿误差进行迭代处理，获得第三运动学参数，可以包括：

S310：获取第二预设数量组串联机构的实际位姿，以及与各实际位姿对应的串联机构中各关节传感器的数据信息；

S320：将第一运动学参数和第二运动学参数代入预设串联机构运动学模型；

S330：通过预设串联机构运动学模型对各关节传感器的数据信息和当前第三运动学参数进行计算，获得第二预设数量组串联机构的理论位姿，并结合第二预设数量组串联机构的实际位姿计算获得串联机构的总位姿误差；

S340：判断串联机构的总位姿误差是否满足第三预设迭代精度；若不满足，则执行S350；若满足，则执行S370；

S350：通过拼装机运动学参数误差模型对串联机构的总位姿误差进行处理，获得第三运动学参数的微分变量；

S360：将第三运动学参数的微分变量叠加至当前第三运动学参数，返回S330进行迭代处理；

S370：获得第三运动学参数。

本申请实施例提供了一种较为具体的串联微调机构的运动学参数标定方法，与上述并联微调机构的运动学参数标定方法类似。具体的，在上述迭代过程中，以总位姿误差是否满足预设迭代精度作为迭代结束的标准，即每进行一次迭代处理，对拼装机的当前总位姿误差进行一次判断，若拼装机的当前总位姿误差不满足上述第三预设迭代精度，则通过拼装机运动学参数误差模型对总位姿误差进行处理，获得对应的运动学参数误差微分量，即上述第三运动学参数的微分变量，并将其叠加至当前第三运动学参数上继续进行迭代处理，直至获得满足第三预设迭代精度的总位姿误差，结束迭代循环，获得第三运动学参数。当然，以上每次迭代处理过程中的当前第三运动学参数均取值上一次迭代处理后的处理结果。另外，上述第二预设数量以及第三预设迭代精度的具体取值均可以由施工人员根据实际需求预先进行设定，其具体取值并不影响本技术方案的实施。

S400：当接收到控制指令时，结合第一运动学参数、第二运动学参数、第三运动学参数控制拼装机运行。

本步骤旨在实现对拼装机的控制，当接收到施工人员发送的控制指令时，即可结合以上标定的第一运动学参数、第二运动学参数、第三运动学参数对拼装机进行对应的运动学计算，以控制拼装机的高精度运行，也就是说，在拼装机的运动学计算过程中，不再使用理论运动学参数，而是将上述三个实际运动学参数替换理论运动学参数进行计算。通过标定的实际运动学参数进行运动学计算，极大地提高了拼装机的运动精度。

需要说明的是，上述S100～S300为拼装机运动学参数的标定过程，该过程执行一次即可，即只需在首次拼装机控制过程中执行，当再次对拼装机进行控制时，可对以上3组运动学参数进行直接调用，方便快捷。另外，可将上述3组运动学参数预先存储至相应的存储介质中，以便调用。

本申请所提供的拼装机控制方法，针对拼装机串并联混合的复杂结构，根据其结构特点对拼装机进行结构划分，并对划分后的各部分结构采用各自对应的特定方法进行运动学参数的标定，首先，分别对拼装机连杆机构的运动学参数和并联微调机构的运动学参数进行标定，然后结合连杆机构的运动学参数和并联微调机构的运动学参数标定获得拼装机串联结构的运动学参数，由此，通过阶段性划分实现了对拼装机整体结构中所有运动学参数的标定，有效提高了拼装机的运动精度，进一步提高了拼装机的作业效率，并降低了对施工人员带来的安全隐患。

在上述各个实施例的基础上，本申请提供了一种更为具体的拼装机控制方法。

(1)连杆机构的运动学参数标定：

请参考图5，图5为本申请所提供的一种连杆机构的运动学模型，该阶段待标定的各关节结构清晰且需标定参数不多，可采用轴线测量的方法进行标定。

首先，令其它关节不运动，单独进行微调机构的俯仰旋转动作，测量多组末端执行器的空间位置，由于是单轴运动，故对应的测量点应在同一平面，拟合测量点所在平面的方程；再联立球面方程求得拟合的空间圆心坐标及法线，从而求得微调机构俯仰运动的关节轴线Ax₄。进一步，单独进行微调机构的偏转旋转动作，通过测量多组末端执行器的空间位置数据并进行拟合，求得微调机构偏转运动的关节轴线Ax₅，由此，联立Ax₄、Ax₅两轴线方程，即可求得Ax₅与Ax₄的公垂线在Ax₄上的垂足Q₁的坐标。

进一步，单独运动待标定侧的相对侧的提升油缸，通过测量多组末端执行器的空间位置数据并进行拟合，求得旋转关节的轴线Ax₃，求出点Q₁到轴线Ax₃的距离即为运动学参数l₂的实际值，同时求出Q₁到Ax₃的垂线l₂及垂足Q₂的坐标。进一步，单独运动待标定侧的提升油缸，通过测量多组末端执行器的空间位置数据并进行拟合，求得空间直线Ax′₂，计算直线l₂与Ax′₂的夹角即为运动学参数α₀的实际值。最后，令其它关节不动，仅进行拼装机的360°旋转动作，通过测量多组末端执行器的空间位置数据并进行拟合，求得旋转关节的轴线Ax₁，再求得与Ax′₂同方向向量且过Q₂点的直线Ax₂，计算Ax₁与Ax₂之间的空间距离即为运动学参数l₁的实际值。

由此，基于上述过程，实现了第一运动学参数l₁，l₂，α₀的标定。

(2)并联微调机构的运动学参数标定：

请参考图6，图6为本申请所提供的一种并联微调机构的运动学模型，该过程采用基于位姿误差模型的运动学参数标定方法。

并联微调机构实质为球形关节，通过俯仰油缸与偏转油缸的动作使得关节绕x轴与z轴旋转运动。令静平台坐标系为o-xyz，动平台坐标系为o′-x′y′z′。相对于o-xyz系，俯仰油缸静平台上的端点为A₁(a_1x,a_1y,a_1z)，偏转油缸静平台上的端点为A₂(a_2x,a_2y,a_2z)；相对于o′-x′y′z′系，俯仰油缸动平台上的端点为B₁(b_1x,b_1y,b_1z)，偏转油缸动平台上的端点为B₂(b_2x,b_2y,b_2z)。俯仰油缸的长度为h₁，偏转油缸的长度为h₂。设动平台绕x轴旋转角度为α，绕z轴旋转角度为γ，则动平台相对静平台的旋转变化矩阵R为：

由于油缸杆长矢量为：h_i＝RB_i-A_i(i＝1,2)，而杆矢量满足关系式：

则有：f_i(α,γ,a_ix,a_iy,a_iz,b_ix,b_iy,b_iz,h_i)＝0(i＝1,2)；进一步，对该方程组进行全微分，变换为矩阵形式，即为：J·D＝dM；其中：

D＝[dα dγ]^T；

由此便建立了位姿误差与并联机构运动学参数误差之间的相互关系，即并联微调机构基于位姿误差的运动学参数误差模型。

令k为迭代次数，ε'为迭代精度，让并联微调机构在工作空间内随意动作，记录多组运动位姿及其对应的两微调油缸的行程数据；计算各组的理论位姿，并将测量的多组实际位姿与理论位姿比较得到位姿误差D'_k。进一步，将D'_k代入上述矩阵，求解即可得到待标定运动学参数的误差dM'_k，将dM'_k修正到当前的运动学参数上，求得新的理论计算位姿，再与测量位姿进行比较到新的位姿误差D'_k+1。由此，当||D'_k+1||_∞≤ε时，迭代计算结束，当前的运动学参数M即为标定出的实际参数；当||D'_k+1||_∞＞ε时，继续将D'_k+1代入上述矩阵，求解运动学参数误差，继续迭代计算，直至满足迭代停止条件。

由此，基于上述过程，实现了第二运动学参数M的标定。

(3)串联机构(等效拼装机整体结构)的运动学参数标定：

请参考图7，图7为本申请所提供的一种串联机构的运动学模型，该过程同样采用基于位姿误差模型的运动学参数标定方法。

首先，建立拼装机串联机构基于位姿误差的运动学参数误差模型。令各关节的D-H参数的误差量为：连杆长度误差Δa_i、连杆转角误差Δα_i、连杆偏距误差Δd_i、关节角误差Δθ_i，其中i＝1,2,3,4,5,6,7。在D-H参数误差的影响下，连杆坐标系变换矩阵由

变为

其中：

令：

可得到：

则有：

令：

又因为：

令：D_i＝[dx_i dy_i dz_i δx_i δy_i δz_i]^T，dB_i＝[Δa_i Δα_i Δd_i Δθ_i]^T；结合上述

可得到表达式：

D_i＝G_i·dB_i；

其中，G_i为误差系数矩阵；

令

表示任意关节坐标系到末端工具坐标系t的变换矩阵，即：

其中，a表示工具坐标系t中z轴的单位矢量，即接近矢量，o表示工具坐标系t中y轴的单位矢量，即方向矢量，n表示工具坐标系t中x轴的单位矢量，通过右手法则，根据n＝o×a求得，即法向矢量。以上三个单位正交矢量n、o和a描述了工具坐标系t的姿态，而工具坐标系t的位置通过位置矢量p来规定，代表工具坐标系t的原点。

令

表示任意关节坐标系到末端工具坐标系t之间的微分误差变换，即：

将微分运动误差和各个关节的微分误差都变换到末端工具坐标系下，令总微分运动误差矢量为D，则有矩阵表达式：D＝H·dB；

其中，

dB为40×1的杆件误差矢量。由此，建立了拼装机串联机构基于位姿误差的运动学参数误差模型。

令k为迭代次数，ε为迭代精度，让拼装机在工作空间内随意动作，测量多个运动位姿并记录其对应的各关节传感器的值，将两个提升油缸的传感器读数结合(1)中标定出的连杆机构运动学参数，计算得到提升油缸与扼架的转动角度和扼架内筒与扼架之间的位移量，将偏转微调油缸及俯仰微调油缸的传感器读数结合(2)中标定出的微调机构运动学参数计算得到微调俯仰角度和微调偏转角度。进一步，计算各组的理论位姿，与实际位姿进行比较获得位姿误差D_k，并计算误差系数矩阵H_k，将D_k代入上述矩阵表达式，求得待标定运动学参数的误差向量dB_k。进一步，将dB_k修正到当前运动学参数上，求得新的理论位姿，再与测量的实际位姿进行比较得到新的位姿误差D_k+1。判断||D_k+1||_∞≤ε是否成立，若成立，则结束迭代计算，当前的运动学参数B即为标定出的实际的运动学参数；若不成立，则继续计算误差系数矩阵H_k+1，将D_k+1代入上述矩阵表达式中求解新的运动学参数误差，继续迭代计算，直至满足迭代停止条件。

由此，基于上述过程，实现了第三运动学参数B的标定。

综上，基于以上三个过程，即完成了对拼装机所有运动学参数的标定。另外，以上每个阶段均可基于激光跟踪仪完成标定工作，无需增加额外的标定工具。

请参考图8，图8为本申请所提供的一种拼装机控制装置的结构示意图，该拼装机控制装置可包括：

第一运动学参数标定模块10，用于第一运动学参数标定模块，用于对拼装机的连杆机构进行运动学参数标定，获得第一运动学参数；

第二运动学参数标定模块20，用于对拼装机的并联微调机构进行运动学参数标定，获得第二运动学参数；

第三运动学参数标定模块30，用于将第一运动学参数和第二运动学参数代入预设串联机构运动学模型，对拼装机的串联机构进行运动学参数标定，获得第三运动学参数；

拼装机控制模块40，用于当接收到控制指令时，结合第一运动学参数、第二运动学参数、第三运动学参数控制拼装机运行。

作为一种优选实施例，上述第一运动学参数标定模块10可具体用于获取连杆机构中的各关节单独动作时，连杆机构末端的空间位置信息；对各空间位置信息进行数据拟合，获得对应的关节轴线；根据各关节轴线的位置关系计算获得第一运动学参数。

作为一种优选实施例，上述第二运动学参数标定模块20可具体用于获取第一预设数量组并联微调机构的实际位姿，以及与各实际位姿对应的并联微调机构中俯仰油缸和偏转油缸的行程数据信息；通过预设并联微调机构运动学模型对行程数据信息和当前第二运动学参数进行计算，获得第一预设数量组并联微调机构的理论位姿，并结合第一预设数量组并联微调机构的实际位姿计算获得并联微调机构的总位姿误差；判断并联微调机构的总位姿误差是否满足第二预设迭代精度；若不满足，则根据预设并联微调机构运动学模型和当前第二运动学参数构建并联微调机构的误差系数矩阵；根据误差系数矩阵和并联微调机构的总位姿误差计算获得第二运动学参数的微分变量；将第二运动学参数的微分变量叠加至当前第二运动学参数，进行迭代处理，直至并联微调机构的总位姿误差满足第二预设迭代精度，获得第二运动学参数。

作为一种优选实施例，上述第三运动学参数标定模块30可具体用于获取第二预设数量组串联机构的实际位姿，以及与各实际位姿对应的串联机构中各关节传感器的数据信息；将第一运动学参数和第二运动学参数代入预设串联机构运动学模型；通过预设串联机构运动学模型对各关节传感器的数据信息和当前第三运动学参数进行计算，获得第二预设数量组串联机构的理论位姿，并结合第二预设数量组串联机构的实际位姿计算获得串联机构的总位姿误差；判断串联机构的总位姿误差是否满足第三预设迭代精度；若不满足，则通过拼装机运动学参数误差模型对串联机构的总位姿误差进行处理，获得第三运动学参数的微分变量；将第三运动学参数的微分变量叠加至当前第三运动学参数，进行迭代处理，直至串联机构的总位姿误差满足第三预设迭代精度，获得第三运动学参数。

对于本申请提供的装置的具体内容，本申请在此不做赘述。

本申请还提供了一种拼装机控制设备，该拼装机可包括拼装机本体，以及上述任意一种拼装机控制装置，其中，拼装机控制设备在拼装机上的安装位置并不影响本技术方案的实施。对于本申请提供的拼装机的具体内容，本申请在此不做赘述。

说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

以上对本申请所提供的拼装机控制方法、装置及设备进行了详细介绍。本文中应用了具体个例对本申请的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本申请的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本申请原理的前提下，还可以对本申请进行若干改进和修饰，这些改进和修饰也落入本申请权利要求的保护范围要素。

Claims (3)

1.一种拼装机控制方法，其特征在于，包括：

获取连杆机构中的各关节单独动作时，所述连杆机构末端的空间位置信息；对各所述空间位置信息进行数据拟合，获得对应的关节轴线；根据各所述关节轴线的位置关系计算获得第一运动学参数；

获取第一预设数量组并联微调机构的实际位姿，以及与各所述实际位姿对应的并联微调机构中俯仰油缸和偏转油缸的行程数据信息；通过预设并联微调机构运动学模型对所述行程数据信息和当前第二运动学参数进行计算，获得所述第一预设数量组所述并联微调机构的理论位姿，并结合所述第一预设数量组所述并联微调机构的实际位姿计算获得所述并联微调机构的总位姿误差；判断所述并联微调机构的总位姿误差是否满足第二预设迭代精度；若不满足，则根据所述预设并联微调机构运动学模型和所述当前第二运动学参数构建所述并联微调机构的误差系数矩阵；根据所述误差系数矩阵和所述并联微调机构的总位姿误差计算获得第二运动学参数的微分变量；将所述第二运动学参数的微分变量叠加至所述当前第二运动学参数，进行迭代处理，直至所述并联微调机构的总位姿误差满足所述第二预设迭代精度，获得第二运动学参数；

获取第二预设数量组串联机构的实际位姿，以及与各所述实际位姿对应的串联机构中各关节传感器的数据信息；将所述第一运动学参数和所述第二运动学参数代入预设串联机构运动学模型；通过所述预设串联机构运动学模型对所述各关节传感器的数据信息和当前第三运动学参数进行计算，获得所述第二预设数量组所述串联机构的理论位姿，并结合所述第二预设数量组所述串联机构的实际位姿计算获得所述串联机构的总位姿误差；判断所述串联机构的总位姿误差是否满足第三预设迭代精度；若不满足，则通过拼装机运动学参数误差模型对所述串联机构的总位姿误差进行处理，获得第三运动学参数的微分变量；将所述第三运动学参数的微分变量叠加至所述当前第三运动学参数，进行迭代处理，直至所述串联机构的总位姿误差满足所述第三预设迭代精度，获得第三运动学参数；

当接收到控制指令时，结合所述第一运动学参数、第二运动学参数、第三运动学参数控制所述拼装机运行。

2.一种拼装机控制装置，其特征在于，包括：

第一运动学参数标定模块，用于获取连杆机构中的各关节单独动作时，所述连杆机构末端的空间位置信息；对各所述空间位置信息进行数据拟合，获得对应的关节轴线；根据各所述关节轴线的位置关系计算获得第一运动学参数；

第二运动学参数标定模块，用于获取第一预设数量组并联微调机构的实际位姿，以及与各所述实际位姿对应的并联微调机构中俯仰油缸和偏转油缸的行程数据信息；通过预设并联微调机构运动学模型对所述行程数据信息和当前第二运动学参数进行计算，获得所述第一预设数量组所述并联微调机构的理论位姿，并结合所述第一预设数量组所述并联微调机构的实际位姿计算获得所述并联微调机构的总位姿误差；判断所述并联微调机构的总位姿误差是否满足第二预设迭代精度；若不满足，则根据所述预设并联微调机构运动学模型和所述当前第二运动学参数构建所述并联微调机构的误差系数矩阵；根据所述误差系数矩阵和所述并联微调机构的总位姿误差计算获得第二运动学参数的微分变量；将所述第二运动学参数的微分变量叠加至所述当前第二运动学参数，进行迭代处理，直至所述并联微调机构的总位姿误差满足所述第二预设迭代精度，获得第二运动学参数；

第三运动学参数标定模块，用于获取第二预设数量组串联机构的实际位姿，以及与各所述实际位姿对应的串联机构中各关节传感器的数据信息；将所述第一运动学参数和所述第二运动学参数代入预设串联机构运动学模型；通过所述预设串联机构运动学模型对所述各关节传感器的数据信息和当前第三运动学参数进行计算，获得所述第二预设数量组所述串联机构的理论位姿，并结合所述第二预设数量组所述串联机构的实际位姿计算获得所述串联机构的总位姿误差；判断所述串联机构的总位姿误差是否满足第三预设迭代精度；若不满足，则通过拼装机运动学参数误差模型对所述串联机构的总位姿误差进行处理，获得第三运动学参数的微分变量；将所述第三运动学参数的微分变量叠加至所述当前第三运动学参数，进行迭代处理，直至所述串联机构的总位姿误差满足所述第三预设迭代精度，获得第三运动学参数；

拼装机控制模块，用于当接收到控制指令时，结合所述第一运动学参数、第二运动学参数、第三运动学参数控制所述拼装机运行。

3.一种拼装机控制设备，其特征在于，包括拼装机本体，以及如权利要求2所述的拼装机控制装置。

Images