一种伺服控制方法、处理器、存储介质和可移动平台

一种伺服控制方法、处理器、存储介质和可移动平台

　　技术领域

　　本发明涉及控制技术领域，尤其涉及一种伺服控制方法、处理器、存储介质和可移动平台。

　　背景技术

　　现代物流系统、仓储系统或工业生产线对精准程度的要求越来越高。工厂朝着智慧化，无人化工厂转型发展，每个零部件都造价不菲，这要求工厂每台生产设备都需要极高的运行精度，才能使工厂整个系统高度鲁棒闭环运作起来。可移动平台作为产线间的搬运设备，对接精度需达到毫米级以上才能满足搬运指标要求。

　　现有的可移动平台中，基于差分底盘或双舵轮底盘这类难于以三自由度快速调整位姿的底盘的移动平台被广泛使用，但与可三自由度快速调整位姿的全向轮底盘相比，上述类型底盘存在一些局限性。例如，对于差分底盘，由于非完整约束限制，无法在狭窄空间下快速收敛横向误差；对于双舵轮底盘，由于设计本身限制，在毫米级高精度横向对接时，高频率全向调整会使舵角频繁改变，无法精确满足运动学理论模型，同时会降低其使用寿命。对于某些应用场景，现有调整方法往往通过完整的约束精准调整到点，计算过程过于繁杂，并且效率低，但实际上可以忽略某些维度的误差，并达到同样的目的，以此简化计算与执行过程，提高执行效率。

　　发明内容

　　本发明针对上述现有产品存在的问题，提供一种采用非完整约束进行的高精度、高效率的伺服控制方法、处理器、存储介质和可移动平台。

　　本发明公开了一种伺服控制方法，

　　确定预设目标位姿；

　　获取至少一个标识，得到当前位姿；

　　根据预设目标位姿和当前位姿的相对位姿关系，确定偏移角度；

　　基于标识和预设目标位姿得到目标直线，根据所述的目标直线和当前位姿的相对位姿关系，确定偏移位移；

　　根据所述偏移位移和偏移角度获取控制指令。

　　作为优选，以预设规则间隔性获取至少同一标识，在每一预设规则间隔内根据所述的标识得到当前时刻的当前位姿，获取控制指令；利用在每一预设规则间隔内获取的控制指令对当前位姿进行伺服控制。

　　作为优选，所述的获取至少一个标识，得到当前位姿，包括：基于所述的标识建立标识坐标系，得到当前位姿在标识坐标系下的位移和角度坐标。

　　作为优选，所述的标识为垂直于地面设置的标识。

　　作为优选，根据所述的根据预设目标位姿和当前位姿的相对位姿关系，确定偏移角度包括：根据所述的预设目标位姿在标识坐标系下的姿态，以及所述的当前位姿在标识坐标系下的姿态，得到预设目标位姿和当前位姿之间的相对位姿关系，提取偏移角度。

　　作为优选，所述的根据目标直线和当前位姿的相对位姿关系，确定偏移位移，包括：基于所述的预设目标位姿的位移坐标创建虚拟目标点，根据预设目标位姿建立预设目标点坐标系；根据所述的虚拟目标点创建虚拟目标位姿；根据所述的虚拟目标位姿在所述的预设目标点坐标系下的位移和角度的坐标，以及当前位姿在预设目标点坐标系下的位移和角度的坐标，得到当前位姿与虚拟目标位姿的相对位姿关系，基于标识和预设目标位姿得到目标直线，提取在与目标直线垂直方向上当前位姿与虚拟目标位姿的偏差位移。

　　作为优选，所述的根据所述的虚拟目标位姿在所述的预设目标点坐标系下的位移和角度的坐标包括：基于所述的虚拟目标位姿建立虚拟目标点坐标系，根据所述的虚拟目标点坐标系与预设目标点坐标系坐标系的坐标轴原点的位移和坐标轴的角度偏差，计算所述的虚拟目标位姿和预设目标位姿的相对位姿关系。

　　作为优选，所述的基于所述的虚拟目标位姿建立虚拟目标点坐标系包括：根据在所述的标识坐标系下所述的标识与预设目标点坐标系原点的向量关系，和在标识坐标系下所述的标识与标识坐标系Z轴方向上距离预设目标点单位向量所取得的点P的向量关系，以及所述的预设目标点坐标系与所述的标识坐标系的转换关系，得到在预设目标点坐标系下的虚拟目标位姿在目标直线方向的单位向量，记为虚拟目标点坐标系的坐标轴Y轴；根据垂直于地面的方向取单位向量，记为虚拟目标点坐标系的坐标轴Z轴；基于空间直角坐标系原则，得到与所述的目标直线垂直的目标位姿坐标系的坐标轴X轴。

　　本发明公开了一种伺服控制的处理器，其用于调用程序代码，当所述程序代码被执行时，所述处理器用于执行以下操作：确定预设目标位姿 ；获取至少一个标识，得到当前位姿；根据预设目标位姿和当前位姿的相对位姿关系，确定偏移角度；基于标识和预设目标位姿得到目标直线，根据所述的目标直线和当前位姿的相对位姿关系，确定偏移位移；根据所述偏移位移和偏移角度进行伺服控制。

　　作为优选，所述的处理器，还用于基于所述的标识建立标识坐标系，得到当前位姿在标识坐标系下的位移和角度坐标。

　　作为优选，所述的处理器，以预设规则间隔性获取至少同一标识，在每一预设规则间隔内根据所述的标识得到当前时刻的当前位姿，获取控制指令；利用在每一预设规则间隔内获取的控制指令对当前位姿进行伺服控制。

　　作为优选，所述的处理器，还用于根据所述的预设目标位姿在标识坐标系下的姿态，以及所述的当前位姿在标识坐标系下的姿态，得到预设目标位姿和当前位姿之间的相对位姿关系，提取偏移角度。

　　作为优选，所述的处理器，还用于基于所述的预设目标位姿的位移坐标创建虚拟目标点，根据预设目标位姿建立预设目标点坐标系；根据所述的虚拟目标点创建虚拟目标位姿；根据所述的虚拟目标位姿在所述的预设目标点坐标系下的位移和角度的坐标，以及当前位姿在预设目标点坐标系下的位移和角度的坐标，得到当前位姿与虚拟目标位姿的相对位姿关系，基于标识和预设目标位姿得到目标直线，提取在与目标直线垂直方向上当前位姿与虚拟目标位姿的偏差位移。

　　作为优选，所述的处理器，还用于基于所述的虚拟目标位姿建立虚拟目标点坐标系，根据所述的虚拟目标点坐标系与预设目标点坐标系坐标系的坐标轴原点的位移和坐标轴的角度偏差，计算所述的虚拟目标位姿和预设目标位姿的相对位姿关系。

　　作为优选，所述的处理器，还用于根据在所述的标识坐标系下所述的标识与预设目标点坐标系原点的向量关系，和在标识坐标系下所述的标识与标识坐标系Z轴方向上距离预设目标点单位向量所取得的点P的向量关系，以及所述的预设目标点坐标系与所述的标识坐标系的转换关系，得到在预设目标点坐标系下的虚拟目标位姿在目标直线方向的单位向量，记为虚拟目标点坐标系的坐标轴Y轴；根据垂直于地面的方向取单位向量，记为虚拟目标点坐标系的坐标轴Z轴；基于空间直角坐标系原则，得到与所述的目标直线垂直的目标位姿坐标系的坐标轴X轴。

　　一种存储介质，所述存储介质上存储有计算机指令，所述计算机指令被执行时，实现所述的一种伺服控制方法。

　　一种可移动平台，包括：包括采集器，用于获取标识；还包括所述的处理器，或者，所述的存储介质。

　　采用以上技术后，本发明与现有技术相比，具有以下优点：

　　本发明用于在非完整约束下，通过只计算确定偏移角度和偏移位移进行伺服控制，以使控制对象调整至目标直线上。由于调整至目标直线上，不精确到点，故本发明计算过程只计算调整角度和一个位移分量，调整方法较简单，计算量小，同时目标直线和角度约束保证了高精度。本发明克服了非全向移动底盘在调节过程中的不足。本发明忽略某个维度上的误差，利用其它自由度进行补偿，达到类似完整约束下的可三自由度快速调整位姿的全向底盘快速、精确到点效果。

