用于移动机器人定位与环境重构的绝对坐标获取方法

摘要

本发明公开用于移动机器人定位与环境重构的绝对坐标获取方法，包含：引入虚拟坐标系；求取当前时刻机器人在以卫星导航接收机天线为原点的局部地理坐标系的坐标，机器人在地心地固坐标系的大地坐标，环境特征点以c点为中心的局部地理坐标系的坐标，环境特征点在地心地固坐标系的大地坐标；求取当前时刻机器人在上一时刻c坐标系的坐标，机器人在上一时刻以c点为原点的局部地理坐标系的坐标，机器人的经度、纬度、高度；求取环境特征点在当前时刻以c点为中心的局部地理坐标系的坐标；求取当前时刻环境特征点在地心地固坐标系的大地坐标。本发明引入虚拟坐标系，等效卫星导航接收机天线中心点与传感器坐标系中心点为一点，节省人力、时间成本。

说明书

技术领域

本发明涉及移动机器人定位与地图构建领域，具体涉及一种用于移动机器人定位与环境重构的绝对坐标获取方法。

背景技术

移动机器人定位与地图构建（Simultaneous localization and mapping，SLAM）是移动机器人工程应用及科研上核心方向之一。随着目前以自导航无人车为代表的轮式机器人成为军用、民用的热点。工程上的应用需求日益增强。在实际的工程应用中首先要解决的就是如何获得对机器人自身位置及所观测的环境特征点的绝对坐标的问题。

移动机器人作业的场景复杂多样，在很多场合以GPS为代表的卫星导航系统信号不稳定或精度较低，且无法完成对环境特征的观测。因此在移动机器人的定位和环境重构应用中通常都是主要使用机器人自搭载的传感器实现，包括：被动式传感器，例如，双目或单目视觉传感器；主动式传感器，例如，声纳传感器，二维或三维激光传感器。无论使用哪种传感器，数据处理过程的大步骤都是十分类似的。都是先通过显著的点特征来描述环境，通过传感器观测获取环境特征点和机器人的空间关系，即特征点在机器人中心坐标系中的坐标；然后，通过全局优化或滤波的方式获得环境特征点和机器人作业过程中的每个采样时刻的方位相对于初始时刻的空间关系，即获得它们在初始时刻机器人中心坐标系中的坐标。如果让其在实际应用中有价值就必须将机器人每个时刻的位置和观测的所有环境特征点相对位置都转换为地心地固坐标系下的经纬度高度，即本发明所指的绝对坐标，这步工作现在很多的科研成果中都回避开展，而仅仅展示相对观测的结果，即，一系列的机器人的轨迹和重构出的环境都是在初始时刻的某一坐标系下的，显然这不符合工程应用的最终需求。少量解决该类问题的现有技术存在如下问题：

1）在一些工作中直接假设卫星导航接收机（例如：GPS接收机）和环境观测传感器（例如：视觉或激光传感器）的安装位置是重合的，但对于像自导航车之类的大型轮式移动机器人这种假设会导致最终求得的结果有较大的误差；但是在对精度要求较高的同类工作中考虑纠正这部分误差的方式是通过系统作业前复杂的校准实现，在实现整个坐标转换流程之前需要经过繁琐的校准，有些方法甚至需要专用的经过详细标定的校准场，可操作性不强；本发明的方法前期校准十分简单，不需要专门的外部设施和场地。

2）卫星导航接收机和其它的环境观测传感器观测数据间的转换和结合时机选择不合理。流程不清晰。而本发明将坐标转换区分为卫星导航接收机信号可靠和卫星导航接收机信号不可靠时的两个流程。而且两种流程得到的结果可以无缝结合，随着机器人的作业这两套流程可以构成统一的迭代循环，在现实工程应用中更易于实现。

发明内容

本发明提供一种用于移动机器人定位与环境重构的绝对坐标获取方法，引入了虚拟坐标系，根据将每一采样时刻机器人方位和环境特征点的相对坐标转换为地心地固坐标系下的大地坐标，即经度、纬度和高度。

为实现上述目的，本发明提供一种用于移动机器人定位与环境重构的绝对坐标获取方法，其特点是，该方法包含以下步骤：

步骤1、传感器装配，将惯性传感组件与环境观测传感器捆绑固连并妥善调整角度，确保环境观测传感器中心坐标系、惯导载体坐标系近似重合，并将重合的坐标系定义为机器人中心坐标系，统称c坐标系；卫星导航接收机天线可放置在机器人顶部的任一位置；

步骤2、确定机器人平台上的虚拟坐标系v，虚拟坐标系的原点取在卫星导航接收机天线的中心点，其坐标轴方向与c坐标系相同，实现环境观测传感器与卫星导航接收机天线之间空间误差的校准；

步骤3、通过卫星导航接收机在两个位置的标定和坐标系的投影，获取c坐标系原点，c点在虚拟坐标系中的坐标，该坐标将作为后续步骤中补偿卫星导航接收机天线装配位置和环境观测传感器、惯性导航传感器及机器人等效点不在同一点所引起的误差；

步骤3.1、测量卫星导航接收机天线中心点go和c点在地心地固坐标系e下的大地坐标和

步骤3.2、通过式（1），求取c点在地心地固坐标系e下的笛卡尔坐标：

$>N^c=\frac{a}{\sqrt{1-e^2{\sin }^2{\phi }^c}}---\left(1\right)$>

其中，a=6378137，为地球的长半轴，单位为米；e=0.081819，为地球的偏心率；

$>x_e^c=(N^c+h^c)\cos \left({\phi }^c\right)\cos \left({\lambda }^c\right)$>

$>y_e^c=(N^c+h^c)\cos \left({\phi }^c\right)\sin \left({\lambda }^c\right)$>

$>z_e^c=[\left({1-e}^2\right)N^c+h^c]\sin \left({\phi }^c\right)$>

步骤3.3、通过式（2），求取卫星导航接收机天线中心点go在地心地固坐标系e下的笛卡尔坐标：

$>N^{\mathrm{go}}=\frac{a}{\sqrt{{1-e}^2{\sin }^2{\phi }^{\mathrm{go}}}}---\left(2\right)$>

其中，a＝6378137，为地球的长半轴，单位为米；e＝0.081819，为地球的偏心率；

$>x_e^{\mathrm{go}}=(N^{\mathrm{go}}+h^{\mathrm{go}})\cos \left({\phi }^{\mathrm{go}}\right)\cos \left({\lambda }^{\mathrm{go}}\right)$>

$>y_e^{\mathrm{go}}=(N^{\mathrm{go}}+h^{\mathrm{go}})\cos \left({\phi }^{\mathrm{go}}\right)\sin \left({\lambda }^{\mathrm{go}}\right)$>

$>z_e^{\mathrm{go}}=[\left({1-e}^2\right)N^{\mathrm{go}}+h^{\mathrm{go}}]\sin \left({\phi }^{\mathrm{go}}\right)$>

步骤3.4、通过式（3），求取c点在以卫星导航接收机天线中心点为原点的局部地理坐标系坐标系g下的坐标即

$>\left(\begin{array}{c}{x}_g^c\\ y_g^c\\ z_g^c\end{array}\right)=\left(\begin{array}{ccc}-\sin \left({\lambda }^{\mathrm{go}}\right)& \cos \left({\lambda }^{\mathrm{go}}\right)& 0\\ -\sin \left({\phi }^{\mathrm{go}}\right)\cos \left({\lambda }^{\mathrm{go}}\right)& -\sin \left({\phi }^{\mathrm{go}}\right)\sin \left({\lambda }^{\mathrm{go}}\right)& \cos \left({\phi }^{\mathrm{go}}\right)\\ \cos \left({\phi }^{\mathrm{go}}\right)\cos \left({\lambda }^{\mathrm{go}}\right)& \cos \left({\phi }^{\mathrm{go}}\right)\sin \left({\lambda }^{\mathrm{go}}\right)& \sin \left({\phi }^{\mathrm{go}}\right)\end{array}\right)\left(\begin{array}{c}{x}_e^c-x_e^{\mathrm{go}}\\ y_e^c-y_e^{\mathrm{go}}\\ z_e^c-z_e^{\mathrm{go}}\end{array}\right)---\left(3\right)$>

步骤3.5、根据上步求取的结果和惯导传感器输出的三个角度航向角ψ、横滚角θ和俯仰角γ，通过式（4），求取c点在虚拟坐标系v下的坐标即$>(x_v^c,y_v^c,z_v^c):$>

$>R_g^v=\left(\begin{array}{ccc}\cos \left(\gamma \right)\cos \left(\psi \right)-\sin \left(\gamma \right)\sin \left(\psi \right)\sin \left(\theta \right)& -\cos \left(\theta \right)\sin \left(\psi \right)& \cos \left(\psi \right)\sin \left(\gamma \right)+\cos \left(\gamma \right)\sin \left(\theta \right)\\ \cos \left(\gamma \right)\sin \left(\psi \right)+\cos \left(\psi \right)\sin \left(\theta \right)& \cos \left(\psi \right)\cos \left(\theta \right)& \sin \left(\gamma \right)\sin \left(\psi \right)-\cos \left(\gamma \right)\cos \left(\psi \right)\sin \left(\theta \right)\\ -\cos \left(\theta \right)\sin \left(\gamma \right)& \sin \left(\theta \right)& \cos \left(\gamma \right)\cos \left(\theta \right)\end{array}\right)---\left(4\right)$>

