具有轨迹预测和随机位置跟踪功能的跟随车及跟随方法与流程

本发明涉及跟随车技术领域，具体是一种具有轨迹预测和随机位置跟踪功能的跟随车及跟随方法。

背景技术：

跟随车可以跟随目标，自动跟随目标运动，当目标停止运动时，跟随车也停止运动。高尔夫球车是一种较为常见的跟随车。

目前跟随技术主要是跟随车辆跟随在目标后方一定距离内，这种跟随方式不仅使得跟随目标上的使用者需要经常转身向后观察车辆的跟随情况，降低了跟随的效率，同时还使用户使用体验变差。

现有的发明专利中部分目标的定位方式主要是用超声波雷达，利用到达时间(toa)的方法，通过多个超声波探头进行目标物体的定位。但超声波定位存在信号传播速度较慢、信号方向性差、发散角度大等问题，由于发散使能量大大降低，导致信号衰减较为严重，因此探测范围有限。

201510498083.6公开了一种跟随式搬运车及其运行方法，车体为拖板车的车体，所述的车体的底部带有轮式结构，所述的轮式结构为四轮结构，所述的四轮结构包括有四个轮子，所述的四个轮子均为动力轮，所述的直立式操作台内还设置有主控板，在所述的车体的最前沿按照从左到右的顺序依次设置有五个超声波测距离传感器，所述的五个超声波测距离传感器、启停按键、模式选择键、电子屏、蓄电池以及所有的电机都同主控板相通信连接，另外所述的跟随式搬运车还配置有遥控器，所述的遥控器用来同主控板相无线通信连接，所述的主控板中带有运行控制模块。该搬运车使用超声波对所需要进行跟随的物体进行定位，存在当跟随目标与跟随车辆之间距离较远时，定位精度较低，信号较弱，当周围存在多个跟随目标时，无法实现正常的跟随功能。

201811417108.5公开了一种基于uwb的跟随系统，包括：第一uwb发射接收模块，设置在被跟随的目标物体上，第一uwb发射接收模块上设有uwb标签；第二、第三uwb发射接收模块，水平间隔地设置在跟随物体上，uwb标签获得与第一uwb发射接收模块之间的距离l2和l3；计算单元，根据uwb标签获得与第一uwb发射接收模块之间的距离l2和l3，计算得到跟随物体与第一uwb发射接收模块之间的距离和夹角；控制单元，控制设置在跟随物体上的驱动装置，使跟随物体朝向目标物体。该发明所要解决的技术问题是现有的跟随系统存在的成本高和对跟随目标的距离和偏角计算不够简洁精准的问题。其跟随系统中没有轨迹预测的功能，车辆到达目标位置附近便会停下来。

201610245269.5公开了一种机器人小车路径跟随方法，属于信息处理技术领域。机器人小车以圆弧路径向目标跟随点运动；机器人小车的运动速度与机器人距目标点的直线距离成正比。本发明的优点是：实现了机器人小车相对被跟随者的实时跟随；跟随算法高效实用；减小机器人小车的抖动，使得跟随运动平滑自然。该发明中机器人小车同样也是没有预测功能，跟随方式是跟随在目标身后。

现有技术的重点主要集中在如何使跟随车辆能够准确地跟随到目标附近，当跟随车辆到达跟随目标附近时，跟随车辆会制动或者停止工作，跟随车辆并不会预测跟随目标下一步的运动。然而多数情况下，跟随目标停下并不是由于到达目标位置，而是由于其他原因，例如中途遇见熟人停下聊天、停下系鞋带、在高尔夫球场停下喝水等，如果此时跟随车辆已知行驶的目的地或者跟随车辆具有预测跟随目标运动的功能，便可以不停下来，而是继续往目标点前进，或者按照预测的轨迹继续跟随，这样可以大大减少跟随车辆的制动次数，具有良好的节能效果，同时提高了工作的效率。鉴于此，本发明提出一种具有轨迹预测和随机位置跟踪功能的跟随车及跟随方法。

技术实现要素：

本发明所要解决的技术问题是，针对现有技术的不足，提供一种具有轨迹预测和随机位置跟踪功能的跟随车及跟随方法。

本发明解决上述技术问题所采用的技术方案为：具有轨迹预测和随机位置跟踪功能的跟随车，包括车身、中央控制单元、车辆驱动转向系统、车辆制动系统、车载uwb、uwb标签、人机交互界面和超声波雷达，所述的车身底部的前侧安装有前轮，所述的车身底部的后侧安装有后轮，所述的中央控制单元、车辆驱动转向系统、车辆制动系统、车载uwb、人机交互界面和超声波雷达分别安装在所述的车身上，所述的uwb标签安装在被跟随目标上；