　　附图说明

　　图1是本发明的流程框图。

　　图2是本发明的伺服控制方法示意图。

　　图3是本发明的方法示意图。

　　图4是本发明的可移动平台的结构图。

　　附图标识说明：1、本体，2、移动底盘，3、对接装置，4、采集器。

　　具体实施方式

　　下面结合附图和具体实施例对本发明作进一步说明。本发明可运用在可移动平台进行高精度对接的场景中，也可以运用在其他非完整约束情况下的高精度定位场景。

　　在可移动平台进行高精度对接场景的实施例中，可应用于如图4所述的一种可移动平台，包括采集器，用于获取标识W；还包括所述的处理器或者所述的存储介质。所述的可移动平台包括安装在本体上的对接装置和移动底盘。所述的移动底盘包括但不限于非全向移动底盘，在如图4所示的可移动平台中，采用了一种容易高频抖动的双舵轮底盘，无法灵活地朝向任意方向前进，其控制难度更大。通过本发明所阐述的方法，可以最大化地减小控制难度、缩小误差。

　　更具体地，所述的对接装置，为伸缩式叉臂，所述的对接装置的伸出长度可依据现场需求进行调节。所述的伸缩式叉臂上安装传感器，用以识别叉臂伸出的长度。由于伸出长度可以调节，故只需将所述的可移动平台的对接装置调整至目标直线上的一个点位，将对接装置以目标角度进行伸缩，即可实现高精度对接。计算方法简便。在如图4所示的可移动平台中，为了实现其精准对接，通过本发明所阐述的方式，可将对接装置的对接精度控制在±2mm，保证了高精度对接。

　　所述的采集器安装在所述的对接装置上，可以避免对接装置与所述的车体之间存在机械误差。

　　在具体实施过程中，将可移动平台导航至目标位姿附近，位于采集器的视野范围之内，根据标识W对可移动平台进行伺服控制，以高精度地调整至目标位姿。

　　如图1所示，本发明公开的一种伺服控制方法，

　　步骤101，确定预设目标位姿。

　　在本发明中，预设目标位姿可以通过示教的方式先控制可移动平台移动到目标位姿读取标识W来实现。所述的预设目标位姿，并不一定是所述的可移动平台实际到达的目标位姿，仅用以实现对偏移角度和偏移位移的计算。

　　步骤102，获取至少一个标识W，得到当前位姿；

　　其中，包括从采集器采集的标识W，所述标识W可以为二维码，或者其他可以用于定位的图案或特征。进一步的，该采集器不限于采集的角度、类型型号、采集的频率。采集器进行多次采集，以保证即时性和准确性。采集器的采集方式包括但不限于：每间隔固定的移动距离采集一次所述的标识W，并计算当前位姿。如图2所示，当前位姿用如图2中的E点示意。可基于所述的标识W建立标识坐标系，再采用建立三维坐标系并进行换算的方式，计算得到当前位姿。所述的位姿包括位置和姿态。

　　步骤103，根据预设目标位姿和当前位姿的相对位姿关系，确定偏移角度；

　　所述的预设目标位姿和当前位姿转换至同一三维坐标系下，可确定相差的空间角度，即相对位姿关系，确定偏移角度。所述的偏移角度，即可移动平台移动至目标位姿，所需要旋转的角度。

　　步骤104，基于标识W和预设目标位姿得到目标直线，根据所述的目标直线和当前位姿的相对位姿关系，确定偏移位移；

　　图2所示，所述的目标直线用如图2中的L示意，所述的目标直线是可移动平台最终移动到的位置的线性约束。所述的可移动平台的目标位姿处于所述的目标直线上的任一可能的位置，所述的预设目标位姿为处于所述的目标直线上的一个具体的点位。所述的目标直线和所述的当前位姿可通过读取相同的标识W得到，误差更小，精度更高。

　　步骤105，根据所述偏移位移和偏移角度获取控制指令。

　　为了进一步提升精度，可以将上述执行步骤拆分成多次执行。更具体地，以预设规则间隔性获取至少同一标识W，在每一预设规则间隔内根据所述的标识W得到当前时刻的当前位姿，获取控制指令；利用在每一预设规则间隔内获取的控制指令对当前位姿进行伺服控制。所述的预设规则间隔，可以是固定时间间隔，也可以是固定距离间隔，或者其他形式。当预设规则间隔周期越短，频率越高，被控制指令控制到达目标位姿的精度越高。