$>\left(\begin{array}{c}{x}_v^c\\ y_v^c\\ z_v^c\end{array}\right)=R_g^v\left(\begin{array}{c}{x}_g^c\\ y_g^c\\ z_g^c\end{array}\right)$>

所述步骤3.1、步骤3.2、步骤3.3只需要做一次，完成后每次即可直接进行步骤3.4；如果机器人上的传感器没有被移动过，那么每次都可跳过步骤3.1、步骤3.2和步骤3.3，直接执行步骤3.4；

步骤4、机器人开始运行，在运行中的当前时刻判断卫星导航接收机是否有可靠的定位数据输出，若是，卫星导航接收机有可靠的数据输出，则跳转到步骤5；若否，卫星导航接收机没有可靠的数据输出，则跳转到步骤10；

步骤5、根据当前时刻惯导传感器输出的三个角度航向角ψ_t、横滚角θ_t和俯仰角γ_t和c点在虚拟坐标系v中的坐标，通过式（5）求取当前时刻机器人在以卫星导航接收机天线中心点为原点的局部地理坐标系g中的坐标即$>(x_g^c,y_g^c,z_g^c):$>

$>R_v^g=\left(\begin{array}{ccc}\cos \left({\gamma }_t\right)\cos \left({\psi }_t\right)-\sin \left({\gamma }_t\right)\sin \left({\psi }_t\right)\sin \left({\theta }_t\right)& \cos \left({\gamma }_t\right)\sin \left({\psi }_t\right)+\cos \left({\psi }_t\right)\sin \left({\theta }_t\right)& -\cos \left({\theta }_t\right)\sin \left({\gamma }_t\right)\\ -\cos \left({\theta }_t\right)\sin \left({\psi }_t\right)& \cos \left({\psi }_t\right)\cos \left({\theta }_t\right)& \sin \left({\theta }_t\right)\\ \cos \left({\psi }_t\right)\sin \left({\gamma }_t\right)+\cos \left({\gamma }_t\right)\sin \left({\psi }_t\right)\sin \left({\theta }_t\right)& \sin \left({\gamma }_t\right)\sin \left({\psi }_t\right)-\cos \left({\gamma }_t\right)\cos \left({\psi }_t\right)\sin \left({\theta }_t\right)& \cos \left({\gamma }_t\right)\cos \left({\theta }_t\right)\end{array}\right)---\left(5\right)$>

$>\left(\begin{array}{c}{x}_g^c\\ y_g^c\\ z_g^c\end{array}\right)=R_v^g\left(\begin{array}{c}{x}_v^c\\ y_v^c\\ z_v^c\end{array}\right)$>

步骤6、根据当前时刻的卫星导航系统输出的经度、纬度和高度数据，即(λ^go，φ^go，h^go)和步骤5中求得的c点在以卫星导航接收机天线中心点为原点的局部地理坐标系g中的坐标即求取当前时刻机器人在地心地固坐标系e下的大地坐标即经度、纬度和高度，即(λ^c，φ^c，h^c);

步骤6.1、通过式（6），将当前时刻卫星导航系统输出的经度、纬度和高度即(λ^go，φ^go，h^go)转换为地心地固坐标系e下的笛卡尔坐标即

$>N^{\mathrm{go}}=\frac{a}{\sqrt{{1-e}^2{\sin }^2{\phi }^{\mathrm{go}}}}---\left(6\right)$>

$>x_e^{\mathrm{go}}=(N^{\mathrm{go}}+h^{\mathrm{go}})\cos \left({\phi }^{\mathrm{go}}\right)\cos \left({\lambda }^{\mathrm{go}}\right)$>

$>y_e^{\mathrm{go}}=(N^{\mathrm{go}}+h^{\mathrm{go}})\cos \left({\phi }^{\mathrm{go}}\right)\sin \left({\lambda }^{\mathrm{go}}\right)$>

$>z_e^{\mathrm{go}}=[\left({1-e}^2\right)N^{\mathrm{go}}+h^{\mathrm{go}}]\sin \left({\phi }^{\mathrm{go}}\right)$>

步骤6.2、通过式（7）求取t时刻c点在地心地固坐标系e下的笛卡尔坐标即$>(x_e^c,y_e^c,z_e^c):$>

$>\left(\begin{array}{c}{x}_g^c\\ y_g^c\\ z_g^c\end{array}\right)={\left(\begin{array}{ccc}-\sin \left({\lambda }^{\mathrm{go}}\right)& \cos \left({\lambda }^{\mathrm{go}}\right)& 0\\ -\sin \left({\phi }^{\mathrm{go}}\right)\cos \left({\lambda }^{\mathrm{go}}\right)& -\sin \left({\phi }^{\mathrm{go}}\right)\sin \left({\lambda }^{\mathrm{go}}\right)& \cos \left({\phi }^{\mathrm{go}}\right)\\ \cos \left({\phi }^{\mathrm{go}}\right)\cos \left({\lambda }^{\mathrm{go}}\right)& \cos \left({\phi }^{\mathrm{go}}\right)\sin \left({\lambda }^{\mathrm{go}}\right)& \sin \left({\phi }^{\mathrm{go}}\right)\end{array}\right)}^{-1}\left(\begin{array}{c}{x}_g^c\\ y_g^c\\ z_g^c\end{array}\right)+\left(\begin{array}{c}{x}_e^{\mathrm{go}}\\ y_e^{\mathrm{go}}\\ z_e^{\mathrm{go}}\end{array}\right)---\left(7\right)$>

步骤6.3、将c点在地心地固坐标系e下的笛卡尔坐标转换为经度、纬度和高度即(λ^c，φ^c，h^c)。

步骤6.3.1、根据式（8）求取经度：

$>{\lambda }^c=\left(\begin{array}{cc}\arctan (y_e^c/x_e^c)& x_e^c\ge 0\\ \arctan (y_e^c/x_e^c)+\pi & x_e^c<0\mathrm{and}{y}_e^c\ge 0\\ \arctan (y_e^c/x_e^c)-\pi & x_e^c<0\mathrm{anb}{y}_e^c<0\end{array}\right)---\left(8\right)$>

步骤6.3.2、求取纬度：

$>r=\sqrt{{\left(x_e^c\right)}^2+{\left(y_e^c\right)}^2}$>

$>F=54{\left({\mathrm{bz}}_e^c\right)}^2$>

η＝1-e²

其中，b＝6356752.3142为地球短半轴，单位为米，e²＝0.006694为地球第一数理偏心率；

$>G=r^2+\eta {\left(z_e^c\right)}^2-e^2(a^2-b^2)$>

$>C=\frac{e^{4}F{r}^2}{G^3}$>

$>S={(1+C+\sqrt{\sqrt{C^2+2C}})}^{\frac{1}{3}}$>

$>P=\frac{F}{3{(S+\frac{1}{S}+1)}^2{G}^2}$>

$>Q=\sqrt{1+{2e}^{4}P}$>

$>\rho =\frac{{-\mathrm{Pe}}^{2}r}{1+Q}+\sqrt{\frac{a^2}{2}(1+\frac{1}{Q})-\frac{{\mathrm{P\eta }\left(z_e^c\right)}^2}{Q(1+Q)}-\frac{{\mathrm{Pr}}^2}{2}}$>

$>{\phi }^p=\arctan \left(\frac{z_e^p+\frac{e^2{\mathrm{az}}_e^p}{V}}{r}\right)$>

步骤6.3.3、根据式（9）求取高度：

步骤7、根据基于环境观测传感器观测值求得的环境特征点p在当前时刻c坐标系中的坐标即和当前时刻惯导传感器输出的数据:三个角度航向角ψ_t、横滚角θ_t和俯仰角γ_t，根据式（10）求取当前时刻环境特征点在当前时刻以c点为中心的局部地理坐标系g中的坐标即

$>R_c^g=\left(\begin{array}{ccc}\cos \left({\gamma }_t\right)\cos \left({\psi }_t\right)-\sin \left({\gamma }_t\right)\sin \left({\psi }_t\right)\sin \left({\theta }_t\right)& \cos \left({\gamma }_t\right)\sin \left({\psi }_t\right)+\cos \left({\psi }_t\right)\sin \left({\theta }_t\right)& -\cos \left({\theta }_t\right)\sin \left({\gamma }_t\right)\\ -\cos \left({\theta }_t\right)\sin \left({\psi }_t\right)& \cos \left({\psi }_t\right)\cos \left({\theta }_t\right)& \sin \left({\theta }_t\right)\\ \cos \left({\psi }_t\right)\sin \left({\gamma }_t\right)+\cos \left({\gamma }_t\right)\sin \left({\psi }_t\right)\sin \left({\theta }_t\right)& \sin \left({\gamma }_t\right)\sin \left({\psi }_t\right)-\cos \left({\gamma }_t\right)\cos \left({\psi }_t\right)\sin \left({\theta }_t\right)& \cos \left({\gamma }_t\right)\cos \left({\theta }_t\right)\end{array}\right)---\left(10\right)$>

