CN104020444B - 一种双轮差动式机器人室内组合定位方法 - Google Patents

Abstract

本发明提供了一种双轮差动式机器人室内组合定位方法，通过蓝牙接收器信号强度指示值得到机器人的当前位置坐标，再通过预测公式得到机器人的预测横坐标和预测纵坐标，使用互补滤波器算法融合预测坐标和当前坐标的计算结果，得到下一时刻机器人的位姿，也就实现了对双轮差动式机器人的组合定位，由于采用了基于蓝牙技术和航位推测技术相组合的双轮差动式机器人室内组合定位方法，该方法具有功耗低、体积小、定位精度高、造价低等特点，可以有效解决现有技术中所存在的定位精度差、成本高、易受干扰等缺点。

Description

一种双轮差动式机器人室内组合定位方法

技术领域

本发明涉及机器人领域，尤其是机器人的组合定位。

背景技术

随着机器人技术和计算机控制技术的兴起，日益智能化的移动服务机器人的用途也越来越广泛，机器人开始从工业领域向军事、医疗、家庭助理等领域渗透。而实时定位在移动服务机器人的工作过程中起着至关重要的作用，可靠、高精度的定位技术是移动服务机器人的关键技术之一。轮式服务机器人具有控制简单，反应灵敏，运动速度快的特点，而且运动快慢也容易控制，因此在移动服务机器人里有着广泛的应用。

现在的室内定位技术主要由超声波定位技术、超宽带定位技术、射频识别定位技术、ZigBee无线定位技术和蓝牙定位技术等。然而，超声波定位技术实现成本低，定位精度相对比较高，但容易受到环境的干扰，而且只能在可视距范围内有效；超带宽定位技术比较适合室内定位，但是目前的实现成本相当高，没有实用意义；射频识别定位技术比较容易实现，定位精度适中，但是多径干扰对其影响比较大；ZigBee无线定位技术功耗低，但其定位受到环境的影响比较大；单独的蓝牙定位技术定位具有体积小、功耗低、信号传输不受视距的影响等特点，但是其定位精度一般。

发明内容

为了克服现有技术的不足，本发明提供了一种基于蓝牙技术和航位推测技术相组合的双轮差动式机器人室内组合定位方法。

本发明解决其技术问题所采用的技术方案是：

(1)机器人在室内地面上长为a米，宽为b米的长方形区域内活动，并且a、b均为2的整数倍，室内地面距离室内天花板的垂直距离为h米，以长方形的一个顶点为原点O，经过这个顶点的长边为x轴正方向，宽边为y轴正方向，经过原点O垂直于地面向上为z轴正方向，建立O-xyz坐标系，这个顶点可以选择长方形四个顶点中任意一个，在O-xyz坐标系中的(2i,2j,h)每一处安装一个蓝牙发射器，这里i＝0,1,2…a/2，j＝0,1,2…b/2，并且每一个蓝牙发射器相同型号，设置一个唯一的ID号，同时每一个蓝牙发射器的坐标信息是唯一且已知的；

(2)在双轮差动式机器人顶部安装一个可以同时接受多个蓝牙信号的蓝牙接收器，蓝牙接收器可以接收到蓝牙发射器的广播信息；

(3)所有蓝牙发射器以相同的功率广播自身信息，自身信息包括ID信息和坐标信息；

(4)蓝牙接收器接收到步骤(3)中蓝牙发射器的广播自身信息之后，根据所接收到信号的功率大小可以直接得到接收信号强度指示值RSSI，并依据RSSI值的大小对接收到的信号强度依次从强到弱排序，根据RSSI值排序的顺序对RSSI值所对应蓝牙发射器进行排序；

(5)取信号强度排序位于前四位的蓝牙发射器广播信息，其余的蓝牙发射器广播信息丢弃；

