CN108267137B - 定位方法、装置、业务处理***以及计算机可读存储介质 - Google Patents

Abstract

本发明提供一种用于双驱移动机器人的定位方法以及定位装置，以及用于双驱移动机器人的定位的业务处理***和计算机可读存储介质，有助于解决现有技术中的定位不准确的技术问题。本发明的用于双驱移动机器人的定位方法包括：获取双驱移动机器人的初始横坐标、初始纵坐标、初始航向角、初始倾斜角、初始侧倾角；根据初始航向角、初始倾斜角以及初始侧倾角确定双驱移动机器人的新航向角、新倾斜角以及新侧倾角；确定双驱移动机器人的左右驱动轮的平均移动距离；根据初始横坐标、初始纵坐标、新航向角以及左右驱动轮的平均移动距离确定双驱移动机器人的新横坐标和新纵坐标。

Description

定位方法、装置、业务处理***以及计算机可读存储介质

技术领域

本发明涉及一种计算机及其软件技术领域，特别涉及一种用于双驱移动机器人的定位的方法、装置、业务处理***以及计算机可读存储介质。

背景技术

随着工业发展，移动机器人逐渐被广泛应用，例如电商公司可以使用移动机器人在仓库内搬运货物。双驱移动机器人通常采用左轮和右轮的双驱模式，车体轮系采用差速固定舵方式，驱动电机为伺服电机，通过控制两个电机的转速从而实现对车的速度控制和姿态控制。

双驱移动机器人的定位技术主要有基于编码器定位和基于惯性导航定位两种。

基于编码器的机器人定位方法具体为：当机器人在两个二维码点之间行走时，如果地面严格水平，排除其他误差的影响，可以认为机器人走直线。在车轮上安装有编码器，当车轮转动时，编码器会发出数个脉冲信号，导航解算单元接收到这个信号后，通过计算脉冲信号的个数，可以得知车轮在测量时间内直线上的距离，这样就可以得到机器人在直线上的坐标。然而，在地面水平度平整度不够、轮子半径不相等、机器人载重不均匀等情况下，机器人不再按直线行走，而是走曲线，尤其是当某个轮子打滑时，机器人会产生大幅度的转向，对编码器方式定位结果造成很大的误差。

基于惯性导航的机器人定位方法具体为：将惯性测量传感器置于载体中心点上，采集机器人运动的加速度、角速度等基础运动数据信息，通过方向余弦矩阵将之转化到导航坐标系下，经过一系列导航算法即可解算出载体的速度、位置以及姿态角信息。然而，惯性导航传感器由于成本原因，实际应用中选择MEMS工艺低成本传感器，器件的零位较不稳定，随机漂移较严重，尤其是加速度计在10秒时间内由于漂移造成的位移计算误差达几十公分，这在实际应用中是不能接受的。

发明内容

有鉴于此，本发明提供一种用于双驱移动机器人的定位方法以及定位装置，以及用于双驱移动机器人的定位的业务处理***和计算机可读存储介质，有助于解决现有技术中的定位不准确的技术问题。

为实现发明目的，根据本发明的第一个方面，提供了一种用于双驱移动机器人的定位方法，包括：获取所述双驱移动机器人的初始横坐标、初始纵坐标、初始航向角、初始倾斜角、初始侧倾角；根据所述初始航向角、所述初始倾斜角以及所述初始侧倾角确定所述双驱移动机器人的新航向角、新倾斜角以及新侧倾角；确定所述双驱移动机器人的左右驱动轮的平均移动距离；根据所述初始横坐标、所述初始纵坐标、所述新航向角以及所述左右驱动轮的平均移动距离确定所述双驱移动机器人的新横坐标和新纵坐标。

可选地，所述根据所述初始航向角、所述初始倾斜角以及所述初始侧倾角确定所述双驱移动机器人的新航向角、新倾斜角以及新侧倾角的步骤包括：

根据所述初始航向角、所述初始倾斜角以及所述初始侧倾角按照下列公式计算四元数：

其中，

表示初始航向角，θ(k)表示初始倾斜角，γ(k)表示初始侧倾角，a、b、c、d表示四元数；

根据所述四元数按照下列公式计算所述新航向角、所述新倾斜角以及所述新侧倾角：

θ(k+1)＝arcsin(2(cd+ab))

其中，