$>\left(\begin{array}{c}{x}_g^p\\ y_g^p\\ z_g^p\end{array}\right)=R_{c\begin{array}{}\end{array}}^g\left(\begin{array}{c}{x}_c^p\\ y_c^p\\ z_c^p\end{array}\right)$>

步骤8、根据步骤7的结果和步骤6求得的机器人的经度、纬度和高度(λ^c，φ^c，h^c)及在地心地固坐标系e下的笛卡尔坐标求取当前时刻环境特征点p在地心地固坐标系下的大地坐标，即经度、纬度、高度(λ^p，φ^p，h^p)；

步骤8.1、根据式（11）求取环境特征点p在地心地固坐标系e下的笛卡尔坐标$>(x_e^p,y_e^p,z_e^p):$>

$>\left(\begin{array}{c}{x}_e^p\\ y_e^p\\ z_e^p\end{array}\right){=\left(\begin{array}{ccc}-\sin \left({\lambda }^c\right)& \cos \left({\lambda }^c\right)& 0\\ -\sin \left({\phi }^c\right)\cos \left({\lambda }^c\right)& -\sin \left({\phi }^c\right)\sin \left({\lambda }^c\right)& \cos \left({\phi }^c\right)\\ \cos \left({\phi }^c\right)\cos \left({\lambda }^c\right)& \cos \left({\phi }^c\right)\sin \left({\lambda }^c\right)& \sin \left({\phi }^c\right)\end{array}\right)}^{-1}\left(\begin{array}{c}{x}_g^p\\ y_g^p\\ z_g^p\end{array}\right)+\left(\begin{array}{c}{x}_e^c\\ y_e^c\\ z_e^p\end{array}\right)---\left(11\right)$>

步骤8.2、将环境特征点p在地心地固坐标系e下的笛卡尔坐标转换为经度、纬度和高度即(λ^p，φ^p，h^p);

步骤8.2.1、根据式（12），求取经度：

:$>{\lambda }^p=\left(\begin{array}{cc}\arctan (y_e^p/x_e^p)& x_e^p\ge 0\\ \arctan (y_e^p/x_e^p)+\pi & x_e^p<0\mathrm{and}{y}_e^p\ge 0\\ \arctan (y_e^p/x_e^p)-\pi & x_e^p<0\mathrm{and}{y}_e^p<0\end{array}\right)---\left(12\right)$>

步骤8.2.2、求取纬度：

$>r=\sqrt{{\left(x_e^p\right)}^2+{\left(y_e^p\right)}^2}$>

$>F=54{\left({\mathrm{bz}}_e^p\right)}^2$>

η＝1-e²

其中，b=6356752.3142为地球短半轴，单位为米，e²＝0.006694为地球第一数理偏心率；

$>G=r^2+\eta {\left(z_e^p\right)}^2-e^2(a^2-b^2)$>

$>C=\frac{e^{4}F{r}^2}{G^3}$>

$>S={(1+C+\sqrt{\sqrt{C^2+2C}})}^{\frac{1}{3}}$>

$>P=\frac{F}{3{(S+\frac{1}{S}+1)}^2{G}^2}$>

$>Q=\sqrt{1+{2e}^{4}P}$>

$>\rho =\frac{-{\mathrm{Pe}}^{2}r}{1+Q}+\sqrt{\frac{a^2}{2}(1+\frac{1}{Q})-\frac{\mathrm{P\eta }{\left(z_e^p\right)}^2}{Q(1+Q)}-\frac{{\mathrm{Pr}}^2}{2}}$>

$>{\phi }^p=\arctan \left(\frac{z_e^p+\frac{e^2{\mathrm{az}}_e^p}{V}}{r}\right)$>

步骤8.2.3、根据式（13），求取高度：

步骤9、完成步骤8，跳转到步骤4；

步骤10、根据基于环境观测传感器观测值求得的机器人在当前时刻t和前一时刻t-1间的相对运动，即平移矩阵T和旋转矩阵γ，求取t时刻机器人在t-1时刻c坐标系中的坐标$>x_{c(t-1)}^c$>即$>(x_{c(t-1)}^c,y_{c(t-1)}^c,z_{c(t-1)}^c);$>

步骤11、根据步骤10的结果和前一时刻t-1惯导传感器输出的三个角度航向角ψ_t-1、横滚角θ_t-1和俯仰角γ_t-1，根据式（14）求取当前时刻t机器人在上一时刻以c点为原点的局部地理坐标系中的坐标即$>(x_{g(t-1)}^c,y_{g(t-1)}^c,z_{g(t-1)}^c):$>

$>R_{c(t-1)}^{g(t-1)}=\left(\begin{array}{ccc}\cos \left({\gamma }_{t-1}\right)\cos \left({\psi }_{t-1}\right)-\sin \left({\gamma }_{t-1}\right)\sin \left({\psi }_{t-1}\right)\sin \left({\theta }_{t-1}\right)& \cos \left({\gamma }_{t-1}\right)\sin \left({\psi }_{t-1}\right)+\cos \left({\psi }_{t-1}\right)\sin \left({\gamma }_{t-1}\right)\sin \left({\theta }_{t-1}\right)& -\cos \left({\theta }_{t-1}\right)\sin \left({\gamma }_{t-1}\right)\\ -\cos \left({\theta }_{t-1}\right)\sin \left({\psi }_{t-1}\right)& \cos \left({\psi }_{t-1}\right)\cos \left({\theta }_{t-1}\right)& \sin \left({\theta }_{t-1}\right)\\ \cos \left({\psi }_{t-1}\right)\sin \left({\gamma }_{t-1}\right)+\cos \left({\gamma }_{t-1}\right)\sin \left({\psi }_{t-1}\right)\sin \left({\theta }_{t-1}\right)& \sin \left({\gamma }_{t-1}\right)\sin \left({\psi }_{t-1}\right)-\cos \left({\gamma }_{t-1}\right)\cos \left({\psi }_{t-1}\right)\sin \left({\theta }_{t-1}\right)& \cos \left({\gamma }_{t-1}\right)\cos \left({\theta }_{t-1}\right)\end{array}\right)---\left(14\right)$>

$>\left(\begin{array}{c}{x}_{g(t-1)}^c\\ y_{g(t-1)}^c\\ z_{g(t-1)}^c\end{array}\right)=R_{c(t-1)}^{g(t-1)}\left(\begin{array}{c}{x}_{c(t-1)}^c\\ y_{c(t-1)}^c\\ z_{c(t-1)}^c\end{array}\right)$>

步骤12、根据步骤11的结果和前一时刻t-1的机器人的经度、纬度和高度，即(λ^c(t-1)，φ^c(t-1)，h^c(-1))及在地心地固坐标系下相应的笛卡尔坐标即求取当前时刻t的机器人的经度、纬度、高度，即(λ^c(t)，φ^c(t)，h^c(t));

步骤12.1、根据式（15）求取t时刻c点在地心地固坐标系e下的笛卡尔坐标即$>(x_e^{c\left(t\right)},y_e^{c\left(t\right)},z_e^{c\left(t\right)}):$>

$>\left(\begin{array}{c}{x}_e^{c\left(t\right)}\\ y_e^{c\left(t\right)}\\ z_e^{c\left(t\right)}\end{array}\right)={\left(\begin{array}{ccc}-\sin \left({\lambda }^{c(t-1)}\right)& \cos \left({\lambda }^{c(t-1)}\right)& 0\\ -\sin \left({\phi }^{c(t-1)}\right)\cos \left({\lambda }^{c(t-1)}\right)& -\sin \left({\phi }^{c(t-1)}\right)\sin \left({\lambda }^{c(t-1)}\right)& \cos \left({\phi }^{c(t-1)}\right)\\ \cos \left({\phi }^{c(t-1)}\right)\cos \left({\lambda }^{c(t-1)}\right)& \cos \left({\phi }^{c(t-1)}\right)\sin \left({\lambda }^{c(t-1)}\right)& \sin \left({\phi }^{c(t-1)}\right)\end{array}\right)}^{-1}\left(\begin{array}{c}{x}_{g(t-1)}^c\\ y_{g(t-1)}^c\\ z_{g(t-1)}^c\end{array}\right)+\left(\begin{array}{c}{x}_e^{c(t-1)}\\ y_e^{c(t-1)}\\ z_e^{c(t-1)}\end{array}\right)---\left(15\right)$>

步骤12.2、将c点在地心地固坐标系e下的笛卡尔坐标转换为经度、纬度和高度即(λ^c(t)，φ^c(t)，h^c(t))；

步骤12.2.1、根据式（16），求取经度：

$>{\lambda }^{c\left(t\right)}=\left(\begin{array}{cc}\arctan (y_e^{c\left(t\right)}/x_e^{c\left(t\right)})& x_e^{c\left(t\right)}\ge 0\\ \arctan (y_e^{c\left(t\right)}/x_e^{c\left(t\right)})+\pi & x_e^{c\left(t\right)}<0\mathrm{and}{y}_e^{c\left(t\right)}\ge 0\\ \arctan (y_e^{c\left(t\right)}/x_e^{c\left(t\right)})-\pi & x_e^{c\left(t\right)}<0\mathrm{and}{y}_e^{c\left(t\right)}<0\end{array}\right)---\left(16\right)$>