　　如图2所示，路径S为所述的伺服控制可能发生的实际路径，目标位姿T为所述的伺服控制可能到达的实际位姿。其实现过程通过只识别至少一个标识W，可以同时调节角度和位移，无需单独反复调试。

　　在步骤102中，所述的获取至少一个标识W，得到当前位姿，包括：基于所述的标识W建立标识坐标系，得到当前位姿在标识坐标系下的位移和姿态。可以采用公告号为CN110209169A，专利名称为一种可移动平台的控制方法、控制装置及可移动平台所公开的方法，计算获取。所述的标识与所述的对接装置待对接的设备之间的相对位姿关系固定不变。在本实施例中，只例举了采用一个标识的方式，同理可证存在多个标识的其他拓展方式。

　　在本实施例中，所述的标识W为垂直于地面设置的标识W，如此，基于所述的标识W建立的标识坐标系的Z轴（垂直于标识W所处平面）便与地面平行。所述的标识坐标系的Z轴用以计算所述的目标直线。在本实施例中，所述的标识坐标系与所述的目标直线相互平行。进一步简化了计算的难度，提高了运算效率。

　　在步骤103中，根据所述的根据预设目标位姿和当前位姿的相对位姿关系，确定偏移角度包括：根据所述的预设目标位姿在标识坐标系下的姿态，以及所述的当前位姿在标识坐标系下的姿态，得到预设目标位姿和当前位姿之间的相对位姿关系，提取偏移角度。

　　更具体地，可以采用以下方法：

　　由得，即当前位姿在预设目标位姿坐标系下的位姿，提取姿态参数，即偏移角度ΔƟ。

　　在步骤104中，如图3所示，所述的根据目标直线和当前位姿的相对位姿关系，确定偏移位移，包括：基于所述的预设目标位姿的位移坐标创建虚拟目标点，根据预设目标位姿建立预设目标点坐标系；根据所述的虚拟目标点创建虚拟目标位姿；根据所述的虚拟目标位姿在所述的预设目标点坐标系下的位移和角度的坐标，以及当前位姿在预设目标点坐标系下的位移和角度的坐标，得到当前位姿与虚拟目标位姿的相对位姿关系，基于标识W和预设目标位姿得到目标直线，提取在与目标直线垂直方向上当前位姿与虚拟目标位姿的偏差位移。

　　更具体地，可以采用以下方法：

　　由得,即当前位姿在虚拟目标位姿坐标系下的位姿，提取垂直于目标直线的位移参数，即偏移位移。

　　所述的虚拟目标点、虚拟目标位姿，是计算过程中为了便于计算所引入的中间参数。为了方便计算，在本实施例中，所述的虚拟目标点与所述的预设目标点坐标系的原点重合，所述的虚拟目标位姿与所述的预设目标位姿之间需旋转矩阵换算。可以理解为，虚拟目标位姿是基于二维码坐标系Z轴所确定的目标直线上的目标位姿的一个点位，而预设目标位姿是基于实际示教结果所得的目标位姿中的一个点位。可以理解为，所述的虚拟目标位姿是从预设目标位姿中提取了位移参数，忽略了姿态的参数，简化了计算过程。其中，所述的虚拟目标点与所述的偏差位移，是可移动平台移动至所述的目标直线的最短直线距离。

　　所述的根据所述的虚拟目标位姿在所述的预设目标点坐标系下的位移和角度的坐标，包括但不限于：基于所述的虚拟目标位姿建立虚拟目标点坐标系，根据所述的虚拟目标点坐标系与预设目标点坐标系坐标系的坐标轴原点的位移和坐标轴的角度偏差，计算所述的虚拟目标位姿和预设目标位姿的相对位姿关系。

