CN106352869A - 移动机器人室内定位系统及其计算方法 - Google Patents

Abstract

本发明是基于超宽带(UWB)技术，并且融合了电机编码器的信息设计出的提高了定位精度的室内移动机器人定位系统，实现了室内移动机器人的精准定位。本发明提供的定位计算方法，是用于室内移动机器人的定位计算方法，包括：一、确定坐标原点，开启基站和室内移动机器人，以开启时室内移动机器人所测得的位姿2为初始位姿；二、通过电机编码器得到t时刻的移动机器人的即时位姿1；三、标签接收基站发射的高频电磁脉冲，通过计算方法确定出移动机器人自身的位置；四、用电子罗盘所测得的航向角扩维步骤三得到的移动机器人位置，得到t时刻的即时位姿2；以及五、用计算法融合即时位姿1和即时位姿2，得到t的位姿估计，保存并回到步骤二的五个步骤。

Description

移动机器人室内定位系统及其计算方法

技术领域

本发明涉及一种移动机器人室内定位系统及其计算方法，具体涉及一种基于超宽带(UWB)技术和电机编码器信息的移动机器人室内定位系统及其定位计算方法。

背景技术

机器人利用传感器感知周围环境来确定自身在环境中位置的自主定位技术，是机器人能够完成诸如路径规划、自主导航等复杂任务的前提，移动机器人是通过自身携带的传感器来获取自身状态和周围环境信息，从而实现自主定位。

目前，常用的传感器有里程计、陀螺仪和加速度计等传感器，常用的定位系统有GPS和北斗导航定位系统。由于里程计、陀螺仪和加速度计传感器在进行计算时，具有误差累积效应，不适合长距离定位。GPS和北斗定位系统可实现室外无遮挡环境下全球的定位，但若应用在室内环境中，由于卫星信号被建筑物等阻隔，定位精度低，难以达到室内定位要求。利用蓝牙、RFID和红外技术组成的网络，传输数据速率低，传输距离短；超声波定位系统定位精度虽然较高，但超声波受多径效应和非视距传播影响很大，同时需要大量的底层硬件设施投资，成本太高。

WLAN(Wireless Local Area Network)技术和RFID(Radio FrequencyIdentification)技术是新兴的定位技术，该技术的显著特点是设备简单、安装方便、信息传输速率高。目前，WLAN应用于小范围的室内定位，成本较低。但无论是用于室内还是室外定位，Wi-Fi收发器都只能覆盖半径90米以内的区域，而且很容易受到其他信号的干扰，从而影响其精度，定位器的能耗也较高。

发明内容

本发明是为了解决上述问题而进行的，目的在于提供一种定位精度高、能耗小、定位精准度及数据传送率高，且适合无线传感器网络的室内移动机器人自主定位系统。

DecaWave公司的DWM1000模块定位精度高、能耗小，集成了射频无线电设计所需的全部元器件，包括DW1000定位芯片、天线、平衡-不平衡变换器、石英晶体及无源元件。其定位精度为±10厘米，数据传输率高达6.8Mb/s，适合无线传感器网络(WSN)应用，直线距离达290米，非直视距离为35米。

本发明是基于超宽带(UWB)技术，并且融合了电机编码器的信息设计出的进一步提高了定位精度的移动机器人室内定位系统，实现了移动机器人在室内的精准定位。

本发明提供了一种室内移动机器人的室内定位系统，是基于超宽带技术和电机编码器信息的自主移动的室内移动机器人定位系统，具有这样的特征，包括：

室内移动机器人，是用美国国家仪器有限公司的myRIO做主控器的，在一定空间范围内可以自主移动的三全向轮移动机器人；

电子罗盘，又称航向角传感器，是设置在室内移动机器人上用于测定该室内移动机器人自身的航向角的；

电机编码器，是设置在室内移动机器人上用于提供自身定位信号的；以及

三块DWM1000模块，第三DWM1000模块设置在移动机器人上，第一和第二DWM1000模块设置在一定空间范围内，

其中，设置在移动机器人上的第三DWM1000模块为标签，固定设置在一定空间范围内的第一和第二DWM1000模块为第一和第二基站。