步骤12.2.2、求取纬度：

$>r=\sqrt{{\left(x_e^{c\left(t\right)}\right)}^2+{\left(y_e^{c\left(t\right)}\right)}^2}$>

$>F=54{\left({\mathrm{bz}}_e^{c\left(t\right)}\right)}^2$>

η＝1-e²

其中，b＝6356752.3142为地球短半轴，单位为米，e²＝0.006694为地球第一数理偏心率；

$>G=r^2+\eta {\left(z_e^{c\left(t\right)}\right)}^2-e^2(a^2-b^2)$>

$>C=\frac{e^{4}F{r}^2}{G^3}$>

$>S={(1+C+\sqrt{\sqrt{C^2+2C}})}^{\frac{1}{3}}$>

$>P=\frac{F}{3{(S+\frac{1}{S}+1)}^2{G}^2}$>

$>Q=\sqrt{1+{2e}^{4}P}$>

$>\rho =\frac{{-\mathrm{Pe}}^{2}r}{1+Q}+\sqrt{\frac{a^2}{2}(1+\frac{1}{Q})-\frac{{\mathrm{P\eta }\left(z_e^{c\left(t\right)}\right)}^2}{Q(1+Q)}-\frac{{\mathrm{Pr}}^2}{2}}$>

$>{\phi }^p=\arctan \left(\frac{z_e^p+\frac{e^2{\mathrm{az}}_e^p}{V}}{r}\right)$>

步骤12.2.3、根据式（17），求取高度：

步骤13、根据基于环境观测传感器观测值求得的环境特征点p在当前时刻t时刻c坐标系中的坐标即和当前时刻惯导传感器输出的三个角度航向角ψ_t、横滚角θ_t和俯仰角γ_t，根据式（18）求取当前时刻环境特征点p在当前时刻t以c点为中心的局部地理坐标系g中的坐标即

$>R_c^g=\left(\begin{array}{ccc}\cos \left({\gamma }_t\right)\cos \left({\psi }_t\right)-\sin \left({\gamma }_t\right)\sin \left({\psi }_t\right)\sin \left({\theta }_t\right)& \cos \left({\gamma }_t\right)\sin \left({\psi }_t\right)+\cos \left({\psi }_t\right)\sin \left({\gamma }_t\right)\sin \left({\theta }_t\right)& -\cos \left({\theta }_t\right)\sin \left({\gamma }_t\right)\\ -\cos \left({\theta }_t\right)\sin \left({\psi }_t\right)& \cos \left({\psi }_t\right)\cos \left({\theta }_t\right)& \sin \left({\theta }_t\right)\\ \cos \left({\psi }_t\right)\sin \left({\gamma }_t\right)+\cos \left({\gamma }_t\right)\sin \left({\psi }_t\right)\sin \left({\theta }_t\right)& \sin \left({\gamma }_t\right)\sin \left({\psi }_t\right)-\cos \left({\gamma }_t\right)\cos \left({\psi }_t\right)\sin \left({\theta }_t\right)& \cos \left({\gamma }_t\right)\cos \left({\theta }_t\right)\end{array}\right)---\left(18\right)$>

$>\left(\begin{array}{c}{x}_g^p\\ y_g^p\\ z_g^p\end{array}\right)=R_c^g\left(\begin{array}{c}{x}_c^p\\ y_c^p\\ z_c^p\end{array}\right)$>

步骤14、根据步骤13求得的当前时刻t的机器人的经度、纬度、高度，即(λ^c(t)，φ^c(t)，h^c(t))，及t时刻c点在地心地固坐标系e下的笛卡尔坐标即求取当前时刻环境特征点p在地心地固坐标系e下的大地坐标即，经度、纬度、高度(λ^p，φ^p，h^p)；

步骤14.1、根据式（19）求取环境特征点p在地心地固坐标系e下的笛卡尔座坐标

$>\left(\begin{array}{c}{x}_e^p\\ y_e^p\\ z_e^p\end{array}\right)={\left(\begin{array}{ccc}-\sin \left({\lambda }^c\right)& \cos \left({\lambda }^c\right)& 0\\ -\sin \left({\phi }^{c\left(t\right)}\right)\cos \left({\lambda }^{c\left(t\right)}\right)& -\sin \left({\phi }^{c\left(t\right)}\right)\sin \left({\lambda }^{c\left(t\right)}\right)& \cos \left({\phi }^{c\left(t\right)}\right)\\ \cos \left({\phi }^{c\left(t\right)}\right)\cos \left({\lambda }^{c\left(t\right)}\right)& \cos \left({\phi }^{c\left(t\right)}\right)\sin \left({\lambda }^{c\left(t\right)}\right)& \sin \left({\phi }^{c\left(t\right)}\right)\end{array}\right)}^{-1}\left(\begin{array}{c}{x}_g^p\\ y_g^p\\ z_g^p\end{array}\right)+\left(\begin{array}{c}{x}_e^{c\left(t\right)}\\ y_e^{c\left(t\right)}\\ z_e^{c\left(t\right)}\end{array}\right)---\left(19\right)$>

步骤14.2、将环境特征点p在地心地固坐标系e下的笛卡尔坐标转换为经度、纬度和高度即(λ^p，φ^p，h^p)；

步骤14.2.1、根据式（20）求取经度：

$>{\lambda }^p=\left(\begin{array}{cc}\arctan (y_e^p/x_e^p)& x_e^p\ge 0\\ \arctan (y_e^p/x_e^p)+\pi & x_e^p<0\mathrm{and}{y}_e^p\ge 0\\ \arctan (y_e^p/x_e^p)-\pi & x_e^p<0\mathrm{and}{y}_e^p<0\end{array}\right)---\left(20\right)$>

步骤14.2.2、求取纬度：

$>r=\sqrt{{\left(x_e^p\right)}^2+{\left(y_e^p\right)}^2}$>

$>F=54{\left({\mathrm{bz}}_e^p\right)}^2$>

η=1-e²

其中，b＝6356752.3142为地球短半轴，单位为米，e²＝0.006694为地球第一数理偏心率；

$>G=r^2+\eta {\left(z_e^c\right)}^2-e^2(a^2-b^2)$>

$>C=\frac{e^{4}F{r}^2}{G^3}$>

$>S={(1+C+\sqrt{\sqrt{C^2+2C}})}^{\frac{1}{3}}$>

$>P=\frac{F}{3{(S+\frac{1}{S}+1)}^2{G}^2}$>

$>Q=\sqrt{1+{2e}^{4}P}$>

$>\rho =\frac{{-\mathrm{Pe}}^{2}r}{1+Q}+\sqrt{\frac{a^2}{2}(1+\frac{1}{Q})-\frac{{\mathrm{P\eta }\left(z_e^c\right)}^2}{Q(1+Q)}-\frac{{\mathrm{Pr}}^2}{2}}$>

$>{\phi }^p=\arctan \left(\frac{z_e^p+\frac{e^2{\mathrm{az}}_e^p}{V}}{r}\right)$>

步骤14.2.3、根据式（21）求取高度：

步骤15、完成步骤14后，绝对坐标获取方法完成，并跳转到步骤4。

上述步骤10中，因平移矩阵T反应了t-1时刻到t时刻机器人在t-1时刻c坐标系中沿三个坐标轴的增量，即(Δx，Δy，Δz)，它实际上就是本步要求的$>x_{c(t-1)}^c,$>因此，本步的操作可简化为：$>x_{c(t-1)}^c=\mathrm{\Delta x},y_{c(t-1)}^c=\mathrm{\Delta y},z_{c(t-1)}^c=\mathrm{\Delta z}.$>

本发明用于移动机器人定位与环境重构的绝对坐标获取方法和现有技术相比，其优点在于，本发明通过引入虚拟坐标系，可以仅用2步就能把卫星导航接收机天线中心点与环境观测传感器（激光传感器或视觉传感器）坐标系中心点等效为一点，不需做大量校准，节省人力成本、时间成本和器件成本。

附图说明

图1为本发明用于移动机器人定位与环境重构的绝对坐标获取方法的总体流程图；

图2为本发明用于移动机器人定位与环境重构的绝对坐标获取方法中的坐标示意图；

图3为本发明用于移动机器人定位与环境重构的绝对坐标获取方法中获取c点在虚拟坐标系中的坐标的流程图。

具体实施方式

以下结合附图，进一步说明本发明的具体实施例。

为了能够更好地说明本发明，这里声明本发明中涉及的坐标系：

1、地心地固坐标系（本发明中用e代表）：

右手坐标系。地球球心为原点，x轴指向本初子午线，y轴指向东经90度，z轴与地球自转轴重合，指向北极。根据表述不同，e下的坐标有两种等效的表达方式：笛卡尔坐标，用(x_e，y_e，z_e)表示；大地坐标，用经度λ，纬度，高度h表示。

2、局部地理坐标系（本发明中用g代表）：

