CN105759292A - 一种差分gps与机器人的相对位置参数标定方法及*** - Google Patents

Abstract

本发明公开了一种差分GPS与机器人相对位置参数标定方法及***，该方法包括：利用速度测量器和角速度测量器对机器人进行定位，获得机器人的速度、角速度信息；利用差分GPS对机器人进行定位，获得机器人的GPS位置信息；根据初始位置航向信息、速度、角速度信息及GPS位置信息获得差分GPS与机器人的相对位置参数。该***包括：速度测量器、角速度测量器、差分GPS以及相对位置参数标定单元。本发明的差分GPS与机器人相对位置参数标定方法及***，实现了对机器人的高精度高可靠的实时定位。

Description

一种差分GPS与机器人的相对位置参数标定方法及***

技术领域

本发明涉及机器人定位领域，特别涉及一种差分GPS与机器人的相对位置参数标定方法及***。

背景技术

GPS是英文GlobalPositioningSystem(全球定位***)的简称。它利用GPS定位卫星，在全球范围内实时进行定位、导航。GPS是由美国国防部研制建立的一种具有全方位、全天候、全时段、高精度的卫星导航***，能为全球用户提供低成本、高精度的三维位置、速度和精确定时等导航信息。

机器人实时自主定位是其精确到点完成任务的先决条件，GPS是一种绝对式定位传感器，它的优点是简单直接，定位精度不受定位时间和距离增加的影响，差分GPS利用已知精确三维坐标的基站和先进的差分算法，可以做到厘米级别的定位精度。因此将差分GPS应用于室外机器人的定位可以让机器人获得非常高精度的定位信息。

然而GPS定位信息描述的是它自己的位置信息，将GPS安装于机器人上后，它定位的点与机器人本体所需定位的点一般是不重合的，为了从GPS的定位数据得到机器人的定位数据，我们需要获得GPS与机器人之间的那个相对位置参数。

传统的最简单的获取GPS与机器人相对位置参数的方式是用尺子进行测量。这种方式受到以下因素的制约：一、GPS本身具有较大的尺寸，它自身的定位点存在于设备内部，用尺子测量时根本测量不到；二、一般理论上定义机器人定位的点位于机器人各驱动轮着地点的几何中心，这个点在理论上也是可测量到的，但是因为机器人体积一般较大，重心分布不均，且各轮子之间总是存在差异，这导致理论上机器人定位的点往往并不是机器人真实定位的点；三、用尺子每次只能测量一条直线，而GPS与车之间的相对位置是空间的，对不同维度进行测量时并不能保证尺子测量的准确性。

发明内容

本发明针对上述现有技术中存在的问题，提出一种差分GPS与机器人的相对位置参数标定方法及***，采用多传感器融合的方式，提高了机器人实时定位的精度及可靠性。

为解决上述技术问题，本发明是通过如下技术方案实现的：

本发明提供一种差分GPS与机器人的相对位置参数标定方法，其包括以下步骤：

S11：利用速度测量器和角速度测量器对机器人进行定位，获得所述机器人的速度、角速度信息；

S12：利用差分GPS对所述机器人进行定位，获得所述机器人的GPS位置信息；

S13：根据初始位置航向信息、所述速度、角速度信息以及所述GPS位置信息获得所述差分GPS与所述机器人的相对位置参数。

差分GPS(differentialGPS-DGPS，DGPS)是首先利用已知精确三维坐标的差分GPS基准台，求得伪距修正量或位置修正量，再将这个修正量实时或事后发送给用户(GPS导航仪)，对用户的测量数据进行修正，以提高GPS定位精度。差分GPS在较理想条件下定位精度可达到厘米级别。陀螺仪是利用高度回转体的动量矩敏感壳体相对惯性空间绕正交与自转轴的一个或二个轴的角运动检测装置，它可以测量并输出物体运动的角速度，对角速度进行积分即可得到物体的转动角度。里程计为测量形成的装置，其可以测量计算出机器人物体前进的距离或者行进速度。陀螺仪和里程计两种传感器稳定性非常好，但是却存在误差累积的缺陷，随着定位时间和定位距离的增加，定位精度会持续下降；而差分GPS是基于卫星的全球定位，每个时刻只需获得足够多的卫星的信号就能解算出高精度的定位信息，它不存在误差累积，然而却存在卫星信号被遮挡的风险。本发明将陀螺仪、里程计的高可靠性和差分GPS的高精度无误差累积结合起来，提高了机器人实时定位的精度及可靠性。

在卫星信号好的短时间内，可以利用里程计和陀螺仪对机器人进行定位，也可以用差分GPS对机器人进行定位，而联系着里程计、陀螺仪定位数据与差分GPS定位数据关系的是这两套定位***之间的相对参数。因为里程计数据是通过对机器人轮子转动测量获得的，陀螺仪与机器人底盘之间的参数可以简单标定获得，因此，里程计、陀螺仪定位***定位的点就是机器人本体定位的点，因此，里程计、陀螺仪定位数据与差分GPS定位数据之间的关系数据就是本发明所需要标定的差分GPS与机器人的相对位置参数。

较佳地，所述步骤S13具体包括以下步骤：

S131：根据初始位置航向信息、所述速度信息以及所述角速度信息获得实时位置信息和航向信息；

S132：根据差分GPS获得的K1时刻GPS位置信息、K2时刻GPS位置信息以及所述实时位置航向信息获得所述相对位置参数。

较佳地，当所述机器人在平整水平路面运动时，所述机器人的运动在二维空间描述，所述速度、角速度信息以及所述位置信息为二维信息。

较佳地，所述步骤S12具体为：机器人从K1时刻运动到K2时刻，获得所述机器人在K1时刻的GPS位置信息P_gk1以及在K2时刻的GPS位置信息P_gk2。

较佳地，所述步骤S13具体为：所述差分GPS与所述机器人的相对位置参数为：

P_c＝P_c^+inv(R_k1-R_k2)*(P_gk2^-P_gk2)，

其中，

P_k1^＝P_gk1-R_k1*P_c^，

P_gk2^＝P_k2^+R_k2*P_c^，

P_k1^、P_k2^为所述机器人的实时位置信息的估计值，P_k2^根据P_k1^以及所述速度信息、所述角速度信息计算得出，P_c^为相对位置参数的预设估计值，P_gk2^为K2时刻的GPS位置信息的估计值，inv()为矩阵求逆运算，R为航向信息θ对应的矩阵， $R=\left[\begin{array}{cc}cos\mathrm{\θ}& -sin\mathrm{\θ}\\ sin\mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right],$ R_k1、R_k2分别为K1、K2时刻的R。

较佳地，当路面不平整时，在步骤S13之后还包括：

S14：对所述机器人进行垂直标定。

较佳地，所述步骤S14具体为：利用尺子测量方法对所述机器人进行垂直标定。

本发明还提供一种差分GPS与机器人的相对位置参数标定***，其包括：

速度测量器，用于对机器人的平动进行测量，获得所述机器人的速度；

角速度测量器，用于对机器人的转动进行测量，获得所述机器人的角速度；

差分GPS，用于对机器人的位置进行测量，获得所述机器人的GPS位置信息；

相对位置参数标定单元，用于根据初始航向角信息、所述速度信息、所述角速度信息以及所述GPS位置信息获得所述差分GPS与所述机器人的相对位置参数。

较佳地，所述相对位置参数标定单元具体包括：

实时航向角信息获得单元，用于根据初始航向角信息、所述位置信息以及所述角速度信息获得实时航向角信息；

相对位置参数获得单元，用于根据所述实时航向角信息以及所述GPS位置信息获得所述差分GPS与机器人的相对位置参数。

较佳地，所述速度测量器为里程计，所述角速度测量器为陀螺仪。

较佳地，标定***还包括：垂直标定单元，用于对机器人进行垂直标定；进一步地，所述垂直标定单元为尺子。

相较于现有技术，本发明具有以下优点：

本发明提供的差分GPS与机器人的相对位置参数标定方法及***，将陀螺仪、里程计的高可靠性和差分GPS的高精度无误差累积结合起来，实现了机器人的实时定位，定位精度高，误差小，可靠性高。

附图说明

下面结合附图对本发明的实施方式作进一步说明：

图1为本发明的差分GPS与机器人的相对位置参数标定方法的流程图；

图2为本发明的差分GPS与机器人的相对位置参数标定***的结构示意图。

