一种无人车通用目标方向寻优方法技术

本发明专利技术公开了一种无人车通用目标方向寻优方法，无人车上设有一个高精度定位RTK，该方法包括：获取无人车在初始时刻RTK的初始位置，计算RTK在初始位置时距离目标点的初始距离；获取无人车在绕其中心点以任意速度旋转任意时刻t后RTK的当前位置，计算RTK在当前位置距离目标点的当前距离；计算当前距离关于时间t的一阶微分及二阶微分；根据一阶微分及二阶微分判断无人车的方向是否为朝向目标点的最优方向。本发明专利技术仅只使用一个RTK设备就可使无车人精准的朝向目标点，提高了可靠性，同时有效降低了RTK设备的成本，采用RTK设备进行定位，不会受到磁场的干扰。

【技术实现步骤摘要】
一种无人车通用目标方向寻优方法
本专利技术涉及一种无人车通用目标方向寻优方法。
技术介绍
在无人驾驶领域，路径寻优是一个研究重点，通常的做法是先确定无人车的位置和方向，再根据目标点的位置来规划无人车的运行轨迹。目前，确定无人车的方向的常用的技术手段主要有以下两种：方法一：通过两个高精度定位终端RTK获取无人车的经纬度坐标点，将两个坐标点连成一线并计算出该线段的指向，该指向即为无人车的朝向。该种方法广泛应用于无人驾驶领域。方法二：使用电子罗盘获取无人车的方向，该方法依靠地磁场的原理通过磁力计分量来计算该无人车所朝的方向。该方法目前主要应用于诸如四旋翼、六旋翼、八旋翼等无人机上。但是上述两种方法在使用中存在如下问题：(1)、方法一中所用到的高精度定位终端RTK的成本较高，且必须同时使用两个。两个RTK在安装时，必须间隔1米以上才能获得准确的方向精度。(2)、方法二中所用到的电子罗盘极易受到周围环境、安装位置的影响，比如无人车身上的电机、发动机、金属罩等都会对电子罗盘产生干扰，导致无法使用或不能得到较为精确的方向。另外，电子罗盘的使用环境要求周围500米内不能有强磁场，不能在变压站或高压线附近，且电子罗盘在每次使用时，必须先进行磁场校准。基于现有技术中存在的上述问题，如何能在降低成本、且不易受到环境干扰的前提下，令无人车精准的朝向目标点，进而规划最优路径，成为现在亟需解决的问题。
技术实现思路
本专利技术的目的在于提供一种无人车通用目标方向寻优方法，本专利技术能在降低成本，且不易受到环境干扰的前提下，令无人车精准的朝向目标点，进而根据目标点的位置规划最优路径。其技术方案如下：本专利技术公开一种无人车通用目标方向寻优方法，无人车上设有一个高精度定位RTK，该方法包括以下步骤：获取无人车的车轮/履带的等效中心点绕无人车的中心点旋转一周所形成的轨迹，该轨迹的半径为Lc；获取RTK绕无人车的中心点旋转一周所形成的轨迹，该轨迹的半径为Ld；获取无人车在任意初始时刻RTK所在位置，该位置为初始位置Site0，获取目标点A的位置，根据初始位置Site0的坐标及目标点A的坐标，计算RTK在初始位置Site0时距离目标点A的初始距离获取无人车在绕其中心点以任意速度Vref旋转任意时刻t后RTK所在位置，该位置为当前位置Sitet，根据当前位置Sitet的坐标及目标点A的坐标，计算RTK在当前位置Sitet时距离目标点A的当前距离根据上述获取的信息计算当前距离关于时间t的一阶微分及二阶微分根据一阶微分及二阶微分判断无人车的方向是否为朝向目标点A的最优方向。该方法还包括：根据初始位置Site0的坐标及目标点A的坐标计算初始位置Site0相对于目标点A的方位角根据当前位置Sitet的坐标及目标点A的坐标计算当前位置Sitet相对于目标点A的方位角根据方位角计算当前位置Sitet相对于初始位置Site0方位角的变化量ΔPCar_A，ΔPCar_A满足公式：该方法还包括计算RTK由初始位置Site0旋转至当前位置Sitet所旋转的角度θt，以及初始位置Site0与当前位置Sitet之间的距离Dt，其中，θt满足以下公式：Dt满足以下公式：其中，Lt为RTK由初始位置Site0旋转至当前位置Sitet所旋转的弧度。根据余弦定理得到：关于时间t的一阶微分满足以下公式：关于时间t的二阶微分满足以下公式：下面对本专利技术的优点或原理进行说明：本专利技术仅在无人车上安装一个高精度定位RTK，通过RTK的初始位置Site0、初始距离当前位置Sitet、当前距离无人车旋转时的速度Vref及旋转时间t，以及Lc、Ld等即可计算出关于时间t的一阶微分及二阶微分表示RTK与目标点A之间的距离在不断增加，L′Car_A＜0，表示RTK与目标点A之间的距离在不断减少。L″Car_A＞0，表示RTK是在加速趋近或远离目标点A，L″Car_A＜0，表示RTK在减速趋近或远离目标点A。本专利技术根据一阶微分的正负即可判断RTK是在趋近/远离目标点A，根据二阶微分的正负即可判断RTK是在加速/减速趋近或远离目标点A。本专利技术通过一阶微分二阶微分即可判断无人车是否朝向目标点A，并可根据判断结果继续旋转无人车，然后继续根据一阶微分二阶微分进行判断，直至无人车精准的朝向目标点A。本专利技术仅只使用一个RTK设备就可使无车人精准的朝向目标点A，而无需知道无人车本身的方向，提高了可靠性，同时有效降低了RTK设备的成本。采用RTK设备进行定位，不会受到磁场的干扰。附图说明图1是本实施例的无人车通用目标方向寻优方法的流程图；图2是在一实施例中的无人车在初始位置相对于目标点的位置关系示意图；图3是在另一实施例中的无人车旋转后相对于目标点的位置关系示意图；附图标记说明：1、无人车；2、RTK；3、无人车的正方向。具体实施方式下面结合附图对本专利技术的较佳实施例进行详细阐述，以使本专利技术的优点和特征能更易于被本领域技术人员理解，从而对本专利技术的保护范围做出更为清楚明确的界定。如图1至图3所示，本实施例公开一种无人车通用目标方向寻优方法，无人车1上设有一个高精度定位RTK2，该方法包括以下步骤：S10：获取无人车1的车轮/履带的等效中心点绕无人车1的中心点旋转一周所形成的轨迹，该轨迹的半径为Lc；获取RTK2绕无人车1的中心点旋转一周所形成的轨迹，该轨迹的半径为Ld；S20：获取无人车1在任意初始时刻RTK2所在位置，该位置为初始位置Site0，获取目标点A的位置，根据初始位置Site0的坐标及目标点A的坐标，计算RTK2在初始位置Site0时距离目标点A的初始距离S30：获取无人车1在绕其中心点以任意速度Vref旋转任意时刻t后RTK2所在位置，该位置为当前位置Sitet，根据当前位置Sitet的坐标及目标点A的坐标，计算RTK2在当前位置Sitet时距离目标点A的当前距离S40：根据上述获取的信息计算当前距离关于时间t的一阶微分及二阶微分S50：根据一阶微分及二阶微分判断无人车1的方向是否为朝向目标点A的最优方向。本实施例的方法还包括：根据初始位置Site0的坐标及目标点A的坐标计算初始位置Site0相对于目标点A的方位角根据当前位置Sitet的坐标及目标点A的坐标计算当前位置Sitet相对于目标点A的方位角如图3所示，初始位置Site0与当前位置Sitet之间的距离Dt与初始距离当前距离正好构成三角形的三条边。根据方位角计算当前位置Sitet相对于初始位置Site0方位角的变化量ΔPCar_A，ΔPCar_A满足公式：本实施例还包括计算RTK2由初始位置Site0旋转至当前位置Sitet所旋转的角度θt，以及初始位置Site0本文档来自技高网...