表示新航向角，θ(k+1)表示新倾斜角，γ(k+1)表示新侧倾角。

可选地，所述确定所述双驱移动机器人的左右驱动轮的平均移动距离的步骤包括：根据下列公式计算所述双驱移动机器人的左右驱动轮的平均移动距离：

其中U(k)表示左右驱动轮平均移动距离，r表示驱动轮半径，π表示圆周率，n_R表示单个采样周期内右驱动轮编码器的脉冲数数量，n_L表示单个采样周期内右驱动轮编码器的脉冲数数量，n_total表示驱动轮旋转一圈的总脉冲数。

可选地，所述根据所述初始横坐标、所述初始纵坐标、所述新航向角以及所述左右驱动轮的平均移动距离确定所述双驱移动机器人的新横坐标和新纵坐标的步骤包括：根据所述初始横坐标、所述新航向角以及所述左右驱动轮的平均移动距离按照下列公式确定所述新横坐标：

其中X(k+1)表示新横坐标，X(k)表示初始横坐标；根据所述初始纵坐标、所述新航向角以及所述左右驱动轮的平均移动距离按照下列公式确定所述新纵坐标：

其中Y(k+1)表示新纵坐标，Y(k)表示初始纵坐标。

为实现发明目的，根据本发明的第二个方面，提供了一种用于双驱移动机器人的定位装置，包括：获取模块，获取所述双驱移动机器人的初始横坐标、初始纵坐标、初始航向角、初始倾斜角、初始侧倾角；角度计算模块，用于根据所述初始航向角、所述初始倾斜角以及所述初始侧倾角确定所述双驱移动机器人的新航向角、新倾斜角以及新侧倾角；距离计算模块，用于确定所述双驱移动机器人的左右驱动轮的平均移动距离；坐标计算模块，用于根据所述初始横坐标、所述初始纵坐标、所述新航向角以及所述左右驱动轮的平均移动距离确定所述双驱移动机器人的新横坐标和新纵坐标。

可选地，所述角度计算模块还用于：

根据所述初始航向角、所述初始倾斜角以及所述初始侧倾角按照下列公式计算四元数：

其中，

表示初始航向角，θ(k)表示初始倾斜角，γ(k)表示初始侧倾角，a、b、c、d表示四元数；

根据所述四元数按照下列公式计算所述新航向角、所述新倾斜角以及所述新侧倾角：

θ(k+1)＝arcsin(2(cd+ab))

其中，

表示新航向角，θ(k+1)表示新倾斜角，γ(k+1)表示新侧倾角。

可选地，所述距离计算模块还用于：根据下列公式计算所述双驱移动机器人的左右驱动轮的平均移动距离：

其中U(k)表示左右驱动轮平均移动距离，r表示驱动轮半径，π表示圆周率，n_R表示单个采样周期内右驱动轮编码器的脉冲数数量，n_L表示单个采样周期内右驱动轮编码器的脉冲数数量，n_total表示驱动轮旋转一圈的总脉冲数。

可选地，所述坐标计算模块还用于：根据所述初始横坐标、所述新航向角以及所述左右驱动轮的平均移动距离按照下列公式确定所述新横坐标：

其中X(k+1)表示新横坐标，X(k)表示初始横坐标；根据所述初始纵坐标、所述新航向角以及所述左右驱动轮的平均移动距离按照下列公式确定所述新纵坐标：

其中Y(k+1)表示新纵坐标，Y(k)表示初始纵坐标。

为实现发明目的，根据本发明的第三个方面，提供了一种用于双驱移动机器人的定位的电子设备。

本发明实施例的一种电子设备，包括：一个或多个处理器；存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现本发明实施例的一种用于双驱机器人的定位方法。

为实现发明目的，根据本发明的第四个方面，提供了一种用于双驱移动机器人的定位的计算机可读存储介质。

本发明实施例的一种计算机可读存储介质，其上存储有计算机程序，所述程序被处理器执行时实现本发明实施例的一种用于双驱机器人的定位方法。

根据本发明的技术方案是将惯性***与编码器相互融合的导航定位技术方案，结合两者的优势，利用惯导算法确定航向角，利用编码测距算法确定位移，然后利用航向角和位移得到一个较精确的机器人位姿信息。本发明实施方式的用于双驱移动机器人的定位方法以及定位装置可以对复杂的工况和地面环境有交强的纠错和抗干扰能力，比单一传感器导航定位有更高的可靠性。需要说明的是，上述的非惯用的可选方式所具有的进一步效果将在下文中结合具体实施方式加以说明。