右手坐标系。该坐标系的原点任意，因此在本发明的后续论述中凡表述局部地理坐标系时，都需指明其原点。x轴指向东向，y轴指向北极点，z轴指向地球球心的反方向。也称为东北天坐标系。本发明中所使用惯导传感器测量特定坐标系坐标轴与g的旋转角度。

3、机器人中心坐标系、环境观测传感器中心坐标系、惯导载体坐标系（本发明中用c代表）：

本发明中，通过合理的传感器装配使这三个坐标系重合，这里的环境观测传感器可以是激光传感器也可以是视觉传感器。在本发明的实施例中使用单目视觉传感器作为环境观测传感器，环境观测传感器中心坐标系对应了相机中心坐标系，它是右手坐标系。原点为相机成像中心，即光心，x轴指向摄像头前方，y轴指向左方，z轴指向相机上方。本发明中将机器人中心坐标系、环境观测传感器中心坐标系、惯导载体坐标系重合。即坐标系原点相同，坐标轴重合。因这三个坐标系完全重合，下文的阐述中不再区分这三个坐标系，将它们统称为c坐标系，其原点称为点c。下文所指机器人在某坐标系中的坐标，实际上就是点c在相应坐标系中的坐标。

4、虚拟坐标系（本发明中用v代表）（该坐标系的引入是本发明简化坐标转换过程的关键，也是区别于同类方法的关键）：

右手坐标系。原点在测量车装配的主卫星导航接收机天线的中心点，坐标轴与相机中心坐标系c的坐标轴平行。

本发明下文中描述作如下约定：

本发明后续的介绍中将频繁出现关于特定点在特定坐标系中坐标的描述。为了方便表述，特将点以如下方式声明：将α点在坐标系z下的坐标，表示为

本发明公开一种用于移动机器人定位与环境重构的绝对坐标获取方法的实施例，本实施例在一个四轮车型移动机器人上实施，使用了Point Grey出品的单目视觉传感器作为环境观测传感器，还使用了NovAtel产品的GPS接收机，和XSense带航向角输出的惯性导航传感器模块。

如图1所示，用于移动机器人定位与环境重构的绝对坐标获取方法包含以下步骤：

步骤1、传感器装配。将惯导传感器与视觉传感器捆绑固定连接，并妥善调整角度，确保环境观测传感器中心坐标系与惯导载体坐标系近似重合，并将重合的坐标系定义为机器人中心坐标系。

如图2所示，经过本步骤的处理，惯导载体坐标系、机器人中心坐标系和环境观测传感器中心坐标系重合，在图中仅为了可视化才分开绘制。如前所述，将机器人中心坐标系、环境观测传感器中心坐标系、惯导载体坐标系统称为c坐标系，其原点称为点c。

通过这一步可以确保惯性导传感器所测量的三个欧拉角：航向角ψ、俯仰角γ和横滚角θ反映了c坐标系与局部地理坐标系坐标轴之间的旋转角度。

本发明中卫星导航接收机可采用各种类型的卫星导航接收机，例如北斗卫星导航系统、伽利略卫星导航接收机，本实施例中采用GPS接收机。

GPS天线（GPS接收机天线）可放置在机器人顶部的任一位置，本实施例中，GPS天线可放置在视觉传感器附近，但其无法与视觉传感器重合装配。对于车型轮式移动机器人，二者可以同时装配在车辆顶部，其相对直线距离可以保持在1米左右。

步骤2、确定机器人平台上的虚拟坐标系v（本发明定义的坐标系，这是区别于同类方法的核心步骤，同类发明中都没有使用该坐标系），环境观测传感器与GPS天线的安装通常无法在空间上的同一点，往往空间上相隔较远，因此二者的观测间都先天混入了空间误差，如果不寻求妥善的等效机制，将让后续的测量引入较大误差。本步的目的就是实施对此的校准，这里的虚拟坐标系的原点取在GPS天线的中心点，但其坐标轴方向与c坐标系相同。

步骤3、获取c点在虚拟坐标系v中的坐标。该坐标将作为后续步骤中补偿GPS天线装配位置和环境观测传感器、惯性导航传感器及机器人等效点不在同一点所引起的误差。本步骤基于GPS在两个位置的标定和坐标系的投影实现。

如图3所示，步骤3中具体包含以下步骤：

步骤3.1、测量GPS接收机天线中心点go和c点在地心地固坐标系e下的大地坐标和具体实施为，将移动机器人停放于GPS信号好的开阔地带进行连续2分钟的采样观测到一组GPS天线中心点go在e下的大地坐标，再将GPS天线中心点移至与c点近似重合处进行连续两分钟的采样观测，本实施例中使用的GPS接收机的采样频率为1Hz，则可分别测得120组和120组每组都由GPS天线中心点的经度λ^go、纬度φ^go和高度h^go组成，同理，每组都由相机装配中心点的经度λ^co、纬度λ^co和高度h^co组成。然后分别对120组和120组取平均，求得后续步骤中使用的即(λ^go，φ^go，h^go)和即(λ^c，φ^c，h^c)；

步骤3.2、通过式（1），求取c点在地心地固坐标系e下的笛卡尔坐标：

$>N^c=\frac{a}{\sqrt{1-e^2{\sin }^2{\phi }^c}}---\left(1\right)$>

其中，a＝6378137，为地球的长半轴，单位为米；e＝0.081819，为地球的偏心率。

$>x_e^c=(N^c+h^c)\cos \left({\phi }^c\right)\cos \left({\lambda }^c\right)$>

$>y_e^c=(N^c+h^c)\cos \left({\phi }^c\right)\sin \left({\lambda }^c\right)$>

$>z_e^c=[\left({1-e}^2\right)N^c+h^c]\sin \left({\phi }^c\right)$>

步骤3.3、通过式（2），求取GPS接收机天线中心点go在地心地固坐标系e下的笛卡尔坐标：

$>N^{\mathrm{go}}=\frac{a}{\sqrt{{1-e}^2{\sin }^2{\phi }^{\mathrm{go}}}}---\left(2\right)$>

其中，a＝6378137，为地球的长半轴，单位为米；e＝0.081819，为地球的偏心率。

$>x_e^{\mathrm{go}}=(N^{\mathrm{go}}+h^{\mathrm{go}})\cos \left({\phi }^{\mathrm{go}}\right)\cos \left({\lambda }^{\mathrm{go}}\right)$>

$>y_e^{\mathrm{go}}=(N^{\mathrm{go}}+h^{\mathrm{go}})\cos \left({\phi }^{\mathrm{go}}\right)\sin \left({\lambda }^{\mathrm{go}}\right)$>

$>z_e^{\mathrm{go}}=[\left({1-e}^2\right)N^{\mathrm{go}}+h^{\mathrm{go}}]\sin \left({\phi }^{\mathrm{go}}\right)$>

步骤3.4、通过式（3），求取c点在以GPS接收机天线中心点为原点的局部地理坐标系坐标系g下的坐标即

$>\left(\begin{array}{c}{x}_g^c\\ y_g^c\\ z_g^c\end{array}\right)=\left(\begin{array}{ccc}-\sin \left({\lambda }^{\mathrm{go}}\right)& \cos \left({\lambda }^{\mathrm{go}}\right)& 0\\ -\sin \left({\phi }^{\mathrm{go}}\right)\cos \left({\lambda }^{\mathrm{go}}\right)& -\sin \left({\phi }^{\mathrm{go}}\right)\sin \left({\lambda }^{\mathrm{go}}\right)& \cos \left({\phi }^{\mathrm{go}}\right)\\ \cos \left({\phi }^{\mathrm{go}}\right)\cos \left({\lambda }^{\mathrm{go}}\right)& \cos \left({\phi }^{\mathrm{go}}\right)\sin \left({\lambda }^{\mathrm{go}}\right)& \sin \left({\phi }^{\mathrm{go}}\right)\end{array}\right)\left(\begin{array}{c}{x}_e^c-x_e^{\mathrm{go}}\\ y_e^c-y_e^{\mathrm{go}}\\ z_e^c-z_e^{\mathrm{go}}\end{array}\right)---\left(3\right)$>

步骤3.5、根据上步求取的结果和惯导传感器输出的三个角度航向角ψ、横滚角θ和俯仰角γ，通过式（4），求取c点在虚拟坐标系v下的坐标即将在后续步骤中使用：

$>R_g^v=\left(\begin{array}{ccc}\cos \left(\gamma \right)\cos \left(\psi \right)-\sin \left(\gamma \right)\sin \left(\psi \right)\sin \left(\theta \right)& -\cos \left(\theta \right)\sin \left(\psi \right)& \cos \left(\psi \right)\sin \left(\gamma \right)+\cos \left(\gamma \right)\sin \left(\theta \right)\\ \cos \left(\gamma \right)\sin \left(\psi \right)+\cos \left(\psi \right)\sin \left(\theta \right)& \cos \left(\psi \right)\cos \left(\theta \right)& \sin \left(\gamma \right)\sin \left(\psi \right)-\cos \left(\gamma \right)\cos \left(\psi \right)\sin \left(\theta \right)\\ -\cos \left(\theta \right)\sin \left(\gamma \right)& \sin \left(\theta \right)& \cos \left(\gamma \right)\cos \left(\theta \right)\end{array}\right)---\left(4\right)$>

$>\left(\begin{array}{c}{x}_v^c\\ y_v^c\\ z_v^c\end{array}\right)=R_g^v\left(\begin{array}{c}{x}_g^c\\ y_g^c\\ z_g^c\end{array}\right)$>

步骤3.1、步骤3.2、步骤3.3只需要做一次，完成后每次即可直接进行步骤3.4。如果机器人上的传感器没有被移动过，那么每次都可跳过步骤3.1、步骤3.2和步骤3.3，直接执行步骤3.4。

步骤4、在完成上述步骤1-3后机器人开始运行，在运行中的当前时刻判断GPS接收机是否有可靠的定位数据输出，若是，GPS有可靠的数据输出，则跳转到步骤5；若否，GPS没有可靠的数据输出，则跳转到步骤10。

步骤5、计当前时刻为t，根据当前时刻t惯导传感器输出的三个角度航向角ψ_t、横滚角θ_t和俯仰角γ_t和步骤3中求得的c点在虚拟坐标系v中的坐标，求取当前t时刻机器人在以GPS天线中心点go为原点的局部地理坐标系中的坐标。即通过式（5）将步骤3求得的c点在虚拟坐标系v中的坐标转换为以go为原点的局部地理坐标系g中的坐标即

$>R_v^g=\left(\begin{array}{ccc}\cos \left({\gamma }_t\right)\cos \left({\psi }_t\right)-\sin \left({\gamma }_t\right)\sin \left({\psi }_t\right)\sin \left({\theta }_t\right)& \cos \left({\gamma }_t\right)\sin \left({\psi }_t\right)+\cos \left({\psi }_t\right)\sin \left({\theta }_t\right)& -\cos \left({\theta }_t\right)\sin \left({\gamma }_t\right)\\ -\cos \left({\theta }_t\right)\sin \left({\psi }_t\right)& \cos \left({\psi }_t\right)\cos \left({\theta }_t\right)& \sin \left({\theta }_t\right)\\ \cos \left({\psi }_t\right)\sin \left({\gamma }_t\right)+\cos \left({\gamma }_t\right)\sin \left({\psi }_t\right)\sin \left({\theta }_t\right)& \sin \left({\gamma }_t\right)\sin \left({\psi }_t\right)-\cos \left({\gamma }_t\right)\cos \left({\psi }_t\right)\sin \left({\theta }_t\right)& \cos \left({\gamma }_t\right)\cos \left({\theta }_t\right)\end{array}\right)---\left(5\right)$>

$>\left(\begin{array}{c}{x}_g^c\\ y_g^c\\ z_g^c\end{array}\right)=R_v^g\left(\begin{array}{c}{x}_v^c\\ y_v^c\\ z_v^c\end{array}\right)$>

步骤6、根据当前时刻即t时刻的GPS输出的经度、纬度和高度数据，即(λ^go，φ^go，h^go)和上步中求得的c点在以go为原点的局部地理坐标系g中的坐标即求取当前时刻机器人在地心地固坐标系e下的大地坐标即经度、纬度和高度，即(λ^c，φ^c，h^c)。这里不能将GPS输出的经度、纬度和高度作为机器人的经度、纬度和高度的原因是：GPS天线很难与本发明中所描述的c点重合，这也是同类技术中前期校准的必要性。

该步骤6具体包含以下步骤：

步骤6.1、通过式（6），将t时刻GPS输出的经度、纬度和高度即(λ^go，φ^go，h^go)转换为地心地固坐标系e下的笛卡尔坐标即如前所述，这里的go指GPS天线的中心点：

$>N^{\mathrm{go}}=\frac{a}{\sqrt{{1-e}^2{\sin }^2{\phi }^{\mathrm{go}}}}---\left(6\right)$>

$>x_e^{\mathrm{go}}=(N^{\mathrm{go}}+h^{\mathrm{go}})\cos \left({\phi }^{\mathrm{go}}\right)\cos \left({\lambda }^{\mathrm{go}}\right)$>

$>y_e^{\mathrm{go}}=(N^{\mathrm{go}}+h^{\mathrm{go}})\cos \left({\phi }^{\mathrm{go}}\right)\sin \left({\lambda }^{\mathrm{go}}\right)$>

$>z_e^{\mathrm{go}}=[\left({1-e}^2\right)N^{\mathrm{go}}+h^{\mathrm{go}}]\sin \left({\phi }^{\mathrm{go}}\right)$>

步骤6.2、通过式（7）求取t时刻c点在地心地固坐标系e下的笛卡尔坐标即$>(x_e^c,y_e^c,z_e^c):$>

$>\left(\begin{array}{c}{x}_g^c\\ y_g^c\\ z_g^c\end{array}\right)={\left(\begin{array}{ccc}-\sin \left({\lambda }^{\mathrm{go}}\right)& \cos \left({\lambda }^{\mathrm{go}}\right)& 0\\ -\sin \left({\phi }^{\mathrm{go}}\right)\cos \left({\lambda }^{\mathrm{go}}\right)& -\sin \left({\phi }^{\mathrm{go}}\right)\sin \left({\lambda }^{\mathrm{go}}\right)& \cos \left({\phi }^{\mathrm{go}}\right)\\ \cos \left({\phi }^{\mathrm{go}}\right)\cos \left({\lambda }^{\mathrm{go}}\right)& \cos \left({\phi }^{\mathrm{go}}\right)\sin \left({\lambda }^{\mathrm{go}}\right)& \sin \left({\phi }^{\mathrm{go}}\right)\end{array}\right)}^{-1}\left(\begin{array}{c}{x}_g^c\\ y_g^c\\ z_g^c\end{array}\right)+\left(\begin{array}{c}{x}_e^{\mathrm{go}}\\ y_e^{\mathrm{go}}\\ z_e^{\mathrm{go}}\end{array}\right)---\left(7\right)$>

步骤6.3、将c点在地心地固坐标系e下的笛卡尔坐标转换为经度、纬度和高度即(λ^c，φ^c，h^c)。该步的操作如下：

步骤6.3.1、根据式（8）求取经度：

$>{\lambda }^c=\left(\begin{array}{cc}\arctan (y_e^c/x_e^c)& x_e^c\ge 0\\ \arctan (y_e^c/x_e^c)+\pi & x_e^c<0\mathrm{and}{y}_e^c\ge 0\\ \arctan (y_e^c/x_e^c)-\pi & x_e^c<0\mathrm{anb}{y}_e^c<0\end{array}\right)---\left(8\right)$>

步骤6.3.2、求取纬度：

$>r=\sqrt{{\left(x_e^c\right)}^2+{\left(y_e^c\right)}^2}$>

$>F=54{\left({\mathrm{bz}}_e^c\right)}^2$>

η＝1-e²

其中，b＝6356752.3142为地球短半轴，单位为米。e²＝0.006694为地球第一数理偏心率。

$>G=r^2+\eta {\left(z_e^c\right)}^2-e^2(a^2-b^2)$>

$>C=\frac{e^{4}F{r}^2}{G^3}$>

$>S={(1+C+\sqrt{\sqrt{C^2+2C}})}^{\frac{1}{3}}$>

$>P=\frac{F}{3{(S+\frac{1}{S}+1)}^2{G}^2}$>

$>Q=\sqrt{1+{2e}^{4}P}$>

$>\rho =\frac{{-\mathrm{Pe}}^{2}r}{1+Q}+\sqrt{\frac{a^2}{2}(1+\frac{1}{Q})-\frac{{\mathrm{P\eta }\left(z_e^c\right)}^2}{Q(1+Q)}-\frac{{\mathrm{Pr}}^2}{2}}$>

$>{\phi }^p=\arctan \left(\frac{z_e^p+\frac{e^2{\mathrm{az}}_e^p}{V}}{r}\right)$>

上述的G、C、S、P、Q、ρ、、V为都是为了替换方便而假设出来的，并没有实际的物理意义，其实完全可以都替换进去，写出一个很长很大的式子，来表示φ^c，但这个式子会很复杂，不太适合理解。

步骤6.3.3、根据式（9）求取高度：

步骤7、根据基于环境观测传感器观测值求得的环境特征点p在当前t时刻c坐标系中的坐标即和当前时刻惯导传感器输出的数据:三个角度航向角ψ_t、横滚角θ_t和俯仰角γ_t，根据式（10）求取当前时刻环境特征点在当前t时刻以c点为中心的局部地理坐标系g中的坐标即

如前所述，作为本发明中的创新点之一的虚拟坐标系v的坐标轴和c坐标系平行，而仅坐标原点不同，因此，本步骤操作中所需的旋转矩阵与步骤5中用到的旋转矩阵相同，则本步的具体操作为：

