CN108955720B - 一种基于四轮独立驱动和转向的里程推算方法及装置 - Google Patents

Abstract

本发明提供了一种基于四轮独立驱动和转向的里程推算方法及装置，其中方法包括：S1、获取设置于单轮上的旋转编码器采集的旋转脉冲数，结合旋转脉冲数和单轮的周长计算单轮的里程数；S2、获取四轮中前外侧轮的转弯角度，并根据转弯角度以及车体的长度和宽度计算单轮的转弯半径；S3、结合单轮的里程数和单轮的转弯半径，计算得到车体坐标系旋转角度；S4、根据旋转变化矩阵，通过车体坐标系旋转角度和旋转前的车体原点坐标计算旋转后的车体原点坐标。本发明实现了对四轮独立驱动和转向的移动机器人的里程计算。

Description

一种基于四轮独立驱动和转向的里程推算方法及装置

技术领域

本发明涉及移动机器人里程推算技术领域，尤其涉及一种基于四轮独立驱动和转向的里程推算方法及装置。

背景技术

移动机器人里程推算是研究移动机器人同时定位与地图构建(simultaneouslocalization and mapping，SLAM)问题的基础。

现有的里程推算方法大多基于GPS定位来实现，还有利用计算机视觉的方法实现里程推算，但GPS定位的准确度无法确定，且存在基站与移动站之间的通信质量无法掌控的问题，而计算机视觉的方法依赖于摄像头的质量，甚至光线的限制。因此，需要提出一种通用的里程推算方法以实现对四轮独立驱动和转向的移动机器人的里程计算。

发明内容

本发明提供了一种基于四轮独立驱动和转向的里程推算方法及装置，实现了对四轮独立驱动和转向的移动机器人的里程计算。

本发明提供了一种基于四轮独立驱动和转向的里程推算方法，包括：

S1、获取设置于单轮上的旋转编码器采集的旋转脉冲数，结合旋转脉冲数和单轮的周长计算单轮的里程数；

S2、获取四轮中前外侧轮的转弯角度，并根据转弯角度以及车体的长度和宽度计算单轮的转弯半径；

S3、结合单轮的里程数和单轮的转弯半径，计算得到车体坐标系旋转角度；

S4、根据旋转变化矩阵，通过车体坐标系旋转角度和旋转前的车体原点坐标计算旋转后的车体原点坐标。

可选地，步骤S3之后，步骤S4之前还包括：

获取设置于单轮上的陀螺仪采集的补偿旋转角度；

将补偿旋转角度与车体坐标系旋转角度进行加权计算，得到车体坐标系补偿旋转角度；

S4、根据旋转变化矩阵，通过车体坐标系补偿旋转角度和旋转前的车体原点坐标计算旋转后的车体原点坐标。

可选地，将补偿旋转角度与车体坐标系旋转角度进行加权计算，得到车体坐标系补偿旋转角度具体为：

通过预置加权公式

将补偿旋转角度与车体坐标系旋转角度进行加权计算，得到车体坐标系补偿旋转角度，其中Δθ_f为车体坐标系补偿旋转角度，Δθ_gyro为补偿旋转角度，w为加权系数。

可选地，步骤S1之前还包括：

构建车体模型，确定车体的长度和宽度。

可选地，步骤S4之后还包括：

根据旋转后的车体原点坐标以及旋转前的车体原点坐标确定车体的运动模式。

本发明提供了一种基于四轮独立驱动和转向的里程推算装置，包括：

第一处理单元，用于获取设置于单轮上的旋转编码器采集的旋转脉冲数，结合旋转脉冲数和单轮的周长计算单轮的里程数；

第二处理单元，用于获取四轮中前外侧轮的转弯角度，并根据转弯角度以及车体的长度和宽度计算单轮的转弯半径；

第三处理单元，用于结合单轮的里程数和单轮的转弯半径，计算得到车体坐标系旋转角度；

第四处理单元，用于根据旋转变化矩阵，通过车体坐标系旋转角度和旋转前的车体原点坐标计算旋转后的车体原点坐标。

可选地，还包括：

获取单元，用于获取设置于单轮上的陀螺仪采集的补偿旋转角度；

第五处理单元，用于将补偿旋转角度与车体坐标系旋转角度进行加权计算，得到车体坐标系补偿旋转角度；

第四处理单元，还用于根据旋转变化矩阵，通过车体坐标系补偿旋转角度和旋转前的车体原点坐标计算旋转后的车体原点坐标。

可选地，第五处理单元还用于通过预置加权公式