附图说明

附图用于更好地理解本发明，不构成对本发明的不当限定。其中：

图1是根据本发明实施方式的用于双驱移动机器人的定位方法的主要步骤的流程示意图；

图2是根据本发明实施方式的用于双驱移动机器人的定位装置的主要模块的结构示意图。

具体实施方式

以下结合附图对本发明的示范性实施方式做出说明，其中包括本发明实施方式的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施方式做出各种改变和修改，而不会背离本发明的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。

图1是根据本发明实施方式的用于双驱移动机器人的定位方法的主要步骤的流程示意图。如图1所示，该实施方式的用于双驱移动机器人的定位方法可以包括如下的步骤A至步骤D。

步骤A：获取双驱移动机器人的初始横坐标、初始纵坐标、初始航向角、初始倾斜角、初始侧倾角。

步骤B：根据初始航向角、初始倾斜角以及初始侧倾角确定双驱移动机器人的新航向角、新倾斜角以及新侧倾角。

步骤C：确定双驱移动机器人的左右驱动轮的平均移动距离。

步骤D：根据初始横坐标、初始纵坐标、新航向角以及左右驱动轮的平均移动距离确定双驱移动机器人的新横坐标和新纵坐标。

可选地，步骤B中可以利用惯导算法根据初始航向角、初始倾斜角以及初始侧倾角确定双驱移动机器人的新航向角、新倾斜角以及新侧倾角，具体过程如下。

首先，根据初始航向角、初始倾斜角以及初始侧倾角按照下列公式计算四元数。其中，

表示初始航向角，θ(k)表示初始倾斜角，γ(k)表示初始侧倾角，a、b、c、d表示四元数。

由于双驱移动机器人运动引起姿态角的变化，四元数随之变化，更新方程为：

用四阶毕卡法求解该微分方程，得到解为：

其中：

设采样间隔为Δt，则：

其中

为陀螺仪测量出来的三个分量。

通过解算出的四元数可以得到更新后的方向余弦矩阵：

从而可以计算新航向角

新倾斜角θ(k+1)、以及新侧倾角γ(k+1)，计算公式如下：

θ(k+1)＝arcsin(2(cd+ab))

可选地，步骤C可以利用编码测距算法来确定双驱移动机器人的左右驱动轮的平均移动距离。具体过程如下。

由于左右驱动轮的平均移动距离应该等于左驱动轮的移动距离和右驱动轮的移动距离的平均值，因此可以根据下列公式计算左右驱动轮的平均移动距离：

其中U(k)表示左右驱动轮平均移动距离，r表示驱动轮半径，π表示圆周率，n_R表示单个采样周期内右驱动轮编码器的脉冲数数量，n_L表示单个采样周期内右驱动轮编码器的脉冲数数量，n_total表示驱动轮旋转一圈的总脉冲数。

可选地，步骤D可以包括如下的步骤D1和步骤D2。

步骤D1：根据初始横坐标、新航向角以及左右驱动轮的平均移动距离按照下列公式确定新横坐标：

其中X(k+1)表示新横坐标，X(k)表示初始横坐标。

步骤D2：根据初始纵坐标、新航向角以及左右驱动轮的平均移动距离按照下列公式确定新纵坐标：

其中Y(k+1)表示新纵坐标，Y(k)表示初始纵坐标。

图2是根据本发明实施方式的用于双驱移动机器人的定位装置的主要模块的结构示意图。如图2所示，该实施方式的用于双驱移动机器人的定位装置20可以包括获取模块21、角度计算模块22、距离计算模块23和坐标计算模块24。

获取模块21用于获取双驱移动机器人的初始横坐标、初始纵坐标、初始航向角、初始倾斜角、初始侧倾角。

角度计算模块22用于根据初始航向角、初始倾斜角以及初始侧倾角确定双驱移动机器人的新航向角、新倾斜角以及新侧倾角。

距离计算模块23用于确定双驱移动机器人的左右驱动轮的平均移动距离。

坐标计算模块24用于根据初始横坐标、初始纵坐标、新航向角以及左右驱动轮的平均移动距离确定双驱移动机器人的新横坐标和新纵坐标。