$>R_c^g=\left(\begin{array}{ccc}\cos \left({\gamma }_t\right)\cos \left({\psi }_t\right)-\sin \left({\gamma }_t\right)\sin \left({\psi }_t\right)\sin \left({\theta }_t\right)& \cos \left({\gamma }_t\right)\sin \left({\psi }_t\right)+\cos \left({\psi }_t\right)\sin \left({\theta }_t\right)& -\cos \left({\theta }_t\right)\sin \left({\gamma }_t\right)\\ -\cos \left({\theta }_t\right)\sin \left({\psi }_t\right)& \cos \left({\psi }_t\right)\cos \left({\theta }_t\right)& \sin \left({\theta }_t\right)\\ \cos \left({\psi }_t\right)\sin \left({\gamma }_t\right)+\cos \left({\gamma }_t\right)\sin \left({\psi }_t\right)\sin \left({\theta }_t\right)& \sin \left({\gamma }_t\right)\sin \left({\psi }_t\right)-\cos \left({\gamma }_t\right)\cos \left({\psi }_t\right)\sin \left({\theta }_t\right)& \cos \left({\gamma }_t\right)\cos \left({\theta }_t\right)\end{array}\right)---\left(10\right)$>

$>\left(\begin{array}{c}{x}_g^p\\ y_g^p\\ z_g^p\end{array}\right)=R_{c\begin{array}{}\end{array}}^g\left(\begin{array}{c}{x}_c^p\\ y_c^p\\ z_c^p\end{array}\right)$>

步骤8、根据步骤7的结果和步骤6求得的机器人t时刻的经度、纬度和高度(λ^c，φ^c，h^c)及在地心地固坐标系e下的笛卡尔坐标（求取过程已在步骤6.2中描述），求取当前时刻即t时刻环境特征点p在地心地固坐标系下的大地坐标，即经度、纬度、高度(λp^，φ^p，h^p)。该步骤8包含以下步骤：

步骤8.1、根据式（11）求取环境特征点p在地心地固坐标系e下的笛卡尔坐标执行如下操作：

$>\left(\begin{array}{c}{x}_e^p\\ y_e^p\\ z_e^p\end{array}\right){=\left(\begin{array}{ccc}-\sin \left({\lambda }^c\right)& \cos \left({\lambda }^c\right)& 0\\ -\sin \left({\phi }^c\right)\cos \left({\lambda }^c\right)& -\sin \left({\phi }^c\right)\sin \left({\lambda }^c\right)& \cos \left({\phi }^c\right)\\ \cos \left({\phi }^c\right)\cos \left({\lambda }^c\right)& \cos \left({\phi }^c\right)\sin \left({\lambda }^c\right)& \sin \left({\phi }^c\right)\end{array}\right)}^{-1}\left(\begin{array}{c}{x}_g^p\\ y_g^p\\ z_g^p\end{array}\right)+\left(\begin{array}{c}{x}_e^c\\ y_e^c\\ z_e^p\end{array}\right)---\left(11\right)$>

步骤8.2、将环境特征点p在地心地固坐标系e下的笛卡尔坐标转换为经度、纬度和高度即(λ^p，φ^p，h^p)。该步操作和步骤6.3相似，该步的操作如下：

步骤8.2.1、根据式（12），求取经度：

$>{\lambda }^p=\left(\begin{array}{cc}\arctan (y_e^p/x_e^p)& x_e^p\ge 0\\ \arctan (y_e^p/x_e^p)+\pi & x_e^p<0\mathrm{and}{y}_e^p\ge 0\\ \arctan (y_e^p/x_e^p)-\pi & x_e^p<0\mathrm{and}{y}_e^p<0\end{array}\right)---\left(12\right)$>

步骤8.2.2、求取纬度：

$>r=\sqrt{{\left(x_e^p\right)}^2+{\left(y_e^p\right)}^2}$>

$>F=54{\left({\mathrm{bz}}_e^p\right)}^2$>

η＝1-e²

其中，b＝6356752.3142为地球短半轴，单位为米。e²＝0.06694为地球第一数理偏心率。

$>G=r^2+\eta {\left(z_e^p\right)}^2-e^2(a^2-b^2)$>

$>C=\frac{e^{4}F{r}^2}{G^3}$>

$>S={(1+C+\sqrt{\sqrt{C^2+2C}})}^{\frac{1}{3}}$>

$>P=\frac{F}{3{(S+\frac{1}{S}+1)}^2{G}^2}$>

$>Q=\sqrt{1+{2e}^{4}P}$>

$>\rho =\frac{{-\mathrm{Pe}}^{2}r}{1+Q}+\sqrt{\frac{a^2}{2}(1+\frac{1}{Q})-\frac{{\mathrm{P\eta }\left(z_e^c\right)}^2}{Q(1+Q)}-\frac{{\mathrm{Pr}}^2}{2}}$>

$>{\phi }^p=\arctan \left(\frac{z_e^p+\frac{e^2{\mathrm{az}}_e^p}{V}}{r}\right)$>

步骤8.2.3、根据式（13），求取高度：

步骤9、完成步骤8，跳转到步骤4。

步骤10、根据基于环境观测传感器（本实施例中为单目视觉传感器）观测值求得的机器人在当前时刻t和前一时刻t-1间的相对运动，即平移矩阵T和旋转矩阵γ，求取t时刻机器人在t-1时刻（上一时刻）c坐标系中的坐标，即基于使用机器视觉方法得到的t-1和t时刻的相对运动求t时刻c点在t-1时刻c坐标系中的坐标即因平移矩阵T反应了t-1时刻到t时刻机器人在t-1时刻c坐标系中沿三个坐标轴的增量，即(Δx，Δy，Δz)，它实际上就是本步要求的因此，本步的操作可简化为：$>y_{c(t-1)}^c=\mathrm{\Delta y},z_{c(t-1)}^c=\mathrm{\Delta z}.$>

步骤11、根据上一步的结果和前一时刻惯导传感器输出的数据，求取当前时刻机器人在上一时刻以c点为原点的局部地理坐标系中的坐标。

根据步骤10的结果和前一时刻t-1惯导传感器输出的三个角度航向角ψ_t-1、横滚角θ_t-1和俯仰角γ_t-1，根据式（14）求取当前时刻t的机器人在以上一时刻即t-1时刻的c点为原点的局部地理坐标系中的坐标即$>(x_{g(t-1)}^c,y_{g(t-1)}^c,z_{g(t-1)}^c).$>本步的操作如下:

$>R_{c(t-1)}^{g(t-1)}=\left(\begin{array}{ccc}\cos \left({\gamma }_{t-1}\right)\cos \left({\psi }_{t-1}\right)-\sin \left({\gamma }_{t-1}\right)\sin \left({\psi }_{t-1}\right)\sin \left({\theta }_{t-1}\right)& \cos \left({\gamma }_{t-1}\right)\sin \left({\psi }_{t-1}\right)+\cos \left({\psi }_{t-1}\right)\sin \left({\gamma }_{t-1}\right)\sin \left({\theta }_{t-1}\right)& -\cos \left({\theta }_{t-1}\right)\sin \left({\gamma }_{t-1}\right)\\ -\cos \left({\theta }_{t-1}\right)\sin \left({\psi }_{t-1}\right)& \cos \left({\psi }_{t-1}\right)\cos \left({\theta }_{t-1}\right)& \sin \left({\theta }_{t-1}\right)\\ \cos \left({\psi }_{t-1}\right)\sin \left({\gamma }_{t-1}\right)+\cos \left({\gamma }_{t-1}\right)\sin \left({\psi }_{t-1}\right)\sin \left({\theta }_{t-1}\right)& \sin \left({\gamma }_{t-1}\right)\sin \left({\psi }_{t-1}\right)-\cos \left({\gamma }_{t-1}\right)\cos \left({\psi }_{t-1}\right)\sin \left({\theta }_{t-1}\right)& \cos \left({\gamma }_{t-1}\right)\cos \left({\theta }_{t-1}\right)\end{array}\right)---\left(14\right)$>

$>\left(\begin{array}{c}{x}_{g(t-1)}^c\\ y_{g(t-1)}^c\\ z_{g(t-1)}^c\end{array}\right)=R_{c(t-1)}^{g(t-1)}\left(\begin{array}{c}{x}_{c(t-1)}^c\\ y_{c(t-1)}^c\\ z_{c(t-1)}^c\end{array}\right)$>

步骤12、根据步骤11的结果和前一时刻t-1的机器人（即c点）的经度、纬度和高度，即(λ^c(t-1)，φ^c(t-1)，h^c(t-1))及在地心地固坐标系下相应的笛卡尔坐标即该数据可在t-1时刻求取机器人（即c点）坐标时缓存。求取当前时刻t的机器人的经度、纬度、高度，即(λ^c(t)，φ^c(t)，h^c(t))。本步骤12包含以下步骤：

步骤12.1、根据式（15）求取t时刻c点在地心地固坐标系e下的笛卡尔坐标即该步骤的操作如下：

