CN117002479B - 一种基于横摆角速度的履带式车辆轨迹跟踪方法 - Google Patents

Abstract

本发明公开了一种基于横摆角速度的履带式车辆轨迹跟踪方法，属于自动驾驶技术领域。该方法是通过对车辆当前位姿的横向误差进行几何学解析，并与航向误差共同进行加权运算获得目标偏转角，并通过车辆的运动学将目标偏转角转化为目标的横摆角速度，并下发给操纵机构进行执行。基于上述算法，以实现实时对车辆的横摆角速度进行反馈控制。

Description

一种基于横摆角速度的履带式车辆轨迹跟踪方法

技术领域

本发明涉及自动驾驶技术领域，具体为一种基于横摆角速度的履带式车辆轨迹跟踪方法。

背景技术

履带式车辆自动驾驶系统是一种根据位姿信息，通过执行机构使左、右履带运动绕卷以实现控制的设备系统。履带式车辆的运动则依赖车辆左、右履带的配合。履带式车辆的整体运动由左、右履带分别的运动状态合作用得到。故车辆的横向、纵向运动是相互耦合的。而履带式车辆执行机构和作业环境常常是非线性的，这导致履带式车辆运动控制难度较大。

履带式车辆运动过程是横、纵向运动耦合的。因此在进行横向控制时，依然需要将纵向运动状态纳入考虑因素。在轨迹跟踪过程中，轨迹跟踪精度与运动稳定性都是重要的考量指标。这要求履带式车辆在运动过程中不仅横向误差要尽可能的小，横摆角在保证精度情况下也要尽可能的小。

在理想情况下，主动轮与履带、履带与地面间不产生滑动，此时的主动轮轮缘的线速度可视为履带速度。通过在主动轮上安装霍尔传感器等方法可测得主动轮转速，从而计算出履带绕卷速度。然而这种方案传感器布置复杂，价格高昂，且泥泞环境中传感器精度和寿命将遭受影响。此外，主动轮与履带、履带与地面间的滑动也将导致控制误差。测量履带速度的方法无法解决滑动误差问题。

在履带式车辆的理想模型中，执行器动作将影响左、右履带的速度，可以将和速度视为纵向速度和差速部分。纵向操纵机构通常为油门，横向执行机构通常为方向盘，或者是手柄。横向操纵机构的动作通过机械传动或电子控制控制左、右履带的速度。从而实现车辆的差速转向。然而由于操纵-传动-执行机构间的传动延迟，使用对履带式车辆的航向进行控制的方案难以满足高精度控制的要求。且由于前述延迟问题的存在，在自动驾驶过程中会导致危险情况的出现。

此外，履带式车辆的执行机构有集中式和分布式的构造差异，基于分布式底盘开发的控制方法能直接对左、右履带进行控制。而集中式驱动的履带式车辆动力传动左右路相互耦合，控制难度更高。

发明内容

为解决履带式车辆的轨迹跟踪控制精度与稳定性不足，以及现有测量方法成本较高、延迟较大的问题。本发明提供了一种针对履带式车辆的基于几何学和运动学的实现高精度轨迹跟踪的目标横摆角速度计算方法，以及对目标横摆角速度进行跟踪的方法。

本发明的目的在于提供一种基于横摆角速度的履带式车辆轨迹跟踪方法，以解决上述背景技术中提出的问题。该方法是通过对车辆当前位姿的横向误差进行几何学解析，并与航向误差共同进行加权运算获得目标偏转角，并通过车辆的运动学将目标偏转角转化为目标的横摆角速度，并下发给操纵机构进行执行。

为了解决上述技术问题，本发明提供如下技术方案：一种基于横摆角速度的履带式车辆轨迹跟踪方法，其特征在于：该跟踪方法包括：

一种基于横摆角速度的履带式车辆轨迹跟踪方法，该跟踪方法包括：

S100：根据GNSS信息计算车辆位置和姿态；

S200：确定车辆的横向误差和航向误差；

S300：计算车辆的目标偏转角；

S400：计算车辆的目标差速比；

S500：计算车辆的目标横摆角速度；

S600：控制操纵机构动作，完成横摆角速度跟踪。

优选的，所述的步骤S200中，横向误差通过车辆位置与目标轨迹之间的几何关系获得。

优选的，所述的步骤S300中，目标偏转角的计算公式为：

其中，δ：目标偏转角；ω_h：航向误差的权重；ω_lat：横向误差的权重；目标轨迹的切向方向角；/>被控车辆的车身朝向角；e_lat：横向误差；n：预计实现横向误差收敛的周期数；T：每个控制周期的时间长度；V_c：当前车辆的速度。

优选的，所述的步骤S400中，目标差速比的计算公式为：

其中，k：目标差速比；f(δ)：目标偏转角δ的自定义函数；B：履带车底盘中轴宽度。

优选的，所述的步骤S400中，目标偏转角δ的自定义函数为：

优选的，所述的S500中，目标横摆角速度的计算公式为：

其中，ω_T：目标横摆角速度。

优选的，所述的S600中，通过控制操纵机构使横摆角速度误差收敛为0，完成横摆角速度跟踪，所述横摆角速度误差的计算公式为：

e_ω＝ω_T一ω_real

其中，e_ω：横摆角速度误差；ω_real：陀螺仪测量获得的当前车辆的实际横摆角速度。

优选的，基于上述算法，可以实时对车辆的横摆角速度进行反馈控制。

优选的，所述航向误差和横向误差的权重为常数。

优选的，所述航向误差和横向误差的权重为函数，该函数为：

与现有技术相比，本发明所达到的有益效果是：本发明不仅能广泛地适用于集中式和分布式两种不同的履带式车辆上；还能够提高履带式车辆的轨迹跟踪精度和稳定性，并通过减小对车辆状态测量的延迟提高系统整体稳定性，避免危险情况的出现。

本发明的工作原理：通过测算履带式车辆的横向误差和航向误差，基于履带式车辆的几何学模型和轨迹跟踪的运动学模型计算出实现轨迹跟踪的目标横摆角速度。再通过对操纵机构进行控制，实现对目标横摆角速度的跟踪拟合。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：

图1是本发明的流程示意图；

图2是本发明履带式车辆轨迹跟踪简图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明提供技术方案，具体流程图参照图1，该方法的具体执行过程如下。

履带式车辆上需安装有GNSS天线、陀螺仪，操纵机构以方向盘为例，方向盘电机上需要安装有编码器。通过GNSS获得车辆位置及航向信息。通过陀螺仪获得横摆角速度，通过编码器获得方向盘转角。操纵机构也可以是手柄等其他可替代结构。

1、根据几何学计算的目标偏转角

根据GNSS获得的车辆与轨迹间的位姿状态，在图中以变形笛卡尔坐标系为例，设正北方向为X轴、正东方向为Y轴。坐标系还可以是东北天坐标系、地心地固坐标系、Frenet坐标系等其他等同坐标系。本发明中的方程以连续或离散形式进行表征具有同等效果。

可将履带车的目标偏转角以式1进行表示：

δ＝ω_h δ_h(t)+ω_lat δ_lat(t) (1)

ω_h、ω_lat：航向误差和横向误差的权重，应用实例上，可以是一固定值，也可以是与速度、横向误差等参数相关的函数。例如式2：

式1中的

δ_h(t)：纠正航向误差所需的目标偏转角；

δ_lat(t)：纠正横向误差所需的目标偏转角。

其中的航向误差的目标偏转角δ_h(t)表示为式3：

目标轨迹的切向方向角，对目标轨迹进行几何计算可获得；

被控车辆的车身朝向角，车身朝向可通过GNSS天线获得。

对式1中的横向误差的目标偏转角δ_lat(t)由式4计算获得，式4中的横向误差e_lat是通过GNSS获得车辆当前的位置状态通过计算车辆位置到轨迹的距离获得：

e_lat：被控车辆与目标轨迹之间的横向误差，通过车辆位置与目标轨迹之间的几何关系获得；

d(t)：速度在轨迹切向上一确定周期内将会前进的距离。可表示为式5：

d(t)＝n V_cT cosδ_h(t) (5)

n：预计实现e_lat收敛的周期数，常数；

T：每个控制周期的时间长度；

V_c：当前车辆的速度。

综合上述，可将式1可表示为：

式6中的函数表征了目标偏转角能够有效跟踪目标轨迹以及目标轨迹的方向。履带车的自动驾驶过程中，对目标偏转角的持续拟合就是车辆横向控制的控制过程。

2、基于运动学的差速运动解耦方法

根据履带车的差速模型运动学，

V_L、V_R：左、右履带的绕卷速度；

ω_c：横摆角速度；

B：履带车底盘中轴宽度，提前通过测量获得。

引入变量k，定义为左、右轮速比：

V_L＝k V_R (8)

将式8与差速模型式7进行联立，得履带式车辆的差速比为：

其中式f(δ)可视为可调参数，是关于偏转角δ的自定义函数。应用实例：

通过该方法计算获得的履带车的目标差速比可有效表征车辆运动目标的计算方式。相比传统的运动学模型，降低了目标值的计算复杂度。并且更加贴近履带式车辆运动控制的底层原理。

3、履带目标横摆角速度计算

目标横摆角速度是目标偏转角在期望动作时间内完成的转向动作，因此由下式计算获得

式11中ΔV_L/R表示左、右履带的速度差。速度差也可由差速比计算获得：

因此，目标横摆角速度计算方法表示为：

由此，目标横摆角速度计算可通过上式，使用差速模型计算获得的差速比、车宽以及履带车当前速度计算获得。通过目标横摆角速度对操纵机构进行自动驾驶控制，整车的运动响应速度能得到极大的提升。相比对目标偏转角进行微分获得的目标角速度，本方案获得的横摆角速度更加的平滑精准。对比使用偏转角直接进行控制，车辆运动过程的平稳性也能得到较大提升。

4、操纵机构执行

通过目标横摆角速度对操纵机构进行自动驾驶控制，方向盘的转动动作可由方向盘编码器读取，方向盘动作量实现的车辆横摆角速度由车辆陀螺仪读取获得。通过目标值与实际值进行做差可计算得操纵机构的操纵误差量：