所述的车辆驱动转向系统包括轮毂电机和电机控制器，所述的轮毂电机用于驱动所述的后轮；

所述的车辆制动系统包括舵机和制动液压缸，所述的舵机用于拉动所述的制动液压缸，向所述的制动液压缸提供制动力，并将制动力传递至制动卡钳，所述的制动卡钳安装在所述的前轮上；

所述的车载uwb用于与所述的uwb标签进行实时通信，获取所述的uwb标签相对于跟随车坐标系原点的实时xy坐标数据，并将所述的实时xy坐标数据以can信号形式传送给所述的中央控制单元；

所述的人机交互界面用于跟随车的跟随模式、跟随目的地位置信息及参数的设置和输出显示，所述的跟随车的跟随模式包括特定跟随位置跟随模式、随机跟随位置跟随模式和默认跟随位置跟随模式；

所述的超声波雷达用于检测其探测范围内是否有障碍物，当检测到跟随车周围有障碍物时，所述的超声波雷达将检测信息传送给所述的中央控制单元，由所述的中央控制单元处理后向所述的车辆制动系统发送制动信号；

所述的中央控制单元分别与所述的电机控制器、舵机、车载uwb、人机交互界面和超声波雷达电连接，所述的中央控制单元用于接收、定期保存并处理数据，并将处理后的数据发送给相应的执行机构。

利用上述具有轨迹预测和随机位置跟踪功能的跟随车实现的跟随方法，包括以下步骤：

步骤一、在被跟随目标上安装uwb标签，uwb标签的xy坐标默认为被跟随目标的位置坐标(x，y)；

步骤二、中央控制单元从车载uwb实时输入的can信号中获取uwb标签相对于跟随车坐标系原点的实时xy坐标数据，据此判断跟随车与被跟随目标之间的距离是否小于设定的阈值d；

如果跟随车与被跟随目标之间的距离小于设定的阈值，即x²+y²＜d²，则中央控制单元向舵机发送pwm信号，舵机向制动液压缸提供制动力，使制动液压缸执行制动动作，直至跟随车与被跟随目标之间的距离大于设定的阈值；

如果跟随车与被跟随目标之间的距离大于设定的阈值，即x²+y²＞d²，则中央控制单元判断使用者是否通过人机交互界面已输入跟随车的特定跟随位置信息；

如果中央控制单元确定使用者已输入跟随车的特定跟随位置信息，则中央控制单元采用特定跟随位置跟随模式，将使用者输入的跟随车的特定跟随位置信息转化为该特定跟随位置相对于跟随车坐标系原点的极坐标参数ρ和θ，并将被跟随目标的位置坐标变换为x1＝x+ρcosθ，y1＝y+ρsinθ，转入步骤三；

如果中央控制单元确定使用者未输入跟随车的特定跟随位置信息但已输入跟随车的跟随模式为随机跟随位置跟随模式，则中央控制单元采用随机跟随位置跟随模式，将被跟随目标的位置坐标变换为x2＝x+ρ'cosθ'，y2＝y+ρ'sinθ'，其中ρ'、θ'为具有一定权重的随机性的参数，即以被跟随目标的位置坐标为中心，以该中心周围某个随机位置为跟随目标，转入步骤三；

如果中央控制单元确定使用者已输入跟随车的跟随模式为默认跟随位置跟随模式或未输入跟随车的跟随模式，则中央控制单元采用默认跟随位置跟随模式，控制跟随车根据uwb标签传送的实时xy坐标数据，保持特定距离跟随在被跟随目标后方，转入步骤三；

步骤三、确定跟随车的跟随模式之后，跟随车对被跟随目标进行跟随，跟随过程中，中央控制单元通过获取的实时xy坐标数据检测被跟随目标是否停止运动；

如果中央控制单元检测到被跟随目标停止运动，则中央控制单元判断使用者是否通过人机交互界面已输入跟随目的地位置信息；如果中央控制单元确定使用者已输入跟随目的地位置信息，则跟随车按照输入的目的地位置信息规划直线运动轨迹，并减速50～75％缓慢向前行进，当跟随车与被跟随目标之间的距离大于设定的阈值d时，若被跟随目标尚未运动，则跟随车停止运行；如果中央控制单元确定使用者未输入跟随目的地位置信息，则中央控制单元根据当前时刻与上一周期保存的被跟随目标的位置与速度数据，运用基于卡尔曼滤波的动态轨迹预测算法对被跟随目标的运动轨迹进行预测及规划，并以规划的运动轨迹位置为中心坐标，结合跟随车的跟随模式，确定被跟随目标的位置坐标，使跟随车按照规划的轨迹，进行减速50～75％的缓慢提前跟随，当跟随车与被跟随目标之间的距离大于设定的阈值d时，若被跟随目标尚未运动，则跟随车停止运行；

