用于移动机器人定位与环境重构的绝对坐标获取方法
【专利摘要】本发明公开用于移动机器人定位与环境重构的绝对坐标获取方法，包含：引入虚拟坐标系；求取当前时刻机器人在以卫星导航接收机天线为原点的局部地理坐标系的坐标，机器人在地心地固坐标系的大地坐标，环境特征点以c点为中心的局部地理坐标系的坐标，环境特征点在地心地固坐标系的大地坐标；求取当前时刻机器人在上一时刻c坐标系的坐标，机器人在上一时刻以c点为原点的局部地理坐标系的坐标，机器人的经度、纬度、高度；求取环境特征点在当前时刻以c点为中心的局部地理坐标系的坐标；求取当前时刻环境特征点在地心地固坐标系的大地坐标。本发明引入虚拟坐标系，等效卫星导航接收机天线中心点与传感器坐标系中心点为一点，节省人力、时间成本。
【专利说明】用于移动机器人定位与环境重构的绝对坐标获取方法
【技术领域】
[0001]本发明涉及移动机器人定位与地图构建领域，具体涉及一种用于移动机器人定位与环境重构的绝对坐标获取方法。
【背景技术】
[0002]移动机器人定位与地图构建(Simultaneouslocalization and mapping, SLAM)是移动机器人工程应用及科研上核心方向之一。随着目前以自导航无人车为代表的轮式机器人成为军用、民用的热点。工程上的应用需求日益增强。在实际的工程应用中首先要解决的就是如何获得对机器人自身位置及所观测的环境特征点的绝对坐标的问题。
[0003]移动机器人作业的场景复杂多样，在很多场合以GPS为代表的卫星导航系统信号不稳定或精度较低，且无法完成对环境特征的观测。因此在移动机器人的定位和环境重构应用中通常都是主要使用机器人自搭载的传感器实现，包括:被动式传感器，例如，双目或单目视觉传感器；主动式传感器，例如，声纳传感器，二维或三维激光传感器。无论使用哪种传感器，数据处理过程的大步骤都是十分类似的。都是先通过显著的点特征来描述环境，通过传感器观测获取环境特征点和机器人的空间关系，即特征点在机器人中心坐标系中的坐标；然后，通过全局优化或滤波的方式获得环境特征点和机器人作业过程中的每个采样时刻的方位相对于初始时刻的空间关系，即获得它们在初始时刻机器人中心坐标系中的坐标。如果让其在实际应用中有价值就必须将机器人每个时刻的位置和观测的所有环境特征点相对位置都转换为地心地固坐标系下的经纬度高度，即本发明所指的绝对坐标，这步工作现在很多的科研成果中都回避开展，而仅仅展示相对观测的结果，即，一系列的机器人的轨迹和重构出的环境都是在初始时刻的某一坐标系下的，显然这不符合工程应用的最终需求。少量解决该类问题的现有技术存在如下问题:
[0004]I)在一些工作中直接假设卫星导航接收机(例如:GPS接收机)和环境观测传感器(例如:视觉或激光传感器)的安装位置是重合的，但对于像自导航车之类的大型轮式移动机器人这种假设会导致最终求得的结果有较大的误差；但是在对精度要求较高的同类工作中考虑纠正这部分误差的方式是通过系统作业前复杂的校准实现，在实现整个坐标转换流程之前需要经过繁琐的校准，有些方法甚至需要专用的经过详细标定的校准场，可操作性不强；本发明的方法前期校准十分简单，不需要专门的外部设施和场地。
[0005]2)卫星导航接收机和其它的环境观测传感器观测数据间的转换和结合时机选择不合理。流程不清晰。而本发明将坐标转换区分为卫星导航接收机信号可靠和卫星导航接收机信号不可靠时的两个流程。而且两种流程得到的结果可以无缝结合，随着机器人的作业这两套流程可以构成统一的迭代循环，在现实工程应用中更易于实现。
【发明内容】
[0006]本发明提供一种用于移动机器人定位与环境重构的绝对坐标获取方法，引入了虚拟坐标系，根据将每一米样时刻机器人方位和环境特征点的相对坐标转换为地心地固坐标系下的大地坐标，即经度、纬度和高度。
[0007]为实现上述目的，本发明提供一种用于移动机器人定位与环境重构的绝对坐标获取方法，其特点是，该方法包含以下步骤:
[0008]步骤1、传感器装配，将惯性传感组件与环境观测传感器捆绑固连并妥善调整角度，确保环境观测传感器中心坐标系、惯导载体坐标系近似重合，并将重合的坐标系定义为机器人中心坐标系，统称c坐标系；卫星导航接收机天线可放置在机器人顶部的任一位置；
[0009]步骤2、确定机器人平台上的虚拟坐标系V，虚拟坐标系的原点取在卫星导航接收机天线的中心点，其坐标轴方向与c坐标系相同，实现环境观测传感器与卫星导航接收机天线之间空间误差的校准；
[0010]步骤3、通过卫星导航接收机在两个位置的标定和坐标系的投影，获取c坐标系原点，c点在虚拟坐标系中的坐标，该坐标将作为后续步骤中补偿卫星导航接收机天线装配位置和环境观测传感器、惯性导航传感器及机器人等效点不在同一点所引起的误差；
[0011]步骤3.1、测量卫星导航接收机天线中心点go和c点在地心地固坐标系e下的大
地坐标: Xf和[0012]步骤3.2、通过式(1)，求取c点在地心地固坐标系e下的笛卡尔坐标:
【权利要求】
1.一种用于移动机器人定位与环境重构的绝对坐标获取方法，其特征在于，该方法包含以下步骤: 步骤1、传感器装配，将惯性传感组件与环境观测传感器捆绑固连并妥善调整角度，确保环境观测传感器中心坐标系、惯导载体坐标系近似重合，并将重合的坐标系定义为机器人中心坐标系，统称C坐标系；卫星导航接收机天线可放置在机器人顶部的任一位置； 步骤2、确定机器人平台上的虚拟坐标系V，虚拟坐标系的原点取在卫星导航接收机天线的中心点，其坐标轴方向与c坐标系相同，实现环境观测传感器与卫星导航接收机天线之间空间误差的校准； 步骤3、通过卫星导航系统在两个位置的标定和坐标系的投影，获取c坐标系原点，c点在虚拟坐标系中的坐标，该坐标将作为后续步骤中补偿卫星导航接收机天线装配位置和环境观测传感器、惯性导航传感器及机器人等效点不在同一点所引起的误差； 步骤4、机器人开始运行，在运行中的当前时刻判断卫星导航接收机是否有可靠的定位数据输出，若是，卫星导航接收机有可靠的数据输出，则跳转到步骤5 ;若否，卫星导航接收机没有可靠的数据输出，则跳转到步骤10 ； 步骤5、根据当前时刻惯导传感器输出的三个角度航向角Vt、横滚角01和俯仰角Yt和c点在虚拟坐标系V中的坐标，通过式(5)求取当前时刻机器人在以卫星导航接收机天线中心点为原点的局部地理坐标系g中的坐标即
2.如权利要求1所述的用于移动机器人定位与环境重构的绝对坐标获取方法，其特征在于，所述步骤3包含以下步骤: 步骤3.1、测量卫星导航接收机天线中心点go和c点在地心地固坐标系e下的大地坐 步骤3.2、通过式(I )，求取c点在地心地固坐标系e下的笛卡尔坐标:
3.如权利要求1所述的用于移动机器人定位与环境重构的绝对坐标获取方法，其特征在于，所述步骤6包含以下步骤: 步骤6.1、通过式(6)，将当前时刻卫星导航接收机输出的经度、纬度和高度即(λ'Φ80, hg0)转换为地心地固坐标系e下的笛卡尔坐标即(Xego,yego,zego);
4.如权利要求3所述的用于移动机器人定位与环境重构的绝对坐标获取方法，其特征在于，所述步骤6.3包含以下步骤: 步骤6.3.1、根据式(8)求取经度:
5.如权利要求1所述的用于移动机器人定位与环境重构的绝对坐标获取方法，其特征在于，所述步骤8包含以下步骤: 步骤8.1、根据式(11)求取环境特征点P在地心地固坐标系e下的笛卡尔坐标:(4Κλ.'41-Stnac)COS(Jc)O T1
6.如权利要求5所述的用于移动机器人定位与环境重构的绝对坐标获取方法，其特征在于，所述步骤8.2包含以下步骤: 步骤8.2.1、根据式(12)，求取经度:
7.如权利要求1所述的用于移动机器人定位与环境重构的绝对坐标获取方法，其特征在于，所述步骤10中，因平移矩阵T反应了 t-1时刻到t时刻机器人在t-1时刻C坐标系中沿三个坐标轴的增量，既(Δx，Δy, Δz)，它实际上就是本步要求xcc(t-1),因此，本步的操作可简化为
8.如权利要求1所述的用于移动机器人定位与环境重构的绝对坐标获取方法，其特征在于，所述步骤12包含以下步骤: 步骤12.1、根据式(15)求取t时刻c点在地心地固坐标系e下的笛卡尔坐标即
9.如权利要求8所述的用于移动机器人定位与环境重构的绝对坐标获取方法，其特征在于，所述步骤12.2包含以下步骤: 步骤12.2.1、根据式(16)，求取经度:
10.如权利要求1所述的用于移动机器人定位与环境重构的绝对坐标获取方法，其特征在于，所述步骤14包含以下步骤: 步骤14.1、根据式(19)求取环境特征点P在地心地固坐标系e下的笛卡尔座坐标
【文档编号】G01C21/20GK103438887SQ201310429674
【公开日】2013年12月11日 申请日期:2013年9月18日 优先权日:2013年9月18日
【发明者】孙作雷, 张波, 曾连荪, 黄平平, 朱大奇 申请人:上海海事大学