用于移动载具的路径跟踪方法技术

本发明专利技术提供了用于移动载具的路径跟踪方法，属于导航领域，包括获取移动载具的当前位置和行驶方向，基于当前位置和行驶方向确定移动载具行驶至目的地的行驶路径；将移动载具简化为差速模型，建立差速模型在行驶路径中的前进目标函数，获取前进目标函数的最优解；将得到的最优解作为最优差速控制量对差速模型对应的移动载具进行速度控制，直至移动载具行驶至目的地。通过将移动载具的运动学模型简化为2轮差速模型，以便寻求最优路径跟踪，采用最小均方误差进行割草机的自动作业。本发明专利技术提供的路径跟踪方法计算简单，且算法本身具有天然稳定性，同时与组合导航相结合，保证跟踪的稳定性和可靠性。

Path tracking method for mobile vehicle

The invention provides a path tracking method for a mobile vehicle, which belongs to the navigation field, including acquiring the current position and direction of the mobile vehicle, determining the traveling path of the mobile vehicle to the destination based on the current position and direction of traveling, simplifying the mobile vehicle as a differential model, and establishing a differential model in the traveling path. The forward objective function is used to obtain the optimal solution of the forward objective function, and the optimal solution is used as the optimal differential control variable to control the speed of the moving vehicle corresponding to the differential model until the moving vehicle travels to its destination. The kinematics model of the moving vehicle is simplified to a two-wheel differential model in order to find the optimal path tracking, and the automatic operation of the mower is carried out with the minimum mean square error. The path tracking method provided by the invention is simple in calculation, and the algorithm itself has natural stability, and combines with integrated navigation to ensure the stability and reliability of the tracking.

【技术实现步骤摘要】
用于移动载具的路径跟踪方法
本专利技术属于导航领域，特别涉及用于移动载具的路径跟踪方法。
技术介绍
包括智能割草机等一系列移动载具在移动过程中，往往由于地形起伏变化或者移动部件误差等原因，经常无法按原计划路线行进，因此如何对移动载具的行驶路径进行跟踪，已经成为亟待解决的问题。
技术实现思路
为了解决现有技术中存在的缺点和不足，本专利技术提供了用于提高行驶路径准确性的用于移动载具的路径跟踪方法。为了达到上述技术目的，本专利技术提供了用于移动载具的路径跟踪方法，所述路径跟踪方法，包括：获取移动载具的当前位置和行驶方向，基于当前位置和行驶方向确定移动载具行驶至目的地的行驶路径；将移动载具简化为差速模型，建立差速模型在行驶路径中的前进目标函数，获取前进目标函数的最优解；将得到的最优解作为最优差速控制量对差速模型对应的移动载具进行速度控制，直至移动载具行驶至目的地。可选的，所述将移动载具简化为差速模型，包括：获取移动载具驱动轮的速度值；根据移动载具的当前位置、行驶方向、驱动轮的速度值，计算移动载具下一时刻所处的位置以及行驶方向；根据移动载具的当前位置以及下一时刻所处的位置，计算得到移动载具在下一时刻的横向偏差、航向偏差。可选的，所述建立差速模型在行驶路径中的前进目标函数，获取前进目标函数的最优解，包括：定义目标函数：hm＝(k·pem)2+(θem)2其中，k为加权系数；计算所有m＝1,2,…n所对应的hm，找到hm取最小值时对应的m＝mmin，则为所要寻求的最优控制量。可选的，所述将得到的最优解作为最优差速控制量对差速模型对应的移动载具进行控制，包括：获取到的最优差速控制量计算车辆驱动轮的转速，控制车辆的行走与转向；根据运动学模型：vlk＝v-δv/2vrk＝v+δv/2则左右轮的转速ωl、ωr分别为ωl＝vl*kredu/π/Dωr＝vr*kredu/π/D其中，kredu为电机减速比，D为车轮直径。本专利技术提供的技术方案带来的有益效果是：通过将移动载具的运动学模型简化为2轮差速模型，以便寻求最优路径跟踪，采用最小均方误差进行割草机的自动作业。本专利技术提供的路径跟踪方法计算简单，且算法本身具有天然稳定性，同时与组合导航相结合，保证跟踪的稳定性和可靠性。附图说明为了更清楚地说明本专利技术的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。图1是本专利技术提供的用于移动载具的路径跟踪方法的流程示意图；图2是本专利技术提供的对移动载具形式路径进行调整的示意图。具体实施方式为使本专利技术的结构和优点更加清楚，下面将结合附图对本专利技术的结构作进一步地描述。实施例一本专利技术提供了用于移动载具的路径跟踪方法，如图1所示，所述路径跟踪方法，包括：11、获取移动载具的当前位置和行驶方向，基于当前位置和行驶方向确定移动载具行驶至目的地的行驶路径；12、将移动载具简化为差速模型，建立差速模型在行驶路径中的前进目标函数，获取前进目标函数的最优解；13、将得到的最优解作为最优差速控制量对差速模型对应的移动载具进行速度控制，直至移动载具行驶至目的地。在实施中，为了能够对移动载具的行驶路径进行准确跟踪，并且基于跟踪结果对移动载具的实际行驶路径进行实时修正，最终提升对路径跟踪的准确性。步骤一、由组合导航实时更新割草机当前的位置及航向，由路径规划方法获取当前需要跟踪的目标路径；若目标路径为直线段，则直接取线段两端构成的直线作为目标路径；若目标路径为曲线段，则根据需求将曲线段进行细分，形成由多条直线段构成的折线段，依序分别对各条直线段进行路径跟踪。步骤二、将车辆运动学模型简化为二轮差速模型，利用割草机的两轮差速实现车辆的行走与转向控制；将两轮速度差δv作为控制量，根据需求从可选控制量范围[δvminδvmax]内选取一个n维的控制量序列[δv1δv2…δvn-1δvn]，结合运动学模型，分别推算出该控制量对应的下一时刻的车辆位置和航向，并计算对应的横向偏差pe、航向偏差θe，找到所有序列中pe、θe的加权均方误差最小值对应的控制量δv，即为所需的最优控制量。步骤三、根据步骤二获取的两轮差速以及割草机的前向行走速度，分别获取左右轮的电机转速，并发送指令到电机驱动器，控制车辆的行走。在实际使用时，可以采用GNSS/INS/车辆组合导航为割草机提供高精度的定位和航向信息；将割草机的运动学模型简化为2轮差速模型，为寻求最优路径跟踪，采用最小均方误差(MMSE)，基于与目标路径的位置偏差，以及与目标期望航向的航向偏差，获取车辆的最优差速控制量；根据获取的控制量为车辆的执行机构，即两轮行走电机，提供控制指令，实现割草机对所规划路径的精确跟踪，进行割草机的自动作业。本专利技术提供的路径跟踪方法计算简单，且算法本身具有天然稳定性，同时与组合导航相结合，保证跟踪的稳定性和可靠性。可选的，为了便于后续步骤的调整，首先需要将移动载具简化为差速模型，包括：获取移动载具驱动轮的速度值；根据移动载具的当前位置、行驶方向、驱动轮的速度值，计算移动载具下一时刻所处的位置以及行驶方向；根据移动载具的当前位置以及下一时刻所处的位置，计算得到移动载具在下一时刻的横向偏差、航向偏差。为了得到最终的控制量，本实施例以常用的两轮驱动的智能割草机为例进行说明。割草机的执行机构是两个驱动轮，通过两轮差速实现行走与转向，将其运动模型简化为二轮差速模型，如图2所示。以二轮差速δv作为控制量，假设δv的可取范围为[δvminδvmax]，则根据需求在可取范围内选取差速δv的n维序列[δv1δv2…δvn-1δvn]，所需的控制精度越高，则可选择越大的序列维数。在当前时刻tk，载体速度为vk，左右轮的线速度分别为vlk和vrk，满足vk＝(vlk+vrk)/2δv＝vrk-vlk载体所处的位置及当前航向为(xk,yk,θk)，根据运动学模型推算δv对应的下一时刻tk+1载体所在的位置及航向(xk+1,yk+1,θk+1)：θk+1＝θk-tan-1(δv·δt/L)xk+1＝xk+vk·δt·sin(θk+1)yk+1＝yk+vk·δt·cos(θk+1)其中，δt＝tk+1-tk为时间间隔，L为车轮的轴距。对于任意δvm(m＝1,2,…n)，计算tk+1时刻的pem、θem，横向偏差pe为载体所处的点到目标路径的垂直距离其中，(x0,y0)为(xk+1,yk+1)点与目标路径之间的垂足点坐标。航向偏差θe为当前航向与目标路径航向之差：θe＝θk+1-θref其中，θref即为图2中目标路径的航向。定义目标函数hm＝(k·pem)2+(θem)2式中k为加权系数，则hm＝k2[(xk+1-x0)2+(yk+1-y0)2]+(θk+1-θref)2该目标函数的意义表示为进行计算的当前时刻移动载具所处位置与目的地的偏差量。计算所有m＝1,2,…n所对应的hm，找到hm取最小值时对应的m＝mmin，则为所要寻求的最优控制量。这里通过引入目标函数并获取其最优解的方式，能够实现对移动载具行驶路径的调整。可选的，所述将得到的最优解作为最优差速控制量对差速模型对应的移动载具进行控制，包括：本文档来自技高网...