(6)蓝牙接收器接收到蓝牙发射器信号的RSSI值与该蓝牙发射器和蓝牙接收器之间距离的关系式为r为蓝牙发射器到蓝牙发射器之间的距离，即是蓝牙发射器与双轮差动式机器人之间的距离，A为r＝1时机器人接收蓝牙信号的RSSI值，n为信号传播常量，这里取n＝2，RSSI取值是当机器人距离蓝牙发射器距离为r时蓝牙接收器读取的数值，即RSSI值是直接可以从蓝牙接收器中读取的数值，即：

$r={10}^{-\frac{RSSI+A}{20}}---\left(1\right)$

(7)A值的获得采取多次测量求平均值的办法：将双轮差动式机器人放在距离蓝牙发射器1米处，连续10次测量蓝牙接收器接收该蓝牙发射器的RSSI值，对这10次求平均值即得到A的值，双轮差动式机器人在自由移动的过程中，可以根据接收到的蓝牙信号RSSI值由式(1)实时得到机器人到相应蓝牙发射器之间的距离r；

(8)在得到第k个蓝牙发射器到双轮差动式机器人的距离r^(k)后，可以得到双轮差动式机器人的定位方程式：

$\left\{\begin{array}{c}\sqrt{{(x^{\left(1\right)}-\stackrel{\‾}{x})}^2+{(y^{\left(1\right)}-\stackrel{\‾}{y})}^2+h^2}=r^{\left(1\right)}\\ \sqrt{{(x^{\left(2\right)}-\stackrel{\‾}{x})}^2+{(y^{\left(2\right)}-\stackrel{\‾}{y})}^2+h^2}=r^{\left(2\right)}\\ \sqrt{{(x^{\left(3\right)}-\stackrel{\‾}{x})}^2+{(y^{\left(3\right)}-\stackrel{\‾}{y})}^2+h^2}=r^{\left(3\right)}\\ \sqrt{{(x^{\left(4\right)}-\stackrel{\‾}{x})}^2+{(y^{\left(4\right)}-\stackrel{\‾}{y})}^2+h^2}=r^{\left(4\right)}\end{array}\right.---\left(2\right)$

式(2)中，x⁽¹⁾和y⁽¹⁾为信号最强的蓝牙发射器的横坐标和纵坐标，x⁽²⁾和y⁽²⁾为信号次强的蓝牙发射器的横坐标和纵坐标，x⁽³⁾和y⁽³⁾为信号第三强的蓝牙发射器的横坐标和纵坐标，x⁽⁴⁾和y⁽⁴⁾为信号第四强的蓝牙发射器的横坐标和纵坐标，为双轮差动式机器人的位置坐标，h为室内天花板到地面的高度，单位为米，h不超过5，具体值可以经过精确测量得到，在式(2)里为已知值；

(9)对非线性方程式(2)运用牛顿迭代及其线性化的方法进行线性化，然后对线性化以后的方程运用最小二乘法求解，解出式(2)中的和

(10)k时刻双轮差动式机器人的位姿为(x_k,y_k,θ_k)^T，其中x_k和y_k分别为双轮差动式机器人在k时刻的x、y坐标，θ_k为0时刻到k时刻的旋转角度，向量的上标“T”代表向量的转置向量，u_k＝(δ_k,Δθ_k)^T为k时刻的输入控制量，其中δ_k为0时刻到k时刻移动的距离，Δθ_k为双轮差动式机器人从k-1时刻到k时刻旋转的角度，对于双轮差动式机器人，δ_k＝(δ_rk+δ_lk)/2，其中δ_lk和δ_rk分别为双轮差动式机器人左轮和右轮移动的距离；Δθ_k＝(δ_rk-δ_lk)/d，d为双轮差动式机器人两轮之间的距离，则机器人的状态方程为：

$\left\{\begin{array}{c}{\hat{x}}_{k+1}=x_k+{\mathrm{\δ}}_{k}cos({\mathrm{\θ}}_k+{\mathrm{\Δ\θ}}_k)\\ {\hat{y}}_{k+1}=y_k+{\mathrm{\δ}}_{k}sin({\mathrm{\θ}}_k+{\mathrm{\Δ\θ}}_k)\\ {\mathrm{\θ}}_{k+1}={\mathrm{\θ}}_k+{\mathrm{\Δ\θ}}_k\end{array}\right.---\left(3\right)$

式(3)中x_k和y_k分别为双轮差动式机器人在k时刻的x、y坐标，和为机器人在K+1时刻横坐标和纵坐标的估计值，θ_k+1为双轮差动式机器人在k+1时刻的旋转角度；

(11)双轮差动式机器人的初始位姿(x₀,y₀,θ₀)^T即初始位置坐标和初始旋转角度，可以精确测量得到，取每一个时刻的间隔为0.1秒，在k时刻，根据式(3)得到k+1时刻机器人的横坐标估计值和纵坐标估计值在k+1时刻,根据式(2)可以计算出k+1时刻的机器人位置的横坐标和纵坐标使用互补滤波器算法融合式(2)和式(3)的计算结果，得到k+1时刻机器人的最终位置，互补滤波器算法如式(4)所示：

$\left\{\begin{array}{c}x=\mathrm{\α}\×\stackrel{\‾}{x}+\mathrm{\β}\×\hat{x}\\ y=\mathrm{\α}\×\stackrel{\‾}{y}+\mathrm{\β}\×\hat{y}\end{array}\right.---\left(4\right)$

式(4)中，α和β为互补滤波器算法系数，α和β的决策策略如下：

当0.1×(k+1)为整数时，α＝0.64，β＝0.36；

当0.1×(k+1)不为整数时，α＝0.22,β＝0.78；

即得到下一个时刻机器人的位姿，也就实现了对双轮差动式机器人的组合定位。

本发明的有益效果是由于采用了一种基于蓝牙技术和航位推测技术相组合的双轮差动式机器人室内组合定位方法，该方法具有功耗低、体积小、定位精度高、造价低等特点，可以有效解决现有技术中所存在的定位精度差、成本高、易受干扰等缺点。

附图说明

图1是本发明工作流程图。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

以下为具体实施步骤：

(1)机器人在室内地面上长为a米，宽为b米的长方形区域内活动，并且a、b均为2的整数倍，室内地面距离室内天花板的垂直距离为h米。以长方形的一个顶点为原点O，经过这个顶点的长边为x轴正方向，宽边为y轴正方向，经过原点O垂直于地面向上为z轴正方向，建立O-xyz坐标系。其中，这个顶点可以为长方形四个顶点中任意一个。在O-xyz坐标系中的(2i,2j,h)点处安装一个相同型号的蓝牙发射器(这里i＝0,1,2…a/2；j＝0,1,2…b/2)，并且为每一个蓝牙发射器设置一个唯一的ID号，同时每一个蓝牙发射器的坐标信息也是唯一的并且是已知的；

(2)所有蓝牙发射器以相同的功率广播自身信息，自身信息包括ID信息和坐标信息；

(3)在双轮差动式机器人顶部安装一个可以同时接受多个蓝牙信号的蓝牙接收器，蓝牙接收器可以接收到蓝牙发射器的广播信息；

(4)蓝牙接收器接收到蓝牙发射器的广播信息以后，根据所接收到信号的功率大小可以直接得到接收信号强度指示值RSSI，并依据RSSI值大小判断接收信号的强弱，根据接收信号强度从强到弱的顺序对蓝牙发射器进行排序；

(5)取出信号强度排序前四位的蓝牙发射器广播信息，使蓝牙广播信息强度排序前四位的广播信息参与双轮差动式机器人室内定位，其余的蓝牙发射器广播信息丢弃，

(6)双轮差动式机器人接收到蓝牙发射器信号RSSI值与蓝牙发射器和双轮差动式机器人之间距离的关系式为r为蓝牙发射器到双轮差动式机器人的距离，A为r＝1时机器人接收蓝牙信号的RSSI值，n为信号传播常量，这里取n＝2，即

$r={10}^{-\frac{RSSI+A}{20}}---\left(1\right)$

(7)A值的获得采取多次测量求平均值的办法，将双轮差动式机器人放在距离蓝牙发射器1米处，连续测量10次蓝牙接收器接收该蓝牙发射器的RSSI值，对这10次求平均值得到A的值，双轮差动式机器人在自由移动的过程中，可以根据接收到的蓝牙信号RSSI值由式1实时得到机器人到相应蓝牙发射器的距离r；

(8)得到第k个蓝牙发射器到双轮差动式机器人的距离r^(k)后，可以得到双轮差动式机器人的定位方程式(2)：

$\left\{\begin{array}{c}\sqrt{{(x^{\left(1\right)}-\stackrel{\‾}{x})}^2+{(y^{\left(1\right)}-\stackrel{\‾}{y})}^2+h^2}=r^{\left(1\right)}\\ \sqrt{{(x^{\left(2\right)}-\stackrel{\‾}{x})}^2+{(y^{\left(2\right)}-\stackrel{\‾}{y})}^2+h^2}=r^{\left(2\right)}\\ \sqrt{{(x^{\left(3\right)}-\stackrel{\‾}{x})}^2+{(y^{\left(3\right)}-\stackrel{\‾}{y})}^2+h^2}=r^{\left(3\right)}\\ \sqrt{{(x^{\left(4\right)}-\stackrel{\‾}{x})}^2+{(y^{\left(4\right)}-\stackrel{\‾}{y})}^2+h^2}=r^{\left(4\right)}\end{array}\right.---\left(2\right)$

式2中x⁽ⁱ⁾和y⁽ⁱ⁾分别为第k(k＝1,2,3,4)个蓝牙发射器的坐标，为双轮差动式机器人位置坐标，h为室内天花板到地面的高度，单位为米，h不超过5。

(9)对非线性方程式(2)运用牛顿迭代及其线性化的方法进行线性化，然后对线性化以后的方程运用最小二乘法求解，解出式(2)中的和

(10)k时刻双轮差动式机器人的位姿为(x_k,y_k,θ_k)^T，其中x_k和y_k分别为双轮差动式机器人在k时刻的x、y坐标，θ_k为其在k时刻的旋转角度，向量的上标“T”代表向量的转置向量，u_k＝(δ_k,Δθ_k)^T为k时刻的输入控制量，其中，δ_k为k时刻移动的距离，Δθ_k为机器人在k时刻旋转的角度，对于双轮差动式机器人，δ_k＝(δ_rk+δ_lk)/2，其中δ_lk和δ_rk分别为左轮和右轮移动的距离；Δθ_k＝(δ_rk-δ_lk)/d，d为双轮差动式机器人两轮之间的距离。则机器人的状态方程为式(3)

$\left\{\begin{array}{c}{\hat{x}}_{k+1}=x_k+{\mathrm{\δ}}_{k}cos({\mathrm{\θ}}_k+{\mathrm{\Δ\θ}}_k)\\ {\hat{y}}_{k+1}=y_k+{\mathrm{\δ}}_{k}sin({\mathrm{\θ}}_k+{\mathrm{\Δ\θ}}_k)\\ {\mathrm{\θ}}_{k+1}={\mathrm{\θ}}_k+{\mathrm{\Δ\θ}}_k\end{array}\right.---\left(3\right)$

式3中为k+1时刻机器人定位结果预测值，(x_k,y_k)^T为k时刻机器人的实际定位结果；

(11)双轮差动式机器人的初始位姿(x₀,y₀,θ₀)^T即初始位置坐标和初始旋转角度可以精确测量得到，取每一个时刻的间隔为0.1秒，在k时刻，根据双轮差动式机器人状态方程式(3)可以预测出k+1时刻机器人的位姿，在k+1时刻，蓝牙发射器根据式(2)也可以计算出k+1时刻的机器人位置坐标，k+1时刻的最终定位结果如下：

当0.1×(k+1)为整数时：

$\left\{\begin{array}{c}x=0.64\×\stackrel{\‾}{x}+0.36\×\hat{x}\\ y=0.64\×\stackrel{\‾}{y}+0.36\×\hat{y}\end{array}\right.---\left(4\right)$

当0.1×(k+1)不为整数时：

$\left\{\begin{array}{c}x=0.22\×\stackrel{\‾}{x}+0.78\×\hat{x}\\ y=0.22\×\stackrel{\‾}{y}+0.78\×\hat{y}\end{array}\right.---\left(5\right)$

即得到下一个时刻机器人的位姿，也就实现了对双轮差动式机器人的组合定位。

Claims (1)

1.一种双轮差动式机器人室内组合定位方法，其特征在于包括下述步骤：

(1)机器人在室内地面上长为a米，宽为b米的长方形区域内活动，并且a、b均为2的整数倍，室内地面距离室内天花板的垂直距离为h米，以长方形的一个顶点为原点O，经过这个顶点的长边为x轴正方向，宽边为y轴正方向，经过原点O垂直于地面向上为z轴正方向，建立O-xyz坐标系，这个顶点可以选择长方形四个顶点中任意一个，在O-xyz坐标系中的(2i,2j,h)每一处安装一个蓝牙发射器，这里i＝0,1,2…a/2，j＝0,1,2…b/2，并且每一个蓝牙发射器相同型号，设置一个唯一的ID号，同时每一个蓝牙发射器的坐标信息是唯一且已知的；

(2)在双轮差动式机器人顶部安装一个可以同时接受多个蓝牙信号的蓝牙接收器，蓝牙接收器可以接收到蓝牙发射器的广播信息；

(3)所有蓝牙发射器以相同的功率广播自身信息，自身信息包括ID信息和坐标信息；

(4)蓝牙接收器接收到步骤(3)中蓝牙发射器的广播自身信息之后，根据所接收到信号的功率大小可以直接得到接收信号强度指示值RSSI，并依据RSSI值的大小对接收到的信号强度依次从强到弱排序，根据RSSI值排序的顺序对RSSI值所对应蓝牙发射器进行排序；

(5)取信号强度排序位于前四位的蓝牙发射器广播信息，其余的蓝牙发射器广播信息丢弃；

(6)蓝牙接收器接收到蓝牙发射器信号的RSSI值与该蓝牙发射器和蓝牙接收器之间距离的关系式为r为蓝牙发射器到蓝牙发射器之间的距离，即是蓝牙发射器与双轮差动式机器人之间的距离，A为r＝1时机器人接收蓝牙信号的RSSI值，n为信号传播常量，这里取n＝2，RSSI取值是当机器人距离蓝牙发射器距离为r时蓝牙接收器读取的数值，即RSSI值是直接可以从蓝牙接收器中读取的数值，即：

$r={10}^{-\frac{RSSI+A}{20}}---\left(1\right)$

(7)A值的获得采取多次测量求平均值的办法：将双轮差动式机器人放在距离蓝牙发射器1米处，连续10次测量蓝牙接收器接收该蓝牙发射器的RSSI值，对这10次求平均值即得到A的值，双轮差动式机器人在自由移动的过程中，可以根据接收到的蓝牙信号RSSI值由式(1)实时得到机器人到相应蓝牙发射器之间的距离r；

(8)在得到第k个蓝牙发射器到双轮差动式机器人的距离r^(k)后，可以得到双轮差动式机器人的定位方程式：

$\left\{\begin{array}{c}\sqrt{{(x^{\left(1\right)}-\stackrel{\‾}{x})}^2+{(y^{\left(1\right)}-\stackrel{\‾}{y})}^2+h^2}=r^{\left(1\right)}\\ \sqrt{{(x^{\left(2\right)}-\stackrel{\‾}{x})}^2+{(y^{\left(2\right)}-\stackrel{\‾}{y})}^2+h^2}=r^{\left(2\right)}\\ \sqrt{{(x^{\left(3\right)}-\stackrel{\‾}{x})}^2+{(y^{\left(3\right)}-\stackrel{\‾}{y})}^2+h^2}=r^{\left(3\right)}\\ \sqrt{{(x^{\left(4\right)}-\stackrel{\‾}{x})}^2+{(y^{\left(4\right)}-\stackrel{\‾}{y})}^2+h^2}=r^{\left(4\right)}\end{array}\right.---\left(2\right)$

式(2)中，x⁽¹⁾和y⁽¹⁾为信号最强的蓝牙发射器的横坐标和纵坐标，x⁽²⁾和y⁽²⁾为信号次强的蓝牙发射器的横坐标和纵坐标，x⁽³⁾和y⁽³⁾为信号第三强的蓝牙发射器的横坐标和纵坐标，x⁽⁴⁾和y⁽⁴⁾为信号第四强的蓝牙发射器的横坐标和纵坐标，为双轮差动式机器人的位置坐标，h为室内天花板到地面的高度，单位为米，h不超过5，具体值可以经过精确测量得到，在式(2)里为已知值；

(9)对非线性方程式(2)运用牛顿迭代及其线性化的方法进行线性化，然后对线性化以后的方程运用最小二乘法求解，解出式(2)中的和

(10)k时刻双轮差动式机器人的位姿为(x_k,y_k,θ_k)^T，其中x_k和y_k分别为双轮差动式机器人在k时刻的x、y坐标，θ_k为0时刻到k时刻的旋转角度，向量的上标“T”代表向量的转置向量，u_k＝(δ_k,Δθ_k)^T为k时刻的输入控制量，其中δ_k为0时刻到k时刻移动的距离，Δθ_k为双轮差动式机器人从k-1时刻到k时刻旋转的角度，对于双轮差动式机器人，δ_k＝(δ_rk+δ_lk)/2，其中δ_lk和δ_rk分别为双轮差动式机器人左轮和右轮移动的距离；Δθ_k＝(δ_rk-δ_lk)/d，d为双轮差动式机器人两轮之间的距离，则机器人的状态方程为：

$\left\{\begin{array}{c}{\hat{x}}_{k+1}=x_k+{\mathrm{\δ}}_{k}cos({\mathrm{\θ}}_k+{\mathrm{\Δ\θ}}_k)\\ {\hat{y}}_{k+1}=y_k+{\mathrm{\δ}}_{k}sin({\mathrm{\θ}}_k+{\mathrm{\Δ\θ}}_k)\\ {\mathrm{\θ}}_{k+1}={\mathrm{\θ}}_k+{\mathrm{\Δ\θ}}_k\end{array}\right.---\left(3\right)$

式(3)中x_k和y_k分别为双轮差动式机器人在k时刻的x、y坐标，和为机器人在K+1时刻横坐标和纵坐标的估计值，θ_k+1为双轮差动式机器人在k+1时刻的旋转角度；

(11)双轮差动式机器人的初始位姿(x₀,y₀,θ₀)^T即初始位置坐标和初始旋转角度，可以精确测量得到，取每一个时刻的间隔为0.1秒，在k时刻，根据式(3)得到k+1时刻机器人的横坐标估计值和纵坐标估计值在k+1时刻,根据式(2)可以计算出k+1时刻的机器人位置的横坐标和纵坐标使用互补滤波器算法融合式(2)和式(3)的计算结果，得到k+1时刻机器人的最终位置，互补滤波器算法如式(4)所示：

$\left\{\begin{array}{c}x=\mathrm{\α}\×\stackrel{\‾}{x}+\mathrm{\β}\×\hat{x}\\ y=\mathrm{\α}\×\stackrel{\‾}{y}+\mathrm{\β}\×\hat{y}\end{array}\right.---\left(4\right)$

式(4)中，α和β为互补滤波器算法系数，α和β的决策策略如下：

当0.1×(k+1)为整数时，α＝0.64，β＝0.36；

当0.1×(k+1)不为整数时，α＝0.22,β＝0.78；

即得到下一个时刻机器人的位姿，也就实现了对双轮差动式机器人的组合定位。