　　其中，所述的基于所述的虚拟目标位姿建立虚拟目标点坐标系包括：根据在所述的标识坐标系下所述的标识W与预设目标点坐标系原点的向量关系，和在标识坐标系下所述的标识W与标识坐标系Z轴方向上距离预设目标点单位向量所取得的点P的向量关系，以及所述的预设目标点坐标系与所述的标识坐标系的转换关系，得到在预设目标点坐标系下的虚拟目标位姿在目标直线方向的单位向量，记为虚拟目标点坐标系的坐标轴Y轴；根据垂直于地面的方向取单位向量，记为虚拟目标点坐标系的坐标轴Z轴；基于空间直角坐标系原则，得到与所述的目标直线垂直的目标位姿坐标系的坐标轴X轴。

　　更具体地，可以采用以下方法：

　　由 ，得在预设目标点坐标系下的虚拟目标位姿在目标直线方向的单位向量，记为；其中，所述的点P为标识坐标系Z轴方向上距离预设目标点单位向量所取得的点。

　　如图3所示，所述的标识坐标系Z轴方向与目标直线方向平行。

　　一种非完整约束下的伺服控制的处理器，其用于调用程序代码，当所述程序代码被执行时，所述处理器用于执行以下操作：确定预设目标位姿 ；获取至少一个标识W，得到当前位姿；根据预设目标位姿和当前位姿的相对位姿关系，确定偏移角度；基于标识W和预设目标位姿得到目标直线，根据所述的目标直线和当前位姿的相对位姿关系，确定偏移位移；根据所述偏移位移和偏移角度进行伺服控制。

　　作为一种实施方式，所述的处理器，还用于基于所述的标识W建立标识坐标系，得到当前位姿在标识坐标系下的位移和角度坐标。

　　作为一种实施方式，所述的处理器，以预设规则间隔性获取至少同一标识W，在每一预设规则间隔内根据所述的标识W得到当前时刻的当前位姿，获取控制指令；利用在每一预设规则间隔内获取的控制指令对当前位姿进行伺服控制。

　　作为一种实施方式，所述的处理器，还用于根据所述的预设目标位姿在标识坐标系下的姿态，以及所述的当前位姿在标识坐标系下的姿态，得到预设目标位姿和当前位姿之间的相对位姿关系，提取偏移角度。

　　作为一种实施方式，所述的处理器，还用于基于所述的预设目标位姿的位移坐标创建虚拟目标点，根据预设目标位姿建立预设目标点坐标系；根据所述的虚拟目标点创建虚拟目标位姿；根据所述的虚拟目标位姿在所述的预设目标点坐标系下的位移和角度的坐标，以及当前位姿在预设目标点坐标系下的位移和角度的坐标，得到当前位姿与虚拟目标位姿的相对位姿关系，基于标识W和预设目标位姿得到目标直线，提取在与目标直线垂直方向上当前位姿与虚拟目标位姿的偏差位移。

　　作为一种实施方式，所述的处理器，还用于基于所述的虚拟目标位姿建立虚拟目标点坐标系，根据所述的虚拟目标点坐标系与预设目标点坐标系坐标系的坐标轴原点的位移和坐标轴的角度偏差，计算所述的虚拟目标位姿和预设目标位姿的相对位姿关系。

　　作为一种实施方式，所述的处理器，还用于根据在所述的标识坐标系下所述的标识W与预设目标点坐标系原点的向量关系，和在标识坐标系下所述的标识W与标识坐标系Z轴方向上距离预设目标点单位向量所取得的点P的向量关系，以及所述的预设目标点坐标系与所述的标识坐标系的转换关系，得到在预设目标点坐标系下的虚拟目标位姿在目标直线方向的单位向量，记为虚拟目标点坐标系的坐标轴Y轴；根据垂直于地面的方向取单位向量，记为虚拟目标点坐标系的坐标轴Z轴；基于空间直角坐标系原则，得到与所述的目标直线垂直的目标位姿坐标系的坐标轴X轴。

　　一种存储介质，所述存储介质上存储有计算机指令，所述计算机指令被执行时，实现所述的一种伺服控制方法。

　　在本说明书的描述中，参考术语“一种实施方式”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一种实施方式或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

　　尽管上面已经示出和描述了本发明的实施例，但不能理解为是对权利要求的限制。本发明不仅限于以上实施例，其具体结构允许有变化，凡在本发明独立权利要求的保护范围内所作的各种变化均在本发明的保护范围内。