e_ω＝ω_T-ω_real (14)

ω_real：实车真实的横摆角速度，可通过陀螺仪测算获得。

操纵机构的控制可使用SISO的经典控制方法进行跟踪，通过方向盘电机的动作量，对目标ω_T进行跟踪，以实现e_ω收敛为0。

参照图2，以下给出一个实施例对本发明进行详细阐释。

本方案需要在履带式车辆上安装有GNSS天线和陀螺仪，方向盘上装有编码器。通过GNSS获取车体与目标轨迹之间的距离，以及车体方向与目标轨迹的切向之间的角度差。编码器用以获取方向盘的转角。

本方案在开始运行之前需要获得两侧履带中轴距B。

本方案通过测算履带式车辆的横向误差和航向误差，基于履带式车辆的几何学模型和轨迹跟踪的运动学模型计算出实现轨迹跟踪的目标横摆角速度。再通过对操纵机构进行控制，实现对目标横摆角速度的跟踪拟合。

履带车的目标横摆角速度的具体计算过程如下：

根据GNSS获得的车辆位置和目标轨迹的位置，通过计算点到曲线的距离，可计算得到横向误差。横向误差可根据几何关系描述成为消除该横向误差应偏转的角度：

其中d(t)描述的是速度的纵向分量在未来n个周期将会前进的距离。

d(t)＝n v_c T cosδ_h(t)

根据车体方向朝向和轨迹曲线的切线朝向之差，可计算获得纠正航向误差所需的目标偏转角。

综合以上两项：横向与航向，可以计算得到应偏转的角度和为：

由于系统的响应特性，因此同时对横向与航向同时加上调节权重ω_h和ω_lat。

根据上式，即根据车辆的位置与姿态，基于几何学模型，计算获得的为实现误差收敛履带车的目标偏转角。

履带车的操纵机构的动作直接控制的是左、右履带各自的速度。在自动驾驶的横向控制中，左、右履带的速度差是自变量。也就是说方向盘的转动角度将影响左、右履带差速比。差速比是横摆角速度的另一种表征形式。而横摆角是横摆角速度的积分，因此对横摆角速度进行反馈控制能够减小执行动作的延迟问题。

目标横摆角速度的计算方法通过履带车的运动学模型计算而来，首先定义为左、右轮速比：

V_L＝k V_R

将上式代入差速运动的运动学方程，可获得差速比为：

目标横摆角速度可由差速比进行表示：

通过该算式，可计算得到目标横摆角速度大小。根据该值，通过操纵机构动作对执行器进行控制。具体到方向盘操纵的车辆中，通过方向盘转角，来对整车的横摆角速度进行反馈控制。反馈误差为：

e_ω＝ω_T-ω_real

其中ω_real为陀螺仪测量获得的当前车辆的实际横摆角速度。通过控制操纵机构使角速度误差收敛为0，可使用pid控制等方法实现。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。

最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于横摆角速度的履带式车辆轨迹跟踪方法，其特征在于，该跟踪方法包括：

S100：根据GNSS信息计算车辆位置和姿态；

S200：确定车辆的横向误差和航向误差；

S300：计算车辆的目标偏转角；

S400：计算车辆的目标差速比；

S500：计算车辆的目标横摆角速度；

S600：控制操纵机构动作，完成横摆角速度跟踪；

所述的S200中，横向误差通过车辆位置与目标轨迹之间的几何关系获得；

所述的S300中，目标偏转角的计算公式为：

其中，δ：目标偏转角；ω_h：航向误差的权重；ω_lat：横向误差的权重；目标轨迹的切向方向角；/>被控车辆的车身朝向角；e_lat：横向误差；n：预计实现横向误差收敛的周期数；T：每个控制周期的时间长度；V_c：当前车辆的速度；

所述的S400中，目标差速比的计算公式为：

其中，k：目标差速比；f(δ)：目标偏转角δ的自定义函数；B：履带车底盘中轴宽度；

目标偏转角δ的自定义函数为：

所述的S500中，目标横摆角速度的计算公式为：

其中，ω_T：目标横摆角速度。

2.根据权利要求1所述的一种基于横摆角速度的履带式车辆轨迹跟踪方法，其特征在于，所述的S600中，通过控制操纵机构使横摆角速度误差收敛为0，完成横摆角速度跟踪，所述横摆角速度误差的计算公式为：

e_ω＝ω_T-ω_real

其中，e_ω：横摆角速度误差；ω_real：陀螺仪测量获得的当前车辆的实际横摆角速度。

3.根据权利要求2所述的一种基于横摆角速度的履带式车辆轨迹跟踪方法，其特征在于，可以实时对车辆的横摆角速度进行反馈控制。

4.根据权利要求1所述的一种基于横摆角速度的履带式车辆轨迹跟踪方法，其特征在于，所述航向误差和横向误差的权重为常数。

5.根据权利要求1所述的一种基于横摆角速度的履带式车辆轨迹跟踪方法，其特征在于，所述航向误差和横向误差的权重为函数，该函数为：