如果中央控制单元检测到被跟随目标在运动，则中央控制单元判断被跟随目标与跟随车之间的实时距离变化，如果距离变小，说明当前预测正确，则中央控制单元控制跟随车继续保持当前的状态，以规划的运动轨迹行进，对被跟随目标进行跟随，跟随过程中，当超声波雷达检测到运动轨迹上有障碍物时，超声波雷达将检测信息传送给中央控制单元，由中央控制单元处理后向舵机发送pwm信号，舵机向制动液压缸提供制动力，使制动液压缸执行制动动作，直至障碍物移除或跟随车绕开障碍物；如果跟随车与被跟随目标之间的距离变大，说明当前规划的运动轨迹出现偏差或者被跟随目标有新的目的地，此时以被跟随目标当前所在的坐标位置为中心位置，结合跟随车的跟随模式，重复步骤三，重新确定需要跟随的被跟随目标的位置并进行新一轮的跟随。

本发明具有轨迹预测和随机位置跟踪功能的跟随车及跟随方法，能够实现在被跟随目标停止运动时，跟随车根据已知的目的地或者通过现有已知的基于卡尔曼滤波的动态轨迹预测算法预测跟随车下一步的运动轨迹，对被跟随目标的动态行为进行状态估计，利用上一周期保存的被跟随目标的位置、速度等状态值和当前时刻状态的观测值更新对状态变量的估计，进而对下一时刻的被跟随目标的轨迹位置与行进速度进行预测。在被跟随目标停止运动期间，跟随车减速缓慢地向规划的运动轨迹前进，当被跟随目标重新运动时，跟随车重新对被跟随目标进行跟随。即使被跟随目标出现了由于某些原因中途停顿的情况，跟随车也能继续行驶，这样不仅提高了跟随效率，还使得跟随车的制动次数大大减少，从而提高了跟随车的续航里程，节约能源。同时本发明针对传统跟随车大多都是跟随在被跟随目标后方的这种单一的跟随模式进行了改进，不仅在使用过程中使用者(即被跟随目标)不需要经常转身观察跟随车的跟随情况，而且使用者可以使跟随车按个人习惯进行跟随，从而提高跟随过程中的趣味性，增强跟随车与被跟随目标的交互性。

与现有技术相比，本发明具有如下优点：

1、本发明运用uwb定位技术和超声波雷达相结合的感知定位方案，定位范围大、方向性好，可以实现避障功能；

2、跟随车在跟随过程中具有运动轨迹规划和预测功能，能够在被跟随目标停止运动的情况下，按照被跟随目标的意图进行不需制动的跟随，提高跟随车的跟随效率和续航里程，节约能源；

3、根据使用者的意愿，可以进行多种跟随模式(即特定跟随位置跟随模式、随机跟随位置跟随模式和默认跟随位置跟随模式)跟随，适应不同的人群，跟随的灵活性大，趣味性高，跟随车与被跟随目标的交互性强。

附图说明

图1为本发明跟随车各主要组成部分之间的连接关系示意图；

图2为本发明跟随方法的基本流程图。

具体实施方式

以下结合附图实施例对本发明作进一步详细描述。

实施例1：一种具有轨迹预测和随机位置跟踪功能的跟随车，如图1所示，包括车身、中央控制单元、车辆驱动转向系统、车辆制动系统、车载uwb、uwb标签、人机交互界面和超声波雷达，车身底部的前侧安装有前轮，车身底部的后侧安装有后轮，中央控制单元、车辆驱动转向系统、车辆制动系统、车载uwb、人机交互界面和超声波雷达分别安装在车身上，uwb标签安装在被跟随目标上。

实施例1中，车辆驱动转向系统包括轮毂电机和电机控制器，轮毂电机用于驱动后轮；车辆制动系统包括舵机和制动液压缸，舵机用于拉动制动液压缸，向制动液压缸提供制动力，并将制动力传递至制动卡钳，制动卡钳安装在前轮上。

实施例1中，车载uwb用于与uwb标签进行实时通信，获取uwb标签相对于跟随车坐标系原点的实时xy坐标数据，并将实时xy坐标数据以can信号形式传送给中央控制单元。

实施例1中，人机交互界面是使用者和跟随车之间的信息交流平台，用于跟随车的跟随模式、跟随目的地位置信息及参数的设置和输出显示，跟随车的跟随模式包括特定跟随位置跟随模式、随机跟随位置跟随模式和默认跟随位置跟随模式；超声波雷达用于检测其探测范围内是否有障碍物，当检测到跟随车周围有障碍物时，超声波雷达将检测信息传送给中央控制单元，由中央控制单元处理后向车辆制动系统发送制动信号；中央控制单元分别与电机控制器、舵机、车载uwb、人机交互界面和超声波雷达电连接，中央控制单元用于接收、定期保存并处理数据，并将处理后的数据发送给相应的执行机构。

