CN116659493B - 一种车载自主定位定向方法 - Google Patents

一种车载自主定位定向方法 Download PDF

Info

Publication number
CN116659493B
CN116659493B CN202310945431.4A CN202310945431A CN116659493B CN 116659493 B CN116659493 B CN 116659493B CN 202310945431 A CN202310945431 A CN 202310945431A CN 116659493 B CN116659493 B CN 116659493B
Authority
CN
China
Prior art keywords
coordinate system
coordinate
axis
coordinates
vehicle body
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202310945431.4A
Other languages
English (en)
Other versions
CN116659493A (zh
Inventor
刘广泉
李国庆
谭春燕
白明杰
庞连海
董树鹏
齐彩利
安林波
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tianjin 764 Communication and Navigation Technology Corp
Original Assignee
Tianjin 764 Communication and Navigation Technology Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tianjin 764 Communication and Navigation Technology Corp filed Critical Tianjin 764 Communication and Navigation Technology Corp
Priority to CN202310945431.4A priority Critical patent/CN116659493B/zh
Publication of CN116659493A publication Critical patent/CN116659493A/zh
Application granted granted Critical
Publication of CN116659493B publication Critical patent/CN116659493B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C15/00—Surveying instruments or accessories not provided for in groups G01C1/00 - G01C13/00
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

本发明提供一种车载自主定位定向方法，属于坐标测量技术领域，包括：构建车体坐标系并选定待测点，响应于第一模式的信号，进行以下步骤：构建本地坐标系与全站仪坐标系；移动测量车辆，使本地坐标系、车体坐标系的坐标原点与待测点重合；计算待测点在大地坐标系下的第一坐标；选取参考点，测量并计算参考点在本地坐标系下的第三坐标；根据第一坐标、第三坐标与待测点在本地坐标系下坐标，计算参考点在大地坐标系下第四坐标，进而计算待测点与参考点之间的第三方位角。上述方式能够在不借助卫星定位的情况同时兼具高精度定位、定向功能。同时，测量装置设置在测量车辆上，大大提高了测量的灵活性，减小了地形对测量设备的限制。

Description

一种车载自主定位定向方法
技术领域
本发明一般涉及坐标测量技术领域，具体涉及一种车载自主定位定向方法。
背景技术
针对未知坐标的待测点进行坐标测量的过程中，现有技术均是通过卫星定位的方式确定待测点的经纬度。
现有的卫星定位系统，如GPS（Global Positioning System），可以通过测量接收器与多颗卫星之间的信号传播时间来确定接收器的位置。这里假设接收器位置与待测点位置相同。具体来说，卫星定位系统通过接收来自至少三颗卫星的信号，并计算信号传播时间来确定接收器与每颗卫星之间的距离。通过将这些距离与卫星的已知位置进行比较，可以计算出接收器的位置。此种方式的定位精度较高，但是定向精度不足。
但是卫星定位的方式十分依赖卫星与地面之间的通讯，当遭遇恶劣天气时，可能会导致信号衰减、传输延迟或中断。特殊的天气现象会干扰信号的传播，使得信号的质量下降或无法正常传输，难以完成高精度定位的任务。
现有卫星的定向精度难以满足要求。同时，现有的其他设备能够满足定向精度，但是定位精度不足，还会受到地形的影响。当地形不平坦时，现有的测量装置难以校准并完成测量。因而现有技术中缺少不借助卫星且兼具高精度定位、定向的方式。
发明内容
鉴于现有技术中的上述缺陷或不足，期望提供一种车载自主定位定向方法。
本发明提供一种车载自主定位定向方法，其中：
全站仪安装在惯导装置上，所述惯导装置安装在测量车辆上；所述测量车辆上具有标志点；所述全站仪包括目镜；所述全站仪用于测量待测点在全站仪坐标系下的坐标；
所述车载自主定位定向方法包括：
构建车体坐标系与全站仪坐标系；所述车体坐标系以所述标志点沿竖直方向在地表所在平面的投影点为坐标原点；所述全站仪坐标系以所述目镜的镜头中心为坐标原点；
选定坐标未知的待测点；
响应于所述测量车辆第一模式的信号，进行以下步骤：
构建本地坐标系；所述本地坐标系以车体坐标系的坐标原点为坐标原点，竖直方向为本地坐标系Z轴，正北方向为本地坐标系Y轴，正东方向为本地坐标系X轴；将当前位置构建的本地坐标系坐标原点作为初始位置；
获取初始位置本地坐标系坐标原点在大地坐标系下的坐标，得到初始坐标；
移动所述测量车辆，使本地坐标系坐标原点、车体坐标系坐标原点与待测点均重合；本地坐标系坐标原点随所述测量车辆平移；
根据所述初始坐标，计算得到待测点在大地坐标系下的坐标，得到第一坐标；
选取待测点处可见且坐标未知的参考点；
测量所述参考点在全站仪坐标系下的坐标，得到第二坐标；
根据所述第二坐标，将所述参考点在全站仪坐标系下的第二坐标变换为所述参考点在车体坐标系下的坐标；
将参考点在车体坐标系下的坐标变换至本地坐标系下的坐标，得到第三坐标；
根据所述第一坐标、所述第三坐标与待测点在本地坐标系下坐标，计算所述参考点在大地坐标系下坐标，得到第四坐标；根据第一坐标与第四坐标，计算待测点与参考点之间的第三方位角。
根据本发明提供的技术方案，响应于所述测量车辆第二模式的信号，进行以下步骤：
测量车体坐标系坐标原点在大地坐标系下的坐标，得到第六坐标；
测量所述待测点在全站仪坐标系下的坐标，得到第七坐标；
根据所述第七坐标，将所述待测点在全站仪坐标系下的坐标变换至车体坐标系下的坐标，得到第八坐标；
根据所述第六坐标、第八坐标与车体坐标系原点在车体坐标系下坐标，将所述待测点在车体坐标系下的第八坐标变换为大地坐标系下的坐标，得到第九坐标；
根据待测点在大地坐标系下的第九坐标与车体坐标系坐标原点在大地坐标系下的第六坐标，计算得到待测点相对车体坐标系坐标原点的第二方位角。
根据本发明提供的技术方案，根据所述初始坐标，计算得到待测点在大地坐标系下的坐标，得到第一坐标的步骤包括：
调整测量车辆的位姿使惯导装置处于大地坐标系下坐标已知位置点；
获取坐标已知位置点在大地坐标系下的坐标，得到初始坐标；
将所述初始坐标输入所述惯导装置；
再次调整测量车辆的位姿，使所述车体坐标系与所述本地坐标系的坐标原点重合且坐标轴分别重合；
惯导装置计算得到本地坐标系坐标原点在大地坐标系下的坐标，得到所述第一坐标。
根据本发明提供的技术方案，测量所述参考点在全站仪坐标系下的坐标，得到第二坐标的步骤包括：
测量所述参考点相对于全站仪的第一高低角与第一方位角；
测量所述参考点与全站仪之间的距离，得到第一距离；
根据所述第一距离、第一高低角与第一方位角计算所述参考点在全站仪坐标系下的坐标，得到第二坐标。
根据本发明提供的技术方案，根据所述第二坐标，将所述参考点在全站仪坐标系下的第二坐标变换为所述参考点在车体坐标系下的坐标的步骤包括：
将所述第二坐标输入初始设置后的修正模型，计算得到所述参考点在车体坐标系下的坐标；所述修正模型用于输入全站仪坐标系下坐标，计算并输出车体坐标系下的坐标。
根据本发明提供的技术方案，初始设置所述修正模型的过程步骤包括：
获取多组样本数据，所述样本数据包括：标定点在全站仪坐标系下的坐标与标定点在本地坐标系下的坐标；
获取初始修正模型；所述初始修正模型包含未知的初始参数矩阵，所述初始修正模型用于表征全站仪坐标系下的坐标转化为车体坐标系下的坐标的变换关系；
对所述初始修正模型进行等价变换，将全站仪坐标系下坐标变换为输入矩阵，初始参数矩阵中未知的参数变换为换算矩阵，车体坐标系下的坐标变换为输出矩阵，使输出矩阵等于输入矩阵乘换算矩阵，得到第四坐标变换公式；所述第四坐标变换公式的输入为全站仪坐标系下的坐标，代入所述输入矩阵；输出为车体坐标系下的坐标；
所述第四坐标变换公式由公式（二）表示；
公式（二）；
其中，为输出矩阵，X3、Y3、Z3分别表示车体坐标系下沿车体坐标系X轴、车体坐标系Y轴与车体坐标系Z轴的坐标；/>为输入矩阵，X1、Y1、Z1分别表示全站仪坐标系下沿全站仪坐标系X轴、全站仪坐标系Y轴与全站仪坐标系Z轴的坐标；/>为换算矩阵，表示输入矩阵与输出矩阵之间的换算关系;换算矩阵中a11、a12、a13、b11、b12、b13、c11、c12、c13、ΔX、ΔY与ΔZ均表示未知的参数；
输入多个所述样本数据至所述第四坐标变换公式中，对所述换算矩阵进行拟合，计算得到参数已知换算矩阵；
将参数已知换算矩阵代入所述第四坐标变换公式，得到所述修正模型。
根据本发明提供的技术方案，所述惯导装置包括：Z轴加速度计、Y轴加速度计与X轴加速度计，三者的正方向两两相互垂直设置；
获取初始修正模型的步骤包括：
构建惯导坐标系；所述惯导坐标系以所述惯导装置的质心为坐标原点，所述Z轴加速度计的正方向为惯导坐标系Z轴，所述Y轴加速度计的正方向为惯导坐标系Y轴，所述X轴加速度计的正方向为惯导坐标系X轴;
根据所述全站仪坐标系与所述惯导坐标系构建第一坐标变换公式；所述第一坐标变换公式中包含有未知参数：所述全站仪坐标系与所述惯导坐标系之间的第一欧拉角与第一平移矢量；所述第一欧拉角包括：惯导坐标系Z轴与全站仪坐标系Z轴之间夹角、惯导坐标系Y轴与全站仪坐标系Y轴之间夹角、惯导坐标系X轴与全站仪坐标系X轴之间夹角；所述第一平移矢量为由全站仪坐标系的坐标原点到惯导坐标系坐标原点，在全站仪坐标系下的矢量；
所述第一坐标变换公式如公式（三）所示；
公式（三）；
其中，表示惯导坐标系下的坐标，/>表示全站仪坐标系下的坐标；
表示惯导坐标系Z轴与全站仪坐标系Z轴之间夹角，/>表示惯导坐标系X轴与全站仪坐标系X轴之间夹角，/>表示惯导坐标系Y轴与全站仪坐标系Y轴之间夹角；
表示代入/>后的第一变换矩阵，/>表示代入/>后的第二变换矩阵，表示代入/>后的第三变换矩阵；
表示第一平移矢量；
根据所述车体坐标系与所述惯导坐标系构建第二坐标变换公式；所述第二坐标变换公式中包含有未知参数：所述惯导坐标系与所述车体坐标系之间的第二欧拉角与第二平移矢量；所述第二欧拉角包括：惯导坐标系Z轴与车体坐标系Z轴之间夹角、惯导坐标系Y轴与车体坐标系Y轴之间夹角、惯导坐标系X轴与车体坐标系X轴之间夹角；所述第二平移矢量为惯导坐标系的坐标原点到车体坐标系坐标原点，在惯导坐标系下的矢量；
所述第二坐标变换公式如公式（四）所示；
公式（四）；
其中，表示车体坐标系下的坐标，/>表示惯导坐标系下的坐标；
表示惯导坐标系Z轴与车体坐标系Z轴之间夹角，/>表示惯导坐标系X轴与车体坐标系X轴之间夹角，/>表示惯导坐标系Y轴与车体坐标系Y轴之间夹角；
表示代入/>后的第一变换矩阵，/>表示代入/>后的第二变换矩阵，表示代入/>后的第三变换矩阵；
表示第二平移矢量；
根据所述第一坐标变换公式与第二坐标变换公式计算得到初始修正模型。
根据本发明提供的技术方案，根据所述全站仪坐标系、所述惯导坐标系与所述车体坐标系构建第一坐标变换公式与第二坐标变换公式的步骤包括：
获取坐标变换公式；所述坐标变换公式包括：第一变换矩阵、第二变换矩阵、第三变换矩阵与第一矢量矩阵；
所述坐标变换公式由公式（五）表示；
公式（五）；
其中，表示变换后的坐标，/>表示变换前的坐标；
表示第一变换矩阵，/>表示第二变换矩阵，/>表示第三变换矩阵；α、β、γ分别表示需要代入的欧拉角；
表示需要代入的平移矢量；
获取将全站仪坐标系Z轴、全站仪坐标系Y轴与全站仪坐标系X轴旋转至惯导坐标系Z轴、惯导坐标系Y轴与惯导坐标系X轴的旋转顺序，得到第一旋转顺序；
获取将惯导坐标系Z轴、惯导坐标系Y轴与惯导坐标系X轴旋转至车体坐标系Z轴、车体坐标系Y轴与车体坐标系X轴的旋转顺序，得到第二旋转顺序；
以所述第一旋转顺序，将所述第一欧拉角依次代入所述第一变换矩阵、第二变换矩阵与第三变换矩阵，并将所述第一平移矢量代入所述第一矢量矩阵，得到第一坐标变换公式；
以所述第二旋转顺序，将所述第二欧拉角依次代入所述第一变换矩阵、第二变换矩阵与第三变换矩阵，并将所述第二平移矢量代入所述第一矢量矩阵，得到第二坐标变换公式。
根据本发明提供的技术方案，根据所述第一坐标变换公式与第二坐标变换公式计算得到初始修正模型的步骤包括：
获取第一坐标变换公式与第二坐标变换公式；所述第一坐标变换公式的输入为全站仪坐标系下的坐标，输出为惯导坐标系下的坐标；所述第二坐标变换公式的输入为惯导坐标系下的坐标，输出为车体坐标系下的坐标；
将第一坐标变换公式的输出作为第二坐标变换公式的输入，使所述第一坐标变换公式代入所述第二坐标变换公式，得到第三坐标变换公式；
对所述第三坐标变换公式进行化简，并将所述第三坐标变换公式中所述全站仪坐标系下的坐标与所述车体坐标系下的坐标之间的矩阵分别替换为初始参数矩阵和初始矢量矩阵，得到所述初始修正模型；
所述第三坐标变换公式由公式（六）表示；
公式（六）。
根据本发明提供的技术方案，将参考点在车体坐标系下的坐标变换至本地坐标系下的坐标，得到第三坐标的步骤包括：
测量车体坐标系与本地坐标系坐标轴之间的偏差，得到第三欧拉角与第三平移矢量；所述第三欧拉角包括：本地坐标系Z轴与车体坐标系Z轴之间夹角、本地坐标系Y轴与车体坐标系Y轴之间夹角、本地坐标系X轴与车体坐标系X轴之间夹角；所述第三平移矢量为本地坐标系的坐标原点到车体坐标系坐标原点，在本地坐标系下的矢量；
获取坐标变换公式；所述坐标变换公式包括：第一变换矩阵、第二变换矩阵、第三变换矩阵与第一矢量矩阵；
获取将车体坐标系Z轴、车体坐标系Y轴与车体坐标系X轴旋转至本地坐标系Z轴、本地坐标系Y轴与本地坐标系X轴的旋转顺序，得到第三旋转顺序；
以所述第三旋转顺序，将所述第三欧拉角依次代入所述第一变换矩阵、第二变换矩阵与第三变换矩阵，并将所述第三平移矢量代入所述第一矢量矩阵，得到第五坐标变换公式；
将所述第三坐标代入所述第五坐标变换公式，计算得到第三坐标。
本发明的有益效果在于：
构建车体坐标系并选定待测点，切换测量车辆的测量模式。当测量车辆处于第一模式时：构建本地坐标系与全站仪坐标系；移动测量车辆，使本地坐标、车体坐标系的坐标原点与待测点重合；计算待测点在大地坐标系下的第一坐标；选取待测点处可见且坐标未知的参考点，测量并计算参考点在本地坐标系下的第三坐标；根据第一坐标、第三坐标与待测点在本地坐标系下坐标，计算参考点在大地坐标系下第四坐标，进而计算待测点与参考点之间的第三方位角。上述方式能够在不借助卫星定位的情况同时兼具高精度定位、定向功能。同时，测量装置设置在测量车辆上，大大提高了测量的灵活性，减小了地形对测量设备的限制。
附图说明
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为本发明提供的一种车载自主定位定向方法的流程示意图；
图2为全站仪与测量车辆的安装结构示意图；
图3为第一模式时测量车辆与全站仪坐标系的示意图；
图4为第一距离与全站仪坐标系几何关系示意图；
图5为第二模式时测量车辆与本地坐标系的示意图；
其中：1、全站仪；2、惯导装置；3、测量车辆；4、标志点；5、目镜；6、待测点；7、参考点。
具体实施方式
下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与发明相关的部分。
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。
请参考图1-图2，全站仪1固定安装在惯导装置2上，所述惯导装置2安装在测量车辆3上；所述测量车辆3上具有标志点4；所述全站仪1包括目镜5、具有角度刻度的水平转盘与具有角度刻度的垂直转盘；所述全站仪1用于测量待测点在全站仪坐标系下的坐标；
所述车载自主定位定向方法包括：
S1:构建车体坐标系与全站仪坐标系；所述车体坐标系以所述标志点4沿竖直方向在地表所在平面的投影点为坐标原点；所述全站仪坐标系以所述目镜5的镜头中心为坐标原点；
S2:选定坐标未知的待测点6；
S3:根据待测点6切换所述测量车辆3的测量模式；所述测量模式包括第一模式与第二模式；当处于第一模式时，所述测量车辆3能够抵达待测点6；当处于第二模式时，所述测量车辆3不能抵达待测点6；
S4:响应于所述测量车辆3第一模式的信号，进行以下步骤：
S11:构建本地坐标系；所述本地坐标系以车体坐标系的坐标原点为坐标原点，竖直方向为本地坐标系Z轴，正北方向为本地坐标系Y轴，正东方向为本地坐标系X轴；将当前位置构建的本地坐标系坐标原点作为初始位置；
S12:获取初始位置本地坐标系坐标原点在大地坐标系下的坐标，得到初始坐标；
S13:移动所述测量车辆3，使本地坐标系坐标原点、车体坐标系坐标原点与待测点6均重合；本地坐标系坐标原点随所述测量车辆3平移；
S14:根据所述初始坐标，计算得到待测点6在大地坐标系下的坐标，得到第一坐标；
S15:选取待测点6处可见且坐标未知的参考点7；
S16:测量所述参考点7在全站仪坐标系下的坐标，得到第二坐标；
S17:根据所述第二坐标，将所述参考点7在全站仪坐标系下的第二坐标变换为所述参考点7在车体坐标系下的坐标；
S18:将参考点7在车体坐标系下的坐标变换至本地坐标系下的坐标，得到第三坐标；
S19:根据所述第一坐标、所述第三坐标与待测点6在本地坐标系下坐标，计算所述参考点7在大地坐标系下坐标，得到第四坐标；根据第一坐标与第四坐标，计算待测点6与参考点7之间的第三方位角。
具体地，一个点在大地坐标系下的坐标表示为经度、纬度与海拔。大地坐标系以本初子午线为0度经线，地球赤道为0度纬线，海平面为0海拔。
在一些实施方式中，所述全站仪1包括目镜5、具有角度刻度的水平转盘与具有角度刻度的垂直转盘。测量车辆3上搭载有车载计算机。
所述全站仪坐标系以所述目镜5的镜头中心为坐标原点，所述水平转盘的中轴线的平行线为全站仪坐标系Z轴，所述水平转盘的零刻度方向为全站仪坐标系Y轴，垂直于所述全站仪坐标系Z轴且垂直于所述全站仪坐标系Y轴的方向为全站仪坐标系X轴。
所述车体坐标系以所述标志点4沿竖直方向在地表所在平面的投影点为坐标原点，垂直于所述测量车辆3车顶所在平面的方向为车体坐标系Z轴，所述测量车辆3的前进方向为车体坐标系Y轴，平行于所述测量车辆3后车轮轴线的方向为车体坐标系X轴。
在一些实施方式中，待测点6由操作人员选定，并输入车载计算机；同时通过操作人员手动选择测量车辆3的测量模式。
在一些实施方式中，将参考点7在车体坐标系下的坐标变换至本地坐标系下的坐标，得到第三坐标前，可以先调整所述测量车辆3的位姿，使所述车体坐标系与所述本地坐标系的坐标原点重合且坐标轴分别重合。这种方式能够使车体坐标系与本地坐标系下的坐标相同，因而使车体坐标系与本地坐标系下的坐标变换至大地坐标系下的坐标也相同，简化了变换过程。
具体地，可以通过惯导装置2测量得到所述第一坐标，或手动将第一坐标输入测量车辆3的车载计算机中。本实施例中，通过惯导装置2测量得到第一坐标。
具体地，若身处未知区域仅知道当前位置坐标，未知正北方向，在未知区域移动一段时间后，难以判断自身的行进方向与位置坐标，依旧会迷路。
因此，根据待测点6在大地坐标系下坐标与参考点7在大地坐标系下的坐标计算得到第三方位角。第三方位角能够方便辨别出未知区域的正北方向，能够根据正北方向与自身位置坐标判断移动路线，避免迷路。
初始位置在大地坐标系下的坐标可以通过现有的各种手段获取。本发明的方案为利用卫星定位获取初始位置在大地坐标系下的坐标。
进而能够利用惯导装置2测量初始位置在惯导坐标系下的坐标；而后在测量车辆3移动至待测点6后，再次利用惯导装置2测量初始位置相对于惯导坐标系的坐标；最终根据上述坐标，结合惯导坐标系与大地坐标系之间的欧拉角与平移矢量计算得到待测点6在大地坐标系下的坐标。
本发明的技术方案则不在需要卫星定位，即可实现计算得到所达位置在大地坐标系下的坐标。
现有的定向装置中以陀螺仪为例，陀螺仪的机动性差，若测量地点地形复杂、不够平坦则难以选出合适的加设地点且移动受限（即待测点6不可抵达的情况），进而难以完成定向任务。
进一步地，参考图5，响应于所述测量车辆3第二模式的信号，进行以下步骤：
S21:测量车体坐标系坐标原点在大地坐标系下的坐标，得到第六坐标；
S22:测量所述待测点6在全站仪坐标系下的坐标，得到第七坐标；
S23:根据所述第七坐标，将所述待测点6在全站仪坐标系下的坐标变换至车体坐标系下的坐标，得到第八坐标；
S24:根据所述第六坐标、第八坐标与车体坐标系原点在车体坐标系下坐标，将所述待测点6在车体坐标系下的第八坐标变换为大地坐标系下的坐标，得到第九坐标；
S25:根据待测点6在大地坐标系下的第九坐标与车体坐标系坐标原点在大地坐标系下的第六坐标，计算得到待测点6相对车体坐标系坐标原点的第二方位角。
在一些实施方式中，一些待测点6所处位置特殊，测量车辆3难以到达，或无法使车体坐标系与本地坐标系的坐标原点重合，则需要切换测量模式至第二模式。测量车体坐标系坐标原点在大地坐标系下的坐标前，需要移动测量车辆3至待测点6周围，使待测点6进入全站仪1的测量范围，方便利用全站仪1测量全站仪坐标系下的坐标。
在一些实施方式中，第二模式下同样使用惯导装置2以同样的方法测量得到第六坐标；使用全站仪1测量待测点6在全站仪坐标系下的坐标，得到第七坐标。
本实施例的技术方案中，设置两种测量模式，使测量车辆3能够带动惯导装置2与全站仪1随意移动，大大提高了测量装置的机动性；同时两种模式可以根据实际的测量情况自由切换，提高了地形的适应性。
在一些实施方式中，将所述待测点6在车体坐标系下的第八坐标变换为大地坐标系下的坐标，得到第九坐标的过程与原理包括：
由于车体坐标系坐标原点与惯导装置2之间的安装距离固定且可以通过直接测量的方式得知；因此可以得知惯导装置2在车体坐标系下的坐标。
根据前文描述惯导装置2定位的方式，先将惯导装置2设置在大地坐标系下坐标已知点；并驾驶测量车辆3，行驶至待测点6附近。
此时惯导装置2在大地坐标系下的坐标可以通过惯导装置2计算得到，再根据惯导装置2在车体坐标系下的坐标，计算得到车体坐标系坐标原点在大地坐标系下的坐标。
根据待测点6在大地坐标系下的第九坐标与车体坐标系坐标原点在大地坐标系下的第六坐标，计算得到待测点6相对车体坐标系坐标原点的第二方位角。第二方位角能够方便分辨待测点6相对车体坐标系的方位，同时辨别出未知区域的正北方向，能够根据正北方向与自身位置坐标判断移动路线，避免迷路。
图5中，X轴、Y轴、Z轴分别为本地坐标系的X轴、Y轴、Z轴。
本地坐标系原点与待测点6连线为图中虚线L，虚线L在本地坐标系XOY平面的投影线为图中虚线L’；
第二方位角α2表示为本地坐标系Y轴与虚线L’之间夹角；
第二高低角β2表示为虚线L与虚线L’之间夹角。
进一步地，根据所述初始坐标，计算得到待测点6在大地坐标系下的坐标，得到第一坐标的步骤包括：
调整测量车辆3的位姿使惯导装置2处于大地坐标系下坐标已知位置点；
获取坐标已知位置点在大地坐标系下的坐标，得到初始坐标；
将所述初始坐标输入所述惯导装置2；
再次调整测量车辆3的位姿，使所述车体坐标系与所述本地坐标系的坐标原点重合且坐标轴分别重合；
惯导装置2计算得到本地坐标系坐标原点在大地坐标系下的坐标，得到所述第一坐标。
在一些实施方式中，使用惯导装置2测量本地坐标系坐标原点在大地坐标系下的坐标。
具体地，惯导装置2中设置有惯性导航系统（Inertial Navigation System，简称INS）是一种通过测量和集成加速度计和陀螺仪的数据来确定位置、速度和方向的导航系统。在INS中，加速度计用于测量加速度，陀螺仪用于测量角速度。
测量坐标的过程通常包括以下步骤：
1、初始化：在开始导航之前，需要对INS进行初始化。通常包括将INS放置在已知的位置，并记录初始的位置、速度和方向。
2、加速度计测量：加速度计用于测量物体的加速度。通过积分加速度计的输出，可以得到速度和位移的估计值。
3、陀螺仪测量：陀螺仪用于测量物体的角速度。通过积分陀螺仪的输出，可以得到物体的方向估计值。
4、数据融合：将加速度计和陀螺仪的测量数据进行融合，可以得到更准确的位置、速度和方向估计值。常用的方法包括卡尔曼滤波和扩展卡尔曼滤波。
INS是一种自主导航系统，其测量结果会随着时间的推移而累积误差。因此，在本实施例中为了提高精度，使用每5分钟校零或每10分钟校零的方式对惯性导航系统进行校正，以减小误差。
在一些实施方式中，所述参考点7选择为全站仪1方向测量的位置点。
由于本地坐标系的坐标轴分别与大地坐标系的经线平行、纬线平行、与海平面垂直。本地坐标系下的坐标只需通过平移变换即可变换为大地坐标系下的坐标。
例如，北京市的经纬度为39.9042°N，116.4074°E，海拔43.5米。北京市的本地坐标系则以39.9042°N，116.4074°E，海拔43.5米所处的点为坐标原点。
假设北京市内某一点相对于北京市的本地坐标系原点的坐标为：（3,4，0），单位：千米。可以知道，该点相对于北京市的本地坐标系坐标原点的坐标差可以理解为：该点相对于北京市的本地坐标系坐标原点沿东西方向向东偏移3千米，沿南北方向向北偏移4千米，海拔相同。
进而可以根据地球的半径将偏移的距离换算为经纬度之间的偏差，再将经纬度之间的偏差分别与39.9042°N，116.4074°E叠加；海拔上的偏差则直接叠加。最终计算得到该点在大地坐标系下的经度、纬度和海拔。
进一步地，测量所述参考点7在全站仪坐标系下的坐标，得到第二坐标的步骤包括：
测量所述参考点7相对于全站仪1的第一高低角与第一方位角；
测量所述参考点7与全站仪1之间的距离，得到第一距离；
根据所述第一距离、第一高低角与第一方位角计算所述参考点7在全站仪坐标系下的坐标，得到第二坐标。
在一些实施方式中，参考图3-图4，X轴、Y轴、Z轴分别为全站仪坐标系的X轴、Y轴、Z轴。
参考点7与全站仪坐标系原点之间的连线为S，S的长度即为第一距离。S’为参考点7与全站仪坐标系原点之间的连线在全站仪坐标系XOY平面的投影线。第一方位角α1表示全站仪坐标系Y轴与投影线S’之间的夹角，第一高低角β1表示参考点7和全站仪坐标系原点之间的连线与投影线S’之间的夹角。
具体地，计算第二坐标的过程包括：
参考点7、参考点7在全站仪坐标系XOY平面的投影点与全站仪坐标系坐标原点之间形成直角三角形；所述直角三角形所在平面且垂直于全站仪坐标系XOY平面。
由于形成直角三角形，参考点7在全站仪坐标系下Z轴的坐标与第一距离S的比值等于第一高低角的正弦值。其中，第一高低角β1已知，则其正弦值可以计算得到；第一距离S也经过测量已知，因此可以计算得到参考点7在全站仪坐标系下Z轴的坐标。
根据第一距离S与第一高低角β1计算得到参考点7与全站仪坐标系原点之间的连线在全站仪坐标系XOY平面的投影线S’的长度。
参考点7在全站仪坐标系下X轴的坐标与S’的比值等于第一方位角α1的正弦值。
参考点7在全站仪坐标系下Y轴的坐标与S’的比值等于第一方位角α1的余弦值。
通过上述过程即可计算得到参考点7在全站仪坐标系下的坐标，即所述第二坐标。
进一步地，根据所述第二坐标，将所述参考点7在全站仪坐标系下的第二坐标变换为所述参考点7在车体坐标系下的坐标的步骤包括：
将所述第二坐标输入初始设置后的修正模型，计算得到所述参考点7在车体坐标系下的坐标；所述修正模型用于输入全站仪坐标系下坐标，计算并输出车体坐标系下的坐标。
在一些实施方式中，所述修正模型中考虑了全站仪1、惯导装置2与测量车辆3之间的各种误差，能够尽可能减小坐标变换过程中误差对计算结果带来的影响，提高计算的准确度。
进一步地，初始设置所述修正模型的过程步骤包括：
获取多组样本数据，所述样本数据包括：标定点在全站仪坐标系下的坐标与标定点在本地坐标系下的坐标；
获取初始修正模型；
对所述初始修正模型进行等价变换，得到第四坐标变换公式；所述第四坐标变换公式中具有参数未知换算矩阵，且输入为全站仪坐标系下的坐标，输出为车体坐标系下的坐标；
输入多个所述样本数据至所述第四坐标变换公式中，计算得到参数已知换算矩阵；
将所述参数已知换算矩阵代入所述第四坐标变换公式，得到初始设置后的所述修正模型。
在一些实施方式中，所述初始修正模型根据公式（一）计算得到；
公式（一）；
其中，X3、Y3、Z3分别表示车体坐标系下沿车体坐标系X轴、车体坐标系Y轴与车体坐标系Z轴的坐标；X1、Y1、Z1分别表示全站仪坐标系下沿全站仪坐标系X轴、全站仪坐标系Y轴与全站仪坐标系Z轴的坐标。a11、a12、a13、b11、b12、b13、c11、c12、c13、ΔX、ΔY与ΔZ均表示未知的参数。
具体地，对所述初始修正模型进行等价变换的过程使用常规的矩阵变换方式。进行等价变换后，所述第四坐标变换公式由公式（二）表示；
公式（二）；
其中，为输出矩阵，X3、Y3、Z3分别表示车体坐标系下沿车体坐标系X轴、车体坐标系Y轴与车体坐标系Z轴的坐标；/>为输入矩阵，X1、Y1、Z1分别表示全站仪坐标系下沿全站仪坐标系X轴、全站仪坐标系Y轴与全站仪坐标系Z轴的坐标；/>为换算矩阵，表示输入矩阵与输出矩阵之间的换算关系;换算矩阵中a11、a12、a13、b11、b12、b13、c11、c12、c13、ΔX、ΔY与ΔZ均表示未知的参数。
进一步地，所述惯导装置2包括：Z轴加速度计、Y轴加速度计与X轴加速度计，三者的正方向两两相互垂直设置；获取初始修正模型的步骤包括：
构建惯导坐标系；所述惯导坐标系以所述惯导装置2的质心为坐标原点，所述Z轴加速度计的正方向为惯导坐标系Z轴，所述Y轴加速度计的正方向为惯导坐标系Y轴，所述X轴加速度计的正方向为惯导坐标系X轴;
根据所述全站仪坐标系与所述惯导坐标系构建第一坐标变换公式；所述第一坐标变换公式中包含有未知参数：所述全站仪坐标系与所述惯导坐标系之间的第一欧拉角与第一平移矢量；所述第一欧拉角包括：惯导坐标系Z轴与全站仪坐标系Z轴之间夹角、惯导坐标系Y轴与全站仪坐标系Y轴之间夹角、惯导坐标系X轴与全站仪坐标系X轴之间夹角；所述第一平移矢量为由全站仪坐标系的坐标原点到惯导坐标系坐标原点，在全站仪坐标系下的矢量；
根据所述车体坐标系与所述惯导坐标系构建第二坐标变换公式；所述第二坐标变换公式中包含有未知参数：所述惯导坐标系与所述车体坐标系之间的第二欧拉角与第二平移矢量；所述第二欧拉角包括：惯导坐标系Z轴与车体坐标系Z轴之间夹角、惯导坐标系Y轴与车体坐标系Y轴之间夹角、惯导坐标系X轴与车体坐标系X轴之间夹角；所述第二平移矢量为惯导坐标系的坐标原点到车体坐标系坐标原点，在惯导坐标系下的矢量；
根据所述第一坐标变换公式与第二坐标变换公式计算得到初始修正模型。
在一些实施方式中，所述第一坐标变换公式如公式（三）所示；
公式（三）；
其中，表示惯导坐标系下的坐标，/>表示全站仪坐标系下的坐标；
表示惯导坐标系Z轴与全站仪坐标系Z轴之间夹角，/>表示惯导坐标系X轴与全站仪坐标系X轴之间夹角，/>表示惯导坐标系Y轴与全站仪坐标系Y轴之间夹角；
表示代入/>后的第一变换矩阵，/>表示代入/>后的第二变换矩阵，表示代入/>后的第三变换矩阵；
表示第一平移矢量。
具体地，所述第二坐标变换公式如公式（四）所示；
/>
公式（四）；
其中，表示车体坐标系下的坐标，/>表示惯导坐标系下的坐标；
表示惯导坐标系Z轴与车体坐标系Z轴之间夹角，/>表示惯导坐标系X轴与车体坐标系X轴之间夹角，/>表示惯导坐标系Y轴与车体坐标系Y轴之间夹角；
表示代入/>后的第一变换矩阵，/>表示代入/>后的第二变换矩阵，表示代入/>后的第三变换矩阵；
表示第二平移矢量。
进一步地，根据所述全站仪坐标系与所述惯导坐标系构建第一坐标变换公式的步骤包括：
获取坐标变换公式；所述坐标变换公式包括：第一变换矩阵、第二变换矩阵、第三变换矩阵与第一矢量矩阵；
获取将全站仪坐标系Z轴、全站仪坐标系Y轴与全站仪坐标系X轴旋转至惯导坐标系Z轴、惯导坐标系Y轴与惯导坐标系X轴的旋转顺序，得到第一旋转顺序；
以所述第一旋转顺序，将所述第一欧拉角依次代入所述第一变换矩阵、第二变换矩阵与第三变换矩阵，并将所述第一平移矢量代入所述第一矢量矩阵，得到第一坐标变换公式。
在一些实施方式中，所述坐标变换公式由公式（五）表示；
公式（五）；
其中，表示变换后的坐标，/>表示变换前的坐标；
表示第一变换矩阵，/>表示第二变换矩阵，/>表示第三变换矩阵；α、β、γ分别表示需要代入的欧拉角；
表示需要代入的平移矢量。
具体地，所述第一旋转顺序为先旋转全站仪坐标系Z轴，再旋转全站仪坐标系X轴，后旋转全站仪坐标系Y轴。
以所述第一旋转顺序，将所述第一欧拉角依次代入所述第一变换矩阵、第二变换矩阵与第三变换矩阵的过程具体为：将代入所述第一变换矩阵，/>代入所述第二变换矩阵，/>代入所述第三变换矩阵。
进一步地，根据所述车体坐标系与所述惯导坐标系构建第二坐标变换公式的步骤包括：
获取坐标变换公式；所述坐标变换公式包括：第一变换矩阵、第二变换矩阵、第三变换矩阵与第一矢量矩阵；
获取将惯导坐标系Z轴、惯导坐标系Y轴与惯导坐标系X轴旋转至车体坐标系Z轴、车体坐标系Y轴与车体坐标系X轴的旋转顺序，得到第二旋转顺序；
以所述第二旋转顺序，将所述第二欧拉角依次代入所述第一变换矩阵、第二变换矩阵与第三变换矩阵，并将所述第二平移矢量代入所述第一矢量矩阵，得到第二坐标变换公式。
具体地，所述第二旋转顺序为先旋转惯导坐标系Z轴，再旋转惯导坐标系X轴，后旋转惯导坐标系Y轴。
以所述第二旋转顺序，将所述第二欧拉角依次代入所述第一变换矩阵、第二变换矩阵与第三变换矩阵的过程具体为：将代入所述第一变换矩阵，/>代入所述第二变换矩阵，/>代入所述第三变换矩阵。
进一步地，根据所述第一坐标变换公式与第二坐标变换公式计算得到初始修正模型的步骤包括：
获取第一坐标变换公式与第二坐标变换公式；所述第一坐标变换公式的输入为全站仪坐标系下的坐标，输出为惯导坐标系下的坐标；所述第二坐标变换公式的输入为惯导坐标系下的坐标，输出为车体坐标系下的坐标；
将第一坐标变换公式的输出作为第二坐标变换公式的输入，使所述第一坐标变换公式代入所述第二坐标变换公式，得到第三坐标变换公式；
对所述第三坐标变换公式进行化简，并将所述第三坐标变换公式中所述全站仪坐标系下的坐标与所述车体坐标系下的坐标之间的矩阵分别替换为初始参数矩阵和初始矢量矩阵，得到所述初始修正模型。
在一些实施方式中，所述第三坐标变换公式由公式（六）表示；
/>
公式（六）。
进而对公式（六）进行化简，并将所述第三坐标变换公式中所述全站仪坐标系下的坐标与所述车体坐标系下的坐标之间的矩阵分别替换为初始参数矩阵和初始矢量矩阵，得到公式（一）。
进一步地，输入多个所述样本数据至所述第四坐标变换公式中，计算得到参数已知换算矩阵的步骤包括：
获取所述第四坐标变换公式；所述第四坐标变换公式中具有用于代入全站仪坐标系下的坐标的输入矩阵，与所述初始参数矩阵具有相同未知参数的所述参数未知换算矩阵，以及用于表示车体坐标系下坐标的输出矩阵；
将所述样本数据中，标定点在全站仪坐标系下的坐标代入所述输入矩阵；
将所述样本数据中，标定点在本地坐标系下的坐标代入所述输出矩阵；
根据所述输入矩阵与所述输出矩阵计算得到所述换算矩阵中的参数值，得到所述参数已知换算矩阵。
在一些实施方式中，将所述输出矩阵作为P矩阵，输入矩阵作为B矩阵，换算矩阵作为Y矩阵；具体如公式（七）、公式（八）与公式（九）表示；
公式（七）；
公式（八）；
公式（九）；
进而所述第四坐标变换公式可由公式（十）表示：
公式（十）；
根据公式（十）进一步推导可得换算矩阵计算公式，具体如公式（十一）表示；
公式（十一）；
其中，B T表示输入矩阵的转置，（·）-1表示矩阵的逆矩阵。
在一些实施方式中，将所述样本数据中，标定点在全站仪坐标系下的坐标代入所述输入矩阵的过程包括：
将多个标定点在全站仪坐标系下的坐标分别代入多个输入子矩阵；如公式（十二）所示；
公式（十二）；
其中，表示第n个输入子矩阵，/>、/>、/>分别表示第n样本数据中全站仪坐标系下沿全站仪坐标系X轴、全站仪坐标系Y轴与全站仪坐标系Z轴的坐标。
再将多个输入子矩阵代入所述输入矩阵，得到带有输入数据的输入矩阵；具体如公式（十三）表示；
公式（十三）；
其中，脚标G表示输入子矩阵个数。
将所述样本数据中，标定点在本地坐标系下的坐标代入所述输出矩阵的过程包括：
将多个标定点在本地坐标系下的坐标分别代入多个输出子矩阵；如公式（十四）所示；
公式（十四）；
其中，表示第n个输出子矩阵，/>、/>、/>分别表示第n样本数据中本地坐标系下沿本地坐标系X轴、本地坐标系Y轴与本地坐标系Z轴的坐标。
再将多个输出子矩阵代入所述输出矩阵，得到带有输出数据的输出矩阵；具体如公式（十五）表示；
公式（十五）；
其中，脚标H表示输出子矩阵个数。
进而将输入数据的输入矩阵公式（十三）与输出数据的输出矩阵公式（十五）代入所述换算矩阵计算公式，即公式（十一），计算得到参数已知换算矩阵。
进一步地，获取多组样本数据的步骤包括：
将本地坐标系的坐标原点作为标定圆心，将水平面作为标定平面，以第一设定距离为半径，绘制得到标定圆周；
在所述标定圆周上选取多个标定点；
获取每个标定点在全站仪坐标系下的坐标与所述本地坐标系下的坐标；
将同一标定点在全站仪坐标系下的坐标与所述本地坐标系下的坐标作为一组样本数据，得到多组样本数据。
具体地，第一设定距离设定为1公里。
获取多组样本数据前需要将车体坐标系与本地坐标系调整至重合，即这边原点重合且各轴分别重合。此时，标定点在本地坐标系下的坐标等于在车体坐标系下的坐标；上述方式能够方便测量标定点在车体坐标系下的坐标。
进一步地，在所述标定圆周上选取多个标定点的方式为：
沿所述标定圆周等间隔随机选取多个标定点。
在一些实施方式中，两相邻标定点之间的间隔与标定圆周周长的比值设定为1:72。
等间隔选取能够对标定圆周内各方向的位置进行采样，避免仅采集少数几个方向的标定点时，同时对一个方向采集多个标定点；以减少同一方向采集数据时产生的误差。
具体地，全站仪坐标到车体坐标的误差修正方法中计算以及操作过程包括：
一、构建全站仪坐标系、惯导坐标系、车体坐标系与本地坐标系；
二、调整测量车辆3的位姿，使车体坐标系与本地坐标系重合；
三、获取多组样本数据，包括：获取每个标定点在全站仪坐标系下的坐标与所述本地坐标系下的坐标；
四、构建第一坐标变换公式与第二坐标变换公式；
五、将第一坐标变换公式代入第二坐标变换公式，计算得到第三坐标变换公式；
六、对第三坐标变换公式中的矩阵进行替换，得到初始修正模型；
七、对初始修正模型进行等价变换，得到第四坐标变换公式；
八、对第四坐标变换公式进行进一步等价变换，得到换算矩阵计算公式；
九、分别将每个标定点在全站仪坐标系下的坐标与所述本地坐标系下的坐标代入所述换算矩阵计算公式，计算得到参数已知换算矩阵；
十、将参数已知换算矩阵代入初始修正模型，得到修正模型；
十一、使用全站仪测量待测点6在全站仪坐标系下的待测坐标；
十二、将待测坐标输入所述待测坐标至所述修正模型，计算得到待测点6在车体坐标系下的坐标。
进一步地，将参考点7在车体坐标系下的坐标变换至本地坐标系下的坐标，得到第三坐标的步骤包括：
测量车体坐标系与本地坐标系坐标轴之间的偏差，得到第三欧拉角与第三平移矢量；所述第三欧拉角包括：本地坐标系Z轴与车体坐标系Z轴之间夹角、本地坐标系Y轴与车体坐标系Y轴之间夹角、本地坐标系X轴与车体坐标系X轴之间夹角；所述第三平移矢量为本地坐标系的坐标原点到车体坐标系坐标原点，在本地坐标系下的矢量；
获取坐标变换公式；所述坐标变换公式包括：第一变换矩阵、第二变换矩阵、第三变换矩阵与第一矢量矩阵；
获取将车体坐标系Z轴、车体坐标系Y轴与车体坐标系X轴旋转至本地坐标系Z轴、本地坐标系Y轴与本地坐标系X轴的旋转顺序，得到第三旋转顺序；
以所述第三旋转顺序，将所述第三欧拉角依次代入所述第一变换矩阵、第二变换矩阵与第三变换矩阵，并将所述第三平移矢量代入所述第一矢量矩阵，得到第五坐标变换公式；
将所述第三坐标代入所述第五坐标变换公式，计算得到第三坐标；
在一些实施方式中，移动测量车辆3的过程中可能会产生调节误差，使本地坐标系与车体坐标系不完全重合，具体表现为坐标轴之间的偏移与旋转。
因此在计算第四坐标前还需要对第三坐标进行进一步变换，进而排除车体坐标系与本地坐标系之间偏差的影响。
具体地，所述第五坐标变换公式由公式（十六）表示；
公式（十六）；
其中，、/>、/>分别表示第三欧拉角的本地坐标系Z轴与车体坐标系Z轴之间夹角、本地坐标系Y轴与车体坐标系Y轴之间夹角、本地坐标系X轴与车体坐标系X轴之间夹角；
表示变换后的坐标，计算时得出第五坐标，/>表示变换前的坐标，计算时代入原有的第三坐标；/>表示第三平移矢量。
计算时，将调节误差代入公式（五），得到公式（十六）；再将第三坐标代入公式（十六）即可计算得到排除调节误差影响的第五坐标。进而将第五坐标替换原有的第三坐标，得到最终的第三坐标代入计算得到待测点6在大地坐标系下的第四坐标。
以上描述仅为本发明的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本发明中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离所述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本发明中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。

Claims (8)

1.一种车载自主定位定向方法，全站仪（1）安装在惯导装置（2）上，所述惯导装置（2）安装在测量车辆（3）上；所述测量车辆（3）上具有标志点（4）；所述全站仪（1）包括目镜（5）；所述全站仪（1）用于测量待测点在全站仪坐标系下的坐标；其特征在于，所述车载自主定位定向方法包括：
构建车体坐标系与全站仪坐标系；所述车体坐标系以所述标志点（4）沿竖直方向在地表所在平面的投影点为坐标原点；所述全站仪坐标系以所述目镜（5）的镜头中心为坐标原点；
选定坐标未知的待测点（6）；
响应于所述测量车辆（3）第一模式的信号，进行以下步骤：
构建本地坐标系；所述本地坐标系以车体坐标系的坐标原点为坐标原点，竖直方向为本地坐标系Z轴，正北方向为本地坐标系Y轴，正东方向为本地坐标系X轴；将当前位置构建的本地坐标系坐标原点作为初始位置；
获取初始位置本地坐标系坐标原点在大地坐标系下的坐标，得到初始坐标；
移动所述测量车辆（3），使本地坐标系坐标原点、车体坐标系坐标原点与待测点（6）均重合；本地坐标系坐标原点随所述测量车辆（3）平移；
根据所述初始坐标，计算得到待测点（6）在大地坐标系下的坐标，得到第一坐标；
选取待测点（6）处可见且坐标未知的参考点（7）；
测量所述参考点（7）在全站仪坐标系下的坐标，得到第二坐标；
根据所述第二坐标，将所述参考点（7）在全站仪坐标系下的第二坐标变换为所述参考点（7）在车体坐标系下的坐标；
将参考点（7）在车体坐标系下的坐标变换至本地坐标系下的坐标，得到第三坐标；
根据所述第一坐标、所述第三坐标与待测点（6）在本地坐标系下坐标，计算所述参考点（7）在大地坐标系下坐标，得到第四坐标；根据第一坐标与第四坐标，计算待测点（6）与参考点（7）之间的第三方位角；
其中，根据所述初始坐标，计算得到待测点（6）在大地坐标系下的坐标，得到第一坐标的步骤包括：
调整测量车辆（3）的位姿使惯导装置（2）处于大地坐标系下坐标已知位置点；
获取坐标已知位置点在大地坐标系下的坐标，得到初始坐标；
将所述初始坐标输入所述惯导装置（2）；
再次调整测量车辆（3）的位姿，使所述车体坐标系与所述本地坐标系的坐标原点重合且坐标轴分别重合；
惯导装置（2）计算得到本地坐标系坐标原点在大地坐标系下的坐标，得到所述第一坐标；
其中，测量所述参考点（7）在全站仪坐标系下的坐标，得到第二坐标的步骤包括：
测量所述参考点（7）相对于全站仪（1）的第一高低角与第一方位角；
测量所述参考点（7）与全站仪（1）之间的距离，得到第一距离；
根据所述第一距离、第一高低角与第一方位角计算所述参考点（7）在全站仪坐标系下的坐标，得到第二坐标。
2.根据权利要求1所述的一种车载自主定位定向方法，其特征在于，响应于所述测量车辆（3）第二模式的信号，进行以下步骤：
测量车体坐标系坐标原点在大地坐标系下的坐标，得到第六坐标；
测量所述待测点（6）在全站仪坐标系下的坐标，得到第七坐标；
根据所述第七坐标，将所述待测点（6）在全站仪坐标系下的坐标变换至车体坐标系下的坐标，得到第八坐标；
根据所述第六坐标、第八坐标与车体坐标系原点在车体坐标系下坐标，将所述待测点（6）在车体坐标系下的第八坐标变换为大地坐标系下的坐标，得到第九坐标；
根据待测点（6）在大地坐标系下的第九坐标与车体坐标系坐标原点在大地坐标系下的第六坐标，计算得到待测点（6）相对车体坐标系坐标原点的第二方位角。
3.根据权利要求1所述的一种车载自主定位定向方法，其特征在于，根据所述第二坐标，将所述参考点（7）在全站仪坐标系下的第二坐标变换为所述参考点（7）在车体坐标系下的坐标的步骤包括：
将所述第二坐标输入初始设置后的修正模型，计算得到所述参考点（7）在车体坐标系下的坐标；所述修正模型用于输入全站仪坐标系下坐标，计算并输出车体坐标系下的坐标。
4.根据权利要求3所述的一种车载自主定位定向方法，其特征在于，初始设置所述修正模型的过程步骤包括：
获取多组样本数据，所述样本数据包括：标定点在全站仪坐标系下的坐标与标定点在本地坐标系下的坐标；
获取初始修正模型；所述初始修正模型包含未知的初始参数矩阵，所述初始修正模型用于表征全站仪坐标系下的坐标转化为车体坐标系下的坐标的变换关系；
对所述初始修正模型进行等价变换，将全站仪坐标系下坐标变换为输入矩阵，初始参数矩阵中未知的参数变换为换算矩阵，车体坐标系下的坐标变换为输出矩阵，使输出矩阵等于输入矩阵乘换算矩阵，得到第四坐标变换公式；所述第四坐标变换公式的输入为全站仪坐标系下的坐标，代入所述输入矩阵；输出为车体坐标系下的坐标；
所述第四坐标变换公式由公式（二）表示；
公式（二）；
其中，为输出矩阵，X3、Y3、Z3分别表示车体坐标系下沿车体坐标系X轴、车体坐标系Y轴与车体坐标系Z轴的坐标；/>为输入矩阵，X1、Y1、Z1分别表示全站仪坐标系下沿全站仪坐标系X轴、全站仪坐标系Y轴与全站仪坐标系Z轴的坐标；/>为换算矩阵，表示输入矩阵与输出矩阵之间的换算关系;换算矩阵中a11、a12、a13、b11、b12、b13、c11、c12、c13、ΔX、ΔY与ΔZ均表示未知的参数；
输入多个所述样本数据至所述第四坐标变换公式中，对所述换算矩阵进行拟合，计算得到参数已知换算矩阵；
将参数已知换算矩阵代入所述第四坐标变换公式，得到所述修正模型。
5.根据权利要求4所述的一种车载自主定位定向方法，其特征在于，所述惯导装置（2）包括：Z轴加速度计、Y轴加速度计与X轴加速度计，三者的正方向两两相互垂直设置；
获取初始修正模型的步骤包括：
构建惯导坐标系；所述惯导坐标系以所述惯导装置（2）的质心为坐标原点，所述Z轴加速度计的正方向为惯导坐标系Z轴，所述Y轴加速度计的正方向为惯导坐标系Y轴，所述X轴加速度计的正方向为惯导坐标系X轴;
根据所述全站仪坐标系与所述惯导坐标系构建第一坐标变换公式；所述第一坐标变换公式中包含有未知参数：所述全站仪坐标系与所述惯导坐标系之间的第一欧拉角与第一平移矢量；所述第一欧拉角包括：惯导坐标系Z轴与全站仪坐标系Z轴之间夹角、惯导坐标系Y轴与全站仪坐标系Y轴之间夹角、惯导坐标系X轴与全站仪坐标系X轴之间夹角；所述第一平移矢量为由全站仪坐标系的坐标原点到惯导坐标系坐标原点，在全站仪坐标系下的矢量；
所述第一坐标变换公式如公式（三）所示；
公式（三）；
其中，表示惯导坐标系下的坐标，/>表示全站仪坐标系下的坐标；
表示惯导坐标系Z轴与全站仪坐标系Z轴之间夹角，/>表示惯导坐标系X轴与全站仪坐标系X轴之间夹角，/>表示惯导坐标系Y轴与全站仪坐标系Y轴之间夹角；
表示代入/>后的第一变换矩阵，/>表示代入/>后的第二变换矩阵，表示代入/>后的第三变换矩阵；
表示第一平移矢量；
根据所述车体坐标系与所述惯导坐标系构建第二坐标变换公式；所述第二坐标变换公式中包含有未知参数：所述惯导坐标系与所述车体坐标系之间的第二欧拉角与第二平移矢量；所述第二欧拉角包括：惯导坐标系Z轴与车体坐标系Z轴之间夹角、惯导坐标系Y轴与车体坐标系Y轴之间夹角、惯导坐标系X轴与车体坐标系X轴之间夹角；所述第二平移矢量为惯导坐标系的坐标原点到车体坐标系坐标原点，在惯导坐标系下的矢量；
所述第二坐标变换公式如公式（四）所示；
公式（四）；
其中，表示车体坐标系下的坐标，/>表示惯导坐标系下的坐标；
表示惯导坐标系Z轴与车体坐标系Z轴之间夹角，/>表示惯导坐标系X轴与车体坐标系X轴之间夹角，/>表示惯导坐标系Y轴与车体坐标系Y轴之间夹角；
表示代入/>后的第一变换矩阵，/>表示代入/>后的第二变换矩阵，表示代入/>后的第三变换矩阵；
表示第二平移矢量；
根据所述第一坐标变换公式与第二坐标变换公式计算得到初始修正模型。
6.根据权利要求5所述的一种车载自主定位定向方法，其特征在于，根据所述全站仪坐标系、所述惯导坐标系与所述车体坐标系构建第一坐标变换公式与第二坐标变换公式的步骤包括：
获取坐标变换公式；所述坐标变换公式包括：第一变换矩阵、第二变换矩阵、第三变换矩阵与第一矢量矩阵；
所述坐标变换公式由公式（五）表示；
公式（五）；
其中，表示变换后的坐标，/>表示变换前的坐标；
表示第一变换矩阵，/>表示第二变换矩阵，/>表示第三变换矩阵；α、β、γ分别表示需要代入的欧拉角；
表示需要代入的平移矢量；
获取将全站仪坐标系Z轴、全站仪坐标系Y轴与全站仪坐标系X轴旋转至惯导坐标系Z轴、惯导坐标系Y轴与惯导坐标系X轴的旋转顺序，得到第一旋转顺序；
获取将惯导坐标系Z轴、惯导坐标系Y轴与惯导坐标系X轴旋转至车体坐标系Z轴、车体坐标系Y轴与车体坐标系X轴的旋转顺序，得到第二旋转顺序；
以所述第一旋转顺序，将所述第一欧拉角依次代入所述第一变换矩阵、第二变换矩阵与第三变换矩阵，并将所述第一平移矢量代入所述第一矢量矩阵，得到第一坐标变换公式；
以所述第二旋转顺序，将所述第二欧拉角依次代入所述第一变换矩阵、第二变换矩阵与第三变换矩阵，并将所述第二平移矢量代入所述第一矢量矩阵，得到第二坐标变换公式。
7.根据权利要求6所述的一种车载自主定位定向方法，其特征在于，根据所述第一坐标变换公式与第二坐标变换公式计算得到初始修正模型的步骤包括：
获取第一坐标变换公式与第二坐标变换公式；所述第一坐标变换公式的输入为全站仪坐标系下的坐标，输出为惯导坐标系下的坐标；所述第二坐标变换公式的输入为惯导坐标系下的坐标，输出为车体坐标系下的坐标；
将第一坐标变换公式的输出作为第二坐标变换公式的输入，使所述第一坐标变换公式代入所述第二坐标变换公式，得到第三坐标变换公式；
对所述第三坐标变换公式进行化简，并将所述第三坐标变换公式中所述全站仪坐标系下的坐标与所述车体坐标系下的坐标之间的矩阵分别替换为初始参数矩阵和初始矢量矩阵，得到所述初始修正模型；
所述第三坐标变换公式由公式（六）表示；
公式（六）。
8.根据权利要求1所述的一种车载自主定位定向方法，其特征在于，将参考点（7）在车体坐标系下的坐标变换至本地坐标系下的坐标，得到第三坐标的步骤包括：
测量车体坐标系与本地坐标系坐标轴之间的偏差，得到第三欧拉角与第三平移矢量；所述第三欧拉角包括：本地坐标系Z轴与车体坐标系Z轴之间夹角、本地坐标系Y轴与车体坐标系Y轴之间夹角、本地坐标系X轴与车体坐标系X轴之间夹角；所述第三平移矢量为本地坐标系的坐标原点到车体坐标系坐标原点，在本地坐标系下的矢量；
获取坐标变换公式；所述坐标变换公式包括：第一变换矩阵、第二变换矩阵、第三变换矩阵与第一矢量矩阵；
获取将车体坐标系Z轴、车体坐标系Y轴与车体坐标系X轴旋转至本地坐标系Z轴、本地坐标系Y轴与本地坐标系X轴的旋转顺序，得到第三旋转顺序；
以所述第三旋转顺序，将所述第三欧拉角依次代入所述第一变换矩阵、第二变换矩阵与第三变换矩阵，并将所述第三平移矢量代入所述第一矢量矩阵，得到第五坐标变换公式；
将所述第三坐标代入所述第五坐标变换公式，计算得到第三坐标。
CN202310945431.4A 2023-07-31 2023-07-31 一种车载自主定位定向方法 Active CN116659493B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310945431.4A CN116659493B (zh) 2023-07-31 2023-07-31 一种车载自主定位定向方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310945431.4A CN116659493B (zh) 2023-07-31 2023-07-31 一种车载自主定位定向方法

Publications (2)

Publication Number Publication Date
CN116659493A CN116659493A (zh) 2023-08-29
CN116659493B true CN116659493B (zh) 2023-10-13

Family

ID=87710159

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310945431.4A Active CN116659493B (zh) 2023-07-31 2023-07-31 一种车载自主定位定向方法

Country Status (1)

Country Link
CN (1) CN116659493B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103821054A (zh) * 2014-03-12 2014-05-28 武汉大学 基于ins与全站仪组合的轨道几何状态测量系统及方法
CN106840088A (zh) * 2017-02-27 2017-06-13 武汉理工大学 一种车载组合导航路面沉降快速测量方法
CN109115150A (zh) * 2018-08-14 2019-01-01 中车唐山机车车辆有限公司 一种基于车体的数据处理方法和装置
EP3502821A1 (de) * 2017-12-22 2019-06-26 Wirtgen GmbH Selbstfahrende baumaschine und verfahren zum steuern einer selbstfahrenden baumaschine
CN110297220A (zh) * 2019-07-19 2019-10-01 西安科技大学 一种双天线InSAR系统基线矢量的测量方法
CN112923921A (zh) * 2021-01-28 2021-06-08 湖北三江航天红峰控制有限公司 一种基于位置修正的轨迹测量方法、装置及电子设备
CN114353792A (zh) * 2022-03-16 2022-04-15 长沙金维信息技术有限公司 车载组合导航系统中惯导系统的杆臂测量方法及导航方法
CN115727843A (zh) * 2022-11-16 2023-03-03 北京觉非科技有限公司 用于航位推算的轮速确定方法、装置及设备

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103821054A (zh) * 2014-03-12 2014-05-28 武汉大学 基于ins与全站仪组合的轨道几何状态测量系统及方法
CN106840088A (zh) * 2017-02-27 2017-06-13 武汉理工大学 一种车载组合导航路面沉降快速测量方法
EP3502821A1 (de) * 2017-12-22 2019-06-26 Wirtgen GmbH Selbstfahrende baumaschine und verfahren zum steuern einer selbstfahrenden baumaschine
CN109115150A (zh) * 2018-08-14 2019-01-01 中车唐山机车车辆有限公司 一种基于车体的数据处理方法和装置
CN110297220A (zh) * 2019-07-19 2019-10-01 西安科技大学 一种双天线InSAR系统基线矢量的测量方法
CN112923921A (zh) * 2021-01-28 2021-06-08 湖北三江航天红峰控制有限公司 一种基于位置修正的轨迹测量方法、装置及电子设备
CN114353792A (zh) * 2022-03-16 2022-04-15 长沙金维信息技术有限公司 车载组合导航系统中惯导系统的杆臂测量方法及导航方法
CN115727843A (zh) * 2022-11-16 2023-03-03 北京觉非科技有限公司 用于航位推算的轮速确定方法、装置及设备

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
吴维军 ; 朱洪涛 ; 曹娟华 ; 熊丽娟 ; 殷华 ; 赵书琦 ; .车载全站仪位姿估计方法研究.铁道标准设计.2018,(12),全文. *
车载捷联惯导系统定位测姿算法研究;陈允芳;叶泽田;钟若飞;;中国惯性技术学报(01);全文 *

Also Published As

Publication number Publication date
CN116659493A (zh) 2023-08-29

Similar Documents

Publication Publication Date Title
CN106990424B (zh) 一种双天线gps测姿方法
CN103994763B (zh) 一种火星车的sins/cns深组合导航系统及其实现方法
CN113311436B (zh) 一种移动平台上激光测风雷达运动姿态测风订正方法
CN106643709B (zh) 一种海上运载体的组合导航方法及装置
GREJNER‐BRZEZINSKA Direct exterior orientation of airborne imagery with GPS/INS system: Performance analysis
CA2777803C (en) Ultra-short baseline gnss receiver
CN106842271B (zh) 导航定位方法及装置
JP2000506604A (ja) 改良された車両ナビゲーションシステム及びその方法
CN108051866A (zh) 基于捷联惯性/gps组合辅助水平角运动隔离的重力测量方法
CN111637888B (zh) 基于惯导和激光雷达单点测距的掘进机定位方法及系统
CN105928515B (zh) 一种无人机导航系统
CN112378399B (zh) 基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法
CN103389092A (zh) 一种系留飞艇姿态测量装置及测量方法
CN111947653A (zh) 一种月面巡视探测器双模式惯性/视觉/天文导航方法
CN103453903A (zh) 一种基于惯性测量组件的管道探伤系统导航定位方法
CN113203418A (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN103604428A (zh) 基于高精度水平基准的星敏感器定位方法
CN113686299B (zh) 一种海上动态目标定位与移速预测方法
CN111811538A (zh) 一种基于常见星体的光电经纬仪定向标校方法
CN108205151B (zh) 一种低成本gps单天线姿态测量方法
CN104501809A (zh) 一种基于姿态耦合的捷联惯导/星敏感器组合导航方法
CN116659493B (zh) 一种车载自主定位定向方法
CN109471102B (zh) 一种惯组误差修正方法
CN106767824B (zh) 一种计算双探测器在地外天体表面相对位置的方法
CN114001756B (zh) 一种小视场星敏感器外场地面寻星方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant