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

摘要

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

主权项

一种用于移动机器人定位与环境重构的绝对坐标获取方法，其特征在于，该方法包含以下步骤：步骤1、传感器装配，将惯性传感组件与环境观测传感器捆绑固连并调整角度，确保环境观测传感器中心坐标系、惯导载体坐标系重合，并将重合的坐标系定义为机器人中心坐标系，统称c坐标系；卫星导航接收机天线可放置在机器人顶部的任一位置；步骤2、确定机器人平台上的虚拟坐标系v，虚拟坐标系的原点取在卫星导航接收机天线的中心点，其坐标轴方向与c坐标系相同，实现环境观测传感器与卫星导航接收机天线之间空间误差的校准；步骤3、通过卫星导航系统在两个位置的标定和坐标系的投影，获取c坐标系原点，c点在虚拟坐标系中的坐标，该坐标将作为后续步骤中补偿卫星导航接收机天线装配位置和环境观测传感器、惯性导航传感器及机器人等效点不在同一点所引起的误差；步骤4、机器人开始运行，在运行中的当前时刻判断卫星导航接收机是否有可靠的定位数据输出，若是，卫星导航接收机有可靠的数据输出，则跳转到步骤5；若否，卫星导航接收机没有可靠的数据输出，则跳转到步骤10；步骤5、根据当前时刻惯导传感器输出的三个角度航向角ψ_t、横滚角θ_t和俯仰角γ_t和c点在虚拟坐标系v中的坐标，通过式(5)求取当前时刻机器人在以卫星导航接收机天线中心点为原点的局部地理坐标系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({\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(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]$局部地理坐标系g的原点任意；步骤6、根据当前时刻的卫星导航接收机输出的经度、纬度和高度数据，即(λ^go，φ^go，h^go)和步骤5中求得的c点在以卫星导航接收机天线中心点为原点的局部地理坐标系g中的坐标即求取当前时刻机器人在地心地固坐标系e下的大地坐标即经度、纬度和高度，即(λ^c，φ^c，h^c)；步骤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({\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(10\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]$步骤8、根据步骤7的结果和步骤6求得的机器人的经度、纬度和高度(λ^c，φ^c，h^c)及在地心地固坐标系e下的笛卡尔坐标求取当前时刻环境特征点p在地心地固坐标系下的大地坐标，即经度、纬度、高度(λ^p，φ^p，h^p)；步骤9、完成步骤8，跳转到步骤4；步骤10、根据基于环境观测传感器观测值求得的机器人在当前时刻t和前一时刻t‑1间的相对运动，即平移矩阵T和旋转矩阵Υ，求取t时刻机器人在t‑1时刻c坐标系中的坐标即步骤11、根据步骤10的结果和前一时刻t‑1惯导传感器输出的三个角度航向角ψ_t‑1、横滚角θ_t‑1和俯仰角γ_t‑1，根据式(14)求取当前时刻t机器人在上一时刻以c点为原点的局部地理坐标系中的坐标即$R_{c\left(t-1\right)}^{g\left(t-1\right)}=\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\left(t-1\right)}^c\\ y_{g\left(t-1\right)}^c\\ z_{g\left(t-1\right)}^c\end{array}\right]=R_{c\left(t-1\right)}^{g\left(t-1\right)}\left[\begin{array}{c}{x}_{c\left(t-1\right)}^c\\ y_{c\left(t-1\right)}^c\\ z_{c\left(t-1\right)}^c\end{array}\right]$步骤12、根据步骤11的结果和前一时刻t‑1的机器人的经度、纬度和高度，即(λ^c(t‑1)，φ^c(t‑1)，h^c(t‑1))及在地心地固坐标系下相应的笛卡尔坐标即求取当前时刻t的机器人的经度、纬度、高度，即(λ^c(t)，φ^c(t)，h^c(t))；步骤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、根据步骤12求得的当前时刻t的机器人的经度、纬度、高度，即(λ^c(t)，φ^c(t)，h^c(t))，及t时刻c点在地心地固坐标系e下的笛卡尔坐标即求取当前时刻环境特征点p在地心地固坐标系e下的大地坐标即，经度、纬度、高度(λ^p，φ^p，h^p)；步骤15、完成步骤14后，绝对坐标获取方法完成，并跳转到步骤4。