CN103308047B - 一种具有人工在线修正功能的移动机器人定位方法 - Google Patents

Abstract

一种具有人工在线修正功能的移动机器人定位方法，包括：人工修正事件检测及处理；运动参数标准化与分发；里程数据、航向角数据、俯仰角数据以及速度和前进后退标志的处理；定位计算及运动姿态估计；坐标变换；显示与处理。本发明的优点是：可通过操控人员在导航图上人工设定机器人位置坐标，修正航迹推算初始值与航向角，及时消除航迹推算的累积误差，从而使得惯性导航始终处于短周期工作状态；修正了移动机器人的回退运动与非结构地面坡度两个因素所导致的水平位移误差。因此，本发明可满足移动机器人的长时间小区域导航要求。

Description

一种具有人工在线修正功能的移动机器人定位方法

技术领域

本发明涉及机器人导航与定位等领域，尤其是在没有GPS信号的情况下，非结构地面移动机器人小区域导航与定位的技术方法。

背景技术

随着传感器技术、信息处理、电子工程、计算机工程、自动化控制工程以及人工智能等相关技术的发展，各类机器人性能不断地得到完善，其应用范围也不断得以扩展，目前不仅已经应用于工业、农业、医疗、服务等日常领域，而且也已经应用于城市安全、国防和空间探测领域等特殊领域。

移动机器人是一个集环境感知、动态决策与规划、行为控制与执行等多功能于一体的综合系统。与一般机器人相比，移动机器人最显著的特点在于移动功能，因而其具有更大的机动性和灵活性，可以代替人类在危险、恶劣（如辐射、有毒等）环境和人所不及的（如宇宙空间、水下等）环境等条件下开展作业。从相对比较成熟的移动机器人工作模式看，非结构地面条件下，移动机器人主要由操控人员通过无线通信系统，实现对移动机器人的远程控制并完成相应的工作任务。

由于移动机器人在一定地域空间内始终处于移动的工作状态，因此操控人员通常需要依赖导航与定位系统，以便实时确定移动机器人位置与姿态。目前，最常见的是利用GPS实现移动机器人的导航与定位，操控人员根据导航地图，可以知道机器人的实时位置和行进方向。同时，为了保证机器人操作的安全性与有效性，机器人通常自带摄像头实时采集周边环境，并传输到远程操控器，操控人员根据实时环境图像与机器人工作状态，决策并下发各项机器人操作指令。这种GPS导航信息与实时图像信息相结合的工作模式，已经开始应用于各种GPS卫星信号可接收的移动机器人应用系统中。从应用效果看，通常出现以下两种情况导致这种工作模式失效：一是没有GPS信号条件下，根据周边实时环境图像，操控人员无法确定移动机器人的实时位置与姿态，容易导致机器人迷航问题；二是GPS信号正常条件下，由于GPS精度有限，如果机器人处于茂密草丛、树林等复杂环境中，根据周边实时环境图像以及导航图，也容易导致机器人迷航问题。

另一类可用于移动机器人导航的是惯性导航技术，它属于一种推算导航方式，即从一已知点的位置根据连续测得的运载体航向角和速度推算出其下一点的位置，因而可连续测出运动体的当前位置。它是一种不依赖于任何外部信息，也不向外部辐射能量的自主式系统，具有隐蔽性好、不受外界电磁干扰、导航信息连续、可全天候全球全时间地工作于空中地球表面乃至水下等优点。然而，由于惯性导航信息是经过积分而产生的，虽然惯性导航器件短期精度和稳定性较好，定位误差仍然会随时间增长而增大，长期精度很差，不能直接用于移动机器人导航服务。

早期，航迹推算应用于求取船舶的航迹和船位，即从已知点位置出发根据连续测得的运动载体姿态以及位移变化推算出其之后的载体连续位置，其计算方法如下式所示：