本发明还提供了一种定位计算方法，是用于前述室内移动机器人的定位系统的定位计算方法，具有这样的特征，包括：

步骤一，确定坐标原点，开启基站，再开启室内移动机器人，以开启时室内移动机器人所测得的位姿2为初始位姿；

步骤二，通过电机编码器得到t时刻的移动机器人的即时位姿1；

步骤三，标签接收基站发射的高频电磁脉冲，通过计算方法确定出移动机器人自身的位置；

步骤四，用电子罗盘所测得的航向角扩维步骤三得到的移动机器人位置，得到t时刻的即时位姿2；

步骤五，用计算法融合即时位姿1和即时位姿2，得到t的位姿估计，后回到步骤二。

在本发明提供的定位计算方法中，还可以具有这样的特征：

其中，步骤一中初始位姿的定位方法：

计算步骤I，在一定空间中建立XOY坐标系，第一和第二基站的高度为h₁、h₂，第一和第二基站的距相邻墙面的距离为l₁、l₂，第一和第二基站与移动机器人间的距离，由TOF算法算得为d1、d2，再设定移动机器人在一定空间中的坐标为(x，y)，则由几何关系可得：

$\left\{\begin{array}{c}{(y-l_1)}^2+x^2+{h_1}^2={d_1}^2\\ {(x-l_2)}^2+(Y_{\max }-y)+{h_2}^2={d_2}^2\end{array}\right.$

式中，y_max为一定空间y轴方向的宽度，令，

$m=\frac{d_1^2-d_2^2+l_2^2-l_1^2+h_2^2-h_1^2+Y_{max}^2}{2l_2},$

$n=\frac{2m(Y_{max}-2l_1)}{l_2},$

$k=\frac{{(Y_{max}-2l_1)}^2}{l_2^2},$

则得到

$\left\{\begin{array}{c}y=\frac{(2l_1+n)\±\sqrt{{(2l_1+n)}^2-4(k+1)(l_1^2+m^2+h_1^2-d_1^2)}}{2(k+1)}\\ x=m-\frac{y(Y_{max}-2l_1)}{l_2}\end{array}\right.$

上式中，舍去不在0～y_max范围内的y值得到移动机器人在一定空间中的坐标(x，y)；

计算步骤II，电子罗盘测量得到航向角θ；

计算步骤III，用室内移动机器人同时刻的位置坐标(x，y)和朝向航向角θ组合描述该时刻室内移动机器人所处状态，得到初始位姿(x，y，θ)。

在本发明提供的定位计算方法中，还可以具有这样的特征：

其中，步骤二中即时位姿的定位方法：

室内移动机器人的参考坐标系为X_ROY_R，

全局坐标系为X_WOY_W坐标系，

θ为室内移动机器人的自转角度，

V_Rx、V_Ry分别为机器人在其参考坐标系X_ROY_R上的运动速度，

V_Wx、V_Wy分别为机器人在其全局坐标系X_WOY_W上的运动速度，V_Wx、V_Wy是由V_Rx、V_Ry分别往X_W轴和Y_W方向分解、合成而得，

T为时间，因此，

在参考坐标系X_ROY_R中室内移动机器人的速度为：

[V_Rx，V_Ry，θ]^T，

在全局坐标系X_WOY_W中室内移动机器人的速度为：

[V_Wx，V_Wy，θ]^T，

定位系统工作时，

每间隔Δt时间内的位移[V_WxΔt，V_WyΔt，θΔt]^T，

循环n次，得到的n组位移[V_WxΔt，V_WyΔt，θΔt]_j ^T

对得到的n组位移[V_WxΔt，V_WyΔt，θΔt]_j ^T做中值滤波处理，得到n组滤波后的位移[V_WxΔt，V_WyΔt，θΔt]_k ^T，

上述中：j、k均为1,2,3，...n的整数，是循环次数，

对滤波处理后的位移求和，即得到n*Δt时间内的即时位姿1(x_r，y_r，θ_r)_t，

$(x_r,y_r,{\mathrm{\θ}}_r)=\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{\[V_{Wx}\mathrm{\Δ}t,V_{Wy}\mathrm{\Δ}t,\stackrel{\·}{\mathrm{\θ}}\mathrm{\Δ}t\]}_k^T.$

在本发明提供的定位计算方法中，还可以具有这样的特征：

其中，步骤三的计算规则是Time of Flight计算法或是Time Diffe-rence ofArrival计算法。

在本发明提供的定位计算方法中，还可以具有这样的特征：

其中，步骤四中，航向角为θ，室内移动机器人的位置为(x，y)，得到的t时刻的即时位姿1(x，y，θ)。

在本发明提供的定位计算方法中，还可以具有这样的特征：

其中，步骤五的即时位姿1和即时位姿2的融合方法，是卡尔曼滤波融合方法。

发明的作用与效果

根据本发明所涉及的定位计算方法，因为包含了：确定坐标原点，开启基站，再开启室内移动机器人，以开启时室内移动机器人所测得的位姿2为初始位姿的步骤一；通过电机编码器得到t时刻的移动机器人的即时位姿1的步骤二；标签接收基站发射的高频电磁脉冲，通过计算方法确定出移动机器人自身的位置的步骤三；用电子罗盘所测得的航向角扩维步骤三得到的室内移动机器人位置，得到t时刻的即时位姿2的步骤四；和用计算法融合即时位姿1和即时位姿2，得到t的位姿估计，后回到步骤二的步骤五，所以，本发明的定位计算方法是对从异质传感器获取的两种位姿进行融合，获得了高精度的室内移动机器人的位姿，使用该定位计算方法的定位系统定位的结果更加可靠、精度更高，且具有很强的环境适用性。

附图说明

图1是本发明实施例的室内移动机器人硬件架构图；

图2是本发明实施例的室内移动机器人的室内定位系统框图；

图3是本发明实施例的室内移动机器人的工作流程图；

图4是本发明实施例的室内移动机器人的水平位置坐标系统的模型图；

图5是本发明实施例的基站和标签的位置关系模型图；

图6是本发明实施例的室内移动机器人的电子罗盘测航向角的程序框图；以及

图7是本发明实施例的基于卡尔曼滤波的效果示意图。

具体实施方式

为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，以下实施例结合附图对本发明的基于超宽带(UWB)技术的移动机器人室内定位系统及其定位计算方法作具体阐述。

图1是本发明实施例的室内移动机器人硬件架构图。

如图1所示，本实施例的室内定位系统使用的是三全向轮移动机器人，其主控制器为National Instruments(美国国家仪器有限公司，简称NI)生产的myRIO。其他还有一个电子罗盘(航向角感应器)及一块DWM 1000模块安装在移动机器人上。另外，还有两块DWM1000模块安放在本实施例的移动机器人活动的空间中。

安装到移动机器人上的一块DWM1000模块作为标签，固定设置在移动机器人活动的空间里的两块DWM1000模块作为基站。

本实施例的移动机器人的软件系统是基于Labview编写的程序，经过调试后，下载到移动机器人的控制器中运行。

图2是本发明实施例的室内移动机器人的室内定位系统框图。

图3是本发明实施例的室内移动机器人的工作流程图。

如图2、图3所示，在本发明实施例中的移动机器人中：

步骤一，以室内的某个墙角作为XOY坐标系的坐标原点，先使基站开始工作，然后移动机器人开机。将开机时刻所测得的移动机器人的位姿2(x_a，y_a，θ_a)_t0作为移动机器人的初始位姿(x，y，θ)_t0。

图4是本发明实施例的室内移动机器人的水平位置坐标系统的模型图。

如图4所示，本实施例使用的带有三个全向轮的，且三个全向轮按等间隔120°分布的室内移动机器人，在图中坐标系X_WOY_W为全局坐标系和图中坐标系X_ROY_R为室内移动机器人参考坐标系的双坐标系统中。d为全向轮距室内移动机器人中心的距离，V₁、V₂、V₃分别为三个轮子的转速，θ表示其自转的角度，а为X_R轴正方向与X_W轴正方向的夹角，X_Rx、Y_Ry分别为机器人在其参考坐标系X_ROY_R上的运动速度，V_Wx、V_Wy分别为机器人在其全局坐标系X_WOY_W上的运动速度，V_Wx、V_Wy是由X_Rx、Y_Ry分别往X_W轴和Y_W方向分解、合成而得，则：

电机所用编码器为增量式光电编码器，编码器的分辨率为P，电机减速箱减速比为G，轮子半径为r，三者都为已知。

机器人运动时，如测得第i(i＝1,2,3)个编码器的脉冲数为M_i，则与之对应的轮速用下式计算：

$V_i=\frac{M_i\·r}{2\mathrm{\π}PG}---\left(2\right)$

在此定义：

在参考坐标系X_ROY_R中室内移动机器人的速度为[V_Rx，V_Ry，θ]^T，

在全局坐标系X_WOY_W中室内移动机器人的速度为[V_Wx，V_Wy，θ]^T，θ是室内移动机器人的自转角速度。

则由机器人模型可得：

$\left[\begin{array}{c}{V}_1\\ V_2\\ V_3\end{array}\right]=\left[\begin{array}{ccc}0& 1& d\\ sin\frac{\mathrm{\π}}{3}& -\cos \frac{\mathrm{\π}}{3}& d\\ -\sin \frac{\mathrm{\π}}{3}& -\cos \frac{\mathrm{\π}}{3}& d\end{array}\right]\·\left[\begin{array}{c}{V}_{Rx}\\ V_{Ry}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right]---\left(3\right)$

由上式(3)可得：

$\left[\begin{array}{c}{V}_{Rx}\\ V_{Ry}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{ccc}0& \frac{\sqrt{3}}{3}& -\frac{\sqrt{3}}{3}\\ \frac{2}{3}& -\frac{1}{3}& -\frac{1}{3}\\ \frac{1}{3d}& \frac{1}{3d}& \frac{1}{3d}\end{array}\right]\·\left[\begin{array}{c}{V}_1\\ V_2\\ V_3\end{array}\right]---\left(4\right)$

由式(2)和式(4)即可求得[V_Rx，V_Ry，θ]^T，

进而即可求得[V_Wx，V_Wy，θ]^T。

定位系统工作时，每间隔Δt(如Δt＝0.01s)读取一次[V_Wx，V_Wy，θ]^T，进而可得Δt时间内的位移[V_WxΔt，V_WyΔt，θΔt]^T，然后循环n次，将得到n组位移[V_WxΔt，V_WyΔt，θΔt]_j ^T(j为循环次数，j＝1,2,...n)。

然而，用编码器得到位移时会有误差，误差分为系统误差和非系统误差。系统误差与外界环境无关，它以常量进行误差累积，因此不适合做长距离的定位；非系统误差与外界环境有关。中值滤波是一种能有效抑制噪声的非线性信号处理技术，其对脉冲噪声有良好的滤除作用。

对上面得到的n组位移[V_WxΔt，V_WyΔt，θΔt]_j ^T(j为循环次数，j＝1,2,...n)做中值滤波处理，以减小测得其误差。处理后的n组位移为[V_WxΔt，V_WyΔt，θΔt]_k ^T(k为循环次数，k＝1,2,...)，然后将处理后的位移求和，即得到n*Δt时间内的位姿1(x_r，y_r，θ_r)_t，即：

$(x_r,y_r,{\mathrm{\θ}}_r)=\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{\[V_{Wx}\mathrm{\Δ}t,V_{Wy}\mathrm{\Δ}t,\stackrel{\·}{\mathrm{\θ}}\mathrm{\Δ}t\]}_k^T$

通过以上的做法获得位姿1。

步骤二，移动机器人开始运动，将移动机器人的电机编码器测得的值经过运算得到在移动机器人参考坐标系下的x、y方向及旋转方向的速度，速度乘以时间Δt则得到位移。再对位移做中值滤波处理，以尽量减小其测量的误差，得到t时刻的位姿1(x_r，y_r，θ_r)_t。

图5是本发明实施例的基站和标签的位置关系模型图。

如图5所示，本实施例共使用三个DWM1000模块，其中，一个DWM1000模块装到室内移动机器人上作为标签，另两个DWM1000模块固定在室内已知位置点作为基站。

基站和室内移动机器人在房间中的位置模型如图5。

两基站的高度为h₁、h₂，两基站距相邻墙面的距离为l₁、l₂，这些均为已知。

d₁、d₂为由TOF算法测得的两基站距室内移动机器人的距离，设室内移动机器人在房间中的坐标为(x，y)，则由几何关系可得：

$\left\{\begin{array}{c}{(y-l_1)}^2+x^2+{h_1}^2={d_1}^2\\ {(x-l_2)}^2+(Y_{\max }-y)+{h_2}^2={d_2}^2\end{array}\right.---\left(5\right)$

式中，y_max为房间在y轴方向的宽度。令，

$m=\frac{d_1^2-d_2^2+l_2^2-l_1^2+h_2^2-h_1^2+Y_{max}^2}{2l_2},$

$n=\frac{2m(Y_{max}-2l_1)}{l_2},$

则得：

$\left\{\begin{array}{c}y=\frac{(2l_1+n)\±\sqrt{{(2l_1+n)}^2-4(k+1)(l_1^2+m^2+h_1^2-d_1^2)}}{2(k+1)}\\ x=m-\frac{y(Y_{max}-2l_1)}{l_2}\end{array}\right.---\left(6\right)$

上式(6)中，y有两个值，舍去不在0～y_max范围内的取值，则得到室内移动机器人在房间中的位置坐标(x，y)。

步骤三，在移动机器人的电机编码器测量计算的同时，基站向标签发射高频电磁脉冲。标签接收到高频电磁脉冲后，通过TOF(Time of Flight)计算法或TDOA(TimeDifference of Arrival)计算法算出标签距基站的距离。将得到的距离根据几何运算法可得到机器人在室内的x、y坐标位置。

步骤四，将电子罗盘(又称航向角传感器)测得的航向角θ对步骤三得到的x、y坐标进行扩维，得到t时刻的移动机器人的位姿2(x_a，y_a，θ_a)_t。

图6是本发明实施例的室内移动机器人的电子罗盘测航向角的程序框图。

对于室内移动机器人的航向角θ，则用电子罗盘测量。

如图6所示，本实施例采用基于Labview的电子罗盘航向角测量，其程序框图即图6。

定位系统工作时，获得同时刻的(x，y)和θ，且每隔n*Δt个时间段更新一次数据，如此便可获得与位姿1(x_r，y_r，θ_r)，同步的位姿2(x_a，y_a，θ_a)。

通过以上做法获得位姿2(x_a，y_a，θ_a)。

步骤五，用卡尔曼(Kalman)滤波算法将位姿1(x_r，y_r，θ_r)_t和位姿2(x_a，y_a，θ_a)_t融合，即用位姿2对位姿1进行修正，以得到移动机器人在t时刻的位姿估计(x，y，θ)_t。

图7是本发明实施例的基于卡尔曼滤波的效果示意图。

如图7所示，位姿1和位姿2按照以下方法融合。

本实施例采用KF算法融合位姿1和位姿2。

设U(t)^T＝(x_r,y_r,θ_r)，Z(t)^T＝(x_a,y_a,θ_a)，X(t)为每个n*Δt时间内的位姿估计，是三维列向量，则融合过程如下：

X(t＝0)＝Z(t＝0) (7)

式(7)是系统的初始化方程；

X(t)＝X(t-1)+U(t)+W(t) (8)

式(8)为系统的状态方程；

Z(t)＝X(t)+V(t) (9)

式(9)为观测方程；

X(t|t-1)＝X(t-1|t-1)+U(t) (10)

式(10)是用t-1时刻的估计值预测t时刻的值；

P(t|t-1)＝P(t-1|t-1)+Q (11)

式(11)是用t-1时刻的P值预测t时刻的P值；

$Kg\left(t\right)=\frac{P\left(t\right|t-1)}{P\left(t\right|t-1)+R}---\left(12\right)$

式(12)是计算t时刻的卡尔曼增益；

X(t|t)＝X(t|t-1)+Kg(t)[Z(t)-X(t|t-1)] (13)

式(13)结合预测值和观测值得到t时刻的估算值；

P(t|t)＝[I-Kg(t)]P(t|t-1) (14)

式(14)用于计算t时刻的P值。

上述式(7)～(14)中，W(t)和V(t)分别为获取位姿1和位姿2时的噪声，Q和R分别为W(t)和V(t)的噪声协方差，P为X(t)的协方差，W(t)、V(t)、Q、R、P均为三维列向量。

对Q、R和P取恰当的值，则由式(10)～(14)即可得到每个n*Δt时间内的位姿估计。

用卡尔曼滤波融合位姿1和位姿2的效果示意图表示在图7中。

然后保存t时刻的位姿估计值，并跳转至步骤二，获得下一个时刻，即t+1时刻的位姿估计，如此不断循环，即可得到每个时刻的位姿估计。

实施例的作用与效果

根据本发明所涉及的定位计算方法，因为包含了：确定坐标原点，开启基站，再开启室内移动机器人，以开启时室内移动机器人所测得的位姿2为初始位姿的步骤一；通过电机编码器得到t时刻的移动机器人的即时位姿1的步骤二；标签接收基站发射的高频电磁脉冲，通过计算方法确定出移动机器人自身的位置的步骤三；用电子罗盘所测得的航向角扩维步骤三得到的室内移动机器人位置，得到t时刻的即时位姿2的步骤四；和用计算法融合即时位姿1和即时位姿2，得到t的位姿估计，后回到步骤二的步骤五，所以，本发明的定位计算方法是对从异质传感器获取的两种位姿进行融合，获得了高精度的室内移动机器人的位姿，使用该定位计算方法的定位系统定位的结果更加可靠、精度更高，且具有很强的环境适用性。

又因为本发明一种基于UWB技术的室内移动机器人的室内自主定位系统应用了本发明所涉及的定位计算方法，所以，与现有技术相比具有以下优点：第一、位姿1的获得是基于电机编码器的，而目前大多数电机都自带编码器，这增强了本方法的适用性；第二、航向角传感器众多，因此测得航向角也比较容易，这也增强了本方法的适用性；第三、DWM1000模块精度高，能耗低，且尺寸小，安装和使用极为方便，这也增强了本方法的适用性；第四、位姿1与位姿2的获得采用的是两种异质传感器，使用融合算法兼顾了位姿1与位姿2，使得定位结果更加可靠。

上述实施方式为本发明的优选案例，并不用来限制本发明的保护范围。

Claims (7)

1.一种室内移动机器人的室内定位系统，是基于超宽带技术和电机编码器信息的自主移动的室内移动机器人定位系统，其特征在于，包括：

移动机器人，是用美国国家仪器有限公司的myRIO做主控器的，在一定空间范围内可以自主移动的三全向轮移动机器人；

电子罗盘，又称航向角传感器，是设置在所述移动机器人上用于测定所述机器人自身的航向角的；

电机编码器，是设置在所述室内移动机器人上用于提供自身定位信号的；以及

三块DWM1000模块，第三DWM1000模块设置在所述移动机器人上，第一和第二DWM1000模块设置在所述一定空间范围内，

其中，设置在所述移动机器人上的所述第三DWM1000模块为标签，固定设置在所述一定空间范围内的所述第一和第二DWM1000模块为第一和第二基站。

2.一种定位计算方法，是用于权利要求1所述的室内移动机器人的定位系统的定位计算方法，其特征在于，包括：

步骤一，确定坐标原点，开启所述基站，再开启所述移动机器人，以开启时所述移动机器人所测得的位姿2为初始位姿；

步骤二，通过所述电机编码器得到t时刻的所述移动机器人的即时位姿1；

步骤三，所述标签接收所述基站发射的高频电磁脉冲，通过计算方法确定出所述移动机器人自身的位置；

步骤四，用所述电子罗盘所测得的所述航向角扩维步骤三得到的所述移动机器人位置，得到t时刻的即时位姿2；

步骤五，用计算法融合所述即时位姿1和即时位姿2，得到t的位姿估计，后回到步骤二。

3.根据权利要求2所述的定位计算方法，其特征在于：

其中，步骤一中所述初始位姿的定位方法：

计算步骤I，在所述一定空间中建立XOY坐标系，所述第一和第二基站的高度为h₁、h₂，所述第一和第二基站的距相邻墙面的距离为l₁、l₂，所述第一和第二基站与所述移动机器人间的距离，由TOF算法算得为d1、d2，再设定所述移动机器人在所述一定空间中的坐标为(x，y)，则由几何关系可得：

$\left\{\begin{array}{c}{(y-l_1)}^2+x^2+{h_1}^2={d_1}^2\\ {(x-l_2)}^2+(Y_{max}-y)+{h_2}^2={d_2}^2\end{array}\right.$

式中，y_max为所述一定空间y轴方向的宽度，令，

$m=\frac{d_1^2-d_2^2+l_2^2-l_1^2+h_2^2-h_1^2+Y_{max}^2}{2l_2},$

$n=\frac{2m(Y_{max}-2l_1)}{l_2},$

$k=\frac{{(Y_{max}-2l_1)}^2}{l_2^2},$

则得到

$\left\{\begin{array}{c}y=\frac{(2l_1+n)\±\sqrt{{(2l_1+n)}^2-4(k+1)(l_1^2+m^2+h_1^2-d_1^2)}}{2(k+1)}\\ x=m-\frac{y(Y_{max}-2l_1)}{l_2}\end{array}\right.$

上式中，舍去不在0～y_max范围内的y值得到所述移动机器人在所述一定空间中的坐标(x，y)；

计算步骤II，所述电子罗盘测量得到所述航向角θ；

计算步骤III，用所述室内移动机器人同时刻的位置坐标(x，y)和朝向航向角θ组合描述该时刻室内移动机器人所处状态，得到初始位姿(x，y，θ)。

4.根据权利要求2所述的定位计算方法，其特征在于：

其中，步骤二中所述即时位姿1的定位方法：

所述室内移动机器人的参考坐标系为X_ROY_R，

全局坐标系为X_WOY_W坐标系，

θ为所述室内移动机器人的自转角度，

V_Rx、V_Ry分别为机器人在其参考坐标系X_ROY_R上的运动速度，

V_Wx、V_Wy分别为机器人在其全局坐标系X_WOY_W上的运动速度，V_Wx、V_Wy是由V_Rx、V_Ry分别往X_W轴和Y_W方向分解、合成而得，

T为时间，因此，

在参考坐标系X_ROY_R中所述室内移动机器人的速度为：

[V_Rx，V_Ry，θ]^T，

在全局坐标系X_WOY_W中所述室内移动机器人的速度为：

[V_Wx，V_Wy，θ]^T，

定位系统工作时，

每间隔Δt时间内的位移[V_WxΔt，V_WyΔt，θΔt]^T，

循环n次，得到的n组位移[V_WxΔt，V_WyΔt，θΔt]_j ^T

对得到的n组位移[V_WxΔt，V_WyΔt，θΔt]_j ^T做中值滤波处理，得到n组滤波后的位移[V_WxΔt，V_WyΔt，θΔt]_k ^T，

上述中：j、k均为1,2,3，…n的整数，是循环次数，

对滤波处理后的位移求和，即得到n*Δt时间内的即时位姿1(x_r，y_r，θ_r)_t，

$(x_r,y_r,{\mathrm{\θ}}_r)=\underset{k=1}{\overset{n}{\Σ}}{\[V_{Wx}\mathrm{\Δ}t,V_{Wy}\mathrm{\Δ}t,\stackrel{\·}{\mathrm{\θ}}\mathrm{\Δ}t\]}_k^T.$

5.根据权利要求2所述的定位计算方法，其特征在于：

其中，步骤三的计算规则是Time ofFlight计算法或是Time Diffe-rence of Arrival计算法。

6.根据权利要求2所述的定位计算方法，其特征在于：

其中，步骤四中，所述航向角为θ，所述室内移动机器人的位置为(x，y)，得到的t时刻的即时位姿2(x，y，θ)。

7.根据权利要求2所述的定位计算方法，其特征在于：

其中，步骤五的所述即时位姿1和所述即时位姿2的融合方法，是卡尔曼滤波融合方法。