CN113064344B - 一种多轴无人重载车辆的轨迹跟踪控制方法 - Google Patents

一种多轴无人重载车辆的轨迹跟踪控制方法 Download PDF

Info

Publication number
CN113064344B
CN113064344B CN202110296428.5A CN202110296428A CN113064344B CN 113064344 B CN113064344 B CN 113064344B CN 202110296428 A CN202110296428 A CN 202110296428A CN 113064344 B CN113064344 B CN 113064344B
Authority
CN
China
Prior art keywords
track
vehicle
point
shaft
speed
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110296428.5A
Other languages
English (en)
Other versions
CN113064344A (zh
Inventor
熊会元
李志航
尹文成
潘跃龙
刘羽
马健
李同同
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China Nuclear Power Engineering Co Ltd
Sun Yat Sen University
Original Assignee
China Nuclear Power Engineering Co Ltd
Sun Yat Sen University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by China Nuclear Power Engineering Co Ltd, Sun Yat Sen University filed Critical China Nuclear Power Engineering Co Ltd
Priority to CN202110296428.5A priority Critical patent/CN113064344B/zh
Publication of CN113064344A publication Critical patent/CN113064344A/zh
Application granted granted Critical
Publication of CN113064344B publication Critical patent/CN113064344B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B11/00—Automatic controllers
    • G05B11/01—Automatic controllers electric
    • G05B11/36—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential
    • G05B11/42—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential for obtaining a characteristic which is both proportional and time-dependent, e.g. P. I., P. I. D.
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明针对多轴无人重载车辆满载质量大、重心高、惯性大以及转角需连续性变化以减少轴间载荷的转移的特点，提出了一种多轴无人重载车辆的轨迹跟踪控制方法，通过对轨迹速度曲线的优化，使得车辆运动的速度、加速度、横向加速度处于安全范围内，最小化急动度的优化目标提高了车辆行驶的平稳度；同时通过运动学建模，构建了适用于前轮转向和全轮转向两种转向模式的模型预测控制算法。模型预测控制算法针对多轴无人重载车辆转向延迟大的特点，在提高轨迹跟踪的精度与稳定性的同时，对车辆转向角的变化进行优化，使转向角变化平缓，减少了轴间载荷的转移。

Description

一种多轴无人重载车辆的轨迹跟踪控制方法
技术领域
本发明涉及点无人车控制技术领域，具体涉及一种多轴无人重载车辆的轨迹跟踪控制方法。
背景技术
交通运输、建筑工程还是军事运载等领域都对车辆的大型化和重型化提出了越来越高的要求，多轴转向重载无人车辆应运而生。随着无人驾驶相关技术的不断突破，相关在恶劣环境下的重型特征车辆逐步实现其无人化与智能化是必然趋势。多轴转向重载无人车辆的轨迹跟踪系统是车辆控制的一项关键的技术，其目标是在满足一定安全性、操纵稳定性和舒适性的条件下，通过控制行驶中车辆的速度和车轮转角，使其以较小的偏差跟踪所规划的轨迹线，目前所采用的控制方法被控对象多为前轮转向汽车，而针对多轴转向的重载无人车辆的轨迹跟踪还存在不少问题。
公告日为2019.01.29，公告号为：CN 108279563 B的中国发明专利：一种速度自适应的无人车轨迹跟踪PID控制方法，试图提供一种轨迹跟踪精度、准时性、平稳性和安全性更优的速度自适应无人车轨迹跟踪方案。但是该方案并不能直接应用在多轴转向的重载无人车辆上，局限性明显。
发明内容
针对现有技术的局限，本发明提出一种多轴无人重载车辆的轨迹跟踪控制方法，本发明采用的技术方案是：
一种多轴无人重载车辆的轨迹跟踪控制方法，在对被控车辆的各控制周期中，包括以下步骤：
S1，从预设的全局参考轨迹中提取所述被控车辆当前位置前方的局部轨迹，以最小化急动度为目标对所述局部轨迹进行优化，获得优化轨迹；
S2，根据所述优化轨迹，结合所述被控车辆对应的多轴无人重载车辆运动学模型获得所述被控车辆的转角控制指令；
S3，根据所述优化轨迹，通过PID控制器计算出所述被控车辆的加速度控制指令；
S4，根据所述被控车辆的转角控制指令以及加速度控制指令对所述被控车辆进行控制。
相较于现有技术，本发明针对多轴无人重载车辆满载质量大、重心高、惯性大以及转角需连续性变化以减少轴间载荷的转移的特点，通过对轨迹速度曲线的优化，使得车辆运动的速度、加速度、横向加速度处于安全范围内，最小化急动度的优化目标提高了车辆行驶的平稳度；同时通过运动学建模，构建了适用于前轮转向和全轮转向两种转向模式的模型预测控制算法。模型预测控制算法针对多轴无人重载车辆转向延迟大的特点，在提高轨迹跟踪的精度与稳定性的同时，对车辆转向角的变化进行优化，使转向角变化平缓，减少了轴间载荷的转移。
作为一种优选方案，所述全局参考轨迹、局部轨迹以及优化轨迹分别由若干轨迹点组成，所述轨迹点的信息包括所述被控车辆的坐标、航向角、期望速度、期望角速度、期望加速度。
进一步的，所述实时定位信息包括车辆当前的X坐标、Y坐标以及航向角。
作为一种优选方案，所述步骤S1以最小化急动度为目标对所述局部轨迹进行优化，获得优化轨迹的过程包括以下步骤：
S11，按预设的最大速度限制对所述局部轨迹中轨迹点的期望速度进行限幅：
Figure BDA0002984529430000021
其中，v0表示所述局部轨迹中轨迹点的期望速度，v1表示步骤S11中的限幅结果，vmax表示所述最大速度限制；
S12，遍历并计算所述局部轨迹中各轨迹点的曲率值，根据所述曲率值结合预设的最大加速度限制对步骤S11中的限幅结果进行限幅：
Figure BDA0002984529430000031
其中，v2表示步骤S12中的限幅结果，amax表示所述最大加速度限制，k表示所述曲率值；
S13，根据以下优化方程以及求解评价函数，以使轨迹点的急动度最小且使优化后速度与步骤S12中的限幅结果之间的偏差最小为目标，使用OSQP求解库对经过步骤S12限幅后的局部轨迹进行凸优化求解，获得优化轨迹；所述优化方程如下：
Figure BDA0002984529430000032
其中，x是由局部轨迹各轨迹点的信息组成的矩阵，局部轨迹各轨迹点由列向量(v2,a,δ,σ)T表示，v2表示车辆纵向期望速度，a表示车辆纵向期望加速度，δ代表关于速度的松弛变量，σ代表关于加速度的松弛变量；
所述评价函数如下：
J(v)＝∫|v(s)2-vr(s)2|+wjjp(s)2+wδδ(s)2+wσσ(s)2ds；
其中，v(s)表示优化后的速度序列，vr(s)表示局部轨迹的期望速度；jp(s)表示优化后的加加速度，通过式子
Figure BDA0002984529430000033
计算获得；wj表示控制加加速度的权重，wδ表示控制速度松弛变量的权重，wσ表示控制加速度松弛变量的权重。
进一步的所述局部轨迹中各轨迹点的曲率值k按以下公式计算：
Figure BDA0002984529430000034
Figure BDA0002984529430000041
Figure BDA0002984529430000042
其中，
Figure BDA0002984529430000043
点为当前计算曲率值的轨迹点，
Figure BDA0002984529430000044
点、
Figure BDA0002984529430000045
点分别为与A点在预设距离上的前后两个轨迹点，△ABC为A、B、C三点所构成的圆的内接三角形。
进一步的，所述优化方程以及求解评价函数受限于以下二次规划的不等式约束：
Figure BDA0002984529430000046
amin-σ<a<amax+σ。
进一步的，所述被控车辆为四轴无人重载车辆；所述多轴无人重载车辆运动学模型的转向模式包括前轮转向模式和全轮转向模式，所述多轴无人重载车辆的运动学模型按以下公式表示：
Figure BDA0002984529430000047
其中，
Figure BDA0002984529430000048
和
Figure BDA0002984529430000049
分别代表Frenet坐标系下的纵向偏差和横向偏差；l为理论轴距：在前轮转向模式下，l代表第一、第四轴之间的轴距；在全轮转向模式下，l代表第一、第四轴之间的半轴距；
Figure BDA0002984529430000051
为参考点的航向角，
Figure BDA0002984529430000052
为被控车辆当前航向角，
Figure BDA0002984529430000053
为被控车辆与轨迹点的航向角偏差，ρ(s)为在s处的参考曲率；在前轮转向模式下，所述参考点为第四轴轴中心点；在全轮转向模式下，所述参考点为被控车辆中心。
进一步的，所述多轴无人重载车辆的运动学模型中的转角关系按以下公式表示：
Figure BDA0002984529430000054
其中，γi(i＝1,2,3,4)为第i轴内轮转角；αi(i＝1,2,3,4)为第i轴外轮转角；B为轮距；l12为第一、二轴之间的轴距，l34为第三、四轴之间的轴距。
进一步的，所述多轴无人重载车辆的运动学模型按以下方式进行线性化以及离散化：
以参考点速度vd近似
Figure BDA0002984529430000055
构建系统状态方程：
Figure BDA0002984529430000061
其中δ为车辆第一轴转角值，τ为考虑车辆转向延迟的转弯动力学时间常数；
得到系统非线性方程：
Figure BDA0002984529430000062
在参考点处，对所述系统非线性方程进行线性化：
v＝vr；
在参考点处的曲率，以车辆模型进行替代：
Figure BDA0002984529430000063
利用一阶泰勒展开后得到：
Figure BDA0002984529430000064
将线性系统方程
Figure BDA0002984529430000065
描述为：
Figure BDA0002984529430000066
所述线性系统方程的输出为：
Figure BDA0002984529430000071
Y＝CX，
Figure BDA0002984529430000072
采用双线性变化法进行离散化，即：
Figure BDA0002984529430000073
Bd＝B·dt，Wd＝W·dt，Cd＝C·dt；
将离散状态系统方程描述为：
X(k)＝AdX(k-1)+Bdu(k-1)+Wd；
离散输出方程为：
Y(k)＝CdX(k)。
进一步的，所述步骤S2包括以下步骤：
S21，根据被控车辆当前的转向模式，确定参考点，根据所述参考点修改理论轴距值l，对所述离散状态系统方程进行前向递推，即将未来一定将距离轨迹纳入统一矩阵内，表现形式为一系列的轨迹点，得到方程：
Figure BDA0002984529430000074
上述方程由递推关系得到的变换式为：
Figure BDA0002984529430000075
S22，根据以下二次规划方程求解期望转角值：
Xex＝Aex*X0+Bex*Uex+Wex；
代价函数为：
Figure BDA0002984529430000081
所述代价函数化简后的二次优化求解的标准格式为：
Figure BDA0002984529430000082
将转角限制与转角速率限制表示的不等式约束添加进行优化求解，即：
Figure BDA0002984529430000083
S23，根据被控车辆当前的转向模式，根据以下内容计算出被控车辆的各轴的转角值，根据各轴的转角值输出对应的转角控制指令：
设置第一轴角度δ1＝δcmd；
在前轮转向模式下，设置第四轴角度δ4＝0；各轴转角的比例等于该轴与第四轴中心点的距离的比例，即δ1∶δ2∶δ3＝l14∶l24∶l34；
在全轮转向模式下，第一、第二轴转角与第四、第三轴转角值相等，方向相反，第一、第二轴转角的比例等于第一、第二轴与质心点的距离的比例，即：δ1∶δ2＝l10∶l20，δ3＝-δ2，δ4＝-δ1。
作为一种优选方案，所述步骤S3包括以下步骤：
S31，以所述优化轨迹上最接近所述被控车辆实时位置的轨迹点的速度值作为PID控制器的控制目标，将该控制目标与所述被控车辆的实际速度值相减并通过低通滤波器，获得速度偏差；
S32，将所述速度偏差输入PID控制器，获得误差加速度补偿值；
S33，将所述误差加速度补偿值与所述优化轨迹上最接近所述被控车辆实时位置的轨迹点的期望加速度相加，获得加速度控制指令。
附图说明
图1为本发明实施例提供的多轴无人重载车辆的轨迹跟踪控制方法的步骤流程图；
图2为本发明实施例步骤S1的步骤流程图；
图3为本发明实施例曲率计算示意图；
图4为本发明实施例优化前后速度随里程变化的对比示意图；
图5为本发明实施例提供的多轴无人重载车辆运动学模型示意图；
图6为本发明实施例多轴转向模式示意图；
图7为本发明实施例多轴阿克曼转向示意图；
图8为本发明实施例步骤S3的步骤流程图；
图9为本发明实施例速度PID控制框图。
具体实施方式
附图仅用于示例性说明，不能理解为对本专利的限制；
应当明确，所描述的实施例仅仅是本申请实施例一部分实施例，而不是全部的实施例。基于本申请实施例中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本申请实施例保护的范围。
在本申请实施例使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请实施例。在本申请实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。
下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。相反，它们仅是如所附权利要求书中所详述的、本申请的一些方面相一致的装置和方法的例子。在本申请的描述中，需要理解的是，术语“第一”、“第二”、“第三”等仅用于区别类似的对象，而不必用于描述特定的顺序或先后次序，也不能理解为指示或暗示相对重要性。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本申请中的具体含义。
此外，在本申请的描述中，除非另有说明，“多个”是指两个或两个以上。“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。以下结合附图和实施例对本发明做进一步的阐述。
为了解决现有技术的局限性，本实施例提供了一种技术方案，下面结合附图和实施例对本发明的技术方案做进一步的说明。
一种多轴无人重载车辆的轨迹跟踪控制方法，请参考图1，在对被控车辆的各控制周期中，包括以下步骤：
S1，从预设的全局参考轨迹中提取所述被控车辆当前位置前方的局部轨迹，以最小化急动度为目标对所述局部轨迹进行优化，获得优化轨迹；
S2，根据所述优化轨迹，结合所述被控车辆对应的多轴无人重载车辆运动学模型获得所述被控车辆的转角控制指令；
S3，根据所述优化轨迹，通过PID控制器计算出所述被控车辆的加速度控制指令；
S4，根据所述被控车辆的转角控制指令以及加速度控制指令对所述被控车辆进行控制。
相较于现有技术，本发明针对多轴无人重载车辆满载质量大、重心高、惯性大以及转角需连续性变化以减少轴间载荷的转移的特点，通过对轨迹速度曲线的优化，使得车辆运动的速度、加速度、横向加速度处于安全范围内，最小化急动度的优化目标提高了车辆行驶的平稳度；同时通过运动学建模，构建了适用于前轮转向和全轮转向两种转向模式的模型预测控制算法。模型预测控制算法针对多轴无人重载车辆转向延迟大的特点，在提高轨迹跟踪的精度与稳定性的同时，对车辆转向角的变化进行优化，使转向角变化平缓，减少了轴间载荷的转移。
具体的，所述全局参考轨迹是在设定好的运载工况内，车辆从起点移动到终点行进的路线，是通过其他模块计算设置好的轨迹，可视为一种已知的信息；对于在本实施例中涉及的轨迹跟踪问题，车辆行进的目标是最小化当前位姿与所述全局参考轨迹之间的横向误差与航向误差。因此，所述全局参考轨迹是确定不变的，所述局部轨迹、优化轨迹、转角控制指令以及加速度控制指令则会随车辆行驶过程中的实时位置以及实时速度的改变而变化。
作为一种可选的实施例，可以将车载自动驾驶工控机的转角控制指令以及加速度控制指令信息通过CAN总线传输到车辆控制器VCU设备，以实现自动驾驶软件对车辆的控制。
作为一种可选的实施例，所述控制周期可以设置为20ms时间，步骤S1中从预设的全局参考轨迹中提取所述被控车辆当前位置前方的局部轨迹，所述局部轨迹的距离可以设置为100米。
作为一种优选实施例，所述全局参考轨迹、局部轨迹以及优化轨迹分别由若干轨迹点组成，所述轨迹点的信息包括所述被控车辆的坐标、航向角、期望速度、期望角速度、期望加速度。
轨迹点表示形式为
Figure BDA0002984529430000111
分别代表在该点的期望X坐标值，期望Y坐标值与期望航向角。
进一步的，请参阅图2，所述步骤S1以最小化急动度为目标对所述局部轨迹进行优化，获得优化轨迹的过程包括以下步骤：
S11，按预设的最大速度限制对所述局部轨迹中轨迹点的期望速度进行限幅：
Figure BDA0002984529430000112
其中，v0表示所述局部轨迹中轨迹点的期望速度，v1表示步骤S11中的限幅结果，vmax表示所述最大速度限制；
S12，遍历并计算所述局部轨迹中各轨迹点的曲率值，根据所述曲率值结合预设的最大加速度限制对步骤S11中的限幅结果进行限幅：
Figure BDA0002984529430000113
其中，v2表示步骤S12中的限幅结果，amax表示所述最大加速度限制，k表示所述曲率值；
S13，根据以下优化方程以及求解评价函数，以使轨迹点的急动度最小且使优化后速度与步骤S12中的限幅结果之间的偏差最小为目标，使用OSQP求解库对经过步骤S12限幅后的局部轨迹进行凸优化求解，获得优化轨迹；所述优化方程如下：
Figure BDA0002984529430000121
其中，x是由局部轨迹各轨迹点的信息组成的矩阵，局部轨迹各轨迹点由列向量(v2,a,δ,σ)T表示，v2表示车辆纵向期望速度，a表示车辆纵向期望加速度，δ代表关于速度的松弛变量，σ代表关于加速度的松弛变量；
所述评价函数如下：
J(v)＝∫|v(s)2-vr(s)2|+wjjp(s)2+wδδ(s)2+wσσ(s)2ds；
其中，v(s)表示优化后的速度序列，vr(s)表示局部轨迹的期望速度；jp(s)表示优化后的加加速度，通过式子
Figure BDA0002984529430000122
计算获得；wj表示控制加加速度的权重，wδ表示控制速度松弛变量的权重，wσ表示控制加速度松弛变量的权重。
进一步的，所述局部轨迹中各轨迹点的曲率值k按以下公式计算：
Figure BDA0002984529430000123
Figure BDA0002984529430000124
Figure BDA0002984529430000125
其中，请参阅图3，
Figure BDA0002984529430000126
点为当前计算曲率值的轨迹点，
Figure BDA0002984529430000127
点、
Figure BDA0002984529430000128
点分别为与A点在预设距离上的前后两个轨迹点，△ABC为A、B、C三点所构成的圆的内接三角形。
具体的，所述预设距离为一个可设置的参数值，如0.1米。
进一步的，所述优化方程以及求解评价函数受限于以下二次规划的不等式约束：
Figure BDA0002984529430000131
amin-σ<a<amax+σ。
具体的，上述不等式约束对速度和加速度进行了限幅，并通过松弛变量的设置，使得优化方程求解过程不会陷入无解。在一种可选实施例中，可以使用OSQP求解库对轨迹进行凸优化求解，获得优化后的轨迹。
通过对速度曲线的优化，车辆能更平缓地进行加减速操作，在各种动态情况下处理良好，优化前后速度随里程变化的对比可参阅图4：虚线为优化前速度曲线，实线为优化后的速度曲线。通过对速度的二阶导(急动度：jerk)的最小优化，速度曲线在尽可能接近设定曲线的前提下，也能平缓地进行速度变化。
进一步的，所述被控车辆为四轴无人重载车辆；请参阅图5，所述多轴无人重载车辆运动学模型的转向模式包括前轮转向模式和全轮转向模式，所述多轴无人重载车辆的运动学模型按以下公式表示：
Figure BDA0002984529430000132
其中，
Figure BDA0002984529430000133
和
Figure BDA0002984529430000134
分别代表Frenet坐标系下的纵向偏差和横向偏差；l为理论轴距：在前轮转向模式下，l代表第一、第四轴之间的轴距；在全轮转向模式下，l代表第一、第四轴之间的半轴距；
Figure BDA0002984529430000135
为参考点的航向角，
Figure BDA0002984529430000136
为被控车辆当前航向角，
Figure BDA0002984529430000137
为被控车辆与轨迹点的航向角偏差，ρ(s)为在s处的参考曲率；在前轮转向模式下，所述参考点为第四轴轴中心点；在全轮转向模式下，所述参考点为被控车辆中心。
具体的，请参阅图6，多轴转向车辆主要可以分为前轮转向、全轮转向、蟹型转向三种转向模式，如图6所示。前轮转向模式下，车辆第四轴的轮不转，前三轴的轮转角值递减，以满足动力学约束。全轮转向模式下，第一二轴与第三四轴对应转角值相等，转向相反，该转向模式可获得更大的转向半径。蟹型转向模式下，车辆每个轮子都转到同一个角度，车辆可以在一定角度范围内平移。
Frenet坐标系是一种以比传统x，y笛卡尔坐标更直观的方式表示道路位置的方式。用Frenet坐标，一般使用变量s和d描述车辆在道路上的位置。该s坐标表示沿道路的距离，也称为纵向位移；d坐标表示道路上的左右位置，也称为横向位移。
作为一种可选的实施例，对所述被控车辆进行建模时，通过将同轴两侧轮简化为单轮，如图7所示，车轮转角满足阿克曼关系。
进一步的，所述多轴无人重载车辆的运动学模型中的转角关系按以下公式表示：
Figure BDA0002984529430000151
其中，γi(i＝1,2,3,4)为第i轴内轮转角；αi(i＝1,2,3,4)为第i轴外轮转角；B为轮距；l12为第一、二轴之间的轴距，l34为第三、四轴之间的轴距。
进一步的，所述多轴无人重载车辆的运动学模型按以下方式进行线性化以及离散化：
以参考点速度vd近似
Figure BDA0002984529430000152
构建系统状态方程：
Figure BDA0002984529430000153
其中δ为车辆第一轴转角值，τ为考虑车辆转向延迟的转弯动力学时间常数；
得到系统非线性方程：
Figure BDA0002984529430000161
在参考点处，对所述系统非线性方程进行线性化：
v＝vr；
在参考点处的曲率，以车辆模型进行替代：
Figure BDA0002984529430000162
利用一阶泰勒展开后得到：
Figure BDA0002984529430000163
将线性系统方程
Figure BDA0002984529430000164
描述为：
Figure BDA0002984529430000165
所述线性系统方程的输出为：
Figure BDA0002984529430000166
Y＝CX，
Figure BDA0002984529430000167
采用双线性变化法进行离散化，即：
Figure BDA0002984529430000168
Bd＝B·dt，Wd＝W·dt，Cd＝C·dt；
将离散状态系统方程描述为：
X(k)＝AdX(k-1)+Bdu(k-1)+Wd；
离散输出方程为：
Y(k)＝CdX(k)。
进一步的，所述步骤S2包括以下步骤：
S21，根据被控车辆当前的转向模式，确定参考点，根据所述参考点修改理论轴距值l，对所述离散状态系统方程进行前向递推，即将未来一定将距离轨迹纳入统一矩阵内，表现形式为一系列的轨迹点，得到方程：
Figure BDA0002984529430000171
上述方程由递推关系得到的变换式为：
Figure BDA0002984529430000172
S22，根据以下二次规划方程求解期望转角值：
Xex＝Aex*X0+Bex*Uex+Wex；
代价函数为：
Figure BDA0002984529430000173
具体的，上述代价函数中，第一个部分，目的是让当前状态误差尽可能地小，即让横向误差与航向误差尽可能小，分别对应两个权重值为
Figure BDA0002984529430000174
和
Figure BDA0002984529430000175
第二个部分，目的是让控制量输入与轨迹上的参考控制量误差尽可能小；
第三个部分，目的是让控制量的输入尽可能小。
所述代价函数化简后的二次优化求解的标准格式为：
Figure BDA0002984529430000181
将转角限制与转角速率限制表示的不等式约束添加进行优化求解，即：
Figure BDA0002984529430000182
具体的，利用不等式约束，可以使得优化结果不出现不合理的转角与突变的转角值，符合多轴转向车辆的特性；
S23，根据被控车辆当前的转向模式，根据以下内容计算出被控车辆的各轴的转角值，根据各轴的转角值输出对应的转角控制指令：
设置第一轴角度δ1＝δcmd；
在前轮转向模式下，设置第四轴角度δ4＝0；各轴转角的比例等于该轴与第四轴中心点的距离的比例，即δ1∶δ2∶δ3＝l14∶l24∶l34；
在全轮转向模式下，第一、第二轴转角与第四、第三轴转角值相等，方向相反，第一、第二轴转角的比例等于第一、第二轴与质心点的距离的比例，即：δ1∶δ2＝l10∶l20，δ3＝-δ2，δ4＝-δ1。
具体的，不同转向模式条件下，选取的参考点不同。判断当前系统处于何种转向模式，如果是前轮转向，选取参考点为后轴中心附近点，理论轴距值为第一四轴的长度；如果是全轮转向，选取参考点为车辆质心附近点，理论轴距值为第一四轴的长度的一半。
作为一种优选实施例，请参阅图8和图9，所述步骤S3包括以下步骤：
S31，以所述优化轨迹上最接近所述被控车辆实时位置的轨迹点的速度值作为PID控制器的控制目标，将该控制目标与所述被控车辆的实际速度值相减并通过低通滤波器，获得速度偏差；
S32，将所述速度偏差输入PID控制器，获得误差加速度补偿值；
S33，将所述误差加速度补偿值与所述优化轨迹上最接近所述被控车辆实时位置的轨迹点的期望加速度相加，获得加速度控制指令。
具体的，PID控制器即比例-积分-微分控制器，由比例单元(P)、积分单元(I)和微分单元(D)组成。透过Kp，Ki和Kd三个参数的设定。PID控制器主要适用于基本上线性，且动态特性不随时间变化的系统。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (8)

1.一种多轴无人重载车辆的轨迹跟踪控制方法，其特征在于，在对被控车辆的各控制周期中，包括以下步骤：
S1，从预设的全局参考轨迹中提取所述被控车辆当前位置前方的局部轨迹，以最小化急动度为目标对所述局部轨迹进行优化，获得优化轨迹；
S2，根据所述优化轨迹，结合所述被控车辆对应的多轴无人重载车辆运动学模型获得所述被控车辆的转角控制指令；
S3，根据所述优化轨迹，通过PID控制器计算出所述被控车辆的加速度控制指令；
S4，根据所述被控车辆的转角控制指令以及加速度控制指令对所述被控车辆进行控制；
所述全局参考轨迹、局部轨迹以及优化轨迹分别由若干轨迹点组成，所述轨迹点的信息包括所述被控车辆的坐标、航向角、期望速度、期望角速度、期望加速度；
所述步骤S1以最小化急动度为目标对所述局部轨迹进行优化，获得优化轨迹的过程包括以下步骤：
S11，按预设的最大速度限制对所述局部轨迹中轨迹点的期望速度进行限幅：
Figure FDA0003601499410000011
其中，v0表示所述局部轨迹中轨迹点的期望速度，v1表示步骤S11中的限幅结果，vmax表示所述最大速度限制；
S12，遍历并计算所述局部轨迹中各轨迹点的曲率值，根据所述曲率值结合预设的最大加速度限制对步骤S11中的限幅结果进行限幅：
Figure FDA0003601499410000021
其中，v2表示步骤S12中的限幅结果，amax表示所述最大加速度限制，k表示所述曲率值；
S13，根据以下优化方程以及求解评价函数，以使轨迹点的急动度最小且使优化后速度与步骤S12中的限幅结果之间的偏差最小为目标，使用OSQP求解库对经过步骤S12限幅后的局部轨迹进行凸优化求解，获得优化轨迹；所述优化方程如下：
Figure FDA0003601499410000022
其中，x是由局部轨迹各轨迹点的信息组成的矩阵，局部轨迹各轨迹点由列向量(v2,a,δ,σ)T表示，v2表示车辆纵向期望速度，a表示车辆纵向期望加速度，δ代表关于速度的松弛变量，σ代表关于加速度的松弛变量；
所述评价函数如下：
J(v)＝∫|v(s)2-vr(s)2|+wjjp(s)2+wδδ(s)2+wσσ(s)2ds；
其中，v(s)表示优化后的速度序列，vr(s)表示局部轨迹的期望速度；jp(s)表示优化后的加加速度，通过式子
Figure FDA0003601499410000023
计算获得；wj表示控制加加速度的权重，wδ表示控制速度松弛变量的权重，wσ表示控制加速度松弛变量的权重。
2.根据权利要求1所述的多轴无人重载车辆的轨迹跟踪控制方法，其特征在于，所述局部轨迹中各轨迹点的曲率值k按以下公式计算：
Figure FDA0003601499410000024
Figure FDA0003601499410000031
Figure FDA0003601499410000032
其中，
Figure FDA0003601499410000033
点为当前计算曲率值的轨迹点，
Figure FDA0003601499410000034
点、
Figure FDA0003601499410000035
点分别为与A点在预设距离上的前后两个轨迹点，△ABC为A、B、C三点所构成的圆的内接三角形。
3.根据权利要求1所述的多轴无人重载车辆的轨迹跟踪控制方法，其特征在于，所述优化方程以及求解评价函数受限于以下二次规划的不等式约束：
Figure FDA0003601499410000039
amin-σ<a<amax+σ。
4.根据权利要求2所述的多轴无人重载车辆的轨迹跟踪控制方法，其特征在于，所述被控车辆为四轴无人重载车辆；所述多轴无人重载车辆运动学模型的转向模式包括前轮转向模式和全轮转向模式，所述多轴无人重载车辆的运动学模型按以下公式表示：
Figure FDA0003601499410000036
其中，
Figure FDA0003601499410000037
和
Figure FDA0003601499410000038
分别代表Frenet坐标系下的纵向偏差和横向偏差；l为理论轴距：在前轮转向模式下，l代表第一、第四轴之间的轴距；在全轮转向模式下，l代表第一、第四轴之间的半轴距；
Figure FDA0003601499410000041
为参考点的航向角，
Figure FDA0003601499410000042
为被控车辆当前航向角，
Figure FDA0003601499410000043
为被控车辆与轨迹点的航向角偏差，ρ(s)为在s处的参考曲率；在前轮转向模式下，所述参考点为第四轴轴中心点；在全轮转向模式下，所述参考点为被控车辆中心。
5.根据权利要求4所述的多轴无人重载车辆的轨迹跟踪控制方法，其特征在于，所述多轴无人重载车辆的运动学模型中的转角关系按以下公式表示：
Figure FDA0003601499410000044
其中，γi(i＝1,2,3,4)为第i轴内轮转角；αi(i＝1,2,3,4)为第i轴外轮转角；B为轮距；l12为第一、二轴之间的轴距，l34为第三、四轴之间的轴距。
6.根据权利要求5所述的多轴无人重载车辆的轨迹跟踪控制方法，其特征在于，所述多轴无人重载车辆的运动学模型按以下方式进行线性化以及离散化：
以参考点速度vd近似
Figure FDA0003601499410000051
构建系统状态方程：
Figure FDA0003601499410000052
其中δ为车辆第一轴转角值，τ为考虑车辆转向延迟的转弯动力学时间常数；
得到系统非线性方程：
Figure FDA0003601499410000053
在参考点处，对所述系统非线性方程进行线性化：
v＝vr；
在参考点处的曲率，以车辆模型进行替代：
Figure FDA0003601499410000054
利用一阶泰勒展开后得到：
Figure FDA0003601499410000055
将线性系统方程
Figure FDA0003601499410000056
描述为：
Figure FDA0003601499410000057
所述线性系统方程的输出为：
Figure FDA0003601499410000061
Y＝CX，
Figure FDA0003601499410000062
采用双线性变化法进行离散化，即：
Figure FDA0003601499410000063
Bd＝B·dt，Wd＝W·dt，Cd＝C·dt；
将离散状态系统方程描述为：
X(k)＝AdX(k-1)+Bdu(k-1)+Wd；
离散输出方程为：
Y(k)＝CdX(k)。
7.根据权利要求6所述的多轴无人重载车辆的轨迹跟踪控制方法，其特征在于，所述步骤S2包括以下步骤：
S21，根据被控车辆当前的转向模式，确定参考点，根据所述参考点修改理论轴距值l，对所述离散状态系统方程进行前向递推，即将未来一定将距离轨迹纳入统一矩阵内，表现形式为一系列的轨迹点，得到方程：
Figure FDA0003601499410000064
上述方程由递推关系得到的变换式为：
Figure FDA0003601499410000065
S22，根据以下二次规划方程求解期望转角值：
Xex＝Aex*X0+Bex*Uex+Wex；
代价函数为：
Figure FDA0003601499410000071
所述代价函数化简后的二次优化求解的标准格式为：
Figure FDA0003601499410000072
将转角限制与转角速率限制表示的不等式约束添加进行优化求解，即：
Figure FDA0003601499410000073
S23，根据被控车辆当前的转向模式，根据以下内容计算出被控车辆的各轴的转角值，根据各轴的转角值输出对应的转角控制指令：
设置第一轴角度δ1＝δcmd；
在前轮转向模式下，设置第四轴角度δ4＝0；各轴转角的比例等于该轴与第四轴中心点的距离的比例，即δ1∶δ2∶δ3＝l14∶l24∶l34；
在全轮转向模式下，第一、第二轴转角与第四、第三轴转角值相等，方向相反，第一、第二轴转角的比例等于第一、第二轴与质心点的距离的比例，即：δ1∶δ2＝l10∶l20，δ3＝-δ2，δ4＝-δ1。
8.根据权利要求1所述的多轴无人重载车辆的轨迹跟踪控制方法，其特征在于，所述步骤S3包括以下步骤：
S31，以所述优化轨迹上最接近所述被控车辆实时位置的轨迹点的速度值作为PID控制器的控制目标，将该控制目标与所述被控车辆的实际速度值相减并通过低通滤波器，获得速度偏差；
S32，将所述速度偏差输入PID控制器，获得误差加速度补偿值；
S33，将所述误差加速度补偿值与所述优化轨迹上最接近所述被控车辆实时位置的轨迹点的期望加速度相加，获得加速度控制指令。
CN202110296428.5A 2021-03-19 2021-03-19 一种多轴无人重载车辆的轨迹跟踪控制方法 Active CN113064344B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110296428.5A CN113064344B (zh) 2021-03-19 2021-03-19 一种多轴无人重载车辆的轨迹跟踪控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110296428.5A CN113064344B (zh) 2021-03-19 2021-03-19 一种多轴无人重载车辆的轨迹跟踪控制方法

Publications (2)

Publication Number Publication Date
CN113064344A CN113064344A (zh) 2021-07-02
CN113064344B true CN113064344B (zh) 2022-06-07

Family

ID=76562479

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110296428.5A Active CN113064344B (zh) 2021-03-19 2021-03-19 一种多轴无人重载车辆的轨迹跟踪控制方法

Country Status (1)

Country Link
CN (1) CN113064344B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113467470B (zh) * 2021-07-23 2023-03-03 西安理工大学 一种无人自主小车的轨迹跟踪控制方法
CN113415276B (zh) * 2021-07-30 2022-10-14 东风商用车有限公司 一种智能驾驶预瞄控制方法、装置和存储介质
CN114137971B (zh) * 2021-11-25 2023-06-09 北京轻舟智航科技有限公司 一种转向系统延迟的离线辨识方法
CN114291117B (zh) * 2022-01-25 2023-05-09 中山大学 一种四轮转向无人车辆轨迹跟踪控制方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102490781A (zh) * 2011-12-01 2012-06-13 中联重科股份有限公司 工程车辆的转向系统及转向方法
CN103217978A (zh) * 2013-03-28 2013-07-24 燕山大学 一种实现液压载重车多车组协调作业的控制方法
CN108508760A (zh) * 2018-03-05 2018-09-07 北京汽车研究总院有限公司 一种多轮独立驱动滑差转向车辆的联合仿真方法及系统
CN112172918A (zh) * 2020-09-29 2021-01-05 北京航天发射技术研究所 一种多轴电液转向系统双闭环控制系统和方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109492599A (zh) * 2018-11-20 2019-03-19 中车株洲电力机车有限公司 一种多轴电客车自导向方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102490781A (zh) * 2011-12-01 2012-06-13 中联重科股份有限公司 工程车辆的转向系统及转向方法
CN103217978A (zh) * 2013-03-28 2013-07-24 燕山大学 一种实现液压载重车多车组协调作业的控制方法
CN108508760A (zh) * 2018-03-05 2018-09-07 北京汽车研究总院有限公司 一种多轮独立驱动滑差转向车辆的联合仿真方法及系统
CN112172918A (zh) * 2020-09-29 2021-01-05 北京航天发射技术研究所 一种多轴电液转向系统双闭环控制系统和方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
A New Dual Axle Drive Optimization Control Strategy for Electric Vehicles Using Vehicle-to-Infrastructure Communications;Huiyuan Xiong等;《IEEE》;20200430;第16卷(第4期);第2574-2582页 *
基于神经网络算法的多轴式机车运动归轨迹优化;甘卉 等;《煤矿机械》;20150731;第36卷(第7期);第275-278页 *

Also Published As

Publication number Publication date
CN113064344A (zh) 2021-07-02

Similar Documents

Publication Publication Date Title
CN113064344B (zh) 一种多轴无人重载车辆的轨迹跟踪控制方法
CN109683614B (zh) 用于无人矿用车辆的车辆路径控制方法及装置
CN110827535A (zh) 非线性车辆队列协同自适应抗扰纵向控制方法
Song et al. A vehicle trajectory tracking method with a time-varying model based on the model predictive control
CN112578672B (zh) 基于底盘非线性的无人驾驶汽车轨迹控制系统及其轨迹控制方法
CN112230651A (zh) 一种基于分层控制理论的分布式无人车路径跟踪控制方法
CN114942642A (zh) 一种无人驾驶汽车轨迹规划方法
CN116048081A (zh) 一种考虑安全边界约束的自动驾驶车辆决策与规控方法
Meiling et al. Model predictive control for UGV trajectory tracking based on dynamic model
Wu et al. Research on unmanned electric shovel autonomous driving path tracking control based on improved pure tracking and fuzzy control
Wang et al. The trajectory planning of the lane change assist based on the model predictive control with multi-objective
Li et al. Path tracking control based on the prediction of tire state stiffness using the optimized steering sequence
Liu et al. Rollover control of AGV combined with differential drive, active steering, and centroid adjustment under slope driving condition
CN114896694A (zh) 一种基于两点预瞄的路径跟踪控制方法
CN114802200A (zh) 一种智能汽车极限工况下轨迹跟踪和稳定性控制方法
Kovacs et al. Integrated path planning and lateral-longitudinal control for autonomous electric vehicles
Zhang et al. Model Predictive Control for Trajectory Planning Considering Constraints on Vertical Load Variation
Wang et al. Cooperative Control of Two-Vehicle Transportation Based on Time-Varying Distance Between Vehicles
Zhuang et al. Model-Predictive-Control-Based Simultaneous Trajectory Tracking and Speed Control for Intelligent Vehicles
Zhang et al. Low-speed vehicle path-tracking algorithm based on model predictive control using QPKWIK solver
Chen et al. LSTM-based trajectory tracking control for autonomous vehicles
Zeng et al. Research on yaw stability control of multi-axle electric vehicle with in-wheel motors based on fuzzy sliding mode control
Fan et al. Intelligent vehicle lane-change trajectory planning on slippery road surface using nonlinear programming
CN114326728B (zh) 高安全裕度的单agv智能车库路径跟踪控制系统及方法
CN113008240B (zh) 基于稳定域的四轮独立驱动智能电动汽车路径规划方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant