一种基于偏航预警的铰接式车辆速度调节控制方法及系统

1.本发明涉及无人驾驶铰接式车辆的运动控制技术领域，特别涉及一种基于偏航预警的铰接式车辆速度调节控制方法及系统。


背景技术：

2.铰接式车辆是一种常用于矿山、建筑等行业的特种车辆，其运动控制难度较大。传统的无人驾驶铰接式车辆运动控制通常将纵向速度调节控制与横向路径跟踪控制分开考虑，但是有文献指出，由于铰接式车辆的转向机构响应速度有限，纵向速度会对路径跟踪控制的效果造成较大影响(bai g,meng y,liu l,et al.a new path tracking method based on multilayer model predictive control[j].applied sciences,2019,9(13):2649)。对此，该文献提出了一种基于多层模型预测控制的路径跟踪控制方法，以通过主动调节纵向速度来提高路径跟踪控制的精确性。不过该方法仅能以固定的间隔调整纵向速度，每两个控制周期之间的纵向速度变化幅度无法变化。因此，该方法虽能够通过主动调节铰接式车辆纵向速度提高路径跟踪控制的精确性，但在调整幅度和平滑性方面仍然存在不足。


技术实现要素：

[0003]
本发明提供了一种基于偏航预警的铰接式车辆速度调节控制方法及系统，以解决现有的速度调节控制方法在调节铰接式车辆纵向速度以提高路径跟踪控制精确性时调整幅度有限以及调整平滑度不足的技术问题。
[0004]
为解决上述技术问题，本发明提供了如下技术方案：
[0005]
一方面，本发明提供了一种基于偏航预警的铰接式车辆速度调节控制方法，所述基于偏航预警的铰接式车辆速度调节控制方法包括：
[0006]
建立铰接式车辆行驶轨迹预测模型；
[0007]
将所述行驶轨迹预测模型导入车身坐标系，利用车身坐标系对车辆横、纵向坐标与航向角解耦，在车身坐标系中设计铰接式车辆偏航预警优化目标函数；
[0008]
对所述优化目标函数进行求解，实现铰接式车辆速度调节控制。
[0009]
进一步地，所述建立铰接式车辆行驶轨迹预测模型，包括：
[0010]
建立如下的铰接式车辆运动学模型：
[0011][0012]
其中，lf为铰接式车辆的前车桥至铰接点的距离，lr为铰接式车辆的后车桥至铰接点的距离，xf、yf为铰接式车辆的前车桥中心在全局坐标系中的坐标，vf为铰接式车辆的纵
向速度，θf为航向角，ω
γ
为铰接角速度，γ为铰接角；
[0013]
在进行偏航预警时，模型的输入量铰接角速度ω
γ
已知，假设未来的铰接角速度均为0，得一系列铰接角速度，公式如下：
[0014][0015]
其中，ω
γ
(t)为初始时刻的铰接角速度，ω
γ
(t|t)为t时刻的铰接角速度，ω
γ
(t+i|t)为t+i时刻的铰接角速度，i＝1,2,
…
,p-1，p为预测步数；
[0016]
以欧拉法离散模型，计算获得未来的铰接角，公式如下：
[0017][0018]
其中，γ(t|t)为t时刻的铰接角，γ(t+i|t)为t+i时刻的铰接角，i＝1,2,
…
,p，t为预测周期；
[0019]
在计算未来的航向角时假设纵向速度恒定为并将未来航向角的计算导入车身坐标系中，即假设航向角初始值为0，得到：
[0020][0021]
其中，θf(t+i|t)为t+i时刻的航向角，i＝1,2,
…
,p；
[0022]
横、纵坐标的初始值也假设为0，得到：
[0023]
[0024][0025]
其中，xf(t+i|t)为t+i时刻的铰接式车辆的前车桥中心在全局坐标系中的x坐标，yf(t+i|t)为t+i时刻的铰接式车辆的前车桥中心在全局坐标系中的y坐标；
[0026]
令：
[0027][0028]
即为铰接式车辆行驶轨迹预测模型。
[0029]
进一步地，所述利用车身坐标系对车辆横、纵向坐标与航向角解耦，在车身坐标系中设计铰接式车辆偏航预警优化目标函数，包括：
[0030]
在所述行驶轨迹预测模型已被导入车身坐标系的情况下，将车辆前方参考路径上的p个参考点也导入车身坐标系，得到：
[0031][0032]
其中，x
ref
(t+i|t)、y
ref
(t+i|t)为参考路径的坐标，θ
ref
(t+i|t)为参考路径的航向角，x
refgc
(t+i|t)、y
refgc
(t+i|t)为全局坐标系下的参考路径的坐标，x
gc
(t)、y
gc
(t)为全局坐标系下的初始坐标，θ
gc
(t)为全局坐标系下的初始航向角，θ
refgc
(t+i|t)为全局坐标系下的参考路径的航向角；
[0033]
将上式写为矢量形式，得到：
[0034][0035]
将参考信息拟合为纵向速度的函数，得到：
[0036][0037]
其中，f
x
(ivft)为横坐标关于纵向速度的函数，fy(ivft)为纵坐标关于纵向速度的函数，f
θ
(ivft)为航向角关于纵向速度的函数；
[0038]
将优化目标函数设计为：
[0039][0040]
其中：
[0041][0042]
其中，q
x
、qy和q
θ
分别为横坐标方向位移误差权重、纵坐标方向位移误差权重和航向误差权重。
[0043]
进一步地，对优化目标函数进行求解，实现铰接式车辆速度调节控制，包括：
[0044]
求解所述优化目标函数的最小值，获得能够使铰接式车辆偏离参考路径较少的且较快的纵向速度，实现铰接式车辆速度调节控制。
[0045]
另一方面，本发明还提供了一种基于偏航预警的铰接式车辆速度调节控制系统，所述基于偏航预警的铰接式车辆速度调节控制系统包括：
[0046]
行驶轨迹预测模型构建模块，用于建立铰接式车辆行驶轨迹预测模型；
[0047]
车辆偏航预警优化目标函数设计模块，用于将所述行驶轨迹预测模型构建模块构建的行驶轨迹预测模型导入车身坐标系，利用车身坐标系对车辆横、纵向坐标与航向角解耦，在车身坐标系中设计铰接式车辆偏航预警优化目标函数；
[0048]
优化目标函数求解模块，用于对所述车辆偏航预警优化目标函数设计模块设计的优化目标函数进行求解，实现铰接式车辆速度调节控制。
[0049]
进一步地，所述行驶轨迹预测模型构建模块具体用于：
[0050]
建立如下的铰接式车辆运动学模型：
[0051][0052]
其中，lf为铰接式车辆的前车桥至铰接点的距离，lr为铰接式车辆的后车桥至铰接点的距离，xf、yf为铰接式车辆的前车桥中心在全局坐标系中的坐标，vf为铰接式车辆的纵向速度，θf为航向角，ω
γ
为铰接角速度，γ为铰接角；
[0053]
在进行偏航预警时，模型的输入量铰接角速度ω
γ
已知，假设未来的铰接角速度均为0，得一系列铰接角速度，公式如下：
[0054][0055]
其中，ω
γ
(t)为初始时刻的铰接角速度，ω
γ
(t|t)为t时刻的铰接角速度，ω
γ
(t+i|t)为t+i时刻的铰接角速度，i＝1,2,
…
,p-1，p为预测步数；
[0056]
以欧拉法离散模型，计算获得未来的铰接角，公式如下：
[0057][0058]
其中，γ(t|t)为t时刻的铰接角，γ(t+i|t)为t+i时刻的铰接角，i＝1,2,
…
,p，t为预测周期；
[0059]
在计算未来的航向角时假设纵向速度恒定为并将未来航向角的计算导入车身坐标系中，即假设航向角初始值为0，得到：
[0060][0061]
其中，θf(t+i|t)为t+i时刻的航向角，i＝1,2,
…
,p；
[0062]
横、纵坐标的初始值也假设为0，得到：
[0063]
[0064][0065]
其中，xf(t+i|t)为t+i时刻的铰接式车辆的前车桥中心在全局坐标系中的x坐标，yf(t+i|t)为t+i时刻的铰接式车辆的前车桥中心在全局坐标系中的y坐标；
[0066]
令：
[0067][0068]
即为铰接式车辆行驶轨迹预测模型。
[0069]
进一步地，所述车辆偏航预警优化目标函数设计模块具体用于：
[0070]
在所述行驶轨迹预测模型已被导入车身坐标系的情况下，将车辆前方参考路径上的p个参考点也导入车身坐标系，得到：
[0071][0072]
其中，x
ref
(t+i|t)、y
ref
(t+i|t)为参考路径的坐标，θ
ref
(t+i|t)为参考路径的航向角，x
refgc
(t+i|t)、y
refgc
(t+i|t)为全局坐标系下的参考路径的坐标，x
gc
(t)、y
gc
(t)为全局坐标系下的初始坐标，θ
gc
(t)为全局坐标系下的初始航向角，θ
refgc
(t+i|t)为全局坐标系下的参考路径的航向角；
[0073]
将上式写为矢量形式，得到：
[0074][0075]
将参考信息拟合为纵向速度的函数，得到：
[0076][0077]
其中，f
x
(ivft)为横坐标关于纵向速度的函数，fy(ivft)为纵坐标关于纵向速度的函数，f
θ
(ivft)为航向角关于纵向速度的函数；
[0078]
将优化目标函数设计为：
[0079][0080]
其中：
[0081][0082]
其中，q
x
、qy和q
θ
分别为横坐标方向位移误差权重、纵坐标方向位移误差权重和航向误差权重。
[0083]
进一步地，所述优化目标函数求解模块具体用于：
[0084]
求解所述优化目标函数的最小值，获得能够使铰接式车辆偏离参考路径较少的且较快的纵向速度，实现铰接式车辆速度调节控制。
[0085]
再一方面，本发明还提供了一种电子设备，其包括处理器和存储器；其中，存储器中存储有至少一条指令，所述指令由处理器加载并执行以实现上述方法。
[0086]
又一方面，本发明还提供了一种计算机可读存储介质，所述存储介质中存储有至少一条指令，所述指令由处理器加载并执行以实现上述方法。
[0087]
本发明提供的技术方案带来的有益效果至少包括：
[0088]
本发明提供了一种基于偏航预警的铰接式车辆速度调节控制方法，该方法采用预测控制的思路，首先建立铰接式车辆行驶轨迹预测模型，然后利用车身局部坐标系对横、纵向坐标与航向角解耦，并在此坐标系中设计偏航预警优化目标函数，从而获取能够使铰接式车辆偏离参考路径较少的较快的纵向速度，从而解决了现有速度调节控制方法调整幅度有限，以及调整平滑度不足等问题。
附图说明
[0089]
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0090]
图1是本发明实施例提供的基于偏航预警的铰接式车辆速度调节控制方法的执行流程示意图。
具体实施方式
[0091]
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。
[0092]
第一实施例
[0093]
本实施例提供了一种基于偏航预警的铰接式车辆速度调节控制方法，该方法可以由电子设备实现。该方法的执行流程如图1所示，包括以下步骤：
[0094]
s1，建立铰接式车辆行驶轨迹预测模型；
[0095]
s2，将行驶轨迹预测模型导入车身坐标系，利用车身坐标系对车辆横、纵向坐标与航向角解耦，在车身坐标系中设计铰接式车辆偏航预警优化目标函数；
[0096]
s3，对优化目标函数进行求解，实现铰接式车辆速度调节控制。
[0097]
进一步地，铰接式车辆行驶轨迹预测模型的建立过程，包括：
[0098]
铰接式车辆行驶轨迹预测模型以铰接式车辆运动学模型为基础：：
[0099][0100]
其中，lf为铰接式车辆前车桥至铰接点的距离，lr为铰接式车辆后车桥至铰接点的距离，xf、yf为铰接式车辆前车桥中心在全局坐标系中的坐标，vf为铰接式车辆的纵向速度，θf为航向角(横摆角)，ω
γ
为铰接角速度，γ为铰接角；
[0101]
在进行偏航预警时，模型的输入量铰接角速度ω
γ
已知，假设未来的铰接角速度均为0，可得一系列铰接角速度：
[0102][0103]
其中，ω
γ
(t)为初始时刻的铰接角速度，ω
γ
(t|t)为t时刻的铰接角速度，ω
γ
(t+i|t)为t+i时刻的铰接角速度，i＝1,2,
…
,p-1，p为预测步数；
[0104]
以欧拉法离散模型，未来的铰接角也可以计算获得：
[0105][0106]
其中，γ(t|t)为t时刻的铰接角，γ(t+i|t)为t+i时刻的铰接角，i＝1,2,
…
,p，t
为预测周期；
[0107]
对于航向角，考虑到航向角速度与纵向速度成正比，因此当优化目标为减小航向误差及其积分量位移误差时，存在能够通过增加纵向速度减小误差的局部最优情况，为避免该情况，在计算未来的航向角时假设纵向速度恒定为vf，同时考虑到航向角与横、纵向误差的耦合关系是变化的，为削弱该变化的影响，将未来航向角的计算导入车身坐标系中，即假设航向角初始值为0：
[0108][0109]
其中，θf(t+i|t)为t+i时刻的航向角，i＝1,2,
…
,p；
[0110]
横、纵坐标的初始值也假设为0：
[0111][0112][0113]
其中，xf(t+i|t)为t+i时刻的铰接式车辆的前车桥中心在全局坐标系中的x坐标，yf(t+i|t)为t+i时刻的铰接式车辆的前车桥中心在全局坐标系中的y坐标；
[0114]
令：
[0115][0116]
即为铰接式车辆行驶轨迹预测模型。
[0117]
进一步地，利用车身坐标系对车辆横、纵向坐标与航向角解耦，在车身坐标系中设计铰接式车辆偏航预警优化目标函数，包括：
[0118]
由于行驶轨迹预测模型已被导入车身坐标系，设计铰接式车辆偏航预警优化目标函数首先需要将车辆前方参考路径上的p个参考点也导入车身坐标系：
[0119][0120]
其中，下标ref指参考路径信息，下标gc指在全局坐标系下的信息。也即：x
ref
(t+i|t)、y
ref
(t+i|t)为参考路径的坐标，θ
ref
(t+i|t)为参考路径的航向角，x
refgc
(t+i|t)、y
refgc
(t+i|t)为全局坐标系下的参考路径的坐标，x
gc
(t)、y
gc
(t)为全局坐标系下的初始坐标，θ
gc
(t)为全局坐标系下的初始航向角，θ
refgc
(t+i|t)为全局坐标系下的参考路径的航向角；
[0121]
将上式写为矢量形式：
[0122][0123]
考虑到参考点选取也与速度有关，将参考信息拟合为纵向速度的函数：
[0124][0125]
其中，f
x
(ivft)为横坐标关于纵向速度的函数，fy(ivft)为纵坐标关于纵向速度的函数，f
θ
(ivft)为航向角关于纵向速度的函数；
[0126]
在此基础上，优化目标函数可以设计为：
[0127][0128]
其中：
[0129][0130]
其中，q
x
、qy和q
θ
分别为横坐标方向位移误差权重、纵坐标方向位移误差权重和航向误差权重，为保障能够及时减速，q
x
通常取较小值。
[0131]
进一步地，对优化目标函数进行求解，实现铰接式车辆速度调节控制，包括：
[0132]
求解上述偏航预警优化目标函数的最小值，即可获得能够使铰接式车辆偏离参考路径较少的且较快的纵向速度，从而实现铰接式车辆速度调节控制。
[0133]
综上，本实施例提供了一种基于偏航预警的铰接式车辆速度调节控制方法，采用预测控制的思路，首先建立铰接式车辆行驶轨迹预测模型，然后利用车身局部坐标系对横、纵向坐标与航向角解耦，并在此坐标系中设计偏航预警优化目标函数，从而获取能够使铰接式车辆偏离参考路径较少的较快的纵向速度，从而解决了现有速度调节控制方法调整幅度有限，以及调整平滑度不足等问题。
[0134]
第二实施例
[0135]
本实施例提供了一种基于偏航预警的铰接式车辆速度调节控制系统，包括：
[0136]
行驶轨迹预测模型构建模块，用于建立铰接式车辆行驶轨迹预测模型；
[0137]
车辆偏航预警优化目标函数设计模块，用于将所述行驶轨迹预测模型构建模块构建的行驶轨迹预测模型导入车身坐标系，利用车身坐标系对车辆横、纵向坐标与航向角解耦，在车身坐标系中设计铰接式车辆偏航预警优化目标函数；
[0138]
优化目标函数求解模块，用于对所述车辆偏航预警优化目标函数设计模块设计的优化目标函数进行求解，实现铰接式车辆速度调节控制。
[0139]
本实施例的基于偏航预警的铰接式车辆速度调节控制系统与上述第一实施例的基于偏航预警的铰接式车辆速度调节控制方法相对应；其中，本实施例的基于偏航预警的铰接式车辆速度调节控制系统中的各功能模块所实现的功能与上述第一实施例的基于偏航预警的铰接式车辆速度调节控制方法中的各流程步骤一一对应；故，在此不再赘述。
[0140]
第三实施例
[0141]
本实施例提供一种电子设备，其包括处理器和存储器；其中，存储器中存储有至少一条指令，所述指令由处理器加载并执行，以实现第一实施例的方法。
[0142]
该电子设备可因配置或性能不同而产生比较大的差异，可以包括一个或一个以上处理器(central processing units，cpu)和一个或一个以上的存储器，其中，存储器中存储有至少一条指令，所述指令由处理器加载并执行上述方法。
[0143]
第四实施例
[0144]
本实施例提供一种计算机可读存储介质，该存储介质中存储有至少一条指令，所述指令由处理器加载并执行，以实现上述第一实施例的方法。其中，该计算机可读存储介质可以是rom、随机存取存储器、cd-rom、磁带、软盘和光数据存储设备等。其内存储的指令可由终端中的处理器加载并执行上述方法。
[0145]
此外，需要说明的是，本发明可提供为方法、装置或计算机程序产品。因此，本发明实施例可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本发明实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质上实施的计算机程序产品的形式。
[0146]
本发明实施例是参照根据本发明实施例的方法、终端设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、嵌入式处理机或其他可编程数据处理终端设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理终端设备的处理器执行的指令产生
用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0147]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理终端设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理终端设备上，使得在计算机或其他可编程终端设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程终端设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0148]
还需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的相同要素。
[0149]
最后需要说明的是，以上所述是本发明优选实施方式，应当指出，尽管已描述了本发明优选实施例，但对于本技术领域的技术人员来说，一旦得知了本发明的基本创造性概念，在不脱离本发明所述原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明实施例范围的所有变更和修改。