将补偿旋转角度与车体坐标系旋转角度进行加权计算，得到车体坐标系补偿旋转角度，其中Δθ_f为车体坐标系补偿旋转角度，Δθ_gyro为补偿旋转角度，w为加权系数。

可选地，还包括：

建模单元，用于构建车体模型，确定车体的长度和宽度。

可选地，还包括：

模式确定单元，用于根据旋转后的车体原点坐标以及旋转前的车体原点坐标确定车体的运动模式。

从以上技术方案可以看出，本发明具有以下优点：

本发明提供了一种基于四轮独立驱动和转向的里程推算方法，包括：S1、获取设置于单轮上的旋转编码器采集的旋转脉冲数，结合旋转脉冲数和单轮的周长计算单轮的里程数；S2、获取四轮中前外侧轮的转弯角度，并根据转弯角度以及车体的长度和宽度计算单轮的转弯半径；S3、结合单轮的里程数和单轮的转弯半径，计算得到车体坐标系旋转角度；S4、根据旋转变化矩阵，通过车体坐标系旋转角度和旋转前的车体原点坐标计算旋转后的车体原点坐标。

本发明通过在移动机器人的四轮处配备旋转编码器，利用旋转编码器测量单轮的旋转脉冲数，从而计算出单轮的移动距离即里程，再根据在移动机器人的四轮处配备的陀螺仪来获取单轮的转弯角度，从而计算出单轮的转弯半径，结合起来获得整个车体坐标系旋转角度，最终根据旋转变化矩阵求出原点的相对变化，能够确定移动后的移动机器人的位置，实现了对四轮独立驱动和转向的移动机器人的里程计算。

附图说明

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

图1为本发明提供的一种基于四轮独立驱动和转向的里程推算方法的一个实施例的流程示意图；

图2为本发明提供的一种基于四轮独立驱动和转向的里程推算方法的另一个实施例的流程示意图；

图3为本发明提供的一种基于四轮独立驱动和转向的里程推算装置的一个实施例的结构示意图；

图4为本发明提供的一种基于四轮独立驱动和转向的里程推算装置的另一个实施例的结构示意图；

图5为本发明提供的一种两轮驱动两轮转向模式的移动机器人模型图。

具体实施方式

本发明实施例提供了一种基于四轮独立驱动和转向的里程推算方法及装置，实现了对四轮独立驱动和转向的移动机器人的里程计算。

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

请参阅图1，本发明提供了一种基于四轮独立驱动和转向的里程推算方法的一个实施例，包括：

101、获取设置于单轮上的旋转编码器采集的旋转脉冲数，结合旋转脉冲数和单轮的周长计算单轮的里程数；

102、获取四轮中前外侧轮的转弯角度，并根据转弯角度以及车体的长度和宽度计算单轮的转弯半径；

103、结合单轮的里程数和单轮的转弯半径，计算得到车体坐标系旋转角度；

104、根据旋转变化矩阵，通过车体坐标系旋转角度和旋转前的车体原点坐标计算旋转后的车体原点坐标。

本发明实施例通过在移动机器人的四轮处配备旋转编码器，利用旋转编码器测量单轮的旋转脉冲数，从而计算出单轮的移动距离即里程，再根据在移动机器人的四轮处配备的陀螺仪来获取单轮的转弯角度，从而计算出单轮的转弯半径，结合起来获得整个车体坐标系旋转角度，最终根据旋转变化矩阵求出原点的相对变化，能够确定移动后的移动机器人的位置，实现了对四轮独立驱动和转向的移动机器人的里程计算。

以上是对本发明提供的一种基于四轮独立驱动和转向的里程推算方法的一个实施例进行的说明，以下将对本发明提供的一种基于四轮独立驱动和转向的里程推算方法的另一个实施例进行说明。

请参阅图2，本发明提供了一种基于四轮独立驱动和转向的里程推算方法的另一个实施例，包括：

201、构建车体模型，确定车体的长度和宽度；

如图5所示，车体模型包括1号轮至4号轮，每号轮独立驱动和转向，确定车体的长度为l_r，车体的宽度为w_r。

202、获取设置于单轮上的旋转编码器采集的旋转脉冲数，结合旋转脉冲数和单轮的周长计算单轮的里程数；

需要说明的是，四轮独立驱动和转向机器人在四轮处配备有旋转编码器，借助旋转编码器，可以测量单轮的旋转脉冲数，若在单轮的周长已知的情况下，即可以计算出移动机器人移动的距离。同样，在电源频率已知的情况下，可以计算出交流电动机的同步转速。