可选地，角度计算模块22还用于：根据初始航向角、初始倾斜角以及初始侧倾角按照下列公式计算四元数：

其中，

表示初始航向角，θ(k)表示初始倾斜角，γ(k)表示初始侧倾角，a、b、c、d表示四元数。

以及角度计算模块22还用于根据四元数按照下列公式计算新航向角

新倾斜角θ(k+1)、以及新侧倾角γ(k+1)。

θ(k+1)＝arcsin(2(cd+ab))

可选地，距离计算模块23还用于根据下列公式计算双驱移动机器人的左右驱动轮的平均移动距离：

其中U(k)表示左右驱动轮平均移动距离，r表示驱动轮半径，π表示圆周率，n_R表示单个采样周期内右驱动轮编码器的脉冲数数量，n_L表示单个采样周期内右驱动轮编码器的脉冲数数量，n_total表示驱动轮旋转一圈的总脉冲数。

可选地，坐标计算模块24还用于：根据初始横坐标、新航向角以及左右驱动轮的平均移动距离按照下列公式确定新横坐标：

其中X(k+1)表示新横坐标，X(k)表示初始横坐标；根据初始纵坐标、新航向角以及左右驱动轮的平均移动距离按照下列公式确定新纵坐标：

其中Y(k+1)表示新纵坐标，Y(k)表示初始纵坐标。

根据本发明实施方式的用于双驱移动机器人的定位方法以及定位装置是将惯性***与编码器相互融合的导航定位技术方案，结合两者的优势，利用惯导算法确定航向角，利用编码测距算法确定位移，然后利用航向角和位移得到一个较精确的机器人位姿信息。本发明实施方式的用于双驱移动机器人的定位方法以及定位装置可以对复杂的工况和地面环境有交强的纠错和抗干扰能力，比单一传感器导航定位有更高的可靠性。

本发明还提供了一种用于双驱移动机器人的定位的业务处理***。该业务处理***包括存储器；以及耦接至所述存储器的处理器，所述处理器被配置为基于存储在所述存储器的指令执行本发明的用于双驱机器人的定位方法。

本发明还提供了一种用于双驱移动机器人的定位的计算机可读存储介质，其上存储有计算机程序指令，该指令被处理器执行时实现本发明的用于双驱机器人的定位方法的步骤。

上述具体实施方式，并不构成对本发明保护范围的限制。本领域技术人员应该明白的是，取决于设计要求和其他因素，可以发生各种各样的修改、组合、子组合和替代。任何在本发明的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明保护范围之内。

Claims (10)

1.一种用于双驱移动机器人的定位方法，其特征在于，包括：

获取所述双驱移动机器人的初始横坐标、初始纵坐标、初始航向角、初始倾斜角、初始侧倾角；

根据所述初始航向角、所述初始倾斜角以及所述初始侧倾角与四元数之间的转换关系计算四元数的初始值，之后结合四阶毕卡法求解四元数微分方程，以确定所述双驱移动机器人的新航向角、新倾斜角以及新侧倾角；

根据单个采样周期内左驱动轮编码器、右驱动轮编码器的脉冲数数量、驱动轮旋转一圈的总脉冲数以及驱动轮半径，确定所述双驱移动机器人的左右驱动轮的平均移动距离；

根据所述初始横坐标、所述初始纵坐标、所述新航向角以及所述左右驱动轮的平均移动距离确定所述双驱移动机器人的新横坐标和新纵坐标。

2.根据权利要求1所述的用于双驱移动机器人的定位方法，其特征在于，所述确定所述双驱移动机器人的新航向角、新倾斜角以及新侧倾角的步骤包括：

根据所述初始航向角、所述初始倾斜角以及所述初始侧倾角按照下列公式计算四元数：

其中，

表示初始航向角，θ(k)表示初始倾斜角，γ(k)表示初始侧倾角，a、b、c、d表示四元数；

根据所述四元数按照下列公式计算所述新航向角、所述新倾斜角以及所述新侧倾角：

θ(k+1)＝arcsin(2(cd+ab))

其中，

表示新航向角，θ(k+1)表示新倾斜角，γ(k+1)表示新侧倾角。

3.根据权利要求2所述的用于双驱移动机器人的定位方法，其特征在于，所述确定所述双驱移动机器人的左右驱动轮的平均移动距离的步骤包括：

根据下列公式计算所述双驱移动机器人的左右驱动轮的平均移动距离：

其中U(k)表示左右驱动轮平均移动距离，r表示驱动轮半径，π表示圆周率，n_R表示单个采样周期内右驱动轮编码器的脉冲数数量，n_L表示单个采样周期内右驱动轮编码器的脉冲数数量，n_total表示驱动轮旋转一圈的总脉冲数。

4.根据权利要求3所述的用于双驱移动机器人的定位方法，其特征在于，所述根据所述初始横坐标、所述初始纵坐标、所述新航向角以及所述左右驱动轮的平均移动距离确定所述双驱移动机器人的新横坐标和新纵坐标的步骤包括：

根据所述初始横坐标、所述新航向角以及所述左右驱动轮的平均移动距离按照下列公式确定所述新横坐标：

其中X(k+1)表示新横坐标，X(k)表示初始横坐标；

根据所述初始纵坐标、所述新航向角以及所述左右驱动轮的平均移动距离按照下列公式确定所述新纵坐标：

其中Y(k+1)表示新纵坐标，Y(k)表示初始纵坐标。

5.一种用于双驱移动机器人的定位装置，其特征在于，包括：

获取模块，获取所述双驱移动机器人的初始横坐标、初始纵坐标、初始航向角、初始倾斜角、初始侧倾角；

角度计算模块，用于根据所述初始航向角、所述初始倾斜角以及所述初始侧倾角与四元数之间的转换关系计算四元数的初始值，之后结合四阶毕卡法求解四元数微分方程，以确定所述双驱移动机器人的新航向角、新倾斜角以及新侧倾角；

距离计算模块，用于根据单个采样周期内左驱动轮编码器、右驱动轮编码器的脉冲数数量、驱动轮旋转一圈的总脉冲数以及驱动轮半径，确定所述双驱移动机器人的左右驱动轮的平均移动距离；

坐标计算模块，用于根据所述初始横坐标、所述初始纵坐标、所述新航向角以及所述左右驱动轮的平均移动距离确定所述双驱移动机器人的新横坐标和新纵坐标。

6.根据权利要求5所述的用于双驱移动机器人的定位装置，所述角度计算模块还用于：

根据所述初始航向角、所述初始倾斜角以及所述初始侧倾角按照下列公式计算四元数：

其中，

表示初始航向角，θ(k)表示初始倾斜角，γ(k)表示初始侧倾角，a、b、c、d表示四元数；

根据所述四元数按照下列公式计算所述新航向角、所述新倾斜角以及所述新侧倾角：

θ(k+1)＝arcsin(2(cd+ab))

其中，

表示新航向角，θ(k+1)表示新倾斜角，γ(k+1)表示新侧倾角。

7.根据权利要求6所述的用于双驱移动机器人的定位装置，其特征在于，所述距离计算模块还用于：

根据下列公式计算所述双驱移动机器人的左右驱动轮的平均移动距离：

其中U(k)表示左右驱动轮平均移动距离，r表示驱动轮半径，π表示圆周率，n_R表示单个采样周期内右驱动轮编码器的脉冲数数量，n_L表示单个采样周期内右驱动轮编码器的脉冲数数量，n_total表示驱动轮旋转一圈的总脉冲数。

8.根据权利要求7所述的用于双驱移动机器人的定位装置，其特征在于，所述坐标计算模块还用于：

根据所述初始横坐标、所述新航向角以及所述左右驱动轮的平均移动距离按照下列公式确定所述新横坐标：

其中X(k+1)表示新横坐标，X(k)表示初始横坐标；

根据所述初始纵坐标、所述新航向角以及所述左右驱动轮的平均移动距离按照下列公式确定所述新纵坐标：

其中Y(k+1)表示新纵坐标，Y(k)表示初始纵坐标。

9.一种用于双驱移动机器人的定位的电子设备，其特征在于，包括：

一个或多个处理器；

存储装置，用于存储一个或多个程序，

当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1至4任一项所述的方法。

10.一种用于双驱移动机器人的定位的计算机可读存储介质，其上存储有计算机程序，其特征在于，所述程序被处理器执行时实现权利要求1至4任一项所述的方法。

Images