一种基于惯导与卫星的路径规划算法

专利主权项内容

1.一种基于惯导与卫星的路径规划算法，包括农田地图构建、组合定位、代价函数设计、启发函数设计和改进A星路径搜索，所述改进A星路径搜索利用传统的A星算法，改进了启发式函数和代价函数；所述代价函数设计是从路径规划的一个节点到相邻节点难易程度的设计方法；所述启发函数设计是路径规划的一个节点到目标节点启示性设计方法；所述代价函数设计包括以下部分：欧式距离：两个节点之间的欧式距离是指两个节点之间的实际的距离，单位是米，因此当两个节点之间的实际距离越小时，他们之间的代价函数也会越小；高程差：相邻两个节点之间的高程存在差异，如果子节点的高程大于父节点的高程，那么从父节点到子节点将消耗更多能量，因此高程差是代价函数设计重要因素；土地类型：相邻两个节点之间的土地类型可能存在差异，农业机器人在黏性土地上移动和普通的农田中移动所消耗的能量差别较大；因此代价函数的数学表达式为：
，上式中，表示节点到节点的代价值，、、表示加权系数，表示两节点之间的距离，表示两节点之间的高程差，当高程升高时，该值为正，当高程减少时，高程为负值；表示两节点之间的土壤性质差异值，将土壤的粘性、稀疏性和表面硬度综合考量，提出了强粘、弱粘、一般和优良四个等级；一个等级到另一个等级的过度使用土壤性质差异值的倍数表示；相邻等级为一倍的，相差两个等级为2倍的，以此类推；从强粘到弱粘土壤性质差异值为负数；所述启发函数设计的步骤如下：gtmXtempXmαβγDtmHtmPtmPtmPtmPtmPtm计算当前节点和目标节点之间的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、根据农业机器人的实际位置确定路径规划的开始节点和目标节点；XsXgB1、维护一个优先级队列，用于存储所有的被扩展的节点；在开始时刻，优先级队列中只有一个开始节点；SpXsB2、为优先级队列中的每个节点映射一个对应的评价分数，评价分数由两部分构成，包括代价函数值g和启发函数值，其中开始节点的代价函数g()为零；其他的所有的没被加入到优先级队列中的节点的代价函数定义为无穷大；FFhXsB3、判断优先级队列中是否为空，如果为空，输出路径规划失败；B4、将具有最低评价分数的节点从优先级队列中弹出，并将弹出的节点标记为被扩展，并将该节点添加到已扩展队列中；FXtempSvB5、判断B4中弹出的节点是否是目标节点，如果是，那么路径规划结束，找到目标节点；XtempXgB6、如果B4中弹出的节点不是目标节点，那么扩展节点，找到其所有的邻居节点，对于所有的邻居节点做如下判断：XtempXtemp如果这个节点的代价函数值为无穷大，那么说明这个节点从未被加入到优先级队列中，那么计算这个节点的代价函数，计算方式为节点的代价函数加上从节点到该扩展节点的代价函数，计算公式为：XtempXtemp
，上式中表示节点扩展的某一个子节点，infinite表示代价函数的值为无穷大，()表示节点的代价函数值，()表示节点的代价函数值，h()表示节点的启发函数值，表示从节点到节点的代价，append表示将节点的加入到优先级队列中；XmXtempgXmXmgXtempXtempXmXm>tm>temp>>>temp>>若这个节点的代价函数值不是无穷大，如果这个节点的代价函数值大于节点的代价函数值加上节点到该节点的代价，那么更新该节点的代价函数，计算公式为：>temp>temp
>由于这个节点的代价函数值不是无穷大，说明该节点已经存在于优先级队列中，因此不必重复添加到优先级队列中；>>>>B7、重复执行B3到B6的步骤，直到算法找到目标节点。