定义单轮的里程数s如式(1)：

s＝n·l (1)

其中，l为一个霍尔脉冲弧长，考虑轮毂电机磁极对数为15，单轮周长/3*2*磁极对数；n为旋转脉冲数，正转输入为正值，反转输入为负值。

203、获取四轮中前外侧轮的转弯角度，并根据转弯角度以及车体的长度和宽度计算单轮的转弯半径；

需要说明的是，如图5所示，图5中转弯角度α即为前外侧轮(1号轮)偏转角的值。已知转弯角度α，结合车体的长度l_r和车体的宽度w_r，得到：

前外侧轮转弯半径r₁：

前内侧轮转弯半径r₂：

后外侧轮转弯半径r₃：r₃＝l_r·cotα；

后内侧轮转弯半径r₄：r₄＝l_r·cotα-w_r。

204、结合单轮的里程数和单轮的转弯半径，计算得到车体坐标系旋转角度；

需要说明的是，一个采样周期内移动机器人车体坐标系旋转角度Δθ定义为：四轮分别求解出旋转角度，对其进行均值计算，即：

205、获取设置于单轮上的陀螺仪采集的补偿旋转角度；

需要说明的是，旋转编码器对通信速度和轮毂控制器处理速度有一定要求，当存在通信延迟时误差比较大；而陀螺仪积分得到的角度不受小车加速度的影响，但是随着时间的增加积分漂移和温度漂移带来的误差比较大。互补融合就是在短时间内采用陀螺仪得到的角度作为最优，采样读取旋转编码器的读数来校正陀螺仪的得到的角度，因此，需要获取设置于单轮上的陀螺仪采集的补偿旋转角度Δθ_gyro。

206、通过预置加权公式

将补偿旋转角度与车体坐标系旋转角度进行加权计算，得到车体坐标系补偿旋转角度，其中Δθ_f为车体坐标系补偿旋转角度，Δθ_gyro为补偿旋转角度，w为加权系数；

需要说明的是，针对如图5所示的两轮驱动两轮转向模式的移动机器人，定义：

其中Δθ_f为车体坐标系补偿旋转角度，Δθ_gyro为补偿旋转角度，w为加权系数。

207、根据旋转变化矩阵，通过车体坐标系补偿旋转角度和旋转前的车体原点坐标计算旋转后的车体原点坐标；

需要说明的是，定义旋转变化矩阵为：

…

其中，原点初始值定义为

后续推导中，将上一采样周期值进行迭代，进而根据旋转变化矩阵求出原点的相对变化，得到旋转后的车体原点坐标。

208、根据旋转后的车体原点坐标以及旋转前的车体原点坐标确定车体的运动模式；

需要说明的是，不同模式下旋转后的车体原点坐标以及旋转前的车体原点坐标的变化存在区别。

机器人质心在二维平面上的运动用[Δx,Δy,Δθ]表示，为机器人车体坐标系质心每一个采样时间变化量(采样时间由轮毂电机控制器与上位机的串口通信速率决定)，其中(Δx,Δy)车体坐标系原点变化，Δθ为车体坐标系旋转角度。

原地转向模式：巡检机器人具有绕着自身的中心旋转，Δx＝0,Δy＝0；

…

两轮驱动两轮转向模式：

直线行走模式：

以上是对本发明提供的一种基于四轮独立驱动和转向的里程推算方法的另一个实施例进行的说明，以下将对本发明提供的一种基于四轮独立驱动和转向的里程推算装置的一个实施例进行说明。

请参阅图3，本发明提供了一种基于四轮独立驱动和转向的里程推算装置的一个实施例，包括：

第一处理单元301，用于获取设置于单轮上的旋转编码器采集的旋转脉冲数，结合旋转脉冲数和单轮的周长计算单轮的里程数；

第二处理单元302，用于获取四轮中前外侧轮的转弯角度，并根据转弯角度以及车体的长度和宽度计算单轮的转弯半径；

第三处理单元303，用于结合单轮的里程数和单轮的转弯半径，计算得到车体坐标系旋转角度；

第四处理单元304，用于根据旋转变化矩阵，通过车体坐标系旋转角度和旋转前的车体原点坐标计算旋转后的车体原点坐标。

以上是对本发明提供的一种基于四轮独立驱动和转向的里程推算装置的一个实施例进行的说明，以下将对本发明提供的一种基于四轮独立驱动和转向的里程推算装置的另一个实施例进行说明。

请参阅图4，本发明提供了一种基于四轮独立驱动和转向的里程推算装置的另一个实施例，包括：