【技术保护点】
1.一种无人车通用目标方向寻优方法，其特征在于，无人车上设有一个高精度定位RTK，该方法包括以下步骤：/n获取无人车的车轮/履带的等效中心点绕无人车的中心点旋转一周所形成的轨迹，该轨迹的半径为L

【技术特征摘要】
1.一种无人车通用目标方向寻优方法，其特征在于，无人车上设有一个高精度定位RTK，该方法包括以下步骤：
获取无人车的车轮/履带的等效中心点绕无人车的中心点旋转一周所形成的轨迹，该轨迹的半径为Lc；获取RTK绕无人车的中心点旋转一周所形成的轨迹，该轨迹的半径为Ld；
获取无人车在任意初始时刻RTK所在位置，该位置为初始位置Site0，获取目标点A的位置，根据初始位置Site0的坐标及目标点A的坐标，计算RTK在初始位置Site0时距离目标点A的初始距离
获取无人车在绕其中心点以任意速度Vref旋转任意时刻t后RTK所在位置，该位置为当前位置Sitet，根据当前位置Sitet的坐标及目标点A的坐标，计算RTK在当前位置Sitet时距离目标点A的当前距离
根据上述获取的信息计算当前距离关于时间t的一阶微分及二阶微分
根据一阶微分及二阶微分判断无人车的方向是否为朝向目标点A的最优方向。


2.如权利要求1所述的无人车通用目标方向寻优方法，其特征在于，该方法还包括：
根据初始位置Site0的坐标及目标点A的坐...

【专利技术属性】
技术研发人员：黄家怿，孟祥宝，刘海峰，谢秋波，潘明，陈艺，刘红刚，钟林忆，
申请(专利权)人：广东省现代农业装备研究所，
类型：发明
国别省市：广东;44