CN117516549B - 一种基于惯导与卫星的路径规划算法 - Google Patents
一种基于惯导与卫星的路径规划算法 Download PDFInfo
- Publication number
- CN117516549B CN117516549B CN202410005088.XA CN202410005088A CN117516549B CN 117516549 B CN117516549 B CN 117516549B CN 202410005088 A CN202410005088 A CN 202410005088A CN 117516549 B CN117516549 B CN 117516549B
- Authority
- CN
- China
- Prior art keywords
- node
- value
- path planning
- inertial navigation
- nodes
- 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
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 74
- 230000006870 function Effects 0.000 claims abstract description 102
- 238000013461 design Methods 0.000 claims abstract description 27
- 238000010276 construction Methods 0.000 claims abstract description 13
- 238000012545 processing Methods 0.000 claims abstract description 10
- 230000000694 effects Effects 0.000 claims abstract description 5
- 238000000034 method Methods 0.000 claims description 22
- 238000007781 pre-processing Methods 0.000 claims description 16
- 238000004364 calculation method Methods 0.000 claims description 14
- 239000002689 soil Substances 0.000 claims description 14
- 230000008569 process Effects 0.000 claims description 12
- 238000005259 measurement Methods 0.000 claims description 10
- 238000012937 correction Methods 0.000 claims description 9
- 238000011156 evaluation Methods 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 8
- PCHJSUWPFVWCPO-UHFFFAOYSA-N gold Chemical compound [Au] PCHJSUWPFVWCPO-UHFFFAOYSA-N 0.000 claims description 7
- 239000010931 gold Substances 0.000 claims description 7
- 229910052737 gold Inorganic materials 0.000 claims description 7
- 238000001514 detection method Methods 0.000 claims description 6
- 238000001914 filtration Methods 0.000 claims description 6
- 230000004927 fusion Effects 0.000 claims description 5
- 230000007613 environmental effect Effects 0.000 claims description 4
- 238000009499 grossing Methods 0.000 claims description 4
- 239000004927 clay Substances 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 230000007246 mechanism Effects 0.000 claims description 3
- 230000000630 rising effect Effects 0.000 claims description 3
- 230000003068 static effect Effects 0.000 claims description 3
- MKXZASYAUGDDCJ-NJAFHUGGSA-N dextromethorphan Chemical compound C([C@@H]12)CCC[C@]11CCN(C)[C@H]2CC2=CC=C(OC)C=C21 MKXZASYAUGDDCJ-NJAFHUGGSA-N 0.000 claims 8
- 230000002159 abnormal effect Effects 0.000 claims 1
- 238000012876 topography Methods 0.000 abstract description 4
- 230000009286 beneficial effect Effects 0.000 abstract description 2
- 238000010845 search algorithm Methods 0.000 description 5
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000005265 energy consumption Methods 0.000 description 3
- 238000003973 irrigation Methods 0.000 description 3
- 230000002262 irrigation Effects 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000010801 machine learning Methods 0.000 description 2
- 230000001105 regulatory effect Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000004880 explosion Methods 0.000 description 1
- 230000004720 fertilization Effects 0.000 description 1
- 238000003306 harvesting Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000000575 pesticide Substances 0.000 description 1
- 238000009331 sowing Methods 0.000 description 1
- 238000005507 spraying Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000009333 weeding Methods 0.000 description 1
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/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/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
- 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
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于惯导与卫星的路径规划算法，包括农田地图构建、组合定位、代价函数设计、启发函数设计和改进A星路径搜索；所述农田地图构建将实际农田地形处理为可以寻址的数据形式，所述组合定位利用GPS和惯导确定农业机器人的实际位置；所述改进A星路径搜索利用传统的A星算法，改进了的启发式函数和代价函数；本发明属于勘测、导航、陀螺仪技术领域，具体为一种基于惯导与卫星的路径规划算法，本发明的优点为：基于惯导与卫星的路径规划算法专利具有提高导航准确性、考虑地形和土地类型、最优路径规划以及实时更新和路径修正等有益效果，这些效果有助于提高农业机器人的导航精度、作业效率和农田作业质量。
Description
技术领域
本发明属于勘测、导航、陀螺仪技术领域，具体为一种基于惯导与卫星的路径规划算法。
背景技术
路径规划算法可用于设计农业机器人、自动驾驶农用车辆的自主导航系统，通过优化路径选择，这些系统可以在农田中自动导航，执行任务如播种、收割、除草等；路径规划可根据作物的生长需求、土壤条件和其他环境因素，指导种植机器人在农田中选择最佳的种植路径，以提高产量和质量；路径规划算法可以在施肥和喷药任务中起到关键作用；路径规划可以帮助优化农田的灌溉策略，通过考虑土壤湿度、植物的水分需求和灌溉设施的布局，算法可以确定最佳的灌溉路径和时间，以提高水资源利用效率，并减少能源消耗。
现有的常用路径规划算法包括Dijkstra算法、A星算法、跳点搜索算法、概率图搜索算法等，其中Dijkstra算法是一种单源最短路径算法，可用于路径规划。它通过遍历节点和边来确定最短路径，然而，Dijkstra算法没有考虑启发式信息，可能会导致计算时间较长，并且在面对大规模农田时可能效果不佳；A星算法是一种经典的启发式搜索算法，可用于路径规划，它通过估计节点到目标节点的代价来选择最佳路径，然而A星算法在处理大规模农田时可能面临搜索空间爆炸的问题，导致计算复杂度增加；跳点搜索（Jump PointSearch）算法是一种优化的路径搜索算法，旨在减少搜索空间并加速路径规划过程，但跳点搜索算法仅适用于网格地图，对于其他类型的地图结构，跳点搜索算法通常需要进行预处理以计算和存储跳点信息，这些预处理步骤可能需要较长的时间和额外的内存开销，跳点搜索算法在路径规划过程中假设障碍物不会发生变化，如果障碍物在搜索过程中发生变化，算法可能需要重新计算路径，增加计算复杂度和规划时间。
发明内容
（一）要解决的技术问题
（1）小型农业机器人大多采用电力驱动，传统路径规划算法没有考虑到使耗电量最低这一重要因素；
（2）传统的路径规划算法将地图处理为栅格地图，但由于农田的特殊地形，如果简单的将其处理为栅格地图，将会增加路径规划过程中搜索节点的数量，从而大大降低实时性；
（3）传统的路径规划算法在引入启发函数时，往往选择将直线距离或曼哈顿距离作为启发函数，但是引入什么样的启发函数才能使路径规划的效果最佳好没有确定的方案。
（二）技术方案
为了解决上述问题，本发明采用的一种技术方案为：一种基于惯导与卫星的路径规划算法，包括农田地图构建、组合定位、代价函数设计、启发函数设计和改进A星路径搜索，其中农田地图构建用于将实际的农田地形处理为可以寻址的数据形式，这样路径规划算法才能基于构建的地图搜索路径，所述组合定位利用GPS和惯导确定农业机器人的起始位置和目标位置，也为路径规划提供位置、速度、加速度以及必要的姿态信息；所述改进A星路径搜索利用传统的A星算法，并结合了改进的启发式函数和机器学习算法，从而增加路径规划算法提升实时性和鲁棒性的同时减少了能源消耗，并能在环境发生动态变化时，实施调整规划路线；所述代价函数设计是从路径规划的一个节点到相邻节点难易程度的设计方法，设计的好坏将影响路径规划算法的整体性能；启发函数设计是路径规划的一个节点到目标节点一种启示，良好的启发函数对加速路径规划与重要作用。
进一步地，所述农田地图构建的步骤如下：
S1、数据采集：通过高精度GPS测量采集农田的地理数据，这些数据提供农田的边界、土地利用类型和地形高程信息；
S2、地图建模：通过数字化处理和地理信息系统（GIS）软件将采集到的数据转化为可供路径规划算法使用的地图模型，根据数据的类型和精度创建栅格-矢量地图，栅格-矢量地图将农田的可耕地范围设置为只能前进和后退的二向通路，将农田的非耕地部分划分为网格单元，并设置为八联通的通路，同时为每个单元分配属性值，如土地类型和高程；农田的边界则使用线段、多边形等几何要素来表示；
S3、地图预处理：对地图进行预处理以准备路径规划，这包括平滑地势、填补空洞和去除噪声操作，以提高地图的质量和准确性，预处理还可以包括将农田地图与其他地图数据（如道路网络）进行集成，以便在路径规划中考虑更多的因素；
S4、地图分辨率和精度选择：根据路径规划算法的要求、农业机器人的尺寸、农田的特点，选择合适的地图分辨率和精度；
S5、障碍物建模：识别和建模农田中的障碍物，如农作物、建筑物、水体、洼地、山坡、水渠、水井和工作台，这些障碍物可以对路径规划产生影响，需要在地图中进行标记或建模，以便路径规划算法能够避开它们；
S6、地图更新：农田地图可能会随着时间和农业活动的变化而更新，因此，需要建立地图更新的机制，定期或根据需要对地图进行更新，以确保路径规划的准确性和实用性。
作为优选地，所述组合定位是一种将全球定位系统（GPS）和惯性导航系统（Inertial Navigation System，INS）相结合的技术，以获得更精确和可靠的位置估计，以下是使用GPS和INS实现组合导航定位的步骤：
A1、数据采集：收集GPS接收器和惯导传感器（如加速度计和陀螺仪）的原始数据。GPS接收器用于接收卫星信号并提供位置和时间信息，而惯导传感器可以测量加速度和角速度；
A2、数据预处理：对采集到的GPS和惯导数据进行预处理，包括去除异常值、滤波处理、校正传感器误差等。滤波算法（如卡尔曼滤波器）可以用于整合GPS和惯导数据，并提供更准确的位置和姿态估计；
A3、初始对准：在开始导航之前，需要进行初始对准（Alignment），初始对准是将惯导坐标系与地理坐标系对齐的过程，这可以通过采集静止状态下的数据，并使用初始位置和姿态估计算法实现；
A4、动态导航：在导航过程中，GPS和惯导数据将被用于实时位置估计。GPS提供全球位置和速度信息，惯导传感器提供连续的加速度和角速度测量，通过组合两者的数据，可以获得更准确的位置和姿态估计；
A5、状态更新：通过周期性地将GPS测量值与惯导预测值进行融合，融合算法（如扩展卡尔曼滤波器）将考虑GPS的测量误差和惯导的漂移，根据其权重更新导航状态；
A6、故障检测和修正：在组合导航中，需要进行故障检测和修正，以确保位置估计的准确性和可靠性，这可能涉及到检测GPS信号异常、惯导传感器故障或外部环境变化，并采取相应的措施进行修正或切换备用导航系统；
A7、输出结果：组合定位将提供更新的位置和姿态估计作为输出结果，这些估计将用于农业机器人的路径规划任务中。
进一步地，所述代价函数设计包括以下部分：
（1）欧式距离：两个节点之间的欧式距离是指两个节点之间的实际的距离，单位是米，因此当两个节点之间的实际距离越小时，他们之间的代价函数也会越小；
（2）高程差：相邻两个节点之间的高程存在差异，如果子节点的高程大于父节点的高程，那么从父节点到子节点将消耗更多能量，因此高程差是代价函数设计重要因素；
（3）土地类型：相邻两个节点之间的土地类型可能存在差异，农业机器人在黏性土地上移动和普通的农田中移动所消耗的能量差别较大；
因此代价函数的数学表达式为：
上式中，gtm表示节点Xtemp到节点Xm的代价值，α、β、γ表示加权系数，Dtm表示两节点之间的距离，Htm表示两节点之间的高程差，当高程升高时，该值为正，当高程减少时，高程为负值；Ptm表示两节点之间的土壤性质差异值，本发明中，将土壤的粘性、稀疏性和表面硬度综合考量，提出了强粘、弱粘、一般和优良四个等级；一个等级到另一个等级的过度使用土壤性质差异值Ptm的倍数表示；相邻等级为一倍的Ptm，相差两个等级为2倍的Ptm，以此类推；从强粘到弱粘土壤性质差异值Ptm为负数。
作为优选地，所述启发函数设计的步骤如下：
（1）计算当前节点和目标节点之间的X方向的差值的绝对值；
（2）计算当前节点和目标节点之间的Y方向的差值的绝对值；
（3）计算系数k，k的计算方式为：
（4）计算系数q，q的值为GPS位置定位的定位精度，其值取值范围在0到1之间；
（5）计算启发函数值：计算方式如下：
上式中，node表示当前节点，node.x表示当前节点的x坐标，node.y表示当前节点的y坐标，goal.x表示目标节点的x坐标，goal.y表示目标节点的y坐标，max表示求最大值，min表示求最小值，h表示当前节点的启发函数值。
进一步地，所述改进A星路径搜索的步骤如下：
B0、根据农业机器人的实际位置确定路径规划的开始节点Xs和目标节点Xg；
B1、维护一个优先级队列Sp，用于存储所有的被扩展的节点；在开始时刻，优先级队列中只有一个开始节点Xs；
B2、为优先级队列中的每个节点映射一个对应的评价分数F，评价分数F由两部分构成，包括代价函数值g和启发函数值h，其中开始节点的代价函数g(Xs)为零；其他的所有的没被加入到优先级队列中的节点的代价函数定义为无穷大；
B3、判断优先级队列中是否为空，如果为空，输出路径规划失败；
B4、将具有最低评价分数F的节点Xtemp从优先级队列中弹出，并将弹出的节点标记为被扩展，并将该节点添加到已扩展队列Sv中；
B5、判断B4中弹出的节点Xtemp是否是目标节点Xg，如果是，那么路径规划结束，找到目标节点；
B6、如果B4中弹出的节点Xtemp不是目标节点，那么扩展节点Xtemp，找到其所有的邻居节点，对于所有的邻居节点做如下判断：
（1）如果这个节点的代价函数值为无穷大，那么说明这个节点从未被加入到优先级队列中，那么计算这个节点的代价函数，计算方式为节点Xtemp的代价函数加上从节点Xtemp到该扩展节点的代价函数，计算公式为：
上式中Xm表示节点Xtemp扩展的某一个子节点，infinite表示代价函数的值为无穷大，
g(Xm)表示节点Xm的代价函数值，g(Xtemp)表示节点Xtemp的代价函数值，h(Xm)表示节点Xm的启发函数值，gtm表示从节点Xtemp到节点Xm的代价，append表示将节点Xtemp的加入到优先级队列Sp中；
（2）若这个节点的代价函数值不是无穷大，如果这个节点的代价函数值大于节点Xtemp的代价函数值加上节点Xtemp到该节点的代价，那么更新该节点的代价函数，计算公式为：
由于这个节点的代价函数值不是无穷大，说明该节点已经存在于优先级队列Sp中，因此不必重复添加到优先级队列Sp中；
B7、重复执行B3到B6的步骤，直到算法找到目标节点。
（三）有益效果
提高了导航准确性：通过组合定位技术，将GPS和惯性导航系统相结合，可以获得农业机器人更准确的位置和姿态信息，从而提高导航的准确性。这有助于农业机器人在农田中精确地规划路径，避免偏离目标或与障碍物发生碰撞；
考虑地形和土地类型：路径规划算法考虑了地形高程和土地类型的差异，将其纳入代价函数中。这样，农业机器人在规划路径时会尽量选择相对平坦和适合行驶的地形，以及适合当前作业任务的土地类型，从而提高机器人在农田中的行进效率和作业质量；
最优路径规划：通过使用改进的A星路径搜索算法，可以快速找到起始点到目标点的最优路径；该算法综合考虑了节点的代价函数值和启发式估计值，能够有效地搜索最优路径，减少路径长度和行进时间，提高农业机器人的效率；
实时更新和路径修正：基于组合定位技术，路径规划算法能够实时更新农业机器人的位置和姿态状态。如果出现定位错误或故障，算法能够检测并修正路径，确保机器人能够在实际环境中准确地导航和作业。
附图说明
图1为本发明提出的一种基于惯导与卫星的路径规划算法的流程图；
图2为本发明提出的农田地图构建的流程图；
图3为本发明提出的组合定位的流程图；
图4为本发明提出的代价函数设计的流程图；
图5为本发明提出的启发函数设计的流程图；
图6为本发明提出的改进A星路径搜索的流程图；
图7为使用传统的A星算法进行路径搜索的仿真图；
图8为使用Dijkstra算法进行路径搜索的仿真图；
图9为使用本发明提出的路径搜索算法进行的仿真图。
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。
具体实施方式
下面将结合本方案实施例中的附图，对本方案实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本方案一部分实施例，而不是全部的实施例；基于本方案中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本方案保护的范围。
一种基于惯导与卫星的路径规划算法，包括农田地图构建、组合定位、代价函数设计、启发函数设计和改进A星路径搜索，其中农田地图构建用于将实际的农田地形处理为可以寻址的数据形式，这样路径规划算法才能基于构建的地图搜索路径，所述组合定位利用GPS和惯导确定农业机器人的起始位置和目标位置，也为路径规划提供位置、速度、加速度以及必要的姿态信息；改进A星路径搜索利用传统的A星算法，并结合了改进的启发式函数和机器学习算法，从而增加路径规划算法提升实时性和鲁棒性的同时减少了能源消耗，并能在环境发生动态变化时，实施调整规划路线。
所述农田地图构建的步骤如下：
S1、数据采集：通过高精度GPS测量采集农田的地理数据，这些数据提供农田的边界、土地利用类型和地形高程信息；
S2、地图建模：通过数字化处理和地理信息系统（GIS）软件将采集到的数据转化为可供路径规划算法使用的地图模型，根据数据的类型和精度创建栅格-矢量地图，栅格-矢量地图将农田的可耕地范围设置为只能前进和后退的二向通路，将农田的非耕地部分划分为网格单元，并设置为八联通的通路，同时为每个单元分配属性值，如土地类型和高程；农田的边界则使用线段、多边形等几何要素来表示；
S3、地图预处理：对地图进行预处理以准备路径规划，这包括平滑地势、填补空洞和去除噪声操作，以提高地图的质量和准确性，预处理还可以包括将农田地图与其他地图数据（如道路网络）进行集成，以便在路径规划中考虑更多的因素；
S4、地图分辨率和精度选择：根据路径规划算法的要求、农业机器人的尺寸、农田的特点，选择合适的地图分辨率和精度；
S5、障碍物建模：识别和建模农田中的障碍物，如农作物、建筑物、水体、洼地、山坡、水渠、水井和工作台，这些障碍物可以对路径规划产生影响，需要在地图中进行标记或建模，以便路径规划算法能够避开它们；
S6、地图更新：农田地图可能会随着时间和农业活动的变化而更新，因此，需要建立地图更新的机制，定期或根据需要对地图进行更新，以确保路径规划的准确性和实用性。
所述组合定位是一种将全球定位系统（GPS）和惯性导航系统（InertialNavigation System，INS）相结合的技术，以获得更精确和可靠的位置估计，以下是使用GPS和INS实现组合导航定位的步骤：
A1、数据采集：收集GPS接收器和惯导传感器（如加速度计和陀螺仪）的原始数据。GPS接收器用于接收卫星信号并提供位置和时间信息，而惯导传感器可以测量加速度和角速度；
A2、数据预处理：对采集到的GPS和惯导数据进行预处理，包括去除异常值、滤波处理、校正传感器误差等。滤波算法（如卡尔曼滤波器）可以用于整合GPS和惯导数据，并提供更准确的位置和姿态估计；
A3、初始对准：在开始导航之前，需要进行初始对准（Alignment），初始对准是将惯导坐标系与地理坐标系对齐的过程，这可以通过采集静止状态下的数据，并使用初始位置和姿态估计算法实现；
A4、动态导航：在导航过程中，GPS和惯导数据将被用于实时位置估计。GPS提供全球位置和速度信息，惯导传感器提供连续的加速度和角速度测量，通过组合两者的数据，可以获得更准确的位置和姿态估计；
A5、状态更新：通过周期性地将GPS测量值与惯导预测值进行融合，融合算法（如扩展卡尔曼滤波器）将考虑GPS的测量误差和惯导的漂移，根据其权重更新导航状态；
A6、故障检测和修正：在组合导航中，需要进行故障检测和修正，以确保位置估计的准确性和可靠性，这可能涉及到检测GPS信号异常、惯导传感器故障或外部环境变化，并采取相应的措施进行修正或切换备用导航系统；
A7、输出结果：组合定位将提供更新的位置和姿态估计作为输出结果，这些估计将用于农业机器人的路径规划任务中。
所述代价函数设计包括以下部分：
（4）欧式距离：两个节点之间的欧式距离是指两个节点之间的实际的距离，单位是米，因此当两个节点之间的实际距离越小时，他们之间的代价函数也会越小；
（5）高程差：相邻两个节点之间的高程存在差异，如果子节点的高程大于父节点的高程，那么从父节点到子节点将消耗更多能量，因此高程差是代价函数设计重要因素；
（6）土地类型：相邻两个节点之间的土地类型可能存在差异，农业机器人在黏性土地上移动和普通的农田中移动所消耗的能量差别较大；
因此代价函数的数学表达式为：
上式中，gtm表示节点Xtemp到节点Xm的代价值，α、β、γ表示加权系数，Dtm表示两节点之间的距离，Htm表示两节点之间的高程差，当高程升高时，该值为正，当高程减少时，高程为负值；Ptm表示两节点之间的土壤性质差异值，本发明中，将土壤的粘性、稀疏性和表面硬度综合考量，提出了强粘、弱粘、一般和优良四个等级；一个等级到另一个等级的过度使用土壤性质差异值Ptm的倍数表示；相邻等级为一倍的Ptm，相差两个等级为2倍的Ptm，以此类推；从强粘到弱粘土壤性质差异值Ptm为负数。
所述启发函数设计的步骤如下：
（1）计算当前节点和目标节点之间的X方向的差值的绝对值；
（2）计算当前节点和目标节点之间的Y方向的差值的绝对值；
（3）计算系数k，k的计算方式为：
（4）计算系数q，q的值为GPS位置定位的定位精度，其值取值范围在0到1之间；
（5）计算启发函数值：计算方式如下：
上式中，node表示当前节点，node.x表示当前节点的x坐标，node.y表示当前节点的y坐标，goal.x表示目标节点的x坐标，goal.y表示目标节点的y坐标，max表示求最大值，min表示求最小值，h表示当前节点的启发函数值。
所述改进A星路径搜索的步骤如下：
B0、根据农业机器人的实际位置确定路径规划的开始节点Xs和目标节点Xg；
B1、维护一个优先级队列Sp，用于存储所有的被扩展的节点；在开始时刻，优先级队列中只有一个开始节点Xs；
B2、为优先级队列中的每个节点映射一个对应的评价分数F，评价分数F由两部分构成，包括代价函数值g和启发函数值h，其中开始节点的代价函数g(Xs)为零；其他的所有的没被加入到优先级队列中的节点的代价函数定义为无穷大；
B3、判断优先级队列中是否为空，如果为空，输出路径规划失败；
B4、将具有最低评价分数F的节点Xtemp从优先级队列中弹出，并将弹出的节点标记为被扩展，并将该节点添加到已扩展队列Sv中；
B5、判断B4中弹出的节点Xtemp是否是目标节点Xg，如果是，那么路径规划结束，找到目标节点；
B6、如果B4中弹出的节点Xtemp不是目标节点，那么扩展节点Xtemp，找到其所有的邻居节点，对于所有的邻居节点做如下判断：
（1）如果这个节点的代价函数值为无穷大，那么说明这个节点从未被加入到优先级队列中，那么计算这个节点的代价函数，计算方式为节点Xtemp的代价函数加上从节点Xtemp到该扩展节点的代价函数，计算公式为：
上式中Xm表示节点Xtemp扩展的某一个子节点，infinite表示代价函数的值为无穷大，
g(Xm)表示节点Xm的代价函数值，g(Xtemp)表示节点Xtemp的代价函数值，h(Xm)表示节点Xm的启发函数值，gtm表示从节点Xtemp到节点Xm的代价，append表示将节点Xtemp的加入到优先级队列Sp中；
（2）若这个节点的代价函数值不是无穷大，如果这个节点的代价函数值大于节点Xtemp的代价函数值加上节点Xtemp到该节点的代价，那么更新该节点的代价函数值、启发函数值和评价分数F，计算公式为：
由于这个节点的代价函数值不是无穷大，说明该节点已经存在于优先级队列Sp中，因此不必重复添加到优先级队列Sp中；
B7、重复执行B3到B6的步骤，直到算法找到目标节点。
将图7、图8和图9进行对比，可见，本发明提出的一种基于惯导与卫星的路径规划算法在搜索到最优路径的同时，扩展的节点数目是最小的，因此具有最快的速度同时由于使用了组合定位的策略，因此在路径规划过程中的加鲁棒增强。
实施例一：
一种基于惯导与卫星的路径规划算法的大体实施步骤如下：
1、数据采集：使用高精度GPS测量收集农田的地理数据，包括边界、土地利用类型和地形高程信息。
2、地图构建与预处理：通过数字化处理和地理信息系统（GIS）软件将采集到的数据转化为路径规划算法可用的地图模型，包括平滑处理、填补空洞、去除噪声等操作。
3、组合定位：将GPS和惯性导航系统（INS）相结合，通过数据采集、预处理、初始对准、动态导航等步骤获得农业机器人的精确位置和姿态信息。
4、设计代价函数：定义衡量路径规划中各个节点的代价函数，包括欧式距离、高程差和土地类型等。
5、设计启发函数：根据当前节点和目标节点的差值计算启发式估计值，用于指导路径搜索。
6、改进的A星路径搜索：初始化起始节点，循环搜索选择代价函数值加启发式估计值最小的节点作为当前节点，计算相邻节点的代价函数值和启发式估计值，并更新节点信息，最终回溯路径形成最优路径。
7、输出最优路径：从起始节点到目标节点的最优路径，作为农业机器人在农田中导航和操作的指导。
以上便是本发明具体的工作流程，下次使用时重复此步骤即可。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。
以上对本发明及其实施方式进行了描述，这种描述没有限制性，附图中所示的也只是本发明的实施方式之一，实际的结构并不局限于此。总而言之如果本领域的普通技术人员受其启示，在不脱离本发明创造宗旨的情况下，不经创造性的设计出与该技术方案相似的结构方式及实施例，均应属于本发明的保护范围。
Claims (3)
1.一种基于惯导与卫星的路径规划算法，包括农田地图构建、组合定位、代价函数设计、启发函数设计和改进A星路径搜索，所述改进A星路径搜索利用传统的A星算法，改进了启发式函数和代价函数；所述代价函数设计是从路径规划的一个节点到相邻节点难易程度的设计方法；所述启发函数设计是路径规划的一个节点到目标节点启示性设计方法；所述代价函数设计包括以下部分：
欧式距离：两个节点之间的欧式距离是指两个节点之间的实际的距离，单位是米，因此当两个节点之间的实际距离越小时，他们之间的代价函数也会越小；
高程差：相邻两个节点之间的高程存在差异，如果子节点的高程大于父节点的高程，那么从父节点到子节点将消耗更多能量，因此高程差是代价函数设计重要因素；
土地类型：相邻两个节点之间的土地类型可能存在差异，农业机器人在黏性土地上移动和普通的农田中移动所消耗的能量差别较大；
因此代价函数的数学表达式为：
，
上式中，g tm表示节点X temp到节点X m的代价值，α、β、γ表示加权系数，D tm表示两节点之间的距离，H tm表示两节点之间的高程差，当高程升高时，该值为正，当高程减少时，高程为负值；P tm表示两节点之间的土壤性质差异值，将土壤的粘性、稀疏性和表面硬度综合考量，提出了强粘、弱粘、一般和优良四个等级；一个等级到另一个等级的过度使用土壤性质差异值P tm的倍数表示；相邻等级为一倍的P tm，相差两个等级为2倍的P tm，以此类推；从强粘到弱粘土壤性质差异值P tm为负数；所述启发函数设计的步骤如下：
计算当前节点和目标节点之间的X方向的差值的绝对值；
，
上式中，node表示当前节点，node.x表示当前节点的x坐标，goal表示目标节点，goal.x表示目标节点的 x 坐标；
计算当前节点和目标节点之间的Y方向的差值的绝对值；
，
上式中，node.y 表示当前节点的y坐标，goal.y 表示目标节点的y坐标；
计算系数k，k的计算方式为：
，
计算系数q，q的值为GPS位置定位的定位精度，其值取值范围在0到1之间；
计算启发函数值：计算方式如下：
，
max表示求最大值，min表示求最小值，h表示当前节点的启发函数值；所述改进A星路径搜索的步骤如下：
B0、根据农业机器人的实际位置确定路径规划的开始节点X s 和目标节点X g ；
B1、维护一个优先级队列S p ，用于存储所有的被扩展的节点；在开始时刻，优先级队列中只有一个开始节点X s ；
B2、为优先级队列中的每个节点映射一个对应的评价分数F，评价分数F由两部分构成，包括代价函数值g和启发函数值h，其中开始节点的代价函数g(X s )为零；其他的所有的没被加入到优先级队列中的节点的代价函数定义为无穷大；
B3、判断优先级队列中是否为空，如果为空，输出路径规划失败；
B4、将具有最低评价分数F的节点X temp 从优先级队列中弹出，并将弹出的节点标记为被扩展，并将该节点添加到已扩展队列S v 中；
B5、判断B4中弹出的节点X temp 是否是目标节点X g ，如果是，那么路径规划结束，找到目标节点；
B6、如果B4中弹出的节点X temp 不是目标节点，那么扩展节点X temp ，找到其所有的邻居节点，对于所有的邻居节点做如下判断：
如果这个节点的代价函数值为无穷大，那么说明这个节点从未被加入到优先级队列中，那么计算这个节点的代价函数，计算方式为节点X temp 的代价函数加上从节点X temp 到该扩展节点的代价函数，计算公式为：
，
上式中X m 表示节点X temp 扩展的某一个子节点，infinite表示代价函数的值为无穷大，g(X m )表示节点X m 的代价函数值，g(X temp )表示节点X temp 的代价函数值，h(X m )表示节点X m 的启发函数值，g tm表示从节点X temp 到节点X m 的代价，append表示将节点X temp 的加入到优先级队列S p 中；
若这个节点的代价函数值不是无穷大，如果这个节点的代价函数值大于节点X temp 的代价函数值加上节点X temp 到该节点的代价，那么更新该节点的代价函数，计算公式为：
，
由于这个节点的代价函数值不是无穷大，说明该节点已经存在于优先级队列S p 中，因此不必重复添加到优先级队列S p 中；
B7、重复执行B3到B6的步骤，直到算法找到目标节点。
2.根据权利要求1所述的一种基于惯导与卫星的路径规划算法，其特征在于：所述农田地图构建的步骤如下：
S1、数据采集：通过高精度GPS测量采集农田的地理数据，这些数据提供农田的边界、土地利用类型和地形高程信息；
S2、地图建模：通过数字化处理和地理信息系统（GIS）软件将采集到的数据转化为可供路径规划算法使用的地图模型，根据数据的类型和精度创建栅格-矢量地图，栅格-矢量地图将农田的可耕地范围设置为只能前进和后退的二向通路，将农田的非耕地部分划分为网格单元，并设置为八联通的通路，同时为每个单元分配属性值；农田的边界则使用 线段、多边形几何要素来表示；
S3、地图预处理：对地图进行预处理以准备路径规划，这包括平滑地势、填补空洞和去除噪声操作，以提高地图的质量和准确性，预处理还包括将农田地图与其他地图数据进行集成，以便在路径规划中考虑更多的因素；
S4、地图分辨率和精度选择：根据路径规划算法的要求、农业机器人的尺寸、农田的特点，选择合适的地图分辨率和精度；
S5、障碍物建模：识别和建模农田中的障碍物，这些障碍物对路径规划产生影响，需要在地图中进行标记或建模，以便路径规划算法能够避开它们；
S6、地图更新：农田地图可能会随着时间和农业活动的变化而更新，因此，需要建立地图更新的机制，定期或根据需要对地图进行更新，以确保路径规划的准确性和实用性。
3.根据权利要求1所述的一种基于惯导与卫星的路径规划算法，其特征在于：所述组合定位是一种将全球定位系统（GPS）和惯性导航系统（Inertial Navigation System，INS）相结合的技术，以获得更精确和可靠的位置估计，以下是使用GPS和INS实现组合导航定位的步骤：
A1、数据采集：收集GPS接收器和惯导传感器的原始数据，GPS接收器用于接收卫星信号并提供位置和时间信息，而惯导传感器测量加速度和角速度；
A2、数据预处理：对采集到的GPS和惯导数据进行预处理，包括去除异常值、滤波处理、校正传感器误差，滤波算法用于整合GPS和惯导数据，并提供更准确的位置和姿态估计；
A3、初始对准：在开始导航之前，需要进行初始对准，初始对准是将惯导坐标系与地理坐标系对齐的过程，通过采集静止状态下的数据，并使用初始位置和姿态估计算法实现；
A4、动态导航：在导航过程中，GPS和惯导数据将被用于实时位置估计，GPS提供全球位置和速度信息，惯导传感器提供连续的加速度和角速度测量，通过组合两者的数据，获得更准确的位置和姿态估计；
A5、状态更新：通过周期性地将GPS测量值与惯导预测值进行融合，融合算法将考虑GPS 的测量误差和惯导的漂移，根据其权重更新导航状态；
A6、故障检测和修正：在组合导航中，需要进行故障检测和修正，以确保位置估计的准确性和可靠性，这可能涉及到检测GPS信号异常、惯导传感器故障或外部环境变化，并采取相应的措施进行修正或切换备用导航系统；
A7、输出结果：组合定位将提供更新的位置和姿态估计作为输出结果，这些估计将用于农业机器人的路径规划任务中。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410005088.XA CN117516549B (zh) | 2024-01-03 | 2024-01-03 | 一种基于惯导与卫星的路径规划算法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410005088.XA CN117516549B (zh) | 2024-01-03 | 2024-01-03 | 一种基于惯导与卫星的路径规划算法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117516549A CN117516549A (zh) | 2024-02-06 |
| CN117516549B true CN117516549B (zh) | 2024-03-29 |
Family
ID=89766739
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410005088.XA Active CN117516549B (zh) | 2024-01-03 | 2024-01-03 | 一种基于惯导与卫星的路径规划算法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117516549B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118625366A (zh) * | 2024-08-08 | 2024-09-10 | 比亚迪股份有限公司 | 定位系统的定位方法及装置、存储介质和计算机程序产品 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102880186A (zh) * | 2012-08-03 | 2013-01-16 | 北京理工大学 | 基于稀疏a*算法和遗传算法的航迹规划方法 |
| CN112612266A (zh) * | 2020-12-04 | 2021-04-06 | 湖南大学 | 一种非结构化道路全局路径规划方法与系统 |
| CN115566428A (zh) * | 2022-10-11 | 2023-01-03 | 江苏领创星通卫星通信科技有限公司 | 动中通天线的寻星方法、装置、设备及介质 |
| CN115562290A (zh) * | 2022-10-20 | 2023-01-03 | 南京航空航天大学 | 一种基于a星惩罚控制优化算法的机器人路径规划方法 |
| CN115686064A (zh) * | 2022-11-10 | 2023-02-03 | 哈尔滨工业大学 | 一种基于改进a星算法的空投飞行器路径规划方法及系统 |
| CN117289322A (zh) * | 2023-11-24 | 2023-12-26 | 江苏领创星通卫星通信科技有限公司 | 一种基于imu、gps与uwb的高精度定位算法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20230331217A1 (en) * | 2022-04-14 | 2023-10-19 | Mitsubishi Electric Research Laboratories, Inc. | System and Method for Motion and Path Planning for Trailer-Based Vehicle |
-
2024
- 2024-01-03 CN CN202410005088.XA patent/CN117516549B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102880186A (zh) * | 2012-08-03 | 2013-01-16 | 北京理工大学 | 基于稀疏a*算法和遗传算法的航迹规划方法 |
| CN112612266A (zh) * | 2020-12-04 | 2021-04-06 | 湖南大学 | 一种非结构化道路全局路径规划方法与系统 |
| CN115566428A (zh) * | 2022-10-11 | 2023-01-03 | 江苏领创星通卫星通信科技有限公司 | 动中通天线的寻星方法、装置、设备及介质 |
| CN115562290A (zh) * | 2022-10-20 | 2023-01-03 | 南京航空航天大学 | 一种基于a星惩罚控制优化算法的机器人路径规划方法 |
| CN115686064A (zh) * | 2022-11-10 | 2023-02-03 | 哈尔滨工业大学 | 一种基于改进a星算法的空投飞行器路径规划方法及系统 |
| CN117289322A (zh) * | 2023-11-24 | 2023-12-26 | 江苏领创星通卫星通信科技有限公司 | 一种基于imu、gps与uwb的高精度定位算法 |
Non-Patent Citations (1)
| Title |
|---|
| 复杂环境下农业机器人路径规划优化方法;殷建军;董文龙;梁利华;谢伟东;项祖丰;;农业机械学报;20190219(第05期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117516549A (zh) | 2024-02-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113029137B (zh) | 一种多源信息自适应融合定位方法及系统 | |
| CN117516549B (zh) | 一种基于惯导与卫星的路径规划算法 | |
| Sun et al. | Robust IMU/GPS/VO integration for vehicle navigation in GNSS degraded urban areas | |
| CN105509738B (zh) | 基于惯导/多普勒雷达组合的车载定位定向方法 | |
| Han et al. | An improved TERCOM-based algorithm for gravity-aided navigation | |
| CN103777220B (zh) | 基于光纤陀螺、速度传感器和gps的实时精确位姿估计方法 | |
| CN101082493B (zh) | 一种农业机械导航的组合定位方法 | |
| CN106197428B (zh) | 一种利用测量信息优化分布式ekf估计过程的slam方法 | |
| CN112697138B (zh) | 一种基于因子图优化的仿生偏振同步定位与构图的方法 | |
| CN113654555A (zh) | 一种基于多传感器数据融合的自动驾驶车辆高精定位方法 | |
| CN107167130A (zh) | 地图匹配定位方法及系统 | |
| CN107797125B (zh) | 一种减小深海探测型auv导航定位误差的方法 | |
| CN110220510B (zh) | 一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法 | |
| CN105547300A (zh) | 用于auv的全源导航系统及方法 | |
| CN107063245A (zh) | 一种基于5阶ssrckf的sins/dvl组合导航滤波方法 | |
| CN109855623B (zh) | 基于Legendre多项式和BP神经网络的地磁模型在线逼近方法 | |
| CN117289322B (zh) | 一种基于imu、gps与uwb的高精度定位算法 | |
| CN113405560A (zh) | 车辆定位和路径规划统一建模方法 | |
| CN108107890B (zh) | 一种非线性不确定系统的安全轨迹规划方法 | |
| CN114879660B (zh) | 一种基于目标驱动的机器人环境感知方法 | |
| CN112229421A (zh) | 基于李群最优估计的捷联惯性导航晃动基座粗对准方法 | |
| CN114915913A (zh) | 一种基于滑窗因子图的uwb-imu组合室内定位方法 | |
| CN113639722B (zh) | 连续激光扫描配准辅助惯性定位定姿方法 | |
| CN109307513B (zh) | 一种基于行车记录的实时道路匹配方法及系统 | |
| CN115451955B (zh) | 基于分布鲁棒滤波的ins/gps紧耦合导航方法及系统 |
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 |