$>\left(\begin{array}{c}{x}_e^{c\left(t\right)}\\ y_e^{c\left(t\right)}\\ z_e^{c\left(t\right)}\end{array}\right)={\left(\begin{array}{ccc}-\sin \left({\lambda }^{c(t-1)}\right)& \cos \left({\lambda }^{c(t-1)}\right)& 0\\ -\sin \left({\phi }^{c(t-1)}\right)\cos \left({\lambda }^{c(t-1)}\right)& -\sin \left({\phi }^{c(t-1)}\right)\sin \left({\lambda }^{c(t-1)}\right)& \cos \left({\phi }^{c(t-1)}\right)\\ \cos \left({\phi }^{c(t-1)}\right)\cos \left({\lambda }^{c(t-1)}\right)& \cos \left({\phi }^{c(t-1)}\right)\sin \left({\lambda }^{c(t-1)}\right)& \sin \left({\phi }^{c(t-1)}\right)\end{array}\right)}^{-1}\left(\begin{array}{c}{x}_{g(t-1)}^c\\ y_{g(t-1)}^c\\ z_{g(t-1)}^c\end{array}\right)+\left(\begin{array}{c}{x}_e^{c(t-1)}\\ y_e^{c(t-1)}\\ z_e^{c(t-1)}\end{array}\right)---\left(15\right)$>

步骤12.2、将c点在地心地固坐标系e下的笛卡尔坐标转换为经度、纬度和高度即(λ^c(t)，φ^c(t)，h^c(t))。该步骤的操作如下：

步骤12.2.1、根据式（16），求取经度：

$>{\lambda }^{c\left(t\right)}=\left(\begin{array}{cc}\arctan (y_e^{c\left(t\right)}/x_e^{c\left(t\right)})& x_e^{c\left(t\right)}\ge 0\\ \arctan (y_e^{c\left(t\right)}/x_e^{c\left(t\right)})+\pi & x_e^{c\left(t\right)}<0\mathrm{and}{y}_e^{c\left(t\right)}\ge 0\\ \arctan (y_e^{c\left(t\right)}/x_e^{c\left(t\right)})-\pi & x_e^{c\left(t\right)}<0\mathrm{and}{y}_e^{c\left(t\right)}<0\end{array}\right)---\left(16\right)$>

步骤12.2.2、求取纬度：

$>r=\sqrt{{\left(x_e^{c\left(t\right)}\right)}^2+{\left(y_e^{c\left(t\right)}\right)}^2}$>

$>F=54{\left({\mathrm{bz}}_e^{c\left(t\right)}\right)}^2$>

η＝1-e²

其中，b＝6356752.3142为地球短半轴，单位为米。e²＝0.006694为地球第一数理偏心率。

$>G=r^2+\eta {\left(z_e^{c\left(t\right)}\right)}^2-e^2(a^2-b^2)$>

$>C=\frac{e^{4}F{r}^2}{G^3}$>

$>S={(1+C+\sqrt{\sqrt{C^2+2C}})}^{\frac{1}{3}}$>

$>P=\frac{F}{3{(S+\frac{1}{S}+1)}^2{G}^2}$>

$>Q=\sqrt{1+{2e}^{4}P}$>

$>\rho =\frac{{-\mathrm{Pe}}^{2}r}{1+Q}+\sqrt{\frac{a^2}{2}(1+\frac{1}{Q})-\frac{{\mathrm{P\eta }\left(z_e^c\right)}^2}{Q(1+Q)}-\frac{{\mathrm{Pr}}^2}{2}}$>

$>{\phi }^p=\arctan \left(\frac{z_e^p+\frac{e^2{\mathrm{az}}_e^p}{V}}{r}\right)$>

步骤12.2.3、根据式17，求取高度：

步骤13、根据基于环境观测传感器观测值求得的环境特征点p在当前时刻t时刻c坐标系中的坐标即和当前时刻惯导传感器输出的三个角度航向角ψ_t、横滚角θ_t和俯仰角γ_t，根据式（18）求取当前时刻环境特征点p在当前时刻t以c点为中心的局部地理坐标系g中的坐标即则本步的具体操作为：

$>R_c^g=\left(\begin{array}{ccc}\cos \left({\gamma }_t\right)\cos \left({\psi }_t\right)-\sin \left({\gamma }_t\right)\sin \left({\psi }_t\right)\sin \left({\theta }_t\right)& \cos \left({\gamma }_t\right)\sin \left({\psi }_t\right)+\cos \left({\psi }_t\right)\sin \left({\gamma }_t\right)\sin \left({\theta }_t\right)& -\cos \left({\theta }_t\right)\sin \left({\gamma }_t\right)\\ -\cos \left({\theta }_t\right)\sin \left({\psi }_t\right)& \cos \left({\psi }_t\right)\cos \left({\theta }_t\right)& \sin \left({\theta }_t\right)\\ \cos \left({\psi }_t\right)\sin \left({\gamma }_t\right)+\cos \left({\gamma }_t\right)\sin \left({\psi }_t\right)\sin \left({\theta }_t\right)& \sin \left({\gamma }_t\right)\sin \left({\psi }_t\right)-\cos \left({\gamma }_t\right)\cos \left({\psi }_t\right)\sin \left({\theta }_t\right)& \cos \left({\gamma }_t\right)\cos \left({\theta }_t\right)\end{array}\right)---\left(18\right)$>

$>\left(\begin{array}{c}{x}_g^p\\ y_g^p\\ z_g^p\end{array}\right)=R_c^g\left(\begin{array}{c}{x}_c^p\\ y_c^p\\ z_c^p\end{array}\right)$>

步骤14、根据上一步的结果，求取当前时刻环境特征点在地心地固坐标系下的大地坐标即，经度、纬度、高度。

根据上一步求得的当前时刻t的机器人的经度、纬度、高度，即(λ^c(t)，φ^c(t)，h^c(t))，及t时刻c点在地心地固坐标系e下的笛卡尔坐标即求取当前时刻环境特征点p在地心地固坐标系e下的大地坐标即，经度、纬度、高度(λ^p，φ^p，h^p)。该步骤14包含以下步骤：

步骤14.1、根据式（19）求取环境特征点p在地心地固坐标系e下的笛卡尔座坐标执行如下操作：

$>\left(\begin{array}{c}{x}_e^p\\ y_e^p\\ z_e^p\end{array}\right)={\left(\begin{array}{ccc}-\sin \left({\lambda }^c\right)& \cos \left({\lambda }^c\right)& 0\\ -\sin \left({\phi }^{c\left(t\right)}\right)\cos \left({\lambda }^{c\left(t\right)}\right)& -\sin \left({\phi }^{c\left(t\right)}\right)\sin \left({\lambda }^{c\left(t\right)}\right)& \cos \left({\phi }^{c\left(t\right)}\right)\\ \cos \left({\phi }^{c\left(t\right)}\right)\cos \left({\lambda }^{c\left(t\right)}\right)& \cos \left({\phi }^{c\left(t\right)}\right)\sin \left({\lambda }^{c\left(t\right)}\right)& \sin \left({\phi }^{c\left(t\right)}\right)\end{array}\right)}^{-1}\left(\begin{array}{c}{x}_g^p\\ y_g^p\\ z_g^p\end{array}\right)+\left(\begin{array}{c}{x}_e^{c\left(t\right)}\\ y_e^{c\left(t\right)}\\ z_e^{c\left(t\right)}\end{array}\right)---\left(19\right)$>

步骤14.2、将环境特征点p在地心地固坐标系e下的笛卡尔坐标转换为经度、纬度和高度即(λ^p，φ^p，h^p)。该步操作和步骤6.3相似，该步的操作如下：

步骤14.2.1、根据式（20）求取经度：

$>{\lambda }^p=\left(\begin{array}{cc}\arctan (y_e^p/x_e^p)& x_e^p\ge 0\\ \arctan (y_e^p/x_e^p)+\pi & x_e^p<0\mathrm{and}{y}_e^p\ge 0\\ \arctan (y_e^p/x_e^p)-\pi & x_e^p<0\mathrm{and}{y}_e^p<0\end{array}\right)---\left(20\right)$>

步骤14.2.2、求取纬度：

$>r=\sqrt{{\left(x_e^c\right)}^2+{\left(y_e^c\right)}^2}$>

$>F=54{\left({\mathrm{bz}}_e^c\right)}^2$>

η＝1-e²

其中，b＝6356752.3142为地球短半轴，单位为米。e²＝0.006694为地球第一数理偏心率。

$>G=r^2+\eta {\left(z_e^c\right)}^2-e^2(a^2-b^2)$>

$>C=\frac{e^{4}F{r}^2}{G^3}$>

$>S={(1+C+\sqrt{\sqrt{C^2+2C}})}^{\frac{1}{3}}$>

$>P=\frac{F}{3{(S+\frac{1}{S}+1)}^2{G}^2}$>

$>Q=\sqrt{1+{2e}^{4}P}$>

$>\rho =\frac{{-\mathrm{Pe}}^{2}r}{1+Q}+\sqrt{\frac{a^2}{2}(1+\frac{1}{Q})-\frac{{\mathrm{P\eta }\left(z_e^c\right)}^2}{Q(1+Q)}-\frac{{\mathrm{Pr}}^2}{2}}$>

$>{\phi }^p=\arctan \left(\frac{z_e^p+\frac{e^2{\mathrm{az}}_e^p}{V}}{r}\right)$>

步骤14.2.3、根据式（21）求取高度：

步骤15、完成步骤14后，绝对坐标获取方法完成，并跳转到步骤4。

尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。