第一处理单元401，用于获取设置于单轮上的旋转编码器采集的旋转脉冲数，结合旋转脉冲数和单轮的周长计算单轮的里程数；

第二处理单元402，用于获取四轮中前外侧轮的转弯角度，并根据转弯角度以及车体的长度和宽度计算单轮的转弯半径；

第三处理单元403，用于结合单轮的里程数和单轮的转弯半径，计算得到车体坐标系旋转角度；

第四处理单元404，用于根据旋转变化矩阵，通过车体坐标系旋转角度和旋转前的车体原点坐标计算旋转后的车体原点坐标。

进一步地，还包括：

获取单元405，用于获取设置于单轮上的陀螺仪采集的补偿旋转角度；

第五处理单元406，用于将补偿旋转角度与车体坐标系旋转角度进行加权计算，得到车体坐标系补偿旋转角度；

第四处理单元，还用于根据旋转变化矩阵，通过车体坐标系补偿旋转角度和旋转前的车体原点坐标计算旋转后的车体原点坐标。

进一步地，第五处理单元406还用于通过预置加权公式

将补偿旋转角度与车体坐标系旋转角度进行加权计算，得到车体坐标系补偿旋转角度，其中Δθ_f为车体坐标系补偿旋转角度，Δθ_gyro为补偿旋转角度，w为加权系数。

进一步地，还包括：

建模单元407，用于构建车体模型，确定车体的长度和宽度。

进一步地，还包括：

模式确定单元408，用于根据旋转后的车体原点坐标以及旋转前的车体原点坐标确定车体的运动模式。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (6)

1.一种基于四轮独立驱动和转向的里程推算方法，其特征在于，包括：

S1、获取设置于单轮上的旋转编码器采集的旋转脉冲数，结合旋转脉冲数和单轮的周长计算单轮的里程数；

S2、获取四轮中前外侧轮的转弯角度，并根据转弯角度以及车体的长度和宽度计算单轮的转弯半径；

S3、结合单轮的里程数和单轮的转弯半径，计算得到车体坐标系旋转角度；

获取设置于单轮上的陀螺仪采集的补偿旋转角度；通过预置加权公式

将补偿旋转角度与车体坐标系旋转角度进行加权计算，得到车体坐标系补偿旋转角度，其中Δθ_f为车体坐标系补偿旋转角度，Δθ_gyro为补偿旋转角度，w为加权系数，

为一个采样周期内移动机器人车体坐标系旋转角度；

S4、根据旋转变化矩阵，通过车体坐标系补偿旋转角度和旋转前的车体原点坐标计算旋转后的车体原点坐标。

2.根据权利要求1所述的基于四轮独立驱动和转向的里程推算方法，其特征在于，步骤S1之前还包括：

构建车体模型，确定车体的长度和宽度。

3.根据权利要求1所述的基于四轮独立驱动和转向的里程推算方法，其特征在于，步骤S4之后还包括：

根据旋转后的车体原点坐标以及旋转前的车体原点坐标确定车体的运动模式。

4.一种基于四轮独立驱动和转向的里程推算装置，其特征在于，包括：

第一处理单元，用于获取设置于单轮上的旋转编码器采集的旋转脉冲数，结合旋转脉冲数和单轮的周长计算单轮的里程数；

第二处理单元，用于获取四轮中前外侧轮的转弯角度，并根据转弯角度以及车体的长度和宽度计算单轮的转弯半径；

第三处理单元，用于结合单轮的里程数和单轮的转弯半径，计算得到车体坐标系旋转角度；

获取单元，用于获取设置于单轮上的陀螺仪采集的补偿旋转角度；

第五处理单元，用于通过预置加权公式

将补偿旋转角度与车体坐标系旋转角度进行加权计算，得到车体坐标系补偿旋转角度，其中Δθ_f为车体坐标系补偿旋转角度，Δθ_gyro为补偿旋转角度，w为加权系数，

为一个采样周期内移动机器人车体坐标系旋转角度；

第四处理单元，用于根据旋转变化矩阵，通过车体坐标系补偿旋转角度和旋转前的车体原点坐标计算旋转后的车体原点坐标。

5.根据权利要求4所述的基于四轮独立驱动和转向的里程推算装置，其特征在于，还包括：

建模单元，用于构建车体模型，确定车体的长度和宽度。

6.根据权利要求4所述的基于四轮独立驱动和转向的里程推算装置，其特征在于，还包括：

模式确定单元，用于根据旋转后的车体原点坐标以及旋转前的车体原点坐标确定车体的运动模式。

Images