CN114879698B - 基于改进人工势场和mpc的机器人驾驶车辆避障方法 - Google Patents
基于改进人工势场和mpc的机器人驾驶车辆避障方法 Download PDFInfo
- Publication number
- CN114879698B CN114879698B CN202210660051.1A CN202210660051A CN114879698B CN 114879698 B CN114879698 B CN 114879698B CN 202210660051 A CN202210660051 A CN 202210660051A CN 114879698 B CN114879698 B CN 114879698B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- model
- robot
- mpc
- control system
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 53
- 230000001133 acceleration Effects 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 33
- 230000008859 change Effects 0.000 claims description 24
- 238000005457 optimization Methods 0.000 claims description 20
- 230000009977 dual effect Effects 0.000 claims description 10
- 238000005070 sampling Methods 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 6
- 238000005096 rolling process Methods 0.000 claims description 6
- 238000013528 artificial neural network Methods 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000012544 monitoring process Methods 0.000 claims description 3
- 239000002245 particle Substances 0.000 claims description 3
- 230000008676 import Effects 0.000 claims description 2
- 238000012360 testing method Methods 0.000 description 29
- 230000007246 mechanism Effects 0.000 description 6
- 230000005540 biological transmission Effects 0.000 description 4
- 238000013016 damping Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 102100034112 Alkyldihydroxyacetonephosphate synthase, peroxisomal Human genes 0.000 description 3
- 101000799143 Homo sapiens Alkyldihydroxyacetonephosphate synthase, peroxisomal Proteins 0.000 description 3
- 238000000848 angular dependent Auger electron spectroscopy Methods 0.000 description 3
- 241000282414 Homo sapiens Species 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 238000007476 Maximum Likelihood Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008092 positive effect Effects 0.000 description 1
- 239000000725 suspension Substances 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0225—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving docking at a fixed facility, e.g. base station or loading bay
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- 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
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Optics & Photonics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于改进人工势场和MPC的机器人驾驶车辆避障方法，将驾驶机器人的控制系统分成上位控制系统和下位控制系统，上位控制系统，其结合人工势场法中的斥力函数并基于行驶速度及道路边界构建避障函数模型及道路边界磁场模型，其基于车辆动力模型的模型预测控制算法得到车辆下一步的运行轨迹；上位控制系统包括MPC控制器，MPC控制器输入车辆下一步的运行轨迹及车辆的当前状态，输出期望航向角及加速度至下位控制系统；下位控制系统基于转向机器人与车体运动模型将期望航向角转换为转向机器人的转向电机的转矩控制信号。本发明完成对障碍物的规避，同时也能较大程度的实现驾驶机器人完成自动驾驶的任务。
Description
技术领域
本发明涉及一种驾驶机器人驾驶车辆自动完成道路测试的控制方法，特别涉及一种基于改进人工势场和MPC的机器人驾驶车辆避障方法。
背景技术
目前，汽车驾驶机器人是自动驾驶发展的另一重要体现，它有着仿人的机械结构特性，机械结构主要由油门/刹车机械腿、转向机械手、换挡机械手组成，它能够在不破坏现有汽车内部机构的同时，简单安装在汽车上实现汽车的驾驶控制。因其控制精度高、重复性好、耐久性强、安全性高等优势广泛代替人类应用于各种汽车测试项目中，从而获得更加准确的测试数据。驾驶机器人已广泛应用于汽车高危测试以及ADAS测试等，ADAS测试主要集中于自动紧急制动系统(AEB)、车道保持系统等的测试，这就需要驾驶机器人能够根据相应的测试工况完成准确的车速跟踪控制和转向控制，即协调控制。另外一方面，针对相应的路况测试主要体现在汽车按照给定的路径进行测试即使用驾驶机器人驾驶汽车实现路径跟踪，而当测试的过程中突然出现障碍物或者紧急状况时，这样很难保证汽车测试的安全性，驾驶过程中实现汽车的局部路径规划实现障碍规避和风险缓解为提高汽车测试安全性具有重要的意义，与此同时对加入局部规划的驾驶机器人控制系统基础上，结合汽车ADAS系统实现对现有汽车的自动驾驶提供了参考价值。
模型预测控制算法是一种先进的控制算法，现已广泛应用于自动驾驶领域，它是一种基于汽车模型的优化算法，对未来汽车的状况进行实时预测，能够高效稳定的实现对轨迹的跟踪，汽车模型的复杂程度以及多约束优化求解的方法决定了该算法的实时性与稳定性；路径规划旨在给汽车提供一条能够到达目的地的最优最快路径，人工势场法已经在局部路径规划中得到了相应的应用，它通过在障碍物和目标物周围建立引力和斥力磁场，通过斥力和引力的合力指引车辆搜索避障路径。为了能够让驾驶机器人驾驶车辆完成在固定路径测试下，并且能够完成障碍物的规避任务，基于改进人工势场和模型预测的方法被用于驾驶机器人控制车辆上。
发明内容
为了克服驾驶机器人驾驶车辆进行道路测试的较低安全性问题，实现驾驶机器人控制车辆在固定的轨迹行驶下，遇到障碍物完成自动规避任务，与此同时在无人的干预下，完成测试后驾驶机器人能够自动的驾驶车辆再次来到测试初始地，本发明为解决公知技术中存在的技术问题而提供一种基于改进人工势场和MPC的机器人驾驶车辆避障方法。
本发明为解决公知技术中存在的技术问题所采取的技术方案是：一种基于改进人工势场和MPC的机器人驾驶车辆避障方法，将驾驶机器人的控制系统分成上位控制系统和下位控制系统，上位控制系统，其结合人工势场法中的斥力函数并基于行驶速度及道路边界构建避障函数模型及道路边界磁场模型，其基于车辆动力模型的模型预测控制算法得到车辆下一步的运行轨迹；上位控制系统包括MPC控制器，MPC控制器输入车辆下一步的运行轨迹及车辆的当前状态，输出期望航向角及加速度至下位控制系统；下位控制系统基于转向机器人与车体运动模型将期望航向角转换为转向机器人的转向电机的转矩控制信号。
进一步地，该方法包括如下步骤：
步骤1，在车辆上安装用于监测道路环境状况的激光雷达与摄像头，以及用于定位车辆实时位置的定位系统；上位控制系统导入预定路径；
步骤2，在上位控制系统中构建避障函数模型及道路边界磁场模型；
步骤3，基于避障函数模型、道路边界磁场模型及控制量的约束，构建车辆运动轨迹模型，通过QP优化算法由导入的预定路径生成最优的轨迹离散点，将这些轨迹离散点传入MPC控制器当中；
步骤4，在MPC控制器中建立基于轮胎滑移及道路曲率的车辆动力学模型；基于模型预测优化算法建立目标函数及约束条件；
步骤5，将目标函数及约束条件转化为序列二次规划问题；基于快速对偶神经网络，将序列二次规划问题转换为对偶问题来优化求解，计算得到期望航向角。
进一步地，步骤2中，将车辆视为质点，将障碍物视为四边形，构建如下避障函数：
式中：
Fobs,vehicle(X)表示车辆所受障碍物的总斥力；
η为正比例系数；
d(xi,x0)为车辆分别到四边形四个点的欧几里德距离；
ρ为障碍物对车辆产生影响的最大距离。
进一步地，步骤2中，将道路划分为两个区域，建立如下道路边界磁场函数：
式中：
Frep,edge为道路两边的斥力；
wedge为斥力势场的权重系数；
d为道路其中一区域的宽度；
y为车辆所在的横向位置；
w为车身的一般宽度；
vx为车辆速度。
进一步地，步骤3中，将障碍物和车辆的位置信号传输到MPC规划器中，采用点质量模型法建立车辆模型；设：x为在车身坐标系下车辆的纵向位置；y为在车身坐标系下车辆的横向位置；X为在惯性坐标系下车辆的纵向位置；Y为在惯性坐标系下车辆的横向位置；为车辆的横摆角；
选择其中状态量为
设控制量u为前轮偏角δ；则有：
式中：
为车辆在自身坐标系下纵向位置的变化率；
为车辆在自身坐标系下横向位置的变化率；
为车辆在惯性坐标系下横向位置的变化率；
为在惯性坐标系下车辆的纵向位置的变化率；
ξ为新建的状态量；
ξi为第i个滚动时域内的变化向量矩阵；
为ξi的转置矩阵；
Hi为第i个减少滚动时域的权重矩阵；
Ui为所有势场的总和；
Np为滚动时域内的预测时域；
Jmin为代价函数最小值；
通过QP优化算法求解生成最优的满足相应条件的离散点，采用贝塞尔曲线对离散点进行曲线拟合，将拟合后的曲线定义为MPC所需要的跟踪的局部路径曲线，并传入MPC控制器中。
进一步地，步骤4中，建立如下所示基于轮胎滑移及道路曲率的车辆动力学模型：
式中：
Iz为车辆在z轴方向上的转动惯量；
m为车辆的质量；
kref为参考路径得到的道路曲率；
vx为在车体坐标系下车辆质心的纵向速度；
vy为在车体坐标系下车辆质心的横向速度；
lf为车辆质心到前轴的距离；
lr为车辆质心到后轴的距离；
为跟踪航向误差；
ed为跟踪距离误差；
Fxf为车辆前轮纵向力；
Fxr为车辆后轮纵向力；
Fyf为前轮侧偏力；
Fyr为后轮侧偏力；
为车辆横摆角速度。
进一步地，设Cf为车辆前轮侧偏刚度；Cr为车辆后轮侧偏刚度；αf为前轮的侧偏角；αr为后轮的侧偏角；
假设Fyf＝Cfαf,Fyr＝-Crαr；
基于非线性卡尔曼滤波器对轮胎侧偏刚度进行估计，不确定的轮胎侧偏刚度表达式如下所示：
式中：
Cf,0为车辆前轮侧偏刚度的线性标准值；
γf为前轮基于时间的变量；
为前轮侧偏刚度变量；
Cr为后轮侧偏刚度；
Cr,0为车辆后轮侧偏刚度的线性标准值；
γr为后轮基于时间的变量；
为后轮侧偏刚度变量。
进一步地，步骤4中，设状态量通过泰勒级展开和前向欧拉法对车辆动力学模型中的非线性方程进行离散化和线性化得到：构建新的状态空间方程：基于状态轨迹预测未来的Np时刻的ε(k+Np)，同时对误差e(k)＝x0(k+1)-Akx0(k)-Bku0(k)进行实时更新；
式中：
A为状态量雅可比矩阵；
B为控制量雅可比矩阵；
为车辆状态量变换率在k+1与k时刻的差值；
为车辆状态量在k+1与k时刻的差值；
为车辆控制量在k+1与k时刻的差值；
Np为滚动时域内的预测时域；
ε(k)为新的状态空间表达式；
e(k)为误差表达式；
u(k-1)为k-1时刻的控制量；
x0为泰勒级展开选取点的状态量；
Ak为在k时刻新构建的状态量雅各比矩阵；
Bk为在k时刻新构建的控制量雅各比矩阵；
k为采样的步数；
ε(k+Np)为未来的Np时刻的状态量和控制量；
u0(k)为k时刻的泰勒级展开选取点的控制量。
进一步地，步骤4中，基于模型预测优化算法建立如下目标函数及约束条件：目标函数为：
约束条件为：
emax-wd≤ed(k+i|k)≤-emin+wd；
δf,min≤δf(k+i|k)≤δf,max；
Δδf,min≤δf(k+i|k)-δf(k+i-1|k)≤Δδf,max；
式中：
Jmin为轨迹跟踪目标函数的最小值；
i为取样步长；i＝1,2,…,Np；
Np为模型预测算法中的预测时域；
Nc为控制时域；
为输出航向误差权重矩阵；
Qd为输出距离误差权重矩阵；
wd为车体行驶过程中定义的安全距离；
Wf为控制增量权重矩阵；
ε为松弛因子权重；
s为松弛因子；
k为采样的步数；
δf为车辆前轮转角；
emax为车辆跟踪横向位置偏差的最大值；
emin为车辆跟踪横向位置偏差的最小值；
δf,min为车辆前轮转角的最小值；
δf,max为车辆前轮转角的最大值；
Δδf,min为车辆前轮转角增量的最小值；
Δδf,max为车辆前轮转角增量的最大值；
为车辆横摆角变化率的最小值；
为车辆横摆角变化率的最大值。
本发明具有的优点和积极效果是：本发明将改进的人工势场法与模型预测规划及控制有效的结合在一起应用于驾驶机器人测试车辆，具有着高控制速度、高安全性、高稳定性等特点。驾驶机器人控制车辆在固定轨迹下进行道路测试的过程中，能够有效的规避突如其来的风险；同时基于快速对偶神经网络来求解模型预测控制目标函数，减轻了多约束目标函数的求解难度，能够使得驾驶机器人根据指令快速的完成对车辆的控制，有效稳定的完成测试工作
附图说明
图1为驾驶机器人-车辆避障控制系统设计框图。
图2为考虑道路曲率、滑移的汽车动力学模型示意图。
图3为简化的驾驶机器人-车辆(转向机器人)机械结构。
图中：
ynew为局部路径规划新参考轨迹的横向位置。
为局部路径规划新参考轨迹的横摆角。
δf为车辆的前轮转角。
x为；车身坐标系x轴。
y为；车身坐标系y轴。
z为；车身坐标系z轴。
X为；全局坐标系X轴。
Y为；全局坐标系Y轴。
为道路参考点所在的切线与全局坐标系之间的夹角。
为车辆在自身坐标系下的横向速度变化率。
为车辆的横摆角。
θvf为；车辆前轮速度方向与车身轴向之间的夹角。
vf为；车辆前轮速度方向。
lf为车辆质心到前轴的距离。
lr为车辆质心到后轴的距离。
为跟踪航向误差。
Fxf为车辆前轮纵向力。
Fxr为车辆后轮纵向力。
Fyf为前轮侧偏力。
Fyr为后轮侧偏力。
为车辆横摆角速度。
αf为前轮的侧偏角。
αr为后轮的侧偏角。
im为电机的齿轮减速比。
ig为转向机构和方向盘之间的传动比。
Ts为转向轴的实际转矩。
Tm为转向电机的输出转矩。
Td为方向盘的输入转矩。
θs为方向盘转角。
Bs为转向轴的阻尼系数。
Jm为转向电机的转动惯量。
MW为车辆转动的阻力矩。
is为小齿轮至轮胎轴销的传动比。
Jr为轮胎的转动惯量。
具体实施方式
为能进一步了解本发明的发明内容、特点及功效，兹列举以下实施例，并配合附图详细说明如下：
本申请中的如下英文单词及英文缩写中文释义如下：
GPS为全球导航定位系统。
Ref point为道路参考点。
Tangent为道路切线。
SQP为序列二次规划求解。
MPC为模型预测控制。
QP为二次规划求解。
请参见图1至图3，一种基于改进人工势场和MPC的机器人驾驶车辆避障方法，将驾驶机器人的控制系统分成上位控制系统和下位控制系统，上位控制系统，其结合人工势场法中的斥力函数并基于行驶速度及道路边界构建避障函数模型及道路边界磁场模型，其基于车辆动力模型的模型预测控制算法得到车辆下一步的运行轨迹；上位控制系统包括MPC控制器，MPC控制器输入车辆下一步的运行轨迹及车辆的当前状态，输出期望航向角及加速度至下位控制系统；下位控制系统基于转向机器人与车体运动模型将期望航向角转换为转向机器人的转向电机的转矩控制信号。
优选地，该方法可包括如下步骤：
步骤1，可在车辆上安装用于监测道路环境状况的激光雷达与摄像头，以及用于定位车辆实时位置的定位系统；上位控制系统可导入预定路径。
步骤2，可在上位控制系统中构建避障函数模型及道路边界磁场模型。
步骤3，可基于避障函数模型、道路边界磁场模型及控制量的约束，构建车辆运动轨迹模型，通过QP优化算法由导入的预定路径生成最优的轨迹离散点，将这些轨迹离散点传入MPC控制器当中。
步骤4，可在MPC控制器中建立基于轮胎滑移及道路曲率的车辆动力学模型；基于模型预测优化算法建立目标函数及约束条件。
步骤5，可将目标函数及约束条件转化为序列二次规划问题；基于快速对偶神经网络，将序列二次规划问题转换为对偶问题来优化求解，计算得到期望航向角。
优选地，步骤2中，可将车辆视为质点，将障碍物视为四边形，构建如下避障函数：
式中：
Fobs,vehicle(X)表示车辆所受障碍物的总斥力。
η为正比例系数。
d(xi,x0)为车辆分别到四边形四个点的欧几里德距离。
ρ为障碍物对车辆产生影响的最大距离。
优选地，步骤2中，可将道路划分为两个区域，建立如下道路边界磁场函数：
式中：
Frep,edge为道路两边的斥力。
wedge为斥力势场的权重系数。
d为道路其中一区域的宽度。
y为车辆所在的横向位置。
w为车身的一般宽度。
vx为车辆速度。
优选地，步骤3中，可将障碍物和车辆的位置信号传输到MPC规划器中，采用点质量模型法建立车辆模型；可设：x为在车身坐标系下车辆的纵向位置；y为在车身坐标系下车辆的横向位置；X为在惯性坐标系下车辆的纵向位置；Y为在惯性坐标系下车辆的横向位置；为车辆的横摆角。
可选择其中状态量为
可设控制量u为前轮偏角δ。则有：
式中：
为车辆在自身坐标系下纵向位置的变化率。
为车辆在自身坐标系下横向位置的变化率。
为车辆在惯性坐标系下横向位置的变化率。
为在惯性坐标系下车辆的纵向位置的变化率。
ξ为新建的状态量。
ξi为第i个滚动时域内的变化向量矩阵。
为ξi的转置矩阵。
Hi为第i个减少滚动时域的权重矩阵。
Ui为所有势场的总和。
Np为滚动时域内的预测时域。
Jmin为代价函数最小值。
可通过QP优化算法求解生成最优的满足相应条件的离散点，采用贝塞尔曲线对离散点进行曲线拟合，将拟合后的曲线定义为MPC所需要的跟踪的局部路径曲线，并传入MPC控制器中。
优选地，步骤4中，可建立如下所示基于轮胎滑移及道路曲率的车辆动力学模型：
式中：
Iz为车辆在z轴方向上的转动惯量。
m为车辆的质量。
kref为参考路径得到的道路曲率。
vx为在车体坐标系下车辆质心的纵向速度。
vy为在车体坐标系下车辆质心的横向速度。
lf为车辆质心到前轴的距离。
lr为车辆质心到后轴的距离。
为跟踪航向误差。
ed为跟踪距离误差。
Fxf为车辆前轮纵向力。
Fxr为车辆后轮纵向力。
Fyf为前轮侧偏力。
Fyr为后轮侧偏力。
为车辆横摆角速度。
优选地，可设Cf为车辆前轮侧偏刚度；Cr为车辆后轮侧偏刚度；αf为前轮的侧偏角；αr为后轮的侧偏角。
可假设Fyf＝Cfαf,Fyr＝-Crαr。
可基于非线性卡尔曼滤波器对轮胎侧偏刚度进行估计，不确定的轮胎侧偏刚度表达式如下所示：
式中：
Cf,0为车辆前轮侧偏刚度的线性标准值。
γf为前轮基于时间的变量。
为前轮侧偏刚度变量。
Cr为后轮侧偏刚度。
Cr,0为车辆后轮侧偏刚度的线性标准值。
γr为后轮基于时间的变量。
为后轮侧偏刚度变量。
优选地，步骤4中，可设状态量可通过泰勒级展开和前向欧拉法对车辆动力学模型中的非线性方程进行离散化和线性化得到：可构建新的状态空间方程：可基于状态轨迹预测未来的Np时刻的ε(k+Np)，同时对误差e(k)＝x0(k+1)-Akx0(k)-Bku0(k)进行实时更新。
式中：
A为状态量雅可比矩阵。
B为控制量雅可比矩阵。
为车辆状态量变换率在k+1与k时刻的差值。
为车辆状态量在k+1与k时刻的差值。
为车辆控制量在k+1与k时刻的差值。
Np为滚动时域内的预测时域。
ε(k)为新的状态空间表达式。
e(k)为误差表达式。
u(k-1)为k-1时刻的控制量。
x0为泰勒级展开选取点的状态量。
Ak为在k时刻新构建的状态量雅各比矩阵。
Bk为在k时刻新构建的控制量雅各比矩阵。
k为采样的步数。
ε(k+Np)为未来的Np时刻的状态量和控制量。
u0(k)为k时刻的泰勒级展开选取点的控制量。
优选地，步骤4中，可基于模型预测优化算法建立如下目标函数及约束条件：
目标函数为：
约束条件为：
emax-wd≤ed(k+i|k)≤-emin+wd。
δf,min≤δf(k+i|k)≤δf,max。
Δδf,min≤δf(k+i|k)-δf(k+i-1|k)≤Δδf,max。
式中：
Jmin为轨迹跟踪目标函数的最小值。
i为取样步长。i＝1,2,…,Np。
Np为模型预测算法中的预测时域。
Nc为控制时域。
为输出航向误差权重矩阵。
Qd为输出距离误差权重矩阵。
wd为车体行驶过程中定义的安全距离。
Wf为控制增量权重矩阵。
ε为松弛因子权重。
s为松弛因子。
k为采样的步数。
δf为车辆前轮转角。
emax为车辆跟踪横向位置偏差的最大值。
emin为车辆跟踪横向位置偏差的最小值。
δf,min为车辆前轮转角的最小值。
δf,max为车辆前轮转角的最大值。
Δδf,min为车辆前轮转角增量的最小值。
Δδf,max为车辆前轮转角增量的最大值。
为车辆横摆角变化率的最小值。
为车辆横摆角变化率的最大值。
下面以本发明的一个优选实施例来进一步说明本发明的工作流程及工作原理：
一种基于改进人工势场和MPC的机器人驾驶车辆避障方法，用于驾驶机器人车辆道路测试，能保证驾驶机器人控制车辆在测试的过程中完成对障碍物的规避，同时也能较大程度的实现驾驶机器人完成自动驾驶的任务，使得测试完成后自动驾驶车辆回到原先的测试场地，具体的方法如下：
步骤一、预先给定测试参考路径，使得汽车按照预定的轨迹行驶，本发明在提前导入预定测试路径的同时，在测试汽车上安装激光雷达与摄像头实时监测测试场地环境状态，采用GPS提供汽车的实时位置记作(xi,t,yi.t)以及障碍物的位置(xp,t,yp.t)。
步骤二、设计上位控制器，得到期望的前轮转角完成轨迹跟踪和障碍规避任务，本方面将改进人工势场法嵌入MPC规划器的优化设计控制当中，实现跟踪与避障的应用。
避障功能函数的选择综合考虑速度,以及车辆与障碍物的相对距离对避障效果的影响，结合改进的人工势场法构建如下功能函数，当车辆越靠近障碍物时候，障碍物对它的斥力也就越大，这里只引进斥力函数。该方法同时考虑了速度以及道路边界对其轨迹规划的限值，避障函数如下：
其中η为正比例系数，d(xi,x0)为汽车前端两个点分别到四边形四个点的欧几里德距离，ρ为障碍物对汽车产生影响的最大距离。
将道路划分为两个区域，道路边界磁场如下所示:
此处的w为车身的一般宽度，为正比例系数，v为汽车速度。
障碍物和汽车的位置信号传输到MPC规划器中，对于汽车的模型依然采用点质量模型，既体现了车辆的动力学特性，也保证了规划的实时性。
选择其中状态量为控制量u为前轮偏角δ。
基于斥力场限制和控制量的限制，通过QP优化算法求解：
来生成最优的满足相应条件的离散点，为了能够顺利的与控制层相互对接，采用贝塞尔曲线对其进行曲线拟合，将这些离散的点传入MPC控制器当中。
步骤三、改进的模型预测控制
为了控制系统的简单化与合理化，将控制系统分成上位控制和下位控制，基于汽车动力模型的模型预测控制算法计算得到期望得前轮转角，再通过驾驶机器人(转向机器人)与车体得模型得到所需要转向电机转矩。
车辆模型常见的二自由度汽车动力学模型，该模型假设了轮胎侧偏角的小角度假设。为了更好跟踪路线，建立考虑道路曲率的车辆动力学模型，道路曲率的影响直接关系到转向特性和行驶稳定性,模型示意图如图2所示。
考虑轮胎滑移、道路曲率的车辆数学模型如下所示：
其中.kref为参考路径得到的道路曲率，vx和vy分别为在车体坐标系下车辆质心的纵向速度和横向速度，lf和lr分别为车辆质心到前后轴的距离，和ed定义为跟踪航向偏差和跟踪距离偏差。Fyf和Fyr分别为前、后轮侧偏力，小角度侧偏角假设使得Fyf＝Cfαf,Fyr＝-Crαr；为车辆横摆角速度.αf和αr分别为前后轮的侧偏角。
汽车在高速行驶的过程或者在转弯的过程中，由于轮胎的模型呈现着强烈的非线性特性，基于非线性卡尔曼滤波器对轮胎侧偏刚度进行估计，不确定的轮胎侧偏刚度如下：γ用来设计来表示侧偏刚度的非线性特性，取值范围在0～1之间。
状态量通过泰勒级展开和前向欧拉法对非线性方程进行离散化和线性化得到：构建新的状态空间方程：基于状态轨迹预测未来的Np时刻的ε(k+Np)，同时对误差e(k)＝x0(K+1)-Akx0(k)-Bku0(k)进行实时更新。
目标函数及优化求解:
约束条件：
emax-wd≤ed(k+i|k)≤-emin+wd。
δf,min≤δf(k+i|k)≤δf,max。
Δδf,min≤δf(k+i|k)-δf(k+i-1|k)≤Δδf,max。
式中,i从1取到Np，Np是模型预测算法中的预测时域,Nc是控制时域，和Qd分别代表了输出横向误差权重矩阵，Wf为控制增量权重矩阵，ε为松弛因子权重，s为松弛因子。
传统的模型预测控制采用SQP方法求解不等式多约束优化问题，为了更好的达到收敛效果和快速的计算速度，设计基于快速对偶神经网络优化求解SQP，将序列二次规划形式转换为对偶问题：此时通过基于模型预测优化算法计算得到期待的前轮转角δf。
式中：ΔU为系统的控制增量。H为权重矩阵。min为求解目标函数的最小值。lmin为约束变量的最小值。lmax为约束变量的最大值。v、w都为对偶问题的优化变量。
步骤五、基于驾驶机器人-车体动力学模型将前轮转角δf转换为转向机器人转向电机所需的转矩。本发明所用的驾驶机器人为天津中汽数据中心的第四代转向机器人，转向机器人通过三抓固定在方向盘上，传动方式为内啮合齿轮带动方向盘转动，简化的驾驶机器人和汽车转向系统结构图如图3所示。按照下式计算转向机器人转向电机所需的转矩：
其中im为电机的齿轮减速比，ig为转向机构和方向盘之间的传动比，Ts为转向轴的实际转矩，Tm为转向电机的输出转矩，JS、BS、KS分别为转向轴的转动惯量、阻尼系数以及刚度系数，θS为方向盘所转的角度。
假设车辆为柱型电动助力转向系统，助力电机转动惯量Jm假设可测，齿轮轮胎模块模型如下：
其中Ta为助力系统的扭矩，MW为车辆转动的阻力矩，is为小齿轮至轮胎轴销的传动比，Jr为轮胎的转动惯量，Br为阻尼系数。Bm为助力电机的阻尼系数。为前轮转角的变化率。为前轮转角转速的变化率。
从上式可得转向机构所需转矩与前轮转角之间的关系Tm＝f(δf)。基于经验同时为了能够快速的应用于工程领域，简化复杂的车身模型，忽略悬架以及复杂的传动过程，将车辆的前轮转角与转向机构输出角度之间建立以下关系：δf＝f(ux)θs，用以对比验证上式数学模型的准确性。
至此输出转向机器人的所需要转矩，转向机构转向电机得到来自于上层控制器的信号，进而控制方向盘完成对车辆的控制，下位控制系统在这不作为本发明的讨论范围。
步骤六、完成测试后，汽车通过GPS和摄像头得到初始位置以及上面提到的路径规划算法得到返回的路径，基于改进的模型预测控制和驾驶机器人下位控制实现对测试地点的自动返回工作。
以上所述的实施例仅用于说明本发明的技术思想及特点，其目的在于使本领域内的技术人员能够理解本发明的内容并据以实施，不能仅以本实施例来限定本发明的专利范围，即凡本发明所揭示的精神所作的同等变化或修饰，仍落在本发明的专利范围内。
Claims (6)
1.一种基于改进人工势场和MPC的机器人驾驶车辆避障方法，其特征在于，将驾驶机器人的控制系统分成上位控制系统和下位控制系统，上位控制系统，其结合人工势场法中的斥力函数并基于行驶速度及道路边界构建避障函数模型及道路边界磁场模型，其基于车辆动力模型的模型预测控制算法得到车辆下一步的运行轨迹；上位控制系统包括MPC控制器，MPC控制器输入车辆下一步的运行轨迹及车辆的当前状态，输出期望航向角及加速度至下位控制系统；下位控制系统基于转向机器人与车体运动模型将期望航向角转换为转向机器人的转向电机的转矩控制信号；
该方法包括如下步骤：
步骤1，在车辆上安装用于监测道路环境状况的激光雷达与摄像头，以及用于定位车辆实时位置的定位系统；上位控制系统导入预定路径；
步骤2，在上位控制系统中构建避障函数模型及道路边界磁场模型；
步骤3，基于避障函数模型、道路边界磁场模型及控制量的约束，构建车辆运动轨迹模型，通过QP优化算法由导入的预定路径生成最优的轨迹离散点，将这些轨迹离散点传入MPC控制器当中；
步骤4，在MPC控制器中建立基于轮胎滑移及道路曲率的车辆动力学模型；基于模型预测优化算法建立目标函数及约束条件；
步骤5，将目标函数及约束条件转化为序列二次规划问题；基于快速对偶神经网络，将序列二次规划问题转换为对偶问题来优化求解，计算得到期望航向角；
步骤2中，将车辆视为质点，将障碍物视为四边形，构建如下避障函数：
式中：
Fobs,vehicle(X)表示车辆所受障碍物的总斥力；
η为正比例系数；
d(xi,x0)为车辆分别到四边形四个点的欧几里德距离；
ρ为障碍物对车辆产生影响的最大距离；
步骤2中，将道路划分为两个区域，建立如下道路边界磁场函数：
式中：
Frep,edge为道路两边的斥力；
wedge为斥力势场的权重系数；
d为道路其中一区域的宽度；
y为车辆所在的横向位置；
w为车身的一般宽度；
vx为车辆速度。
2.根据权利要求1所述的基于改进人工势场和MPC的机器人驾驶车辆避障方法，其特征在于，步骤3中，将障碍物和车辆的位置信号传输到MPC规划器中，采用点质量模型法建立车辆模型；设：x为在车身坐标系下车辆的纵向位置；y为在车身坐标系下车辆的横向位置；X为在惯性坐标系下车辆的纵向位置；Y为在惯性坐标系下车辆的横向位置；为车辆的横摆角；
选择其中状态量为
设控制量u为前轮偏角δ；则有：
式中：
为车辆在自身坐标系下纵向位置的变化率；
为车辆在自身坐标系下横向位置的变化率；
为车辆在惯性坐标系下横向位置的变化率；
为在惯性坐标系下车辆的纵向位置的变化率；
ξ为新建的状态量；
ξi为第i个滚动时域内的变化向量矩阵；
为ξi的转置矩阵；
Hi为第i个减少滚动时域的权重矩阵；
Ui为所有势场的总和；
Np为滚动时域内的预测时域；
Jmin为代价函数最小值；
通过QP优化算法求解生成最优的满足相应条件的离散点，采用贝塞尔曲线对离散点进行曲线拟合，将拟合后的曲线定义为MPC所需要的跟踪的局部路径曲线，并传入MPC控制器中。
3.根据权利要求1所述的基于改进人工势场和MPC的机器人驾驶车辆避障方法，其特征在于，步骤4中，建立如下所示基于轮胎滑移及道路曲率的车辆动力学模型：
式中：
Iz为车辆在z轴方向上的转动惯量；
m为车辆的质量；
kref为参考路径得到的道路曲率；
vx为在车体坐标系下车辆质心的纵向速度；
vy为在车体坐标系下车辆质心的横向速度；
lf为车辆质心到前轴的距离；
lr为车辆质心到后轴的距离；
为跟踪航向误差；
ed为跟踪距离误差；
Fxf为车辆前轮纵向力；
Fxr为车辆后轮纵向力；
Fyr为前轮侧偏力；
Fyr为后轮侧偏力；
为车辆横摆角速度。
4.根据权利要求3所述的基于改进人工势场和MPC的机器人驾驶车辆避障方法，其特征在于，设Cf为车辆前轮侧偏刚度；Cr为车辆后轮侧偏刚度；αf为前轮的侧偏角；αr为后轮的侧偏角；
假设Fyf＝Cfαf,Fyr＝-Crαr；
基于非线性卡尔曼滤波器对轮胎侧偏刚度进行估计，不确定的轮胎侧偏刚度表达式如下所示：
式中：
Cf,0为车辆前轮侧偏刚度的线性标准值；
γf为前轮基于时间的变量；
为前轮侧偏刚度变量；
Cr为后轮侧偏刚度；
Cr,0为车辆后轮侧偏刚度的线性标准值；
γr为后轮基于时间的变量；
为后轮侧偏刚度变量。
5.根据权利要求3所述的基于改进人工势场和MPC的机器人驾驶车辆避障方法，其特征在于，步骤4中，设状态量通过泰勒级展开和前向欧拉法对车辆动力学模型中的非线性方程进行离散化和线性化得到：构建新的状态空间方程：基于状态轨迹预测未来的Np时刻的ε(k+Np)，同时对误差e(k)＝x0(k+1)-Akx0(k)-Bku0(k)进行实时更新；
式中：
A为状态量雅可比矩阵；
B为控制量雅可比矩阵；
为车辆状态量变换率在k+1与k时刻的差值；
为车辆状态量在k+1与k时刻的差值；
为车辆控制量在k+1与k时刻的差值；
Np为滚动时域内的预测时域；
ε(k)为新的状态空间表达式；
e(k)为误差表达式；
u(k-1)为k-1时刻的控制量；
x0为泰勒级展开选取点的状态量；
Ak为在k时刻新构建的状态量雅各比矩阵；
Bk为在k时刻新构建的控制量雅各比矩阵；
k为采样的步数；
ε(k+Np)为未来的Np时刻的状态量和控制量；
u0(k)为k时刻的泰勒级展开选取点的控制量。
6.根据权利要求5所述的基于改进人工势场和MPC的机器人驾驶车辆避障方法，其特征在于，步骤4中，基于模型预测优化算法建立如下目标函数及约束条件：
目标函数为：
约束条件为：
emax-wd≤ed(k+i|k)≤-emin+wd；
δf,min≤δf(k+i|k)≤δf,max；
Δδf,min≤δf(k+i|k)-δf(k+i-1|k)≤Δδf,max；
式中：
Jmin为轨迹跟踪目标函数的最小值；
i为取样步长；i＝1,2,…,Np；
Np为模型预测算法中的预测时域；
Nc为控制时域；
为输出航向误差权重矩阵；
Qd为输出距离误差权重矩阵；
wd为车体行驶过程中定义的安全距离；
Wf为控制增量权重矩阵；
ε为松弛因子权重；
s为松弛因子；
k为采样的步数；
δf为车辆前轮转角；
emax为车辆跟踪横向位置偏差的最大值；
emin为车辆跟踪横向位置偏差的最小值；
δf,min为车辆前轮转角的最小值；
δf,max为车辆前轮转角的最大值；
Δδf,min为车辆前轮转角增量的最小值；
Δδf,max为车辆前轮转角增量的最大值；
为车辆横摆角变化率的最小值；
为车辆横摆角变化率的最大值。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210660051.1A CN114879698B (zh) | 2022-06-13 | 2022-06-13 | 基于改进人工势场和mpc的机器人驾驶车辆避障方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210660051.1A CN114879698B (zh) | 2022-06-13 | 2022-06-13 | 基于改进人工势场和mpc的机器人驾驶车辆避障方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114879698A CN114879698A (zh) | 2022-08-09 |
| CN114879698B true CN114879698B (zh) | 2024-08-09 |
Family
ID=82682159
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210660051.1A Active CN114879698B (zh) | 2022-06-13 | 2022-06-13 | 基于改进人工势场和mpc的机器人驾驶车辆避障方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114879698B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115320709B (zh) * | 2022-08-29 | 2023-04-18 | 东风悦享科技有限公司 | 一种基于四轮转向的自动驾驶混合控制方法 |
| CN117471972B (zh) * | 2023-11-20 | 2024-08-27 | 山东建筑大学 | 一种自平衡主动规划路线智能单车控制方法及装置 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113515125A (zh) * | 2021-07-05 | 2021-10-19 | 中国石油大学(华东) | 一种无人驾驶汽车全工况避障控制方法及性能评价方法 |
| CN113581201A (zh) * | 2021-07-22 | 2021-11-02 | 重庆邮电大学 | 一种基于势场模型的无人汽车避撞控制方法及系统 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106843235B (zh) * | 2017-03-31 | 2019-04-12 | 深圳市靖洲科技有限公司 | 一种面向无人自行车的人工势场路径规划法 |
| CN114384919B (zh) * | 2022-01-17 | 2023-06-27 | 北京格睿能源科技有限公司 | 基于大型障碍物形态信息的车辆避障路径规划方法及系统 |
| CN114572251A (zh) * | 2022-04-08 | 2022-06-03 | 苏州易驱汽车科技有限公司 | 一种预测控制的高速自动驾驶汽车轨迹跟踪方法 |
-
2022
- 2022-06-13 CN CN202210660051.1A patent/CN114879698B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113515125A (zh) * | 2021-07-05 | 2021-10-19 | 中国石油大学(华东) | 一种无人驾驶汽车全工况避障控制方法及性能评价方法 |
| CN113581201A (zh) * | 2021-07-22 | 2021-11-02 | 重庆邮电大学 | 一种基于势场模型的无人汽车避撞控制方法及系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114879698A (zh) | 2022-08-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111258323B (zh) | 一种智能车辆轨迹规划与跟踪的联合控制方法 | |
| Xu et al. | Preview path tracking control with delay compensation for autonomous vehicles | |
| Li et al. | Development of a new integrated local trajectory planning and tracking control framework for autonomous ground vehicles | |
| CN107415939B (zh) | 一种分布式驱动电动汽车转向稳定性控制方法 | |
| Dominguez et al. | Comparison of lateral controllers for autonomous vehicle: Experimental results | |
| Wurts et al. | Collision imminent steering at high speed using nonlinear model predictive control | |
| CN114879698B (zh) | 基于改进人工势场和mpc的机器人驾驶车辆避障方法 | |
| Kang et al. | Skid steering-based control of a robotic vehicle with six in-wheel drives | |
| CN108791290A (zh) | 基于在线增量式dhp的双车协同自适应巡航控制方法 | |
| Bulsara et al. | Obstacle avoidance using model predictive control: An implementation and validation study using scaled vehicles | |
| Kim et al. | Vehicle path tracking control using pure pursuit with MPC-based look-ahead distance optimization | |
| Cheng et al. | Planning and control for a fully-automatic parallel parking assist system in narrow parking spaces | |
| Sahoo et al. | Design and development of a heading angle controller for an unmanned ground vehicle | |
| Farag | Model-predictive-control complex-path tracking for self-driving cars | |
| CN113911103A (zh) | 一种混合动力履带车辆速度与能量协同优化方法及系统 | |
| Fehér et al. | Proving ground test of a ddpg-based vehicle trajectory planner | |
| Nietzschmann et al. | Trajectory optimization for car-like vehicles in structured and semi-structured environments | |
| Liikanen et al. | Path-following controller for 4wds hydraulic heavy-duty field robots with nonlinear internal dynamics | |
| Reda et al. | Model-based control strategy for autonomous vehicle path tracking task | |
| Jian et al. | An Optimal Controller for Trajectory Tracking of Automated Guided Vehicle | |
| Huang et al. | Dynamic model construction and simulation based on intelligent driving vehicle | |
| Wang | Trajectory Planning for Four WheelSteering Autonomous Vehicle | |
| CN110716561A (zh) | 用于轨迹跟踪的扫地机前视位姿控制方法和系统 | |
| Yazar et al. | Path tracking methods employed in lane centering systems | |
| Szͩcs et al. | Experimental verification of a control system for autonomous navigation |
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 |