标号说明：1-速度测量器，2-角速度测量器，3-差分GPS，4-相对位置参数标定单元。

具体实施方式

下面对本发明的实施例作详细说明，本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

本发明的差分GPS与机器人的相对位置参数标定方法包括以下步骤：

S11：利用速度测量器和角速度测量器对机器人进行定位，获得机器人的速度、角速度信息；

S12：利用差分GPS对机器人进行定位，获得机器人的GPS位置信息；

S13：根据初始航向角信息、速度、角速度信息以及GPS位置信息获得差分GPS与机器人的相对位置参数。

其中，步骤S13具体包括：

S131：根据初始航向信息、速度信息以及角速度信息获得实时航向角信息；

S132：根据差分GPS获得的K1时刻GPS位置信息、K2时刻GPS位置信息以及实时航向角信息获得相对位置参数。

下面结合具体实施例，详细推导本发明的差分GPS与机器人的相对位置参数标定的计算公式。

实施例1：

本实施例标定时让机器人在平整水平路面运动，所以整个算法只需在水平二维空间展开。

1)用速度测量器和角速度测量器对机器人进行定位的公式

让机器人在平整水平路面上运动，位置航向信息为 ${\left[\begin{array}{c}x\\ y\\ \mathrm{\θ}\end{array}\right]}_k$ (x、y是位置，θ是航向，k指时刻)，速度测量器测量的机器人速度大小为v_k，角速度测量器测量的机器人运动的角速度为ω_k，速度测量器和角速度测量器的测量周期为t，则有

${\left[\begin{array}{c}x\\ y\\ \mathrm{\θ}\end{array}\right]}_k={\left[\begin{array}{c}x\\ y\\ \mathrm{\θ}\end{array}\right]}_{k-1}+\left[\begin{array}{c}{v}_{k-1}tcos{\mathrm{\θ}}_{k-1}\\ v_{k-1}t{\mathrm{sin\θ}}_{k-1}\\ {\mathrm{\ω}}_{k-1}t\end{array}\right]---\left(1\right)$

2)机器人位置与差分GPS位置之间关系的公式

假设差分GPS相对于机器人的二维水平位置为 $\left[\begin{array}{c}{x}_c\\ y_c\end{array}\right],$ GPS输出的定位数据为 ${\left[\begin{array}{c}{x}_g\\ y_g\end{array}\right]}_k,$ 则有

${\left[\begin{array}{c}{x}_g\\ y_g\end{array}\right]}_k={\left[\begin{array}{c}x\\ y\end{array}\right]}_k+{\left[\begin{array}{cc}cos\mathrm{\θ}& -sin\mathrm{\θ}\\ sin\mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]}_k*\left[\begin{array}{c}{x}_c\\ y_c\end{array}\right]---\left(2\right)$

3)差分GPS相对于机器人的二维水平位置标定过程及公式

假设机器人位置的真值用 $\left[\begin{array}{c}x\\ y\end{array}\right]=P$ 表示，估计值用 ${\left[\begin{array}{c}x\\ y\end{array}\right]}^{^}=P^{^}$ 表示；机器人的航向真值为θ，航向信息θ对应的矩阵 $\left[\begin{array}{cc}cos\mathrm{\θ}& -sin\mathrm{\θ}\\ sin\mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]=R;$ 差分GPS相对于机器人的二维水平位置真值用 $\left[\begin{array}{c}{x}_c\\ y_c\end{array}\right]=P_c$ 表示，估计值用 ${\left[\begin{array}{c}{x}_c\\ y_c\end{array}\right]}^{^}=P_c^{^}$ 表示；差分GPS输出的定位值为 $\left[\begin{array}{c}{x}_g\\ y_g\end{array}\right]=P_g,$ 估计值为 ${\left[\begin{array}{c}{x}_g\\ y_g\end{array}\right]}^{^}={P_g}^{^};$ 取k1，k2两个时刻的状态，则有真值公式为

P_gk1＝P_k1+R_k1*P_c(3)

P_gk2＝P_k2+R_k2*P_c(4)

在k1时刻，我们先对待标定参数做一个估计然后可以根据GPS的真实输出P_gk1，及R_k1与求出机器人位置的估计值所用公式为

P_gk1＝P_k1^+R_k1*P_c^(5)

在得到k1时刻机器人位置的估计值后，让机器人走圆弧到k2时刻，我们可以用公式(1)得到k2时刻机器人位置的估计值如果k2和k1之间的间隔为一个测量周期t，则使用公式(1)一次即可得出，如果k2和k1之间的间隔为多个测量周期，则多次使用公式(1)也可得出，由及R_k2与可以得到k2时刻GPS位置的估计值，所用公式为

P_gk2^＝P_k2^+R_k2*P_c^(6)

将公式(3)(4)左右对应相减，公式(5)(6)左右对应相减，有

P_gk1-P_gk2＝P_k1-P_k2+(R_k1-R_k2)*P_c(7)

P_gk1-P_gk2^＝P_k1^-P_k2^+(R_k1-R_k2)*P_c^(8)

另外在机器人航向已知的情况下，由公式(1)可知再将公式(7)(8)左右对应相减，有

P_gk2^-P_gk2＝(R_k1-R_k2)*(P_c-P_c^)(9)

则有

(P_c-P_c^)＝inv(R_k1-R_k2)*(P_gk2^-P_gk2)(10)

其中，inv()是矩阵求逆运算，由式(10)可得，

P_c＝P_c^+inv(R_k1-R_k2)*(P_gk2^-P_gk2)(11)

公式(11)就是差分GPS相对于机器人的二维水平位置 $\left[\begin{array}{c}{x}_c\\ y_c\end{array}\right]$ 的求解公式。

上面公式推导一个很重要的前提是，机器人的初始航向角信息是已知的，这可以通过预先估计一个初始航向，然后可以对初始航向角进行对准，对准方法为让机器人走一定距离的直线做初始对准得到机器人的精确初始航向角，在初始航向角对准后，再执行上述相对位置参数标定过程，标定过程中的航向角始终通过角速度测量器的数据进行精确求解，因为整个标定过程时间很短，角速度测量器和速度测量器对机器人的位置航向的估计都是很准确的，所以上述方法准确度非常高，可靠性高。

实施例2：

本实施例对差分GPS与机器人在高度方向的相对位置参数标定进行讨论。

轮式机器人一般在水平路面作业，关注的主要是二维水平定位精度，而GPS输出的定位数据中，高程方向精度是较差的，所以一般也只使用GPS的水平定位数据，那么z_c有没有必要估计呢。

当路面存在不平整情况时，假设存在α的坡度，则z_c将对GPS在水平方向的投影带来z_c*sinα的偏移，当z_c较大时，这个偏移量是不容忽视的，所以必须对z_c进行估计。但是假设z_c的估计值为z_c^，则侧向偏移的误差为假设一般路面不平整的坡度为2°，则可以看到这个侧偏移对z_c估计误差是极其不敏感的，所以可以用一般的尺子测量方法对z_c进行测量，即使测量值存在几厘米的误差，其对最终机器人水平定位的影响也是可以忽略的。

实施例3：

本实施例详细描述本发明的差分GPS与机器人的相对位置参数标定***，其是用于实现实施例1所描述的标定方法的标定***，其结构示意图如图2所示，其包括：速度测量器1、角速度测量器2、差分GPS3以及相对位置参数标定单元4，其中，速度测量器1用于对机器人的平动进行测量，获得机器人的速度；角速度测量器2用于对机器人的转动进行测量，获得机器人的角速度；差分GPS3用于对机器人的位置进行测量，获得机器人的GPS位置信息；相对位置参数标定单元4用于根据初始航向角信息、速度信息、角速度信息以及GPS位置信息获得差分GPS与机器人的相对位置参数。

实施例4：

本实施例是在实施例3的基础上增加了垂直标定单元，其是用于实现实例2所描述的标定方法的标定***，垂直标定单元用于对差分GPS与机器人的垂直相对位置参数进行标定。

上述实施例中，速度测量器1可以为里程计，角速度测量器2可以为陀螺仪，垂直标定单元可以为尺子；里程计可以为码盘测量装置，用于测量机器人的轮子的转动，进而换算成机器人前进的距离或行走速度。

此处公开的仅为本发明的优选实施例，本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，并不是对本发明的限定。任何本领域技术人员在说明书范围内所做的修改和变化，均应落在本发明所保护的范围内。

Claims (11)

1.一种差分GPS与机器人的相对位置参数标定方法，其特征在于，包括以下步骤：

S11：利用速度测量器和角速度测量器对机器人进行定位，获得所述机器人的速度、角速度信息；

S12：利用差分GPS对所述机器人进行定位，获得所述机器人的GPS位置信息；

S13：根据初始位置航向信息、所述速度、角速度信息以及所述GPS位置信息获得所述差分GPS与所述机器人的相对位置参数。

2.根据权利要求1所述的差分GPS与机器人的相对位置参数标定方法，其特征在于，所述步骤S13具体包括以下步骤：

S131：根据初始位置航向信息、所述速度信息以及所述角速度信息获得实时位置信息和航向信息；

S132：根据差分GPS获得的K1时刻GPS位置信息、K2时刻GPS位置信息以及所述实时位置航向信息获得所述相对位置参数。

3.根据权利要求1所述的差分GPS与机器人的相对位置参数标定方法，其特征在于，当所述机器人在平整水平路面运动时，所述机器人的运动在二维空间描述，所述速度、角速度信息以及所述位置信息为二维信息。

4.根据权利要求3所述的差分GPS与机器人相对位置参数的标定方法，其特征在于，所述步骤S12具体为：机器人从K1时刻运动到K2时刻，获得所述机器人在K1时刻的GPS位置信息P_gk1以及在K2时刻的GPS位置信息P_gk2。

5.根据权利要求4所述的差分GPS与机器人相对位置参数标定方法，其特征在于，所述步骤S13具体为：所述差分GPS与所述机器人的相对位置参数为：

P_c＝P_c ^{^}+inv(R_k1-R_k2)*(P_gk2 ^{^}-P_gk2)，

其中，

P_k1 ^{^}＝P_gk1-R_k1*P_c ^{^}，

P_gk2 ^{^}＝P_k2 ^{^}+R_k2*P_c ^{^}，

P_k1 ^{^}、P_k2 ^{^}为所述机器人的实时位置信息的估计值，P_k2 ^{^}根据P_k1 ^{^}以及所述速度信息、所述角速度信息计算得出，P_c ^{^}为相对位置参数的预设估计值，P_gk2 ^{^}为K2时刻的GPS位置信息的估计值，inv()为矩阵求逆运算，R为航向信息θ对应的矩阵， $R=\left[\begin{array}{cc}cos\mathrm{\θ}& -sin\mathrm{\θ}\\ sin\mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right],$ R_k1、R_k2分别为K1、K2时刻的R。

6.根据权利要求1所述的差分GPS与机器人相对位置参数标定方法，其特征在于，当路面不平整时，在步骤S13之后还包括：

S14：对所述机器人进行垂直标定。

7.根据权利要求6所述的差分GPS与机器人相对位置参数标定方法，其特征在于，所述步骤S14具体为：利用尺子测量方法对所述机器人进行垂直标定。

8.一种差分GPS与机器人的相对位置参数标定***，其特征在于，包括：

速度测量器，用于对机器人的平动进行测量，获得所述机器人的速度；

角速度测量器，用于对机器人的转动进行测量，获得所述机器人的角速度；

差分GPS，用于对机器人的位置进行测量，获得所述机器人的GPS位置信息；

相对位置参数标定单元，用于根据初始航向角信息、所述速度信息、所述角速度信息以及所述GPS位置信息获得所述差分GPS与所述机器人的相对位置参数。

9.根据权利要求8所述的差分GPS与机器人的相对位置参数标定***，其特征在于，所述相对位置参数标定单元具体包括：

实时航向角信息获得单元，用于根据初始航向角信息、所述位置信息以及所述角速度信息获得实时航向角信息；

相对位置参数获得单元，用于根据所述实时航向角信息以及所述GPS位置信息获得所述差分GPS与机器人的相对位置参数。

10.根据权利要求8所述的差分GPS与机器人的相对位置参数标定***，其特征在于，所述速度测量器为里程计，所述角速度测量器为陀螺仪。

11.根据权利要求8所述的差分GPS与机器人的相对位置参数标定***，其特征在于，还包括：垂直标定单元，用于对机器人进行垂直标定；

进一步地，所述垂直标定单元为尺子。