一种全地形越野车跟随方法、系统及电子设备

专利名称：一种全地形越野车跟随方法、系统及电子设备

专利类型：发明专利

专利申请号：CN202311720709.4

专利申请（专利权）人：中科华芯(东莞)科技有限公司
权利人地址：广东省东莞市塘厦镇蛟坪路95号4栋107室

专利发明（设计）人：张超,吴海军,戴一诺,吴浩歌

专利摘要：本发明公开一种全地形越野车跟随方法、系统及电子设备，涉及车辆跟随领域，该方法包括利用UWB基站确定车辆与移动标签的偏离值；根据偏离值确定车辆与移动标签的夹角角度；根据夹角角度与预设的偏差容忍角度控制行驶方向；根据移动标签的移动速度确定车辆的最大移动速度；根据偏离值动态调整车辆的移动速度；根据车辆的移动速度调整预设的最大跟随程度和最小跟随程度；根据偏离值确定车辆与移动标签之间的水平偏离值；根据水平偏离值与预设的最小跟随程度和最大跟随程度之间的关系确定的偏离程度和地形复杂度控制车辆的移动；当发现障碍物时，根据障碍物位置调整车辆的行驶方向。本发明能够实现全地形越野车对移动标签的精准跟随和安全行驶。

主权利要求：
1.一种全地形越野车跟随方法，其特征在于，包括：
利用安装在全地形越野车上的第一UWB基站、第二UWB基站和第三UWB基站确定全地形越野车与移动标签之间的偏离值；所述第一UWB基站和所述第二UWB基站分别设置在全地形越野车的两侧，所述第三UWB基站设置在全地形越野车的车头中间位置；所述第一UWB基站、第二UWB基站和第三UWB基站形成等边三角形；
根据全地形越野车与移动标签之间的偏离值确定全地形越野车与移动标签之间的夹角角度；并根据夹角角度与预设的偏差容忍角度控制全地形越野车的行驶方向；
获取移动标签的移动速度；并根据移动标签的移动速度确定全地形越野车的最大移动速度；根据全地形越野车与移动标签之间的偏离值动态调整全地形越野车的移动速度；
根据全地形越野车的移动速度实时调整预设的最大跟随程度和最小跟随程度；
根据全地形越野车与移动标签之间的偏离值确定全地形越野车与移动标签之间的水平偏离值；并根据水平偏离值与预设的最小跟随程度和预设的最大跟随程度之间的关系，确定偏离程度；并根据偏离程度和地形复杂度控制全地形越野车的移动；
当发现障碍物时，获取车辆周围的环境信息，确定障碍物位置；并根据障碍物位置调整全地形越野车的行驶方向；
所述地形复杂度包括以下公式：
Complexity_Score＝k1*Slope+k2*Roughness+k3*ObstacleCount；
其中，Complexity_Score为地形复杂度，Slope为坡度，Slope＝atan2(Δh，Δd)，Δh表示车辆当前高度和地面高度的差异，Δd表示水平距离，Roughness为颠簸度，Roughness＝2
sqrt(Σ(Δh_i) /n)，Δh_i表示相邻点之间的高度差，n表示采样点数量，ObstacleCount为障碍物数量，通过图像处理算法检测和计算得到，k1、k2和k3是权重系数；
所述并根据偏离程度和地形复杂度控制全地形越野车的移动，具体包括：
若水平偏离值D<预设的最小跟随程度D_min且地形复杂度Complexity_Score小于设定阈值，则控制全地形越野车向移动标签靠近、移动速度为高档；
如果水平偏离值D<预设的最小跟随程度D_min且地形复杂度Complexity_Score大于设置阈值，则控制让越野车向移动标签靠近、移动速度为低档；
若水平偏离值D>预设的最大跟随程度D_max，则控制让越野车减速或停止，拉大与移动标签的距离；
若预设的最小跟随程度D_min≤水平偏离值D≤预设的最大跟随程度D_max，则维持越野车与移动标签的偏离值不变。
2.根据权利要求1所述的一种全地形越野车跟随方法，其特征在于，所述根据全地形越野车与移动标签之间的偏离值确定全地形越野车与移动标签之间的夹角角度；并根据夹角角度与预设的偏差容忍角度控制全地形越野车的行驶方向，具体包括：利用公式θ＝atan2(yt‑yc,xt‑xc)确定全地形越野车与移动标签之间的夹角角度；其中，θ为全地形越野车与移动标签之间的夹角角度，xc,yc为全地形越野车的位置坐标，xt,yt为移动目标的位置坐标；
当|θ|>θ_thresh时，调整全地形越野车的行驶方向。
3.根据权利要求1所述的一种全地形越野车跟随方法，其特征在于，当全地形越野车本体与移动标签的水平偏离值大于最大跟随程度的时间超过时间阈值时，触发警报。
4.一种全地形越野车跟随系统，用于实现权利要求1‑3任一项所述的一种全地形越野车跟随方法；其特征在于，包括：偏离程度检测模块，用于利用安装在全地形越野车上的第一UWB基站、第二UWB基站和第三UWB基站确定全地形越野车与移动标签之间的偏离值；所述第一UWB基站和所述第二UWB基站分别设置在全地形越野车的两侧，所述第三UWB基站设置在全地形越野车的车头中间位置；所述第一UWB基站、第二UWB基站和第三UWB基站形成等边三角形；
角度计算和判断模块，用于根据全地形越野车与移动标签之间的偏离值确定全地形越野车与移动标签之间的夹角角度；并根据夹角角度与预设的偏差容忍角度控制全地形越野车的行驶方向；
速度控制模块，用于获取移动标签的移动速度；并根据移动标签的移动速度确定全地形越野车的最大移动速度；根据全地形越野车与移动标签之间的偏离值动态调整全地形越野车的移动速度；
偏离程度设置模块，用于根据全地形越野车的移动速度实时调整预设的最大跟随程度和最小跟随程度；
跟随策略模块，用于根据全地形越野车与移动标签之间的偏离值确定全地形越野车与移动标签之间的水平偏离值；并根据水平偏离值与预设的最小跟随程度和预设的最大跟随程度之间的关系，确定偏离程度；并根据偏离程度和地形复杂度控制全地形越野车的移动；
避障模块，用于当发现障碍物时，获取车辆周围的环境信息，确定障碍物位置；并根据障碍物位置调整全地形越野车的行驶方向。
5.一种电子设备，其特征在于，包括存储器及处理器，所述存储器用于存储计算机程序，所述处理器运行所述计算机程序以使所述电子设备执行根据权利要求1至3中任一项所述的一种全地形越野车跟随方法。
6.根据权利要求5所述的一种电子设备，其特征在于，所述存储器为计算机可读存储介质。 说明书 : 一种全地形越野车跟随方法、系统及电子设备技术领域[0001] 本发明涉及车辆跟随领域，特别是涉及一种全地形越野车跟随方法、系统及电子设备。背景技术[0002] 目前，自动化地形跟随系统在野外探险、搜救任务等领域具有重要意义。然而，现有技术中的地形跟随系统存在一些问题，如精度不高、适应性不足、安全性不强等。为了解决这些问题，提高越野车地形跟随系统的效率和安全性，亟需提供一种新的跟随方法或系统。发明内容[0003] 本发明的目的是提供一种全地形越野车跟随方法、系统及电子设备，能够实现全地形越野车对移动标签的精准跟随和安全行驶。[0004] 为实现上述目的，本发明提供了如下方案：[0005] 一种全地形越野车跟随方法，包括：[0006] 利用安装在全地形越野车上的第一UWB基站、第二UWB基站和第三UWB基站确定全地形越野车与移动标签之间的偏离值；所述第一UWB基站和所述第二UWB基站分别设置在全地形越野车的两侧，所述第三UWB基站设置在全地形越野车的车头中间位置；所述第一UWB基站、第二UWB基站和第三UWB基站形成等边三角形；[0007] 根据全地形越野车与移动标签之间的偏离值确定全地形越野车与移动标签之间的夹角角度；并根据夹角角度与预设的偏差容忍角度控制全地形越野车的行驶方向；[0008] 获取移动标签的移动速度；并根据移动标签的移动速度确定全地形越野车的最大移动速度；根据全地形越野车与移动标签之间的偏离值动态调整全地形越野车的移动速度；[0009] 根据全地形越野车的移动速度实时调整预设的最大跟随程度和最小跟随程度；[0010] 根据全地形越野车与移动标签之间的偏离值确定全地形越野车与移动标签之间的水平偏离值；并根据水平偏离值与预设的最小跟随程度和预设的最大跟随程度之间的关系，确定偏离程度；并根据偏离程度和地形复杂度控制全地形越野车的移动；[0011] 当发现障碍物时，获取车辆周围的环境信息，确定障碍物位置；并根据障碍物位置调整全地形越野车的行驶方向。[0012] 可选地，所述地形复杂度包括以下公式：[0013] Complexity_Score＝k1*Slope+k2*Roughness+k3*ObstacleCount；[0014] 其中，Complexity_Score为地形复杂度，Slope为坡度，Slope＝[0015] atan2(Δh，Δd)，Δh表示车辆当前高度和地面高度的差异，Δd表示水平距离，2Roughness为颠簸度，Roughness＝sqrt(Σ(Δh_i) /n)，Δh_i表示相邻点之间的高度差，n表示采样点数量，ObstacleCount为障碍物数量，通过图像处理算法检测和计算得到，k1、k2和k3是权重系数。[0016] 可选地，所述并根据偏离程度和地形复杂度控制全地形越野车的移动，具体包括：[0017] 若水平偏离值D<预设的最小跟随程度D_min且地形复杂度Complexity_Score小于设定阈值，则控制全地形越野车向移动标签靠近、移动速度为高档；[0018] 如果水平偏离值D<预设的最小跟随程度D_min且地形复杂度Complexity_Score大于设置阈值，则控制让越野车向移动标签靠近、移动速度为低档；[0019] 若水平偏离值D>预设的最大跟随程度D_max，则控制让越野车减速或停止，拉大与移动标签的距离；[0020] 若预设的最小跟随程度D_min≤水平偏离值D≤预设的最大跟随程度D_max，则维持越野车与移动标签的偏离值不变。[0021] 可选地，所述根据全地形越野车与移动标签之间的偏离值确定全地形越野车与移动标签之间的夹角角度；并根据夹角角度与预设的偏差容忍角度控制全地形越野车的行驶方向，具体包括：[0022] 利用公式θ＝atan2(yt‑yc,xt‑xc)确定全地形越野车与移动标签之间的夹角角度；其中，θ为全地形越野车与移动标签之间的夹角角度，xc,yc为全地形越野车的位置坐标，xt,yt为移动目标的位置坐标；[0023] 当|θ|>θ_thresh时，调整全地形越野车的行驶方向。[0024] 可选地，当全地形越野车本体与移动标签的水平偏离值大于最大跟随程度的时间超过时间阈值时，触发警报。[0025] 一种全地形越野车跟随系统，包括：[0026] 偏离程度检测模块，用于利用安装在全地形越野车上的第一UWB基站、第二UWB基站和第三UWB基站确定全地形越野车与移动标签之间的偏离值；所述第一UWB基站和所述第二UWB基站分别设置在全地形越野车的两侧，所述第三UWB基站设置在全地形越野车的车头中间位置；所述第一UWB基站、第二UWB基站和第三UWB基站形成等边三角形；[0027] 角度计算和判断模块，用于根据全地形越野车与移动标签之间的偏离值确定全地形越野车与移动标签之间的夹角角度；并根据夹角角度与预设的偏差容忍角度控制全地形越野车的行驶方向；[0028] 速度控制模块，用于获取移动标签的移动速度；并根据移动标签的移动速度确定全地形越野车的最大移动速度；根据全地形越野车与移动标签之间的偏离值动态调整全地形越野车的移动速度；[0029] 偏离程度设置模块，用于根据全地形越野车的移动速度实时调整预设的最大跟随程度和最小跟随程度；[0030] 跟随策略模块，用于根据全地形越野车与移动标签之间的偏离值确定全地形越野车与移动标签之间的水平偏离值；并根据水平偏离值与预设的最小跟随程度和预设的最大跟随程度之间的关系，确定偏离程度；并根据偏离程度和地形复杂度控制全地形越野车的移动；[0031] 避障模块，用于当发现障碍物时，获取车辆周围的环境信息，确定障碍物位置；并根据障碍物位置调整全地形越野车的行驶方向。[0032] 一种电子设备，包括存储器及处理器，所述存储器用于存储计算机程序，所述处理器运行所述计算机程序以使所述电子设备执行所述的一种全地形越野车跟随方法。[0033] 可选地，所述存储器为计算机可读存储介质。[0034] 根据本发明提供的具体实施例，本发明公开了以下技术效果：[0035] 本发明所提供的一种全地形越野车跟随方法、系统及电子设备，通过安装在全地形越野车上的第一UWB基站、第二UWB基站和第三UWB基站确定全地形越野车与移动标签之间的偏离值，之后根据偏离值确定全地形越野车与移动标签之间的夹角角度；以及根据移动标签的移动速度和全地形越野车与移动标签之间的偏离值动态调整全地形越野车的移动速度，进而根据全地形越野车的移动速度实时调整的预设的最大跟随程度和最小跟随程度，确定偏离程度；并根据偏离程度和地形复杂度控制全地形越野车的移动；但存在障碍物时，进行行驶方向的调整；通过使用UWB基站进行距离检测和计算，以及角度计算和判断模块的协同作用，能够精准地跟随移动标签的位置和行动；通过速度控制和偏离程度的设置，能够根据移动标签的速度和偏离程度调整越野车的移动方式，达到自适应的跟随效果；根据障碍物位置和相对距离，及时调整越野车的行驶方向，避免碰撞和阻碍；本发明全地形越野车可以高效跟随移动标签，实现自动化的跟踪和控制，提高工作效率，并减少人工操作的风险。附图说明[0036] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。[0037] 图1为本发明所提供的一种全地形越野车跟随方法流程示意图；[0038] 图2为全地形越野车跟随原理示意图；[0039] 图3为躲避障碍物示意图。具体实施方式[0040] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。[0041] 本发明的目的是提供一种全地形越野车跟随方法、系统及电子设备，能够实现全地形越野车对移动标签的精准跟随和安全行驶。[0042] 为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。[0043] 如图1所示，本发明所提供的一种全地形越野车跟随方法，包括：[0044] S101，利用安装在全地形越野车上的第一UWB基站、第二UWB基站和第三UWB基站确定全地形越野车与移动标签之间的偏离值；如图2所示，所述第一UWB基站和所述第二UWB基站分别设置在全地形越野车的两侧，所述第三UWB基站设置在全地形越野车的车头中间位置；所述第一UWB基站、第二UWB基站和第三UWB基站形成等边三角形；[0045] 假设第一UWB基站在全地形越野车左侧位置为(x1,y1)，第二UWB基站在全地形越野车右侧位置为(x2,y2)，第三UWB基站在车头中间位置为(x3,y3)。移动标签的位置为(xt,yt)，则第一UWB基站与标签之间的偏离值为 第二UWB基站与标签之间的偏离值为 第三UWB基站与标签之间的偏离值为[0046] S102，根据全地形越野车与移动标签之间的偏离值确定全地形越野车与移动标签之间的夹角角度；并根据夹角角度与预设的偏差容忍角度控制全地形越野车的行驶方向；[0047] S101具体包括：[0048] 利用公式θ＝atan2(yt‑yc,xt‑xc)确定全地形越野车与移动标签之间的夹角角度；其中，θ为全地形越野车与移动标签之间的夹角角度，xc,yc为全地形越野车的位置坐标，xt,yt为移动目标的位置坐标；[0049] 当|θ|>θ_thresh时，调整全地形越野车的行驶方向。[0050] S103，获取移动标签的移动速度v_t；并根据移动标签的移动速度确定全地形越野车的最大移动速度v_max；根据全地形越野车与移动标签之间的偏离值动态调整全地形越野车的移动速度；[0051] S104，根据全地形越野车的移动速度实时调整预设的最大跟随程度和最小跟随程度；[0052] 移动速度越快，最小跟随程度越大，最大跟随程度越小。根据移动标签的移动速度改变预设的最大跟随程度D_max和最小跟随程度D_min。移动速度越快，最小跟随程度D_min越大，最大跟随程度D_max越小。可以使用移动标签的速度v_t进行比例调整。[0053] S105，根据全地形越野车与移动标签之间的偏离值确定全地形越野车与移动标签之间的水平偏离值；并根据水平偏离值与预设的最小跟随程度和预设的最大跟随程度之间的关系，确定偏离程度；并根据偏离程度和地形复杂度控制全地形越野车的移动；[0054] 利用公式D＝(d1+d2+d3)/3确定水平偏离值。[0055] 地形复杂度包括以下公式：[0056] Complexity_Score＝k1*Slope+k2*Roughness+k3*ObstacleCount；[0057] 其中，Complexity_Score为地形复杂度，Slope为坡度，Slope＝[0058] atan2(Δh，Δd)，Δh表示车辆当前高度和地面高度的差异，Δd表示水平距离，2Roughness为颠簸度，Roughness＝sqrt(Σ(Δh_i) /n)，Δh_i表示相邻点之间的高度差，n表示采样点数量，ObstacleCount为障碍物数量，通过图像处理算法检测和计算得到，k1、k2和k3是权重系数，用于调整每个变量对地形复杂度的贡献。这些系数可以根据车辆特性和地形情况进行调整。[0059] 根据偏离程度和地形复杂度控制全地形越野车的移动，具体包括：[0060] 若水平偏离值D<预设的最小跟随程度D_min且地形复杂度Complexity_Score小于设定阈值，则控制全地形越野车向移动标签靠近、移动速度为高档；[0061] 如果水平偏离值D<预设的最小跟随程度D_min且地形复杂度Complexity_Score大于设置阈值，则控制让越野车向移动标签靠近、移动速度为低档；[0062] 若水平偏离值D>预设的最大跟随程度D_max，则控制让越野车减速或停止，拉大与移动标签的距离；[0063] 若预设的最小跟随程度D_min≤水平偏离值D≤预设的最大跟随程度D_max，则维持越野车与移动标签的偏离值不变。[0064] S106，当发现障碍物时，获取车辆周围的环境信息，确定障碍物位置；并根据障碍物位置调整全地形越野车的行驶方向。[0065] 利用红外传感器、超声波雷达和摄像头的数据获取车辆周围的环境信息；如图3所示，障碍物检测是通过分析激光雷达扫描数据中的反射强度和反射时间来实现的。利用激光雷达返回的反射信号强度数据，结合雷达波束模式和探测距离，可以检测出障碍物的存在。通过将连续扫描数据，可以准确识别出障碍物的位置、大小。假设车辆的坐标系是以车辆中心为原点的笛卡尔坐标系，障碍物的位置可以表示为(X_obstacle,Y_obstacle)，其中X_obstacle是障碍物相对于车辆中心的右侧位置，Y_obstacle是障碍物相对于车辆中心的前进方向位置，vehicle_width为车辆宽度，turn_angle为转向角度。[0066] 根据障碍物的位置，决定车辆应该向左转还是向右转。判断X_obstacle的正负值来确定障碍物相对车辆中心的位置。如果X_obstacle>0，表示障碍物在车辆右侧；如果X_obstacle<0，表示障碍物在车辆左侧；如果X_obstacle在[‑vehicle_width/2,vehicle_width/2]范围内，表示障碍物在车辆前方。[0067] 根据障碍物的相对水平位置X_obstacle来决定转向方向。如果X_obstacle>0且X_obstacle<＝vehicle_width/2，则障碍物在车辆右侧，车辆应该向左转，转向角度为arctan((vehicle_width/2‑X_obstacle)/Y_obstacle)；如果X_obstacle<0且‑X_obstacle<＝vehicle_width/2，则障碍物在车辆左侧，车辆应该向右转，转向角度为arctan((vehicle_width/2+X_obstacle)/Y_obstacle)。其他情况保持当前直行方向或执行其他策略(停止、后退等)。[0068] 当全地形越野车本体与移动标签的水平偏离值大于最大跟随程度的时间超过时间阈值时，触发警报。[0069] 本发明可以实现跟踪人员位置并控制越野车的移动，提高效率和安全性。[0070] 对应上述方法，本发明还提供一种全地形越野车跟随系统，包括：[0071] 偏离程度检测模块，用于利用安装在全地形越野车上的第一UWB基站、第二UWB基站和第三UWB基站确定全地形越野车与移动标签之间的偏离值；所述第一UWB基站和所述第二UWB基站分别设置在全地形越野车的两侧，所述第三UWB基站设置在全地形越野车的车头中间位置；所述第一UWB基站、第二UWB基站和第三UWB基站形成等边三角形；[0072] 角度计算和判断模块，用于根据全地形越野车与移动标签之间的偏离值确定全地形越野车与移动标签之间的夹角角度；并根据夹角角度与预设的偏差容忍角度控制全地形越野车的行驶方向；[0073] 速度控制模块，用于获取移动标签的移动速度；并根据移动标签的移动速度确定全地形越野车的最大移动速度；根据全地形越野车与移动标签之间的偏离值动态调整全地形越野车的移动速度；[0074] 偏离程度设置模块，用于根据全地形越野车的移动速度实时调整预设的最大跟随程度和最小跟随程度；[0075] 跟随策略模块，用于根据全地形越野车与移动标签之间的偏离值确定全地形越野车与移动标签之间的水平偏离值；并根据水平偏离值与预设的最小跟随程度和预设的最大跟随程度之间的关系，确定偏离程度；并根据偏离程度和地形复杂度控制全地形越野车的移动；[0076] 避障模块，用于当发现障碍物时，获取车辆周围的环境信息，确定障碍物位置；并根据障碍物位置调整全地形越野车的行驶方向。[0077] 为了执行上述实施例对应的方法，以实现相应的功能和技术效果，本发明还提供一种电子设备，包括存储器及处理器，所述存储器用于存储计算机程序，所述处理器运行所述计算机程序以使所述电子设备执行所述的一种全地形越野车跟随方法。[0078] 所述存储器为计算机可读存储介质。[0079] 基于上述描述，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机、服务器或者网络设备等)执行本发明各个实施例方法的全部或部分步骤。而前述的计算机存储介质包括：U盘、移动硬盘、只读存储器、随机存取存储器、磁碟或者光盘等各种可以存储程序代码的介质。[0080] 本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。[0081] 本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

专利地区：广东

专利申请日期：2023-12-14

专利公开日期：2024-07-02

专利公告号：CN117687415B