$\left\{\begin{array}{c}{x}_i=x_{i-1}+d_i\sin {\mathrm{\θ}}_i\\ y_i=y_{i-1}+d_i\cos {\mathrm{\θ}}_i\\ {\mathrm{\θ}}_i={\mathrm{\θ}}_{i-1}+{\mathrm{\Δ\θ}}_i\end{array}\right.---\left(1\right)$

式中(x_i,y_i)和θ_i分别为载体在第i时刻的位置坐标和航向角，d_i和Δθ_i分别为载体从第i-1时刻位置(x_i-1,y_i-1)到第i时刻位置(x_i,y_i)的位移变化量和航向角变化量。

式(1)中的位移变化量d_i是由载体内安装的加速度计所提供的加速度对时间的两次积分而得到；航向角变化量Δθ_i是由载体内安装的陀螺仪所提供的角速度对时间的一次积分而得到；计算初始坐标(x₀,y₀)和初始航向角θ₀在导航开始时刻一次设定。

然而地面移动机器人在运动过程中不同于船舶的运动特点，主要有：一是移动机器人工作于非结构地面，俯仰角变化剧烈，严重影响位移变化量d_i的计算准确性；二是移动机器人在操作过程中经常处于回退运动状态，位移变化量d_i会出现方向性错误；三是由于一次性设定初始坐标(x₀,y₀)和初始航向角θ₀，会造成长时间的导航累积误差，导致移动机器人在小区域范围内工作时达不到理想的定位精度要求。

发明内容

本发明要克服现有技术的上述缺点，提出一种具有人工在线修正功能的移动机器人定位方法，它采用改进的惯性导航与实时图像相结合的工作模式，避免了GPS信号失效时的机器人导航困难。一种具有人工在线修正功能的移动机器人定位方法，包括以下步骤：

1）、检测是否有人工修正事件发生，若有则进行人工修正事件处理，若无则直接转步骤2）；

2）、对机器人端传输过来的运动参数进行标准化工作，包括定点数与浮点数的转化处理，数据有效位数的处理等，然后将标准化了的里程数据、航向角数据、俯仰角数据及速度和前进后退标志分发给相应的各个单元；

3）、里程数据、航向角数据、俯仰角数据以及速度和前进后退标志的有效性判断及处理；

4）、机器人运动姿态估计及定位计算；

5）、将计算得到的机器人实际坐标转化成相应的地图坐标；

6）、将最终处理得到的机器人位置坐标、运动姿态等在导航图中予以显示，并保存相关运动参数。

进一步，所述的步骤1）中人工修正事件的发生是由于操控人员观察到机器人的实际位置与导航图上的显示位置出现偏差，从而进行人工修正操作的，人工修正事件处理包括以下步骤：

(1.1）获取操控人员在导航图上所设定的机器人准确位置坐标设定方式可以是人工输入坐标、人工点击地图获取坐标或人工拖曳地图获取坐标等；

(1.2）根据第k次人工修正前瞬间位置坐标第k次修正时所设定的位置坐标以及第k-1次修正时所设定的位置坐标计算得到本次的航向角修正量

(1.3）将人工修正事件标志置1，以通知定位计算。

进一步，所述的步骤4）中的定位计算包括以下步骤：

(4.1）检查人工修正事件标志是否为1，若是则转步骤(4.2），若否则转步骤(4.3）；

(4.2）使用当前人工修正后的位置坐标作为本次定位计算的坐标输出，同时将作为下一次航迹推算的初始值，步骤4）结束；

(4.3）计算当前机器人位置坐标：

其中，k表示第k次人工在线修正事件；j表示从第0到第k次的人工在线修正事件索引参量；i表示第k次人工修正事件发生后的第i时刻，下一次人工修正事件发生时重新计数；表示第k次人工在线修正后，第i时刻的机器人位置坐标；分别为导航过程中第k次人工在线修正后，第i时刻机器人端传送过来的机器人俯仰角、相对里程以及前进后退标志；分别为导航过程中第k次人工在线修正后，第i时刻机器人端传送过来的航向角和修正后的航向角；为第j次人工修正的航向角增量。

位移变化量是对传统航迹推算方法中位移变化量d_i（见式（1））的修正：用以修正移动机器人在回退运动时的反向位移，其修正效果可参见附图3；|用以修正移动机器人在非结构地面工作时的水平位移，其修正效果可参见附图4。是用来修正移动机器人累积的航向角误差。

考虑到移动机器人使用过程中由操控人员远程实时操作，因此本发明提出一种可通过人工在线修正航迹推算初始值的惯性导航方法。该方法应用于机器人导航时，操控人员可根据惯性导航信息实时掌握移动机器人的大致位置和方向，同时利用地标找到机器人的准确位置，从而人工设定航迹推算的初始定位值，以充分利用惯性导航系统短时精度高、稳定性好的特点，解决惯性导航系统长时间导航精度差的问题。本发明所提出的具有人工在线修正功能的移动机器人定位方法，除了传统惯性导航方法所具备的特点外，还具有如下几个特点：1）由于引入了人工在线设定航迹推算初始值功能，使得惯性导航算法始终处于短周期工作状态，从而使得惯性导航算法的累积误差得到及时消除，保证了惯性导航信息的准确性；2）由于导航系统初始工作时，可采用人工修正的方式迫使机器人实际运动轨迹与导航轨迹相匹配，使得本发明所述的导航技术不再依赖于地球坐标系而直接实现载体坐标系和导航图的匹配，避免了机器人指北方位的测量问题，同时降低了导航地图制作的复杂性；3)考虑到机器人在运动过程中路面坡度与回退运动等情况对传统惯性导航定位精度影响较大，本发明修改了传统航迹推算的位移变化量计算方法，使得惯性导航满足非结构化地面移动机器人的导航与定位需求。

本发明的优点是：隐蔽性好、不受外界电磁干扰、导航信息连续、可全天候全球全时间地工作，可满足非GPS信号依赖的移动机器人小区域导航与定位服务。

附图说明

图1为本发明公开的具有人工在线修正功能的移动机器人定位方法的总体流程图。

图2为本发明公开的人工在线修正工作机理示意图。

图3为本发明公开的定位方法中机器人回退运动时的反向位移修正示意图。

图4为本发明公开的定位方法中机器人非结构地面工作时的水平位移修正示意图。

具体实施方式

下面参照附图1、2、3、4，详细说明本发明的具体实施方式。

实施案例

一种具有人工在线修正功能的移动机器人定位方法，包括以下步骤：

1）、检测是否有人工修正事件发生，若有则进行人工修正事件处理，若无则直接转步骤2）；

(1.1）获取操控人员在导航图上所设定的机器人准确位置坐标设定方式可以是人工输入坐标、人工点击地图获取坐标或人工拖曳地图获取坐标等；

(1.2）根据第k次人工修正前瞬间位置坐标第k次修正时所设定的位置坐标以及第k-1次修正时所设定的位置坐标计算得到本次的航向角修正量

(1.3）将人工修正事件标志置1，以通知定位计算。

2）、对机器人端传输过来的运动参数进行标准化工作，包括定点数与浮点数的转化处理，数据有效位数的处理等，然后将标准化了的里程数据、航向角数据、俯仰角数据及速度和前进后退标志分发给相应的各个单元；

3）、里程数据、航向角数据、俯仰角数据以及速度和前进后退标志的有效性判断及处理；

4）、机器人运动姿态估计及定位计算；

(4.1）检查人工修正事件标志是否为1，若是则转步骤(4.2），若否则转步骤(4.3）；

(4.2）使用当前人工修正后的位置坐标作为本次定位计算的坐标输出，同时将作为下一次航迹推算的初始值，步骤4）结束；

(4.3）计算当前机器人位置坐标：

其中，k表示第k次人工在线修正事件；j表示从第0到第k次的人工在线修正事件索引参量；i表示第k次人工修正事件发生后的第i时刻，下一次人工修正事件发生时重新计数；表示第k次人工在线修正后，第i时刻的机器人位置坐标；分别为导航过程中第k次人工在线修正后，第i时刻机器人端传送过来的机器人俯仰角、相对里程以及前进后退标志；分别为导航过程中第k次人工在线修正后，第i时刻机器人端传送过来的航向角和修正后的航向角；为第j次人工修正的航向角增量。

5）、将计算得到的机器人实际坐标转化成相应的地图坐标；

6）、将最终处理得到的机器人位置坐标、运动姿态等在导航图中予以显示，并保存相关运动参数。

所述的人工修正工作机理在附图2中展示，图中A为第k-1次人工修正前瞬间机器人位置坐标为第k-1次修正时所设定的位置坐标B为第k次人工修正前瞬间位置坐标为第k次修正时所设定的位置坐标航向角修正量的计算方法如下：

优选的，先计算出导航图中显示运动轨迹终始点连线跟导航图期望运动轨迹终始点连线之间的夹角求解过程如下：

$\∠\stackrel{\‾}{B}\stackrel{\‾}{A}B=\frac{{\left|\stackrel{\‾}{A}\stackrel{\‾}{B}\right|}^2+{\left|\stackrel{\‾}{A}B\right|}^2-{\left|B\stackrel{\‾}{B}\right|}^2}{2\left|\stackrel{\‾}{A}\stackrel{\‾}{B}\right|\left|\stackrel{\‾}{A}B\right|},$ 其中

$\left|\stackrel{\‾}{B}\stackrel{\‾}{A}\right|=\sqrt{{(x_0^{k-1}-x_0^k)}^2+{(y_0^{k-1}-y_0^k)}^2},$

$\left|B\stackrel{\‾}{B}\right|=\sqrt{{(x_0^k-x_i^{k-1})}^2+{(y_0^k-y_i^{k-1})}^2},$

再根据向量和向量的相对位置确定正负符号，如附图2中应为负号，即 $\mathrm{\Δ}{\mathrm{\θ}}_0^k=$ $-\∠\stackrel{\‾}{B}\stackrel{\‾}{A}B.$

所述的里程数据、航向角数据、俯仰角数据以及速度和前进后退标志的有效性判断及处理方法如下:

里程数据有效性判断及处理：优选的，以第i次和第i-1次的平均速度估计出的里程(v_i-1+v_i)/2*(t_i-t_i-1)作为第i次的相对里程参考量（若刚好出现第i次和第i-1次机器人一次为前进，一次为后退的情况，由于两次间的间隔很小，误差可忽略不计，故仍用此方法估计）。若第i次传送过来的相对里程与相对里程参考量的差值在一定范围内，即（ε为给定的最大允许误差），则认为此次里程数据有效；如果里程数据无效，则以相对里程参考量作为此次的里程数据。

航向角数据有效性判断及处理：优选的，首先判断航向角是否落在[-180°,180°）区间，其次判断是否小于单位时间内航向角的最大变化量θ_max与时间间隙t_i-t_i-1的乘积θ_max*（t_i-t_i-1）。只有满足以上条件的航向角数据被判定为有效；如果航向角数据无效，则以第i-1次的航向角数据作为此次的航向角数据。

俯仰角数据有效性判断及处理：优选的，如果俯仰角处于（-90°,90°）区间，则被判定为有效；如果俯仰角数据无效，则以第i-1次的俯仰角数据作为此次的俯仰角数据。

速度和前进后退标志有效性判断及处理：优选的，如果速度v_i小于机器人最大移动速度v_max，则被判定为有效；如果速度数据无效，则以第i-1次的速度数据作为此次的速度数据。优选的，如果前进后退标志为1或-1，则被判定为有效；如果前进后退标志无效，则以第i-1此的前进后退标志作为此次的前进后退标志。

所述的定位计算对机器人回退运动时的反向位移和机器人在非结构地面工作时的水平位移进行了修正。机器人回退运动时的反向位移修正示意图见附图3。由于航向角无法反应机器人的回退运动，故在式（1）中加入因子进行修正。修正前，机器人位移变化量如线段2所示，它是前进的运动里程和后退的运动里程的累加；修正后，机器人位移变化量如线段3所示，它是前进的运动里程和后退的运动里程的差值。机器人在非结构地面工作时的水平位移修正示意图见附图4。由于导航图是平面图，因此必须将坡度路面下的运动里程（实线3所表示的线段长度）映射到水平面中，才能得到准确的里程数据（实线2所表示的线段长度），即在式（1）中加入因子进行修正。

本说明书实施例所述的内容仅仅是对发明构思的实现形式的列举，本发明的保护范围不应当被视为仅限于实施例所陈述的具体形式，本发明的保护范围也及于本领域技术人员根据本发明构思所能够想到的等同技术手段。

Claims (3)

1.一种具有人工在线修正功能的移动机器人定位方法，其特征在于，将可人工在线修正航迹推算初始值的策略整合到惯性导航技术中，包括下述步骤：

1)、检测是否有人工修正事件发生，若有则进行人工修正事件处理，若无则直接转步骤2)；所述的人工修正事件的发生是因为操控人员观察到机器人的实际位置与导航图上的显示位置出现偏差，从而进行人工修正操作的，其中机器人的实际位置由操控人员观察机器人周边环境图像的地标获知,或由机器人进入特定区域后传送过来的信号获知；

所述的步骤1)中的人工修正事件处理包括下述步骤：

(1.1)获取操控人员在导航图上所设定的机器人准确位置坐标设定方式是人工输入坐标、人工点击地图获取坐标或人工拖曳地图获取坐标；

(1.2)根据第k次人工修正前瞬间位置坐标第k次修正时所设定的位置坐标以及第k-1次修正时所设定的位置坐标计算得到本次的航向角修正量

(1.3)将人工修正事件标志置1，以通知定位计算；

2)、对机器人端传输过来的运动参数进行标准化工作，包括定点数与浮点数的转化处理，数据有效位数的处理，然后将标准化了的里程数据、航向角数据、俯仰角数据及速度和前进后退标志分发给相应的各个单元；

3)、里程数据、航向角数据、俯仰角数据以及速度和前进后退标志的有效性判断及处理；

4)、机器人运动姿态估计及定位计算；所述的步骤4)中的定位计算包括以下步骤：

(4.1)检查人工修正事件标志是否为1，若是则转到步骤(4.2)，若否则转到步骤(4.3)；

(4.2)使用当前修正后的位置坐标作为本次定位计算的坐标输出，同时将作为下一次航迹推算的初始值，转到步骤(4.4)；

(4.3)计算当前机器人位置坐标：

其中，k表示第k次人工在线修正事件；j表示从第0到第k次的人工在线修正事件索引参量；i表示第k次人工修正事件发生后的第i时刻，下一次人工修正事件发生时重新计数；表示第k次人工在线修正后，第i时刻的机器人位置坐标；分别为导航过程中第k次人工在线修正后，第i时刻机器人端传送过来的机器人俯仰角、相对里程以及前进后退标志；分别为导航过程中第k次人工在线修正后，第i时刻机器人端传送过来的航向角和修正后的航向角；为第j次人工修正的航向角增量；

(4.4)转到步骤5)；

5)、将计算得到的机器人实际坐标转化成相应的地图坐标；

6)、将最终处理得到的机器人位置坐标、运动姿态在导航图中予以显示，并保存相关运动参数。

2.根据权利要求1所述的移动机器人定位方法，其特征在于，步骤(4.3)所述的定位计算考虑了机器人的回退运动，采用因子对机器人回退运动时的反向位移进行了修正。

3.根据权利要求1所述的移动机器人定位方法，其特征在于，步骤(4.3)所述的定位计算考虑了机器人运动过程中的路面坡度，采用因子对机器人在非结构地面工作时的水平位移进行了修正。