【技术保护点】
1.用于移动载具的路径跟踪方法，其特征在于，所述路径跟踪方法，包括：获取移动载具的当前位置和行驶方向，基于当前位置和行驶方向确定移动载具行驶至目的地的行驶路径；将移动载具简化为差速模型，建立差速模型在行驶路径中的前进目标函数，获取前进目标函数的最优解；将得到的最优解作为最优差速控制量对差速模型对应的移动载具进行速度控制，直至移动载具行驶至目的地。

【技术特征摘要】
1.用于移动载具的路径跟踪方法，其特征在于，所述路径跟踪方法，包括：获取移动载具的当前位置和行驶方向，基于当前位置和行驶方向确定移动载具行驶至目的地的行驶路径；将移动载具简化为差速模型，建立差速模型在行驶路径中的前进目标函数，获取前进目标函数的最优解；将得到的最优解作为最优差速控制量对差速模型对应的移动载具进行速度控制，直至移动载具行驶至目的地。2.根据权利要求1所述的用于移动载具的路径跟踪方法，其特征在于，所述将移动载具简化为差速模型，包括：获取移动载具驱动轮的速度值；根据移动载具的当前位置、行驶方向、驱动轮的速度值，计算移动载具下一时刻所处的位置以及行驶方向；根据移动载具的当前位置以及下一时刻所处的位置，计算得到移动载具在下一时刻的横向偏差、航向偏差。3.根据权利要求2所述...

【专利技术属性】
技术研发人员：陈勇，龚建飞，陈凤梧，张哲，朱海微，杨养文，汤冯炜，管国祥，
申请(专利权)人：浙江亚特电器有限公司，无锡卡尔曼导航技术有限公司，
类型：发明
国别省市：浙江,33