一种面向铰接式车辆的泊车轨迹规划和跟踪控制方法及系统

1.本发明属于铰接式车辆无人驾驶技术领域，具体涉及一种面向铰接式车辆的泊车轨迹规划和跟踪控制方法及系统。


背景技术：

2.铰接式车辆作为一种灵活、机动的运输设备，常用于矿山和建筑工地等非结构区域的物料运输作业，整车由前后车体两部分构成，两个车体之间通过铰接点和摆动环相连，利用液压执行机构使前后车体相对偏转实现整车的转向，转向过程中车轮与车架之间无相对偏转，从而省略了转向驱动桥等部件。该结构形式也决定了其自动驾驶领域的避障轨迹规划和轨迹跟踪控制不能照搬传统乘用车辆。
3.无人驾驶铰接车在轨迹跟踪过程中会受到模型误差及外界扰动等不确定性因素的干扰，然而现有方法较少考虑以上因素。例如，一种现有技术将目标函数和约束函数设定为恒定值，这无疑降低了无人驾驶铰接车的轨迹控制方法的鲁棒性。
4.综上所述，亟需一种面向铰接式车辆且具有鲁棒性能良好的泊车轨迹规划和跟踪控制方法及系统，以解决现有的无人驾驶铰接式车辆泊车轨迹规划和跟踪控制方法无法实现高效跟踪控制及控制方法问题。


技术实现要素：