实施例2：一种利用实施例1的具有轨迹预测和随机位置跟踪功能的跟随车实现的跟随方法，包括以下步骤：

步骤一、在被跟随目标上安装uwb标签，uwb标签的xy坐标默认为被跟随目标的位置坐标(x，y)；

步骤二、中央控制单元从车载uwb实时输入的can信号中获取uwb标签相对于跟随车坐标系原点的实时xy坐标数据，据此判断跟随车与被跟随目标之间的距离是否小于设定的阈值d；

如果跟随车与被跟随目标之间的距离小于设定的阈值，即x²+y²＜d²，则中央控制单元向舵机发送pwm信号，舵机向制动液压缸提供制动力，使制动液压缸执行制动动作，直至跟随车与被跟随目标之间的距离大于设定的阈值；

如果跟随车与被跟随目标之间的距离大于设定的阈值，即x²+y²＞d²，则中央控制单元判断使用者是否通过人机交互界面已输入跟随车的特定跟随位置信息；

如果中央控制单元确定使用者已输入跟随车的特定跟随位置信息，例如：使用者(即被跟随目标)习惯跟随车位于自己的右前方，则中央控制单元采用特定跟随位置跟随模式，将使用者输入的跟随车的特定跟随位置信息转化为该特定跟随位置相对于跟随车坐标系原点的极坐标参数ρ和θ，并将被跟随目标的位置坐标变换为x1＝x+ρcosθ，y1＝y+ρsinθ，转入步骤三；

如果中央控制单元确定使用者未输入跟随车的特定跟随位置信息但已输入跟随车的跟随模式为随机跟随位置跟随模式，则中央控制单元采用随机跟随位置跟随模式，将被跟随目标的位置坐标变换为x2＝x+ρ'cosθ'，y2＝y+ρ'sinθ'，其中ρ'、θ'为具有一定权重的随机性的参数，即以被跟随目标的位置坐标为中心，以该中心周围某个随机位置为跟随目标(随机跟随位置跟随模式类似于“遛狗式”跟随方法，可以使跟随车与被跟随目标之间有更多的交互)，转入

步骤三；

如果中央控制单元确定使用者已输入跟随车的跟随模式为默认跟随位置跟随模式或未输入跟随车的跟随模式，则中央控制单元采用默认跟随位置跟随模式，控制跟随车根据uwb标签传送的实时xy坐标数据，保持特定距离跟随在被跟随目标后方，转入步骤三；

步骤三、确定跟随车的跟随模式之后，跟随车对被跟随目标进行跟随，跟随过程中，中央控制单元通过获取的实时xy坐标数据检测被跟随目标是否停止运动；

如果中央控制单元检测到被跟随目标停止运动，则中央控制单元判断使用者是否通过人机交互界面已输入跟随目的地位置信息；如果中央控制单元确定使用者已输入跟随目的地位置信息，则跟随车按照输入的目的地位置信息规划直线运动轨迹，并减速50～75％缓慢向前行进，当跟随车与被跟随目标之间的距离大于设定的阈值d时，若被跟随目标尚未运动，则跟随车停止运行；如果中央控制单元确定使用者未输入跟随目的地位置信息，则中央控制单元根据当前时刻与上一周期保存的被跟随目标的位置与速度数据，运用基于卡尔曼滤波的动态轨迹预测算法对被跟随目标的运动轨迹进行预测及规划，并以规划的运动轨迹位置为中心坐标，结合跟随车的跟随模式，确定被跟随目标的位置坐标，使跟随车按照规划的轨迹，进行减速50～75％的缓慢提前跟随，当跟随车与被跟随目标之间的距离大于设定的阈值d时，若被跟随目标尚未运动，则跟随车停止运行；

如果中央控制单元检测到被跟随目标在运动，则中央控制单元判断被跟随目标与跟随车之间的实时距离变化，如果距离变小，说明当前预测正确，则中央控制单元控制跟随车继续保持当前的状态，以规划的运动轨迹行进，对被跟随目标进行跟随，跟随过程中，当超声波雷达检测到运动轨迹上有障碍物时，超声波雷达将检测信息传送给中央控制单元，由中央控制单元处理后向舵机发送pwm信号，舵机向制动液压缸提供制动力，使制动液压缸执行制动动作，直至障碍物移除或跟随车绕开障碍物；如果跟随车与被跟随目标之间的距离变大，说明当前规划的运动轨迹出现偏差或者被跟随目标有新的目的地，此时以被跟随目标当前所在的坐标位置为中心位置，结合跟随车的跟随模式，重复步骤三，重新确定需要跟随的被跟随目标的位置并进行新一轮的跟随。