CN112764074B - 一种定位导航轨迹的方法、装置及电子设备 - Google Patents
一种定位导航轨迹的方法、装置及电子设备 Download PDFInfo
- Publication number
- CN112764074B CN112764074B CN202011607056.5A CN202011607056A CN112764074B CN 112764074 B CN112764074 B CN 112764074B CN 202011607056 A CN202011607056 A CN 202011607056A CN 112764074 B CN112764074 B CN 112764074B
- Authority
- CN
- China
- Prior art keywords
- target point
- offset
- calculating
- angle
- distance
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 56
- 230000006870 function Effects 0.000 claims description 44
- 238000004364 calculation method Methods 0.000 claims description 18
- 238000012545 processing Methods 0.000 claims description 18
- 238000001914 filtration Methods 0.000 claims description 5
- 238000005516 engineering process Methods 0.000 abstract description 5
- 230000008569 process Effects 0.000 description 9
- 241000251468 Actinopterygii Species 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000007906 compression Methods 0.000 description 4
- 230000006835 compression Effects 0.000 description 4
- 238000004590 computer program Methods 0.000 description 4
- 238000012544 monitoring process Methods 0.000 description 4
- 238000006243 chemical reaction Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 3
- 241000238557 Decapoda Species 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000009360 aquaculture Methods 0.000 description 1
- 244000144974 aquaculture Species 0.000 description 1
- 230000009193 crawling Effects 0.000 description 1
- 238000007405 data analysis Methods 0.000 description 1
- 238000013144 data compression Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 230000002431 foraging effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000000977 initiatory effect Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 238000007726 management method Methods 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009182 swimming Effects 0.000 description 1
- 238000012876 topography Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
-
- 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
- G01C21/203—Specially adapted for sailing ships
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明涉及定位、导航技术领域，特别涉及一种定位导航轨迹的方法、装置及电子设备。该方法包括：获取第一轨迹集合，所述第一轨迹集合包括多个目标点；获取偏移距离；根据所述第一轨迹集合中的目标点和所述偏移距离确定偏移点，全部的所述偏移点组成第二轨迹集合；根据所述第二轨迹集合生成导航轨迹。本发明能够规划出特定区域的导航轨迹，丰富了轨迹规划方法，并且实现简单。
Description
技术领域
本发明涉及定位、导航技术领域，特别涉及一种定位导航轨迹的方法、装置及电子设备。
背景技术
GPS和北斗是现如今最为常用的全球定位系统，在车辆管理和智能设备的建设之中，高端可靠的GPS、北斗定位系统表现出了良好的属性。而由于定位功能的重要性和应用的广泛性，也对这种高质量的GPS、北斗定位导航模块有了更高的标准要求，让其不断的技术改进和系统升级使其拥有了更多的优势。
目前定位系统的建设过程之中本身的定位模块精准度更高，而专业的GPS、北斗定位导航模块能够通过其卫星定位信号实现有效的定位，其本身依赖高精度卫星定位和优质的惯性导航技术系统让其拥有了更好的反应能力，复杂的城市环境和偏远的山间隧道等地区都能够精准的进行检测。因此在一系列车辆使用和其地理监测的过程之中，高品质的GPS、北斗定位导航模块能够实现有效连续的高精度定位。
同时结合无人船的发展趋势，在水产养殖、河道环保监测、电子围栏区域监控等方面的发展趋势，自动化程度会越来越高，无人作业和任务的情况越来越普遍。这就要求设备能够在指定区域进行精准定位，并通过导航完成作业任务。
由于指定区域的导航需要设备具备初始化轨迹，但是，由于地形或条件限制等原因，导致不能直接采集到所需轨迹。因此，结合以上GPS、北斗全球卫星定位系统技术的优势和特性，规划出无人船导航的路径具有重要意义。
发明内容
本发明实施方式主要解决的技术问题是如何规划指定区域的导航路径。
为解决上述技术问题，本发明实施方式采用的一个技术方案是：提供一种定位导航轨迹的方法，包括：
获取第一轨迹集合，所述第一轨迹集合包括多个目标点；
获取偏移距离；
根据所述第一轨迹集合中的目标点和所述偏移距离确定偏移点，全部的所述偏移点组成第二轨迹集合；
根据所述第二轨迹集合生成导航轨迹。
可选地，所述根据所述第一轨迹集合中的目标点和所述偏移距离确定偏移点，包括：
选取第一目标点和第二目标点；
计算所述第一目标点与所述第二目标点的第一距离；
计算所述第二目标点相对于所述第一目标点的第一航向角；
根据所述偏移距离和所述第一距离，计算第一夹角；
根据所述第一航向角和所述第一夹角获得第二航向角，所述第二航向角是所述偏移点相对于所述第一目标点的航向角；
根据所述第二航向角和所述第一目标点的坐标计算所述偏移点的坐标。
可选地，所述计算所述第一目标点与所述第二目标点的第一距离，包括：
获取所述第一目标点经度、纬度，以及所述第二目标点的经度、纬度；
根据所述第一目标点经度、纬度，所述第二目标点的经度、纬度，以及球面余弦公式，计算所述第一目标点与所述第二目标点对应的角度；
将所述角度转化为弧度，并根据所述弧度计算所述第一目标点与所述第二目标点的第一距离。
可选地，所述计算所述第二目标点相对于所述第一目标点的第一航向角，包括：
根据所述第一目标点与所述第二目标点对应的角度获取所述角度的正弦函数；
根据所述正弦函数和球面正弦公式计算所述第一目标点的正弦函数；
通过反正弦函数计算所述第一目标点对应的角度；
确定所述第一目标点和所述第二目标点在坐标轴上的象限位置，并根据所述象限位置和所述角度计算得到所述第一航向角。
可选地，所述根据所述第二轨迹集合生成导航轨迹包括：
将所述第二轨迹集合中的偏移点进行过滤和压缩处理，得到处理后的偏移点；
基于所述处理后的偏移点生成导航轨迹。
为解决上述技术问题，本发明实施方式采用的另一个技术方案是：提供一种定位导航轨迹的装置，包括：
第一获取模块，用于获取第一轨迹集合，所述第一轨迹集合包括多个目标点；
第二获取模块，用于获取偏移距离；
第一处理模块，用于根据所述第一轨迹集合中的目标点和所述偏移距离确定偏移点，全部的所述偏移点组成第二轨迹集合；
第二处理模块，用于根据所述第二轨迹集合生成导航轨迹。
可选地，所述第一处理模块包括：
目标点选取单元，用于选取第一目标点和第二目标点；
第一计算单元，用于计算所述第一目标点与所述第二目标点的第一距离；
第二计算单元，用于计算所述第二目标点相对于所述第一目标点的第一航向角；
第三计算单元，用于根据所述偏移距离和所述第一距离，计算第一夹角；
第四计算单元，用于根据所述第一航向角和所述第一夹角获得第二航向角，所述第二航向角是所述偏移点相对于所述第一目标点的航向角；
第五计算单元，用于根据所述第二航向角和所述第一目标点的坐标计算所述偏移点的坐标。
可选地，所述第一计算单元具体用于：
获取所述第一目标点经度、纬度，以及所述第二目标点的经度、纬度；
根据所述第一目标点经度、纬度，所述第二目标点的经度、纬度，以及球面余弦公式，计算所述第一目标点与所述第二目标点对应的角度；
将所述角度转化为弧度，并根据所述弧度计算所述第一目标点与所述第二目标点的第一距离。
可选地，所述第二计算单元具体用于：
根据所述第一目标点与所述第二目标点对应的角度获取所述角度的正弦函数；
根据所述正弦函数和球面正弦公式计算所述第一目标点的正弦函数；
通过反正弦函数计算所述第一目标点对应的角度；
确定所述第一目标点和所述第二目标点在坐标轴上的象限位置，并根据所述象限位置和所述角度计算得到所述第一航向角。
为解决上述技术问题，本发明实施方式采用的又一个技术方案是：提供一种电子设备，包括：至少一个处理器；以及，与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如上所述的方法。
区别于相关技术的情况，本发明实施例提供了一种定位导航轨迹的方法、装置及电子设备，通过获取第一轨迹集合，所述第一轨迹集合包括多个目标点；获取偏移距离；根据所述第一轨迹集合中的目标点和所述偏移距离确定偏移点，全部的所述偏移点组成第二轨迹集合；根据所述第二轨迹集合生成导航轨迹。本发明实施例提供的定位导航轨迹的方法、装置和电子设备能够规划出特定区域的导航轨迹，丰富了轨迹规划方法，并且实现简单。
附图说明
一个或多个实施例通过与之对应的附图进行示例性说明，这些示例性说明并不构成对实施例的限定，附图中具有相同参考数字标号的元件表示为类似的元件，除非有特别申明，附图中的图不构成比例限制。
图1是本发明实施例提供的一种定位导航轨迹的方法的流程图；
图2是本发明实施例中根据所述第一轨迹集合中的目标点和所述偏移距离确定偏移点的方法的流程图；
图3，图4以及图5是计算所述导航轨迹的过程示意图；
图6是本发明实施例提供的一种定位导航轨迹的装置的结构示意图；
图7是本发明实施例提供的一种电子设备的硬件结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
请参阅图1，图1是本发明实施例提供的一种定位导航轨迹的方法的流程图。该方法包括：
S11、获取第一轨迹集合，所述第一轨迹集合包括多个目标点。
所述第一轨迹集合是由多个目标点构成的点集合，第一轨迹集合中的全部所述目标点构成第一轨迹，该第一轨迹可理解为原始轨迹。该原始轨迹可以利用北斗、GPS、格洛纳斯等卫星信号接收设备，获取原始的定位轨迹点数据。设备从起点运动到终点，同时卫星信号接收设备输出对应经纬度点坐标，由这些点坐标构成运动轨迹，即所述第一轨迹。
可以知道的，在特定的一些环境中，会导致目标对象不能在预定的轨迹上行驶，比如，将无人船应用于某水库中时，利用该无人船来投放饲料以喂养鱼、虾、蟹等，根据上述方法设置所述无人船行驶的第一轨迹为沿着水库的岸边行驶一圈的轨迹，然而，由于水库中的鱼、虾、蟹的觅食、游动和爬行等习性并不在岸边沿，如果无人船按照该设置的第一轨迹行驶并不能达到其喂养鱼饲料的目的，因此，无人船不能在所述第一轨迹上行驶。如何准确确定无人船的新的导航轨迹就很有必要。
下述方法步骤通过其他轨迹采集设备根据所述第一轨迹集合中的目标点进行轨迹采集，然后对采集的数据进行一定的轨迹偏移处理，以此达到变相获得预定轨迹的目的。
S12、获取偏移距离。
所述偏移距离是用于计算偏移轨迹的其中一个参数，所述偏移轨迹即是针对特定区域和特定应用环境重新确定的所述导航轨迹。所述偏移距离可以由用户根据实际应用的环境和区域进行任意设定，比如偏移距离为1米，2米等。
所述偏移距离与具体应用的环境中的参数相关，比如，应用于无人船设备，无人船工作在水域中，无人船用于监测水域中的A种鱼类，获得水域中该A种鱼类的生活习性一般是在距离岸边2米的地方栖息，那么可以根据该A种鱼类栖息的位置确定所述偏移距离，如，设该偏移距离为2米。
需要说明的是，所述偏移距离在设定后可以根据应用的环境的变化进行更新，以便能更准确的确定所述导航轨迹。
在获得所述偏移距离时，用户可以通过多次重复试验获得相关参数，最后计算该相关参数的均值，所述均值作为所述偏移距离。
S13、根据所述第一轨迹集合中的目标点和所述偏移距离确定偏移点，全部的所述偏移点组成第二轨迹集合。
所述偏移点用于组成重新确定的所述导航轨迹。其中，如图2所示，所述根据所述第一轨迹集合中的目标点和所述偏移距离确定偏移点，包括：
S131、选取第一目标点和第二目标点；
从所述第一轨迹集合中选择两个目标点，分别是第一目标点和第二目标点，所述第一目标点和所述第二目标点在所述第一轨迹集合中可以是连续的点，即相邻的两个点，也可以不是相邻的两个点。
S132、计算所述第一目标点与所述第二目标点的第一距离；
所述第一目标点作为起始点，所述第二目标点作为终点，根据所述起始点和所述终点计算所述第一距离。所述第一距离即是第一目标点与第二目标点之间的距离。
具体地，所述计算所述第一目标点与所述第二目标点的第一距离，包括：
获取所述第一目标点经度、纬度，以及所述第二目标点的经度、纬度；
根据所述第一目标点经度、纬度，所述第二目标点的经度、纬度，以及球面余弦公式，计算所述第一目标点与所述第二目标点对应的角度；
将所述角度转化为弧度，并根据所述弧度计算所述第一目标点与所述第二目标点的第一距离。
S133、计算所述第二目标点相对于所述第一目标点的第一航向角；
所述第一航向角即是设备从第一目标点运动到第二目标点的过程中设备的质心速度与横轴的夹角。
其中，所述计算所述第二目标点相对于所述第一目标点的第一航向角，包括：
根据所述第一目标点与所述第二目标点对应的角度获取所述角度的正弦函数；
根据所述正弦函数和球面正弦公式计算所述第一目标点的正弦函数；
通过反正弦函数计算所述第一目标点对应的角度；
确定所述第一目标点和所述第二目标点在坐标轴上的象限位置，并根据所述象限位置和所述角度计算得到所述第一航向角。
S134、根据所述偏移距离和所述第一距离，计算第一夹角。
其中，可以基于所述第一距离对应的直线作中垂线，在所述中垂线上确定所述偏移距离，然后基于三角函数中的正切函数计算所述第一夹角。其中，可以根据所述第一目标点和所述第二目标点确定偏移点，第一目标点与第二目标点连接构成第一直线，第一目标点与所述偏移点连接构成第二直线，所述第一夹角即是所述第一直线与所述第二直线的夹角。
S135、根据所述第一航向角和所述第一夹角获得第二航向角，所述第二航向角是所述偏移点相对于所述第一目标点的航向角。
其中，所述第二航向角为所述第一航向角与所述第一夹角的和，通过求和运算后获得所述第二航向角。
S136、根据所述第二航向角和所述第一目标点的坐标计算所述偏移点的坐标。
S14、根据所述第二轨迹集合生成导航轨迹。
在本实施例中，可以将所述第二轨迹集合中的全部的偏移点的坐标联系起来，从而得到所述导航轨迹。
在其他一些实施例中，所述根据所述第二轨迹集合生成导航轨迹包括：将所述第二轨迹集合中的偏移点进行过滤和压缩处理，得到处理后的偏移点；基于所述处理后的偏移点生成导航轨迹。
其中，所述过滤处理是由于卫星接收设备精度的影响、周围环境情况的影响、以及采集过程中不规律运动等，从而产生漂移点或者导致轨迹变形，将这些异常的坐标点过滤掉，以优化所述导航轨迹。
其中，轨迹坐标点是浮点数表示，且精度要求较高。轨迹数据一般是按秒采集，所以轨迹数据的量很大，在进行数据分析时，轨迹数据量太大会影响运算效率，而且很多轨迹点是没有必要分析的。因此需要对数据进行压缩，轨迹数据压缩技术的主要目标是在不影响轨迹数据精度的情况下减小轨迹数据的大小。轨迹压缩可使用Douglas-Peucker轨迹压缩算法，又称DP算法，是轨迹压缩中应用广泛的成熟算法。具体的步骤包括：
(1)在轨迹曲线在曲线首尾两点A、B之间连接一条直线AB，该直线为曲线的弦；
(2)遍历曲线上其他所有点，求每个点到直线AB的距离，找到最大距离的点C，最大距离记为dmax；
(3)比较该距离dmax与预先定义的阈值Dmax大小，如果dmax<Dmax，则将该直线AB作为曲线段的近似，曲线段处理完毕；
(4)若dmax>＝Dmax，则使C点将曲线AB分为AC和CB两段，并分别对这两段进行(1)～(3)步处理；
(5)当所有曲线都处理完毕时，依次连接各个分割点形成的折线，即为原始曲线的路径。
本发明实施例提供的定位导航轨迹的方法能够简单的实现特定区域中轨迹的规划，并且可以获得准确的导航轨迹。
在一些实施例中，在获得所述导航轨迹后，可发送所述导航轨迹至应用的设备，以使设备根据所述导航轨迹进行导航。在导航的过程中，设备运行后不断获取当前位置的坐标、速度，并通过自身携带的电子罗盘判断当前的航向。然后遍历所有点，判断当前位置离哪个偏移点最近，以此为起点，逐个寻点，判断离下一个目标点的方位角之差，通过PID调节设备运行的速度，使设备慢慢靠近目标点。到达目标点之后，再次调整下一个目标点，逐一寻点完成轨迹的导航任务。其中，PID指的是PID控制，是一种自动控制方法对应的三个控制参数，P即比例系数、I指积分系数、D指微分系数。
下面通过具体的实例说明上述定位导航轨迹的方法。
例如，如图3所示，设定求B相对于A的航向，即A为当前位置，B为目标位置。其中，Aj：A点经度，Aw：A点纬度；Bj：B点经度，Bw：B点纬度；北纬为正，南纬为负；东经为正，西经为负。经纬度使用十进制度，如DDD.DDDDDD°，非度分或度分秒。度数未加说明均采用角度制。
如图3所示，已知A、B、C表示球面上的三个点及球面上“弧线”在该点处所夹的角，C所处位置为北极点。a,b,c表示A,B,C三点的对“弧”两端点与地心连线所夹的角。O即球心，L是AB两点间球面距离，也叫做大圆距离，即过AOB三点的平面与球相交所产生的圆弧中劣弧AB的长度。R是地球平均半径。Bearing起始真航向，也叫大圆始航向。以真北为0度起点，由东向南向西顺时针旋转360度。
算法可以简化成：已知Aj、Aw、Bj、Bw、R，求L和Bearing，求AB之间的距离和B相对于A的航向。
求AB之间的距离包括：
第一步：知道AB经纬度后，要用到第一个公式，球面余弦公式：
cos(c)＝cos(a)×cos(b)+sin(a)×sin(b)×cos(C)
这里角C等于角A～OC～B，也就是面AOC与面BOC的二面角，也就是Bj-Aj。这里我们将已知数据代入，公式便写成：
cos(c)＝cos(90-Bw)×cos(90-Aw)+sin(90-Bw)×sin(90-Aw)×cos(Bj-Aj)
求得c的余弦值后用反余弦函数便可求得角c：
c＝arccos(cos(90-Bw)×cos(90-Aw)+sin(90-Bw)×sin(90-Aw)×cos(Bj-Aj))
将角度化为弧度后求取距离：
L＝R×c(弧度)
这里要注意，L的单位与R的单位是一致的，单位不同的不要忘记换算。另外最后的Bj-Aj的顺序在这里就无所谓，因为cos函数的Y轴对称性值都是一样的。
求B相对于A的航向Bearing包括：
在距离的求算中求得了角c的余弦值，要求得它的正弦值，所用的公式就是三角函数里最基本正弦余弦转换函数的一个变形：
求得正弦后，接下来我们用球面正弦公式：
将已知数据代入并稍微变形一下，公式写为：
用反正弦函数求角度，于是上式就写成：
这里需要注意一点，我们一开始的假设便是求B点相对于A点的航向，因此这里是Bj-Aj，不要写反，否则后面得不到正确结果。
得到的结果并不总符合我们对航向的定义，因此要根据B相对于A的位置在四个象限两个轴上进行讨论，依据不同情况对计算结果进行不同处理。假设A点固定于原点，则：
B点在第一象限，Bearing＝A；
B在第二象限，Bearing＝360+A；
B在第三四象限，Bearing＝180-A。
以上是距离求算和航向求算的理论依据，基于此理论依据偏移即根据当前已知轨迹进行定距离轨迹偏移。如图4所示，假设已知轨迹上A、B两点，然后利用三角函数的中垂线计算偏移后的轨迹点，两个轨迹点产生一个偏移后的轨迹点，如此将已知轨迹的所有点遍历即可产生偏移轨迹。
计算方法如图5所示：先根据以上理论，由A、B坐标获得AB之间的距离，根据需要偏移的距离d计算偏移后轨迹点A'的偏移角度θ*，然后根据A点的坐标和A'相对于A点的航向θ*，求出A'的坐标；其中，θ*是图5中的θ，与B点相对于A点的航向角的和。如此遍历A、B、C、D、E、F……点得到图4中的A'、B'、C'、D'、E'……，即新的轨迹坐标点，从而可以确定新的导航轨迹。
求A'坐标的问题可转化为如上示例部分已知坐标点Aj、Aw、R、L、Bearing等信息求B点坐标。已知Aj、Aw、R、L'(即A与A'的距离)、θ*，求A'j、A'w的问题。如下：
第一步：已知AB之间的距离L和d，利用勾股定理可计算出L'，如下：
然后，使用球面余弦公式：
cos(a)＝cos(b)×cos(c)+sin(b)×sin(c)×cos(A)
这里角A等于坐标点A'相对于坐标点A的航向角θ*。即我们将已知数据代入，公式便写成：
cos(a)＝cos(90-Aw)×cos(c)+sin(90-Aw)×sin(c)×cos(θ*)
计算得a：
a＝arccos(cos(90-Aw)×cos(c)+sin(90-Aw)×sin(c)×cos(θ*))
然后，使用球面正弦公式的反正弦函数计算如下：
然后，A'点的坐标为：A'w＝90-a，A'j＝Aj+C。
请参阅图6，图6是本发明实施例提供的一种定位导航轨迹的装置的结构示意图。该装置20包括：第一获取模块21、第二获取模块22、第一处理模块23和第二处理模块24。
所述第一获取模块21，用于获取第一轨迹集合，所述第一轨迹集合包括多个目标点；所述第二获取模块22，用于获取偏移距离；所述第一处理模块23，用于根据所述第一轨迹集合中的目标点和所述偏移距离确定偏移点，全部的所述偏移点组成第二轨迹集合；所述第二处理模块24，用于根据所述第二轨迹集合生成导航轨迹。
其中，同样请参阅图6，所述第一处理模块23包括目标点选取单元231、第一计算单元232、第二计算单元233、第三计算单元234、第四计算单元235和第五计算单元236。
所述目标点选取单元231，用于选取第一目标点和第二目标点；所述第一计算单元232，用于计算所述第一目标点与所述第二目标点的第一距离；所述第二计算单元233，用于计算所述第二目标点相对于所述第一目标点的第一航向角；所述第三计算单元234，用于根据所述偏移距离和所述第一距离，计算第一夹角；所述第四计算单元235，用于根据所述第一航向角和所述第一夹角获得第二航向角，所述第二航向角是所述偏移点相对于所述第一目标点的航向角；所述第五计算单元236，用于根据所述第二航向角和所述第一目标点的坐标计算所述偏移点的坐标。
其中，所述第一计算单元232具体用于：获取所述第一目标点经度、纬度，以及所述第二目标点的经度、纬度；根据所述第一目标点经度、纬度，所述第二目标点的经度、纬度，以及球面余弦公式，计算所述第一目标点与所述第二目标点对应的角度；将所述角度转化为弧度，并根据所述弧度计算所述第一目标点与所述第二目标点的第一距离。
其中，所述第二计算单元233具体用于：根据所述第一目标点与所述第二目标点对应的角度获取所述角度的正弦函数；根据所述正弦函数和球面正弦公式计算所述第一目标点的正弦函数；通过反正弦函数计算所述第一目标点对应的角度；确定所述第一目标点和所述第二目标点在坐标轴上的象限位置，并根据所述象限位置和所述角度计算得到所述第一航向角。
其中，所述第二处理模块24具体用于将所述第二轨迹集合中的偏移点进行过滤和压缩处理，得到处理后的偏移点；基于所述处理后的偏移点生成导航轨迹。
需要说明的是，上述定位导航轨迹的装置可执行本发明实施例所提供的定位导航轨迹的方法，具备执行方法相应的功能模块和有益效果。未在定位导航轨迹的装置实施例中详尽描述的技术细节，可参见本发明实施例所提供的定位导航轨迹的装置。
请参阅图7，图7是本发明实施例提供的一种电子设备30的硬件结构示意图，该电子设备30可用于执行如上所述的定位导航轨迹的方法，该电子设备30包括：一个或多个处理器31以及存储器32，图7中以一个处理器31为例。
处理器31和存储器32可以通过总线或其他方式连接，图7中以通过总线连接为例。
存储器32作为一种非易失性计算机可读存储介质，可用于存储非易失性软件程序、非易失性计算机可执行程序以及模块，如本发明实施例中的定位导航轨迹的方法对应的程序指令/模块。处理器31通过运行存储在存储器32中的非易失性软件程序、指令以及模块，从而执行电子设备的各种功能应用以及数据处理，即实现上述方法实施例定位导航轨迹的方法。
存储器32可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储根据定位导航轨迹的装置的使用所创建的数据等。此外，存储器32可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中，存储器32可选包括相对于处理器31远程设置的存储器，这些远程存储器可以通过网络连接至定位导航轨迹的装置。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
所述一个或者多个模块存储在所述存储器32中，当被所述一个或者多个处理器31执行时，执行上述任意方法实施例中的定位导航轨迹的方法，例如，执行以上描述的图1和图2中的方法步骤，实现图6中的模块和单元的功能。
上述产品可执行本发明实施例所提供的方法，具备执行方法相应的功能模块和有益效果。未在本实施例中详尽描述的技术细节，可参见本发明实施例所提供的方法。
本发明实施例的电子设备以多种形式存在，包括但不限于：
超移动个人计算机设备：这类设备属于个人计算机的范畴，有计算和处理功能，一般也具备移动上网特性。这类终端包括：PDA、MID和UMPC设备等。
服务器：提供计算服务的设备，服务器的构成包括处理器、硬盘、内存、系统总线等，服务器和通用的计算机架构类似，但是由于需要提供高可靠的服务，因此在处理能力、稳定性、可靠性、安全性、可扩展性、可管理性等方面要求较高。
其他具有数据交互功能的电子装置。
本发明实施例提供了一种非易失性计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，该计算机可执行指令被一个或多个处理器执行，例如图7中的一个处理器31，可使得上述一个或多个处理器可执行上述任意方法实施例中的定位导航轨迹的方法，例如，执行以上描述的图1和图2中的方法步骤，实现图6中的模块和单元的功能。
本发明实施例提供了一种计算机程序产品，所述计算机程序产品包括存储在非易失性计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被电子设备执行时，使所述电子设备执行上述任意方法实施例中的定位导航轨迹的方法，例如，执行以上描述的图1和图2中的方法步骤，实现图6中的模块和单元的功能。
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。
通过以上的实施方式的描述，本领域普通技术人员可以清楚地了解到各实施方式可借助软件加通用硬件平台的方式来实现，当然也可以通过硬件。本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory,ROM)或随机存储记忆体(Random Access Memory,RAM)等。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；在本发明的思路下，以上实施例或者不同实施例中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本发明的不同方面的许多其它变化，为了简明，它们没有在细节中提供；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
Claims (8)
1.一种定位导航轨迹的方法，其特征在于，包括：
获取第一轨迹集合，所述第一轨迹集合包括多个目标点；
获取偏移距离；
根据所述第一轨迹集合中的目标点和所述偏移距离确定偏移点，全部的所述偏移点组成第二轨迹集合；
根据所述第二轨迹集合生成导航轨迹；
其中，所述根据所述第一轨迹集合中的目标点和所述偏移距离确定偏移点，包括：
选取第一目标点和第二目标点；
计算所述第一目标点与所述第二目标点的第一距离；
计算所述第二目标点相对于所述第一目标点的第一航向角；
根据所述偏移距离和所述第一距离，计算第一夹角；
根据所述第一航向角和所述第一夹角获得第二航向角，所述第二航向角是所述偏移点相对于所述第一目标点的航向角；
根据所述第二航向角和所述第一目标点的坐标计算所述偏移点的坐标；
其中，根据所述偏移距离和所述第一距离，计算第一夹角，包括：
基于所述第一距离对应的直线作中垂线，并在所述中垂线上确定所述偏移距离；
基于所述偏移距离和所述第一距离，采用三角函数中的正切函数计算所述第一夹角。
2.根据权利要求1所述的方法，其特征在于，所述计算所述第一目标点与所述第二目标点的第一距离，包括：
获取所述第一目标点经度、纬度，以及所述第二目标点的经度、纬度；
根据所述第一目标点经度、纬度，所述第二目标点的经度、纬度，以及球面余弦公式，计算所述第一目标点与所述第二目标点对应的角度；
将所述角度转化为弧度，并根据所述弧度计算所述第一目标点与所述第二目标点的第一距离。
3.根据权利要求2所述的方法，其特征在于，所述计算所述第二目标点相对于所述第一目标点的第一航向角，包括：
根据所述第一目标点与所述第二目标点对应的角度获取所述角度的正弦函数；
根据所述正弦函数和球面正弦公式计算所述第一目标点的正弦函数；
通过反正弦函数计算所述第一目标点对应的角度；
确定所述第一目标点和所述第二目标点在坐标轴上的象限位置，并根据所述象限位置和所述角度计算得到所述第一航向角。
4.根据权利要求1至3任一项所述的方法，其特征在于，所述根据所述第二轨迹集合生成导航轨迹包括：
将所述第二轨迹集合中的偏移点进行过滤和压缩处理，得到处理后的偏移点；
基于所述处理后的偏移点生成导航轨迹。
5.一种定位导航轨迹的装置，其特征在于，包括：
第一获取模块，用于获取第一轨迹集合，所述第一轨迹集合包括多个目标点；
第二获取模块，用于获取偏移距离；
第一处理模块，用于根据所述第一轨迹集合中的目标点和所述偏移距离确定偏移点，全部的所述偏移点组成第二轨迹集合；
第二处理模块，用于根据所述第二轨迹集合生成导航轨迹；
其中，所述第一处理模块包括：
目标点选取单元，用于选取第一目标点和第二目标点；
第一计算单元，用于计算所述第一目标点与所述第二目标点的第一距离；
第二计算单元，用于计算所述第二目标点相对于所述第一目标点的第一航向角；
第三计算单元，用于根据所述偏移距离和所述第一距离，计算第一夹角；
第四计算单元，用于根据所述第一航向角和所述第一夹角获得第二航向角，所述第二航向角是所述偏移点相对于所述第一目标点的航向角；
第五计算单元，用于根据所述第二航向角和所述第一目标点的坐标计算所述偏移点的坐标；
其中，所述第三计算单元具体用于：
基于所述第一距离对应的直线作中垂线，并在所述中垂线上确定所述偏移距离；
基于所述偏移距离和所述第一距离，采用三角函数中的正切函数计算所述第一夹角。
6.根据权利要求5所述的装置，其特征在于，所述第一计算单元具体用于：
获取所述第一目标点经度、纬度，以及所述第二目标点的经度、纬度；
根据所述第一目标点经度、纬度，所述第二目标点的经度、纬度，以及球面余弦公式，计算所述第一目标点与所述第二目标点对应的角度；
将所述角度转化为弧度，并根据所述弧度计算所述第一目标点与所述第二目标点的第一距离。
7.根据权利要求5所述的装置，其特征在于，所述第二计算单元具体用于：
根据所述第一目标点与所述第二目标点对应的角度获取所述角度的正弦函数；
根据所述正弦函数和球面正弦公式计算所述第一目标点的正弦函数；
通过反正弦函数计算所述第一目标点对应的角度；
确定所述第一目标点和所述第二目标点在坐标轴上的象限位置，并根据所述象限位置和所述角度计算得到所述第一航向角。
8.一种电子设备，其特征在于，包括：
至少一个处理器；以及，
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1-4任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011607056.5A CN112764074B (zh) | 2020-12-30 | 2020-12-30 | 一种定位导航轨迹的方法、装置及电子设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011607056.5A CN112764074B (zh) | 2020-12-30 | 2020-12-30 | 一种定位导航轨迹的方法、装置及电子设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112764074A CN112764074A (zh) | 2021-05-07 |
| CN112764074B true CN112764074B (zh) | 2024-01-12 |
Family
ID=75697447
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011607056.5A Active CN112764074B (zh) | 2020-12-30 | 2020-12-30 | 一种定位导航轨迹的方法、装置及电子设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112764074B (zh) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113836123A (zh) * | 2021-07-22 | 2021-12-24 | 南京沃旭通讯科技有限公司 | 一种基于距离、角度的轨迹清洗方法 |
| CN116088414A (zh) * | 2021-11-08 | 2023-05-09 | 苏州维嘉科技股份有限公司 | 纠偏方法、装置、设备及存储介质、叠板生产线 |
| CN115755976A (zh) * | 2022-12-02 | 2023-03-07 | 安徽送变电工程有限公司 | 巡检无人机航线规划方法、系统、无人飞行器及存储介质 |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH10161728A (ja) * | 1996-11-29 | 1998-06-19 | Toyota Motor Corp | 移動軌跡算出方法および移動速度算出方法 |
| CN105424026A (zh) * | 2015-11-04 | 2016-03-23 | 中国人民解放军国防科学技术大学 | 一种基于点云轨迹的室内导航定位方法与系统 |
| CN110533730A (zh) * | 2019-08-26 | 2019-12-03 | 智久(厦门)机器人科技有限公司 | 基于b样条曲线的路径规划方法、装置、计算机可读存储介质 |
| JP6622436B1 (ja) * | 2018-12-29 | 2019-12-18 | 深セン市優必選科技股▲ふん▼有限公司Ｕｂｔｅｃｈ Ｐｏｂｏｔｉｃｓ Ｃｏｒｐ Ｌｔｄ | 位置決め・ナビゲーション方法、機器及び記憶装置 |
| CN110646002A (zh) * | 2018-06-27 | 2020-01-03 | 百度在线网络技术（北京）有限公司 | 用于处理信息的方法和装置 |
| CN110749319A (zh) * | 2018-12-19 | 2020-02-04 | 北京嘀嘀无限科技发展有限公司 | 一种路线偏移检测的方法、装置和电子设备 |
| CN111102980A (zh) * | 2019-11-14 | 2020-05-05 | 广东博智林机器人有限公司 | 路径生成方法、装置及电子设备 |
| CN111196271A (zh) * | 2020-01-21 | 2020-05-26 | 北京百度网讯科技有限公司 | 自动泊车方法、装置、设备及存储介质 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10248124B2 (en) * | 2016-07-21 | 2019-04-02 | Mobileye Vision Technologies, Inc. | Localizing vehicle navigation using lane measurements |
| US11193783B2 (en) * | 2017-04-04 | 2021-12-07 | Microsoft Technology Licensing, Llc | Guidance audio cues for navigation assistance |
| US11181919B2 (en) * | 2018-11-27 | 2021-11-23 | Wipro Limited | Method and system for determining an optimal trajectory for navigation of an autonomous vehicle |
-
2020
- 2020-12-30 CN CN202011607056.5A patent/CN112764074B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH10161728A (ja) * | 1996-11-29 | 1998-06-19 | Toyota Motor Corp | 移動軌跡算出方法および移動速度算出方法 |
| CN105424026A (zh) * | 2015-11-04 | 2016-03-23 | 中国人民解放军国防科学技术大学 | 一种基于点云轨迹的室内导航定位方法与系统 |
| CN110646002A (zh) * | 2018-06-27 | 2020-01-03 | 百度在线网络技术（北京）有限公司 | 用于处理信息的方法和装置 |
| CN110749319A (zh) * | 2018-12-19 | 2020-02-04 | 北京嘀嘀无限科技发展有限公司 | 一种路线偏移检测的方法、装置和电子设备 |
| JP6622436B1 (ja) * | 2018-12-29 | 2019-12-18 | 深セン市優必選科技股▲ふん▼有限公司Ｕｂｔｅｃｈ Ｐｏｂｏｔｉｃｓ Ｃｏｒｐ Ｌｔｄ | 位置決め・ナビゲーション方法、機器及び記憶装置 |
| CN110533730A (zh) * | 2019-08-26 | 2019-12-03 | 智久(厦门)机器人科技有限公司 | 基于b样条曲线的路径规划方法、装置、计算机可读存储介质 |
| CN111102980A (zh) * | 2019-11-14 | 2020-05-05 | 广东博智林机器人有限公司 | 路径生成方法、装置及电子设备 |
| CN111196271A (zh) * | 2020-01-21 | 2020-05-26 | 北京百度网讯科技有限公司 | 自动泊车方法、装置、设备及存储介质 |
Non-Patent Citations (4)
| Title |
|---|
| Study on navigation path correction in neuro-navigation based on near infrared spectroscopy;X. Zhu and L. Tao;《2010 3rd International Conference on Biomedical Engineering and Informatics》;1881-1885 * |
| 侯春晓 ; 许劲松 ; 杨荣武.内河无人船局部路径规划和循迹控制.《船舶工程》.2017,41-45. * |
| 喷漆机器人虚拟示教系统中喷枪轨迹插补点位姿的算法及应用研究;夏薇;王科荣;廖小平;彭勇;;现代制造工程(第10期);11-16 * |
| 夏薇 ; 王科荣 ; 廖小平 ; 彭勇 ; .喷漆机器人虚拟示教系统中喷枪轨迹插补点位姿的算法及应用研究.现代制造工程.2009,(第10期),11-16. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112764074A (zh) | 2021-05-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112764074B (zh) | 一种定位导航轨迹的方法、装置及电子设备 | |
| US20190265048A1 (en) | Map-centric map matching method and apparatus | |
| CN107067038B (zh) | 一种基于pdr轨迹绘制的室内楼宇轮廓修整方法 | |
| CN110930495A (zh) | 基于多无人机协作的icp点云地图融合方法、系统、装置及存储介质 | |
| CN112797987B (zh) | 一种无人船避障的航行方法、装置、计算机设备及存储介质 | |
| CN108426576B (zh) | 基于标识点视觉导航与sins的飞行器路径规划方法及系统 | |
| CN111339802A (zh) | 实时相对地图的生成方法及装置、电子设备和存储介质 | |
| Cai et al. | Mobile robot localization using gps, imu and visual odometry | |
| CN111649723A (zh) | 面向复杂地形的贴近摄影三维航迹与姿态规划方法及装置 | |
| CN110441760B (zh) | 一种基于先验地形图的大范围海底地形图拓展构图方法 | |
| CN112665557B (zh) | 一种波浪数据处理方法、装置、电子设备和可读存储介质 | |
| CN114063622B (zh) | 无人船自主停泊定位方法、装置及相关组件 | |
| CN111273312A (zh) | 一种智能车辆定位与回环检测方法 | |
| Deng et al. | Long-range binocular vision target geolocation using handheld electronic devices in outdoor environment | |
| CN114046792A (zh) | 一种无人船水面定位与建图方法、装置及相关组件 | |
| CN110243364B (zh) | 无人机航向确定方法、装置、无人机及存储介质 | |
| CN115079168B (zh) | 基于激光雷达与毫米波雷达融合的建图方法、装置及设备 | |
| CN115164936A (zh) | 高精地图制作中用于点云拼接的全局位姿修正方法及设备 | |
| CN113406961B (zh) | 一种无人船过桥控制方法、装置、电子设备及存储介质 | |
| CN108921896B (zh) | 一种融合点线特征的下视视觉罗盘 | |
| CN113965646A (zh) | 定位控制方法及装置、电子设备、存储介质 | |
| JP2024519424A (ja) | 複数の磁気ナビゲーションデバイスからのオーバーラップする磁気測定データの相関付け及びこのデータによる地磁気マップの更新 | |
| CN113686333B (zh) | 一种针对雷达阵面旋转的捷联惯导全姿态表示方法 | |
| CN116429121A (zh) | 基于多传感器的定位方法、装置、自移动设备及存储介质 | |
| CN115542277A (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 |