5.针对现有技术存在的不足，本发明提出了一种面向铰接式车辆的泊车轨迹规划和跟踪控制方法及系统，该方法包括：
6.s1：构建铰接车运动学模型和跟踪偏差模型；
7.s2：根据铰接车运动模型和跟踪偏差模型，设计不确定非线性系统；
8.s3：根据不确定非线性系统，构建目标函数和约束函数，得到不确定非线性系统最优化问题描述；
9.s4：求解不确定非线性系统最优化问题，得到泊车轨迹规划和跟踪控制方案；铰接式车辆根据泊车轨迹规划和跟踪控制方案实现自动泊车。
10.优选的，铰接车运动学模型的表达式为：
[0011][0012]
其中，xf表示前桥中点的横坐标，yf表示前桥中点的纵坐标，θf表示前桥航向角，lf表示前桥与铰接点的距离，lr表示后桥与铰接点的距离，γ表示铰接转向角，vf表示铰接车前桥的运动速度。
[0013]
优选的，跟踪偏差模型的表达式为：
[0014][0015]
其中，e1表示横向位置偏差，e2表示航向角偏差，v表示整车的运动速度，k表示行驶路径的曲率，θ表示前桥航向角。
[0016]
优选的，不确定非线性系统的表达式为：
[0017][0018]
其中，表示铰接车在第j种泊车场景下k+1时刻的状态，表示铰接车在对应的父节点第j种泊车场景下k时刻的状态，表示铰接车在第j种泊车场景下k时刻的控制输入，表示需要实现控制输入对应的第j种泊车场景下k时刻的不确定参数。
[0019]
优选的，得到不确定非线性系统最优化问题描述的过程包括：采用场景树表示泊车过程中面临的不确定因素；根据场景树，将泊车过程划分为若干阶段；在不同阶段构建与泊车场景相统一的目标函数和约束函数；根据目标函数和约束函数，得到不确定非线性系统最优化问题描述。
[0020]
进一步的，目标函数为：
[0021][0022]
其中，ji表示第i种泊车场景的目标，n
p
表示预测时域，表示铰接车在第j种泊车场景下k+1时刻的状态，xs表示期望状态，表示铰接车在第j种泊车场景下k时刻的控制输入，us表示期望输入，δuk表示k时刻的输入增量，q表示第一对角矩阵，r表示第二对角矩阵，r
δ
表示第三对角矩阵。
[0023]
优选的，不确定非线性系统最优化问题描述为：
[0024][0025][0026][0027][0028]
若则
[0029]
其中，ωi表示第i种泊车场景的目标权重，ji表示第i种泊车场景的目标，i表示场景树分支取值范围，n表示泊车场景数量，表示铰接车在第j种泊车场景下k时刻的状态，表示铰接车在对应的父节点第j种泊车场景下k时刻状态，x表示铰接车状态取值范围，表示铰接车在第j种泊车场景下k时刻的控制输入，u表示铰接车控制输入取值范围，表示需要实现控制输入对应的第j种泊车场景下k时刻的不确定参数，表示铰接车
在对应的父节点第l种泊车场景下k时刻的状态，表示铰接车在第l种泊车场景下k时刻的控制输入。
[0030]
一种面向铰接式车辆的泊车轨迹规划和跟踪控制系统，包括：模型构建模块、不确定非线性系统设计模块、优化问题构建模块、解算模块和控制模块；
[0031]
所述模型构建模块用于构建铰接车运动模型和跟踪偏差模型；
[0032]
所述不确定非线性系统设计模块用于根据铰接车运动模型和跟踪偏差模型，设计不确定非线性系统；
[0033]
所述优化问题构建模块用于根据不确定非线性系统构建不确定非线性系统最优化问题；
[0034]
所述解算模块用于求解不确定非线性系统最优化问题，得到泊车轨迹规划和跟踪控制方案；
[0035]
所述控制模块用于根据泊车轨迹规划和跟踪控制方案实现自动泊车。
[0036]
本发明的有益效果为：本发明设计的一种面向铰接式车辆的泊车轨迹规划和跟踪控制方法，考虑了泊车轨迹规划和跟踪控制过程中的不确定因素并采用场景树表示泊车过程中面临的不确定因素，采用约束函数和多阶的目标函数描述轨迹规划和跟踪控制过程；由于多阶目标函数和约束函数对当前阶段的系统模型及输入参数更具有适应性，因此，相较于以恒定目标函数和恒定约束为基础构建的轨迹规划和跟踪控制方法，基于场景树的多阶目标函数和约束函数构建的轨迹规划和跟踪控制方法具有更好的鲁棒性，即当面对未来时刻的新信息时，最优化的变量可以作为辅助变量将不确定参数抵消，进而改善原有策略的低泛化性；规划的轨迹和跟踪控制过程更符合实际场景的需要，实用性高。
附图说明
[0037]
图1为本发明中面向铰接式车辆的泊车轨迹规划和跟踪控制方法流程图；
[0038]
图2为本发明中铰接车运动模型示意图；
[0039]
图3为本发明中铰接车路径跟踪示意图；
[0040]
图4为本发明中泊车过程不确定场景的场景树示意图。
具体实施方式
[0041]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0042]
本发明提出了一种面向铰接式车辆的泊车轨迹规划和跟踪控制方法及系统，如图1所示，所述方法包括：
[0043]
s1：构建铰接车运动学模型和跟踪偏差模型。
[0044]
如图2所示，根据铰接车的状态参数构建铰接车运动模型，其中，铰接车的状态参数包括铰接车前桥中点坐标、前桥航向角、前桥与铰接点的距离、后桥与铰接点的距离、铰接转向角、铰接车前桥的运动速度等；铰接车运动模型的表达式为：
[0045][0046]
其中，xf表示前桥中点的横坐标，yf表示前桥中点的纵坐标，θf表示前桥航向角，lf表示前桥与铰接点的距离，lr表示后桥与铰接点的距离，γ表示铰接转向角，vf表示铰接车前桥的运动速度。
[0047]
如图3所示，跟踪偏差模型的表达式为：
[0048][0049]
其中，e1表示横向位置偏差，e2表示航向角偏差，v表示整车的运动速度，k表示行驶路径的曲率，θ表示前桥航向角。
[0050]
对于理想状态，横向位置偏差与航向角偏差均为0。
[0051]
s2：根据铰接车运动模型和跟踪偏差模型，设计不确定非线性系统。
[0052]
为了提高使得轨迹规划和跟踪控制与实际场景的适配性，本发明考虑泊车过程中的不确定性参数，不确定性参数包括但不限于路径曲率，优选的，本发明的不确性参数为路径曲率；根据铰接车运动模型和跟踪偏差模型，设计不确定非线性系统，其表达式为：
[0053][0054]
其中，表示铰接车在第j种泊车场景下k+1时刻的状态，表示铰接车在对应的父节点第j种泊车场景下k时刻的状态，表示铰接车在第j种泊车场景下k时刻的控制输入，表示需要实现控制输入对应的第j种泊车场景下k时刻的不确定参数。
[0055]
上式表明铰接车在同种影响(相同的不确定因素和控制变量)下，铰接车在k+1时刻的状态取决于前一时刻的状态、对应的控制输入和不确定参数；状态x＝[xf,yf,θf,γ,e1,e2]
t
，控制输入u＝[vf,γ]
t
，不确定参数d＝[k]。
[0056]
s3：根据不确定非线性系统，构建目标函数和约束函数，得到不确定非线性系统最优化问题描述。
[0057]
根据不确定非线性系统将铰接车的轨迹规划和跟踪控制整合为一个动态优化问题，通过对该问题在线求解，便可以实现在线泊车轨迹轨迹及跟踪控制。
[0058]
得到不确定非线性系统最优化问题描述的过程为：
[0059]
泊车过程的不确定场景是离散的，采用场景树表示泊车过程中面临的不确定因素；如图4所示，在每个节点的每个分支都代表一种未知的不确定因素(参数误差、外部扰动等)和控制变量对系统的影响。由树结构可知，对于泊车场景，以当前时刻为主节点，每个节点有3个分支，当定义鲁棒时域为2时，其对应的分支节点有9个，为降低计算开销，当时域超过2时，树节点不再增加。因此，当预测时域为4时，其对应的分支节点依然为9个。
[0060]
根据场景树，将泊车过程划分为若干阶段；在不同阶段构建与泊车场景相统一的
目标函数和约束函数；目标函数为：
[0061][0062]
其中，ji表示第i种泊车场景的目标，n
p
表示预测时域，表示铰接车在第j种泊车场景下k+1时刻的状态，xs表示期望状态，表示铰接车在第j种泊车场景下k时刻的控制输入，us表示期望输入，δuk表示k时刻的输入增量，q表示第一对角矩阵，r表示第二对角矩阵，r
δ
表示第三对角矩阵。
[0063]
在上式目标函数中，第一项表示状态变量跟踪目标值的惩罚项，第二项表示控制变量跟踪目标值的惩罚项，第三项表示控制变量变化幅度的惩罚项，防止控制变量的剧烈变化。
[0064]
根据目标函数和约束函数，得到不确定非线性系统最优化问题描述，该不确定非线性系统最优化问题描述为：
[0065][0066][0067][0068][0069]
若则
[0070]
其中，ωi表示第i种泊车场景的目标权重，其权重值越高，表示与之对应的场景受到的关注度越高，目标权重根据场景的受关注程度取值；ji表示第i种泊车场景的目标，i表示场景树分支取值范围，n表示泊车场景数量，表示铰接车在第j种泊车场景下k时刻的状态，表示铰接车在对应的父节点第j种泊车场景下k时刻状态，x表示铰接车状态取值范围，表示铰接车在第j种泊车场景下k时刻的控制输入，u表示铰接车控制输入取值范围，表示需要实现控制输入对应的第j种泊车场景下k时刻的不确定参数，表示铰接车在对应的父节点第l种泊车场景下k时刻的状态，表示铰接车在第l种泊车场景下k时刻的控制输入。
[0071]
对于每个泊车阶段皆要满足上述不确定非线性系统最优化问题描述。
[0072]
s4：求解不确定非线性系统最优化问题，得到泊车轨迹规划和跟踪控制方案；铰接式车辆根据泊车轨迹规划和跟踪控制方案实现自动泊车。
[0073]
采用隐式欧拉方法对模型的微分方程即不确定非线性系统表达式进行离散化，将连续时域模型转换为符合泊车场景的离散模型，即：
[0074][0075]
其中，δt表示时间步长，表示数学模型在连续时域中的常微分方
程。
[0076]
上式可采用高斯伪谱配置泊车场景的初始值，以降低计算开销。
[0077]
每个采样时间即每个阶段需要求解的优化问题为：
[0078][0079][0080][0081]
若则
[0082][0083]
采用二次规划方法对上式非规划问题进行求解，包括以下内容：
[0084]
采用隐式欧拉方法对模型的微分方程进行离散化后，每个欧拉离散点对应泊车过程中的一个场景，即离散点与场景树的节点相对应，则泊车过程中每一阶段的目标函数及相应约束可表示为：
[0085][0086]
s.t.x
l
≤x
opt
≤xu[0087]bl
≤ax
opt
≤bu[0088]cl
≤c(x
opt
)≤cu[0089]
其中，x
opt
表示增广优化向量，a和c分别表示线性约束和非线性约束对应的约束矩阵，包括场景树的所有状态节点和控制输入，目标函数f中包含优化问题中目标函数的所有项目；约束x
l
≤x
opt
≤xu表示施加在泊车系统上的状态约束和控制约束，b
l
≤ax
opt
≤bu表示包含非预期约束的线性约束，c
l
≤c(x
opt
)≤cu表示包含场景树节点的离散模型的非线性约束；其中，一阶导数可采用tomlab有限差分计算，二阶导数采用snopt有限记忆牛顿法计算，它利用sqp方法求解带约束非线性规划，特别适用于对于泊车过程中对模型离散后大规模稀疏问题。采用以上方法得到相对精确的雅克比矩阵和hessian矩阵，从而更有效的解决优化问题，最终使场景树不同节点的局部最优解为泊车阶段对应的优化问题的解。
[0090]
根据泊车阶段对应的优化问题最优问题的解，得到具有良好鲁棒性的泊车轨迹规划和跟踪控制方案；铰接式车辆根据泊车轨迹规划和跟踪控制方案实现自动泊车。
[0091]
本发明还提供一种用于执行上述面向铰接式车辆的泊车轨迹规划和跟踪控制方法的系统，该系统包括：模型构建模块、不确定非线性系统设计模块、优化问题构建模块、解算模块和控制模块；
[0092]
所述模型构建模块用于构建铰接车运动模型和跟踪偏差模型；
[0093]
所述不确定非线性系统设计模块用于根据铰接车运动模型和跟踪偏差模型，设计不确定非线性系统；
[0094]
所述优化问题构建模块用于根据不确定非线性系统构建不确定非线性系统最优化问题；
[0095]
所述解算模块用于求解不确定非线性系统最优化问题，得到泊车轨迹规划和跟踪控制方案；
[0096]
所述控制模块用于根据泊车轨迹规划和跟踪控制方案实现自动泊车。
[0097]
本发明设计的一种面向铰接式车辆的泊车轨迹规划和跟踪控制方法，考虑了泊车轨迹规划和跟踪控制过程中的不确定因素并采用场景树表示泊车过程中面临的不确定因素，采用约束函数和多阶的目标函数描述轨迹规划和跟踪控制过程；由于多阶目标函数和约束函数对当前阶段的系统模型及输入参数更具有适应性，因此，相较于以恒定目标函数和恒定约束为基础构建的轨迹规划和跟踪控制方法，基于场景树的多阶目标函数和约束函数构建的轨迹规划和跟踪控制方法更具有适配性；规划的轨迹和跟踪控制过程更符合实际场景的需要，实用性高。
[0098]
以上所举实施例，对本发明的目的、技术方案和优点进行了进一步的详细说明，所应理解的是，以上所举实施例仅为本发明的优选实施方式而已，并不用以限制本发明，凡在本发明的精神和原则之内对本发明所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。