CN115092121A - 一种全自动泊车系统路径跟随装置及方法 - Google Patents
一种全自动泊车系统路径跟随装置及方法 Download PDFInfo
- Publication number
- CN115092121A CN115092121A CN202210489943.XA CN202210489943A CN115092121A CN 115092121 A CN115092121 A CN 115092121A CN 202210489943 A CN202210489943 A CN 202210489943A CN 115092121 A CN115092121 A CN 115092121A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- error
- path
- reference point
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 54
- 230000010354 integration Effects 0.000 claims abstract description 14
- 230000003993 interaction Effects 0.000 claims abstract description 12
- 238000000605 extraction Methods 0.000 claims abstract description 11
- 230000008859 change Effects 0.000 claims description 16
- 238000012937 correction Methods 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 8
- 238000013461 design Methods 0.000 abstract description 26
- 230000000694 effects Effects 0.000 abstract description 13
- 230000007246 mechanism Effects 0.000 abstract description 4
- 230000008030 elimination Effects 0.000 abstract 1
- 238000003379 elimination reaction Methods 0.000 abstract 1
- 230000000875 corresponding effect Effects 0.000 description 15
- 238000004364 calculation method Methods 0.000 description 13
- 230000006870 function Effects 0.000 description 12
- 230000008569 process Effects 0.000 description 11
- 238000010586 diagram Methods 0.000 description 8
- 238000004422 calculation algorithm Methods 0.000 description 6
- 230000001276 controlling effect Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 3
- 238000009795 derivation Methods 0.000 description 3
- 238000007620 mathematical function Methods 0.000 description 3
- 239000011159 matrix material Substances 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- IXKSXJFAGXLQOQ-XISFHERQSA-N WHWLQLKPGQPMY Chemical compound C([C@@H](C(=O)N[C@@H](CC=1C2=CC=CC=C2NC=1)C(=O)N[C@@H](CC(C)C)C(=O)N[C@@H](CCC(N)=O)C(=O)N[C@@H](CC(C)C)C(=O)N1CCC[C@H]1C(=O)NCC(=O)N[C@@H](CCC(N)=O)C(=O)N[C@@H](CC(O)=O)C(=O)N1CCC[C@H]1C(=O)N[C@@H](CCSC)C(=O)N[C@@H](CC=1C=CC(O)=CC=1)C(O)=O)NC(=O)[C@@H](N)CC=1C2=CC=CC=C2NC=1)C1=CNC=N1 IXKSXJFAGXLQOQ-XISFHERQSA-N 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000002238 attenuated effect Effects 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000002349 favourable effect Effects 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
- B60W30/06—Automatic manoeuvring for parking
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W10/00—Conjoint control of vehicle sub-units of different type or different function
- B60W10/04—Conjoint control of vehicle sub-units of different type or different function including control of propulsion units
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W10/00—Conjoint control of vehicle sub-units of different type or different function
- B60W10/04—Conjoint control of vehicle sub-units of different type or different function including control of propulsion units
- B60W10/08—Conjoint control of vehicle sub-units of different type or different function including control of propulsion units including control of electric propulsion units, e.g. motors or generators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W10/00—Conjoint control of vehicle sub-units of different type or different function
- B60W10/20—Conjoint control of vehicle sub-units of different type or different function including control of steering systems
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
- B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
- B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
- B60W40/105—Speed
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
- B60W2050/0001—Details of the control system
- B60W2050/0019—Control system elements or transfer functions
- B60W2050/002—Integrating means
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
- B60W2050/0001—Details of the control system
- B60W2050/0043—Signal treatments, identification of variables or parameters, parameter estimation or state estimation
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W2520/00—Input parameters relating to overall vehicle dynamics
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W2520/00—Input parameters relating to overall vehicle dynamics
- B60W2520/10—Longitudinal speed
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W2530/00—Input parameters relating to vehicle conditions or values, not covered by groups B60W2510/00 or B60W2520/00
Landscapes
- Engineering & Computer Science (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Chemical & Material Sciences (AREA)
- Combustion & Propulsion (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Human Computer Interaction (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种全自动泊车系统路径跟随装置及方法，所述装置包括：参考点信息提取单元，用于基于有效距离积分方法计算总参考距离，并获得目标车速与参考点位置；信息交互单元，用于获取车辆的实际车速与实际位置，与目标车速与参考点位置进行相应比对，获得速度误差和位置误差；底盘控制单元，用于获得速度误差和位置误差，产生相应的驱动力矩指令和转向力矩指令，实现对车辆的纵向控制和横向控制。与现有技术相比，本发明实现了横纵向控制的解耦，还进一步将车辆与参考点之间的误差进行横纵向解耦，并交由对应执行机构进行控制，更加高效地实现了车辆与参考点之间误差的有效消除，最终能够达到使车辆能够高精度跟随路径，同时设计简便的效果。
Description
技术领域
本发明属于汽车技术领域，属于驾驶辅助技术，涉及一种全自动泊车系统路径跟随装置及方法。
背景技术
自动泊车技术作为汽车智能化的一项代表性技术，已经受到了高校与企业的高度关注。具有高精度性能的自动泊车指令控制算法，不仅可以使得车辆泊车无需过大的安全裕度，有助于改善库位规划增大土地利用效率，也可与未来需要车辆精准泊车的自动充电技术等进行有机结合。
现行的自动泊车指令控制算法大致可分为两种：①端到端直接根据环境信息输出控制指令；②先规划路径再输出控制指令跟踪路径。前者多为启发式算法如神经网络等，十分依赖传感器与车辆相关零件如转向器等的性能，若传感器信息存在噪声或车辆零件惯性影响过大，控制效果将受到影响；后者可依靠启发式算法如模糊逻辑，或闭环控制器如滑模控制器、等，其效果的好坏取决于反馈控制状态变量的选取，如相对路径的横向偏距+角度误差、相对参考点的x向误差+y向误差+角度误差等，控制理论相对成熟，效果相对可靠。
由于自动泊车的场景在未来会更加多种多样，泊车可行路径在绝大多数情况下难以直接用数学函数进行表示，致使路径规划输出的信息会越来越多地为无法直接用数学函数表示的、仅能用形状与长度等信息定义的隐式路径，或是通过一系列离散的散点所表示的散点路径。这使得车辆难以直接求解相对路径的横向偏距与角度误差，设计单独的横向控制器以跟随路径。而对于隐式路径与散点路径，相对简便的方法是通过设定沿路径移动的参考点，而后使车辆跟随参考点，如此，参考点与车辆之间的位置与角度误差可作为反馈控制的状态变量。
良好的控制器应该既能够保证车辆能够高精度跟随路径，又能够设计简便，便于大规模应用。一种有效的方法是通过时间尺度变换，使系统以弧长为自变量，实现横纵向控制的解耦，便于独立设计；而反馈线性化又可将系统投射至线性空间，便于使用成熟的线性系统的控制理论进行设计，能够进一步降低设计难度。中国专利CN106372758B即使用时间尺度变换使得系统以弧长为自变量，使得车速与转角控制可根据弧长进行独立设计，进一步地，针对横向控制，基于微分平坦理论，将系统反馈线性化至微分平坦空间，并利用线性二次调节器(LQR)，辅以α衰减因子，实现误差的有效控制。不过这种方法仅实现了横纵向控制的解耦，没有考虑误差对速度指令与转角指令产生的影响，精度不够高；同时，全程设定固定的α衰减因子，使得控制率不能随路径的形状变化而有自适应的变化，误差的衰减尚未达到最优效果。
因此，设计一种保证车辆能够高精度跟随路径，同时设计简便的全自动泊车系统路径跟随装置，具有重要的实际意义。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种全自动泊车系统路径跟随装置及方法，以达到使车辆能够高精度跟随路径，同时设计简便的效果。
本发明的目的可以通过以下技术方案来实现：
一种全自动泊车系统路径跟随装置，包括：
参考点信息提取单元，用于基于有效距离积分方法计算总参考距离sd，根据所述总参考距离sd与规划路径长度获得目标车速与参考点位置；
信息交互单元，用于获取车辆的实际车速与实际位置，与所述目标车速与参考点位置进行相应比对，获得速度误差和位置误差；
底盘控制单元，用于获得所述速度误差和位置误差，产生相应的驱动力矩指令和转向力矩指令，实现对车辆的纵向控制和横向控制。
进一步地，所述参考点信息提取单元包括：
参考行驶距离计算器，用于根据车辆的实际位置，基于有效距离积分方法计算总参考距离sd，具体地：循环基于每个步长的参考点信息，近似计算出车辆行驶时的行驶路径长度向路径投影后的长度，以此作为该步长的有效距离，对每个步长的有效距离进行积分，获得所述总参考距离sd；
车辆速度规划器，用于根据所述总参考距离sd，获取参考点的目标速度；
路径形状规划器，用于根据所述总参考距离sd，获取参考点位置，包括参考点的坐标与航向角信息。
进一步地，所述参考行驶距离计算器中，在每个步长内，获得所述有效距离后，通过比对参考点与车辆实际位置信息，近似计算出车辆沿路径的纵向误差，对该步长的有效距离进行修正，以修正后的有效距离参与所述积分。
进一步地，在所述纵向误差大于设定阈值时进行所述修正，所述修正具体为：
当车辆落后于参考点时，该步长的有效距离按σ1的倍数缩小；
当车辆领先于参考点时，该步长的有效距离按σ2的倍数放大；
其中，σ1、σ2均为可调节的纵向补偿系数。
进一步地，所述信息交互单元包括：
车辆传感器，用于获取轮速与方向盘转角；
车辆定位器，用于根据所述轮速与方向盘转角进行车辆位姿估计，获得车辆的实际车速与实际位置，与所述目标车速与参考点位置进行相应比对，获得速度误差和位置误差。
进一步地，所述底盘控制单元包括：
车辆速度控制器，用于接收所述速度误差，基于该速度误差获取驱动力矩指令；
驱动电机，用于接收所述驱动力矩指令，根据所述驱动力矩指令产生用于对车辆进行纵向控制的驱动力矩；
车辆转角控制器，用于接收所述位置误差，根据该位置误差获取转向力矩指令；
转向电机，用于接收所述转向力矩指令，根据所述转向力矩指令产生用于对车辆进行横向控制的转向力矩；
其中，所述车辆转角控制器基于微分平坦理论，通过反馈线性化将变量由时域空间投影至微分平坦空间，通过时间尺度变换使微分平坦空间中以弧长为自变量，并在微分平坦空间中利用线性二次型最优控制器将路径曲率变化率与可以加速误差衰减的衰减因子α建立联系。
本发明还提供一种全自动泊车系统路径跟随方法，包括以下步骤：
基于有效距离积分方法计算总参考距离sd，根据所述总参考距离sd与规划路径长度获得目标车速与参考点位置；
获取车辆的实际车速与实际位置，与所述目标车速与参考点位置进行相应比对，获得速度误差和位置误差；
获得所述速度误差和位置误差，产生相应的驱动力矩指令和转向力矩指令，实现对车辆的纵向控制和横向控制。
进一步地，所述基于有效距离积分方法计算总参考距离sd具体包括：
获取车辆的实际位置，循环基于每个步长的参考点信息，近似计算出车辆行驶时的行驶路径长度向路径投影后的长度，以此作为该步长的有效距离，对每个步长的有效距离进行积分，获得所述总参考距离sd。
进一步地，在每个步长内，获得所述有效距离后，通过比对参考点与车辆实际位置信息，近似计算出车辆沿路径的纵向误差，对该步长的有效距离进行修正，以修正后的有效距离参与所述积分。
进一步地，产生所述转向力矩指令时，基于微分平坦理论，通过反馈线性化将变量由时域空间投影至微分平坦空间，通过时间尺度变换使微分平坦空间中以弧长为自变量，并在微分平坦空间中利用线性二次型最优控制器将路径曲率变化率与可以加速误差衰减的衰减因子α建立联系。
与现有技术相比，本发明具有以下有益效果：
(1)性能可靠：本发明通过将误差横纵向解耦并与执行器相对应，使得误差能够可靠地保持在较小值，最终使车辆高精度跟随路径；
(2)设计简便：本发明通过时间尺度变换，能够以弧长为自变量，实现了横纵向控制的解耦，便于独立设计。
附图说明
图1是本发明的整体原理框图；
图2是车辆以全部行驶距离作为总参考距离sd时车辆跟随不佳时的示意图；
图3是车辆纵向误差与横向误差的示意图；
图4是车辆在直线部分行驶的有效距离示意图；
图5是车辆在定曲率部分行驶的有效距离示意图；
图6是车辆在变曲率部分行驶的有效距离示意图；
图7是车辆在多段路径交界处溜车的示意图；
图8是车辆跟随路径时有效距离的近似计算示意图；
图9是参考行驶距离计算器设计框图；
图10是隐式路径示意图；
图11是隐式路径递推参考点示意图；
图12是散点路径示意图；
图13是散点路径递推参考点示意图；
图14是车辆转角控制器设计框图；
图15是车辆在定曲率路径上行驶，存在纵向误差的示意图；
图16是车辆行驶过程中与参考点之间的横向误差示意图；
图17是车辆在变曲率处的运行示意图；
图18是车辆垂直泊车路径跟随效果示意图；
图19是车辆垂直泊车路径跟随时，车辆后轴中点与参考点之间x、y方向的误差数据；
图20是车辆平行泊车路径跟随效果示意图；
图21是车辆平行泊车路径跟随时，车辆后轴中点与参考点之间x、y方向的误差数据。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
本发明提出了一种全自动泊车系统路径跟随装置，如图1所示，整套装置包括参考点信息提取单元、信息交互单元与底盘控制单元三部分。
由于在本发明的背景技术部分已经提到，在未来的自动泊车算法中，路径规划的结果将以隐式路径或散点路径两种输出形式为主，而对于这两种形式，相对简便的方法是通过设定沿路径移动的参考点，而后使车辆跟随参考点。如此，车辆与参考点之间的误差将成为重要的反馈量，实现反馈控制。中国专利CN106372758B公开一种辅助泊车系统的路径跟随方法及装置，其虽然能达到一定的精度，但在实际使用中信息反馈过程还存在发生错误的情况。本申请发明人通过创造性研究发现，导致信息反馈有误的主要原因是专利CN106372758B仅以车辆全部的行驶距离作为总参考距离sd，未能进行误差上的解耦。图2为车辆进行路径跟随时的一种常见场景。车辆后轴中点由P点出发并运行至O点，此时弧长
参考点为Or。O点与Or点之间的误差可分为车辆相对于路径的侧向偏离误差OQ，与沿路径的纵向误差OrQ。其造成的影响在于：
①纵向控制中，期望速度vd应确保车辆在全程行驶完毕后停车，因此设计时，当总参考行驶距离sd达到规划路径的总长度l时，vd被设计为0。此时，若sd计算不当，则存在如图3左侧所示的沿路径的纵向误差时，参考点Or到达终点导致车速指令值为0，使得车辆将提前停车，车辆后轴中点O无法到达终点。
②横向控制中，车辆后轴中点O与参考点Or之间的误差将被反馈给横向控制器进行横向控制。若车辆后轴中点与参考点之间的误差同时包含图3所示的纵向误差与横向误差两部分，则横向控制将一同接受不能被其控制的纵向误差信息，而纵向误差反而将对横向控制造成影响，这一点将在后续说明。
可见，sd计算不当将对横纵向控制均造成影响，而通过设计合理的计算sd的方法，即可消除纵向误差，实现良好的纵向控制；而纵向误差消除后，设计横向控制器消除剩余的横向误差，最终即可达到最佳性能。在本发明的装置中，纵向控制在参考点信息提取单元的参考行驶距离计算器中实现，而横向控制可在底盘控制单元的车辆转角控制器中实现。信息交互单元在整套装置中承担起参考点信息提取单元与底盘控制单元之间的信息交互作用。基于以上分析，现对本发明涉及的三个单元进行详细介绍。
1、参考点信息提取单元
参考点信息提取单元的主要作用是在车辆运行过程中，实时接受车辆定位信息，并输出路径参考点信息，便于信息交互单元将车辆状态与参考点状态相比较，并将误差送入底盘控制单元进行底盘控制。该单元对参考点的提取方法可适应不同的路径规划输出结果，如以CC曲线为基础规划的、无法直接用数学函数表示、仅能用形状与长度定义的隐式路径，或以如机器学习方法规划出的一系列离散散点路径。该单元的具体功能为：①进行纵向控制，消除纵向误差；②获取参考点信息。
(1)纵向控制设计
先前分析已经提到，车辆引入纵向误差的原因是总参考距离sd的计算不当，因此需要设计总参考距离sd的计算方法。本发明采用有效距离积分方法进行sd的计算。
有效距离积分方法是借助每个步长的参考点的坐标、航向角等信息，近似计算出车辆行驶时的行驶路径长度向路径投影后的长度，以此作为有效距离并利用该有效距离进行积分，获取有效参考行驶距离sd，最大程度避免车辆由于跟随不佳、偏离路径行驶时将无效距离计入sd。具体过程如下：
车辆尽管始终在前进，但车辆走过的全部距离并非都是沿路径行驶的。仍以图2为例，车辆虽然由P点出发并运行至O点，但在沿路径的方向上，车辆实际上仅行驶到了Q点。若直接以车辆行驶过的全部距离，即弧长PO的长度作为总参考行驶距离sd，则会导致参考点位于如Or所在的领先于Q点的位置，如此则导致了纵向误差。
为应对上述问题，引入有效距离积分方法。有效距离被定义为车辆行驶时行驶路径长度向规划路径投影后的长度。由于在车辆行驶时，车辆会在直线部分、定曲率圆弧部分、变曲率曲线部分处行驶，在三种情况下，对应的有效距离按如下规则定义：
A.直线部分：有效距离为实际行驶路径OO′在直线上的直接投影长度CD，如图4所示；
B.定曲率圆弧部分：车辆行驶时，通过使用车辆后轴中点起点O与终点O′的坐标，可计算出车辆相对圆弧行驶过的有效角度，有效距离为这部分角度对应的圆弧弧长CD，如图5所示；
C.变曲率曲线部分：由于这部分有效距离难以精确算出，因此在计算时，利用起点O与终点O′对应的曲率按B中计算方式分别计算前后曲率下对应的长度，而后取二者平均值近似为有效距离CD，如图6所示。
通过这种方式，每当车辆运行一个时间间隔，则将车辆在这个时间间隔内的前进距离按照上述规则进行修正获取本段时间间隔内运行的有效距离Δsd，并用这样的Δsd进行积分，获取总长度sd。
但有时候，仅仅对有效距离进行积分，仍不足以达到最优性能，这是由于有效距离积分的计算会受到偶然误差的影响导致计算存在偏差。此外，如图7所示，由于车辆进行多段泊车时，在跟随完第一段路径BA时，处于换段交界处A点时存在溜车的情况，这导致车辆会在跟随下一段AC时存在箭头标识出的初始纵向误差OA，而这部分不能被上述积分机制所弥补。在优选的实施方式中，还需要设计对纵向积分的修正方法来消除这部分误差。
纵向积分修正方法：在每个步长内，通过比对参考点与车辆的坐标、航向角等信息，近似计算出车辆沿路径的纵向误差，从而对有效距离进行修正后进行积分，最大程度消除车辆行驶时由于近似计算有效距离产生的累积纵向误差，以及车辆在多段行驶时由于溜车引入的纵向误差。具体过程如下：
存在初始的纵向误差时，一个最直观的想法是，当车辆落后于参考点时，希望参考点移动较少使得车辆追上参考点；反之，希望参考点移动较多使得车辆与参考点之间的误差减少。按此思路，可设置纵向积分修正方法。在计算出有效距离Δsd后，还需进行以下修正才能被计入总长度sd：
A)车辆落后于参考点时：希望参考点不要移动过大，本次计入的有效距离按σ1的倍数缩小；
B)车辆领先于参考点时：希望计入更多距离，使得参考点追上车辆，本次计入的有效距离按σ2的倍数放大。
σ1、σ2为可调节的纵向补偿系数。在补偿时，车辆的超前与落后需要计算沿路径的纵向误差，并结合车辆行驶方向来判断。同时，为避免系统过于敏感，这种修正应在纵向误差超过某一阈值后才能进行，该阈值可设为一个码盘的刻度值。
由于仅有一个参考点的信息，因此纵向补偿时需要的纵向误差实际上无法计算出精确值，仅能通过使用参考点的信息对纵向误差进行近似计算。
上述有效距离积分方法和纵向积分修正方法可内置于参考行驶距离计算器中，以实现相应功能。
图8展示了一种跟随过程常见的场景，车辆后轴中点O与参考点Or之间同时存在横向与纵向误差，显然最理想的横向参考点为S点，而Or与S之间的距离为纵向误差。然而目前仅有关于Or的信息，无法求出二者之间的路径长度。但是由于Or的坐标、航向角、所在处曲率、转向方向均已知，因此Or所在的、与其航向角方向相切的圆R是唯一确定的。直线OrR与OR之间对应的圆R弧长，即认为是近似的纵向误差，其正负结合车辆的前进方向确定。实际运行时，在数据每一次更新时均计算一次纵向误差，若超过阈值，则对该时刻的有效距离Δsd进行修正并计入总长度sd。由于纵向误差无法获取精确值，其对纵向补偿实际上起到的是“警告”作用。
同时，如图1所示，由于横纵向控制均基于同一个有效距离sd，若在实际跟随时存在纵向误差，则在纵向误差未被补偿完毕之前，横向控制器仍然受到纵向误差的影响而无法良好跟随路径，引入额外的横向误差。然而在泊车的低速工况下，近似计算能够引入的纵向误差很小，同时，即使出现溜车，引入的纵向误差也仅为10厘米左右。因此，纵向误差在实时数据更新三至四次，在很短时间内即可补偿完毕，而补偿完毕后横向控制器即可根据相应设计，消除在纵向误差未补偿完毕时引入的横向误差。
纵向控制的整体框图如图9所示。
(2)参考点信息获取
车辆速度规划器与路径形状规划器在车辆运行之前已经存储好了路径规划的输出结果，而后才能结合规划路径长度与总参考距离sd的比较结果。其中，车辆速度规划器用于根据所述总参考距离sd，获取参考点的目标速度；路径形状规划器用于根据所述总参考距离sd，获取参考点位置，包括参考点的坐标与航向角信息。
在本发明的背景技术部分已经提到，在未来的自动泊车算法中，路径规划的结果将以隐式路径或散点路径两种输出形式为主。两种规划输出形式对应的参考点的获取方式稍有区别，以下进行详细介绍。
①规划结果为隐式路径
隐式路径如图10所示。隐式路径的输出结果可视为一系列A1至A6之间的直线段与CC曲线的拼接。而规划结果的表达形式为分段的隐式形式如式(1.1)所示。
式中l1、l2、l3为沿弧长的分段边界，可理解为图10中A1A2、A1A3等之间的弧长距离；ρ1、ρ2、ρ3为本段内的曲率值；q1、q2、q3为本段路径的车辆行驶方向(前进为1，倒车为-1)；π1、π2、π3为本段路径的方向盘转向方向(左转为1，右转为-1)；sd为沿路径的长度。
参考点坐标及航向角的计算是根据几何形状递推获得。将总参考距离，也即沿路径的长度sd，与分段边界值l1、l2、l3等相比较，从而获得此时参考点Or1的曲率ρ、行驶方向u和转向方向π。若此时参考点坐标及航向角分别为xd、yd、θd，沿路径的长度增加Δsd,则可如图11，递推下一时刻参考点Or2坐标及航向角x′d、y′d、θ′d为
车辆速度规划器内部存储目标车速vd与沿路径长度sd、路径总长度l相关的函数vd＝f(sd,l)，sd获取之后即可直接获取期望速度。
②规划结果为散点路径
散点路径如图12所示。散点路径即为一系列前后关联的散点组成的路径，而散点即可为单纯的坐标点，也可为包含速度的轨迹点。其递推方式如图13所示。散点Bk与Bk+1之间可近似为一小段圆弧，当前参考点为Or1且与Bk+1距离为Δsd1，而若由参考行驶距离计算器计算出本次参考点应前进Δsd，则参考点将跨过Bk+1并处于Bk+1与Bk+2之间，且跨过的距离为Δsd2＝Δsd-Δsd1。由于Bk+1与Bk+2的信息已知，则可获取处于圆弧Bk+1Bk+2上，且与Bk+1距离Δsd2的位置的坐标点信息，该点即可视为参考点。
如规划结果为轨迹点序列，则期望速度vd可由参考点两侧的两个轨迹点的速度插值获取；若规划结果为单纯的离散坐标点，则在规划结束后即可计算路径全部长度，并设计车速vd与沿路径长度sd、路径总长度l相关的函数vd＝f(sd,l)，而后根据实际sd获取期望速度。
2、底盘控制单元
底盘控制单元的主要作用是接收车辆实际速度与期望速度之间的速度误差、车辆后轴中点与参考点之间的位置误差，输出驱动力矩指令与转向力矩指令至驱动电机与转向电机，最终输出驱动力矩与转向力矩直接控制车辆运行。该单元的主要功能为：①进行横向控制，消除横向误差；②调整车辆实际速度至期望速度。
(1)横向控制设计
A.车辆运动学模型
由于在泊车过程中，车辆运动速度很低，因此忽略车辆侧偏角的影响，使用车辆运动学模型：
其中x(t)、y(t)为车辆后轴中点坐标，θ(t)为车辆航向角，v(t)为后轴中点速度，
为前轮转角，L为轴距。
进一步地，车辆的转向机构也可简化为惯性环节：
其中，u1(t)为执行机构的输入，T为惯性环节的时间常数。
由上两式看出，运动学方程中包含了时变量速度v(t)，后续控制指令设计时，不利于速度与转角的独立设计。因此，首先引入弧长作为新自变量，以实现横纵向解耦。同时，为便于后续反馈线性化处理，引入时间尺度函数u2(t)：
sd即为沿路径的总行进距离；u2(t)实际上可理解为微小时间dt内，车辆实际行走距离ds与沿路径行走距离dsd之比，而其用于反馈线性化的方式将在后文说明。显然，若车辆能够完全跟随路径，意味着车辆的全部行走距离等同于沿路径的行走距离，此时u2(t)＝1。如此，通过链式法则利用式(2.3)对式(2.1)(2.2)进行处理，可将x(t)、y(t)、θ(t)、
均转化为与弧长为自变量的关系式：
同时
这里需说明，式(2.4)中包含的速度v＝v(sd)。
B.反馈线性化
式(2.4)为车辆关于弧长的运动学模型，仍然为非线性模型，不便于后续状态方程的处理和反馈控制的设计，因此还需进行反馈线性化的过程。
在微分平坦空间中仍然以弧长为自变量，并将微分平坦输出设置为车辆后轴中点横纵坐标x(sd)、y(sd)。这里略去对系统微分平坦性的证明。
通过整理，可将系统涉及到的变量整理为微分平坦输出元素本身及其有限阶导数形式：
状态方程的整理同样需要理想点处的相应值。由于前文所述的参考点递推关系，参考点坐标xr、yr、参考点处理想航向角θr、参考点所在处曲率ρr可以直接获取。
则
通过将xr对弧长sd求导，可计算反馈控制所需的x′r、x″′r、x″′r为
yr及其各阶导数等计算与xr等相同。
根据式(2.4)一二式平方相加有
两次求导即有
同时，利用式(2.4)对
进行整理并代入
相关式有
式(2.6)利用车辆相关变量表示出了计算
所需的x、x′、x″、y、y′、y″，而反馈控制实际上是通过对x″′、y″′的进行设计的。计算
后两次积分获得
即可配合
求出实际指令值y1。这里可以看到，时间尺度函数u2的引入，为车辆相关变量向微分平坦空间的投影与指令投影回时域空间提供了渠道。
记δx＝x-xr。在控制过程中，希望误差向0衰减。为便于后续状态方程的整理，以x方向为例，构造以下等式：
δ″′x+k3δ″x+k2δ′x+k1δx＝0 (2.13)
上式为三阶线性微分方程，可求解δx＝δx(sd)。通过对k1、k2、k3的设计，可使得δx初值不为0时向0衰减，以实现误差的稳定。
将式(2.13)展开，令w1＝x″′，w2＝y″′，有
将w1、w2始终视为x″′、y″′，意味着式(2.13)始终成立，如此会使得误差始终向0衰减。通过使用w1、w2替换式(2.11)(2.12)的x″′、y″′对
进行计算，最终实现由误差至前轮转角控制量的转化。这里需说明，由于微分平坦空间中是对弧长求导，因此其导数表示方式与式(2.1)有所区别。
基于以上说明，车辆转角控制器实现的横向控制整体设计框图如图14所示。
这里对于先前提及的纵向误差对横向的影响进行简要说明。如图15所示，车辆在定曲率、半径为R的圆弧上行驶，根据阿克曼转向模型，车辆前轮转角保持在
即可完全跟随路径。
若存在纵向误差OOr，则将式(2.6)(2.14)代入(2.10)(2.12)并整理可发现，对于指令u1(t)，其不仅包含了式(18)中的
还包含了额外的与δx、δy、θ-θr相关的附加量，而δx、δy、θ-θr在半径R一定时与纵向误差OOr的大小成正相关。这样的结果会使得前轮转角指令无法保持
而偏离出路径，而若纵向误差OOr越大，该影响越大。
C.反馈控制说明
上一部分提到，通过使用设计好的w1、w2替换x″′、y″′进行计算。实际上，是真实存在的变量，其与设计好的w1、w2存在误差，可表示为
以x方向误差为例，记δx＝x-xr,根据式(2.13)(2.16)，以δx、δ′x、δ″x为状态变量，整理状态方程为
实际上，若车辆能够完全跟随路径，Δx＝0。这一点可以通过完全跟随路径时u2(t)≡1，代入式(2.6)中x″并对其求导，与式(2.9)相比较证明。同时，根据先前公式推导可见，w1、w2的计算涉及δx、δ′x、δ″x与k1、k2、k3的乘积，而通过(2.6)对实际的x″′、y″′进行计算不设计上述量，也就是说，δx、δ′x、δ″x与k1、k2、k3的乘积过大，可能将导致Δx过大，控制的设计不准确，因此，当误差δx、δ′x、δ″x较小且k1、k2、k3不大时，Δx才可能较小并可忽略。
后续设计时，假定上述条件满足，并将该项的影响忽略。由于δy等状态方程与式(2.17)(2.18)相同，因此若对δx、δy的状态方程以相同方式处理，则δx、δy的衰减率是相同的。
上述状态方程的整理是为了便于确定式(2.13)中的反馈系数k1、k2、k3，从而决定误差项δx、δy及其各阶导数的衰减速度。中国专利CN106372758B指出，误差的衰减速度取决于系统的主导极点，因此在使用LQR确定反馈系数的基础上，引入了衰减因子α。其原理在于，对于状态方程
利用LQR计算时，使用矩阵A+αI代替矩阵A来进行计算，这就使得系统的主导极点更加远离虚轴，从而使得误差以更快的速度衰减。这里I为单位矩阵。
D.误差一致性说明
路径本身与车辆的横向误差体现为横向偏距与角度误差，而本发明的控制率是针对δx、δy及其导数体现的，因此，需说明上述误差的统一性，从而证明对δx、δy衰减率的设计是有效的。误差示意图如图16所示。其中，l为横向偏距。
对于横向偏距l显然有
当然，若δx、δy均以指数
的速度衰减，则
对于角度偏差θ-θr，结合
与小角度假设(θ-θr小于5°)，有
因此
当然，若δ′x、δ′y均以指数
的速度衰减，则
式(2.20)至(2.24)说明，横向偏距与角度误差和δx、δy等存在对应关系，而对δx、δy等的衰减率设计和对横向偏距与角度误差的衰减率设计是等效的。
E.反馈控制设计
由于车辆的初始误差为0，而在跟随过程中出现了误差，这说明是由于路径的形状变化引入了误差。而又由于路径的形状是连续变化的，因此误差的引入也是一个平滑的过程。因此，若能对误差的引入规律进行研究，则可以结合其引入规律将误差在泊车全程均控制在较小值。
若将误差视为一个函数f(sd)，则根据矩形积分公式，按Δsd步长离散化，配合阶跃函数，可改写为
u(0)、u(Δsd)等为阶跃函数。上式说明，误差可视为一系列阶跃函数值的依次叠加，其在数值上的正确性可通过代入sd＝Δsd、sd＝2Δsd等进行验证。因此，若能获得f′(0)、f′(Δsd)等的规律，则可在误差刚刚开始叠加时即设计控制率进行消除，使得误差值不会累积至较大值。
由于车辆在保持前轮转角固定时，车辆的运行轨迹即为固定的圆弧。若规划路径为定曲率圆弧，则车辆在前轮转角稳定后，不会继续出现横向误差。因此，误差的累积是在曲率发生变化时产生的。
图17为车辆在变曲率处的运行示意图。当车辆处于A点时，车辆与参考点Or重合，由于没有误差反馈，车辆保持圆弧路径行驶至B点，然而此时由于曲率发生了变化，车辆前进Δsd后参考点实际应为O′r。如此，系统产生了横向误差，而该误差的累积最终将表现为车辆偏离了路径。
图17中的BO′r之间的距离即为前进Δsd后引入的横向偏距，B与O′r之间的航向角偏差即为引入的角度误差。记圆R1与R2半径为r1与r2，曲率为ρ1与ρ2，则
利用余弦定理有BR2长度
则
考虑到分母根号中
同
相比小得多，式(2.29)继续整理为
这里ρ′为曲率变化率。式(2.30)表明，经过固定的Δsd长度，由曲率变化率ρ′引入系统的横向偏距
与ρ′本身近似成正比例函数关系。
同理，由ρ′引入的角度误差为
可见，引入的角度误差σ也与曲率变化率ρ′近似成正比例函数关系。
如中国专利CN106372758B所述，若系统的主导极点实部为-μ，则误差以
的速度衰减。若假设系统有一初始误差τ(0)，在经过Δsd后，误差变为
以横向偏距为例，若曲率变化率为ρ′，则结合介入的横向偏距误差，Δsd后，误差变为
若此时加入与曲率变化率相关的衰减因子α＝β1ρ′，则
观察式(2.34)第二部分可见，如果参数β1能够适度调整，则在误差积累到某一值之后，由衰减因子α增加的衰减速度可以将后续引入的误差完全抵消掉，也就是说，误差将维持在某一上限值τ(0)不会继续增加。同时，为保持车辆在定曲率路径行驶时有足够的误差衰减速度，可设定α＝β1ρ′+β2，其中β1、β2为可调参数，由车辆实际跟踪效果确定。
这里说明，实际运行时，误差上限值τ(0)不能根据式(2.34)由β1精确确定，这是由于α因子的加入不严格使得主导极点更远离虚轴α的距离；同时，纵向误差虽然可保持在较小值，但无法完全消除至0，残余的纵向误差仍会对横向控制造成影响；在推导中的近似也会造成些微影响。
(2)车速控制
车速控制采用传统方法如PID控制即可，无特定要求。
在一个具体实施方式中，底盘控制单元包括：
车辆速度控制器，用于接收所述速度误差，基于该速度误差获取驱动力矩指令；
驱动电机，用于接收所述驱动力矩指令，根据所述驱动力矩指令产生用于对车辆进行纵向控制的驱动力矩；
车辆转角控制器，用于接收所述位置误差，根据该位置误差获取转向力矩指令；
转向电机，用于接收所述转向力矩指令，根据所述转向力矩指令产生用于对车辆进行横向控制的转向力矩。
其中，所述车辆转角控制器基于微分平坦理论，通过反馈线性化将变量由时域空间投影至微分平坦空间，通过时间尺度变换使微分平坦空间中以弧长为自变量，并在微分平坦空间中利用线性二次型最优控制器(LQR)将路径曲率变化率与可以加速误差衰减的衰减因子α建立联系，确保车辆在变曲率时也能避免横向误差累积，从而高精度跟随参考点，进而高精度跟随路径。
反馈线性化后的误差项可与未反馈线性化时车辆相对路径的横向偏距、角度误差建立等式关系，如此可确保在微分平坦空间中对误差控制率的设计与在时域空间中对车辆相对路径的横向偏距、角度误差的误差控制率的设计是等效的。
若纵向误差未完全消除，则将车辆与参考点之间的误差送入车辆转角控制器后，会出现与纵向误差成比例的干扰项，影响车辆对路径的跟随，由此可侧面说明对纵向误差补偿的必要性。在车辆与参考点之间的纵向误差未完全补偿完毕时，纵向误差对车辆转角仍有影响并引入额外的横向误差，而在纵向误差补偿完毕后，车辆转角控制器即可根据设计好的控制率，对纵向误差未补偿完毕时引入的额外的横向误差进行消除。
3、信息交互单元
信息交互单元的主要作用是在车辆运行时利用各种车载传感器，提取车辆轮速与方向盘转角送入车辆定位模块，由车辆定位模块将轮速转化为实际车速、进行位姿估计获取实际位置，并将车辆运行状态送入参考点信息提取单元便于参考点的更新，将偏差用于后续控制。这部分对硬件依赖较大，因此在本发明中不作详细介绍。
本装置的实车效果如图18至图21所示。实车效果表明，本装置可使车辆实现高精度跟随，x、y方向误差均可保持在5cm以内。
上述装置通过时间尺度变换，以弧长为自变量，实现了横纵向控制的解耦，便于二者独立设计；同时，进一步将横纵向误差解耦，并交由对应执行器进行控制，更加高效地实现了车辆与参考点之间误差的有效消除，最终使得车辆能够以更高精度跟随路径。
基于上述参考点信息提取单元、信息交互单元与底盘控制单元，可以实现全自动泊车系统路径跟随方法，包括以下步骤：基于有效距离积分方法计算总参考距离sd，根据所述总参考距离sd与规划路径长度获得目标车速与参考点位置；获取车辆的实际车速与实际位置，与所述目标车速与参考点位置进行相应比对，获得速度误差和位置误差；获得所述速度误差和位置误差，产生相应的驱动力矩指令和转向力矩指令，实现对车辆的纵向控制和横向控制。
上述方法也可以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。
Claims (10)
1.一种全自动泊车系统路径跟随装置，其特征在于，包括：
参考点信息提取单元，用于基于有效距离积分方法计算总参考距离sd，根据所述总参考距离sd与规划路径长度获得目标车速与参考点位置；
信息交互单元，用于获取车辆的实际车速与实际位置，与所述目标车速与参考点位置进行相应比对，获得速度误差和位置误差；
底盘控制单元，用于获得所述速度误差和位置误差，产生相应的驱动力矩指令和转向力矩指令，实现对车辆的纵向控制和横向控制。
2.根据权利要求1所述的全自动泊车系统路径跟随装置，其特征在于，所述参考点信息提取单元包括：
参考行驶距离计算器，用于根据车辆的实际位置，基于有效距离积分方法计算总参考距离sd，具体地：循环基于每个步长的参考点信息，近似计算出车辆行驶时的行驶路径长度向路径投影后的长度，以此作为该步长的有效距离，对每个步长的有效距离进行积分，获得所述总参考距离sd；
车辆速度规划器，用于根据所述总参考距离sd，获取参考点的目标速度；
路径形状规划器，用于根据所述总参考距离sd，获取参考点位置，包括参考点的坐标与航向角信息。
3.根据权利要求2所述的全自动泊车系统路径跟随装置，其特征在于，所述参考行驶距离计算器中，在每个步长内，获得所述有效距离后，通过比对参考点与车辆实际位置信息，近似计算出车辆沿路径的纵向误差，对该步长的有效距离进行修正，以修正后的有效距离参与所述积分。
4.根据权利要求3所述的全自动泊车系统路径跟随装置，其特征在于，在所述纵向误差大于设定阈值时进行所述修正，所述修正具体为：
当车辆落后于参考点时，该步长的有效距离按σ1的倍数缩小；
当车辆领先于参考点时，该步长的有效距离按σ2的倍数放大；
其中，σ1、σ2均为可调节的纵向补偿系数。
5.根据权利要求1所述的全自动泊车系统路径跟随装置，其特征在于，所述信息交互单元包括：
车辆传感器，用于获取轮速与方向盘转角；
车辆定位器，用于根据所述轮速与方向盘转角进行车辆位姿估计，获得车辆的实际车速与实际位置，与所述目标车速与参考点位置进行相应比对，获得速度误差和位置误差。
6.根据权利要求1所述的全自动泊车系统路径跟随装置，其特征在于，所述底盘控制单元包括：
车辆速度控制器，用于接收所述速度误差，基于该速度误差获取驱动力矩指令；
驱动电机，用于接收所述驱动力矩指令，根据所述驱动力矩指令产生用于对车辆进行纵向控制的驱动力矩；
车辆转角控制器，用于接收所述位置误差，根据该位置误差获取转向力矩指令；
转向电机，用于接收所述转向力矩指令，根据所述转向力矩指令产生用于对车辆进行横向控制的转向力矩；
其中，所述车辆转角控制器基于微分平坦理论，通过反馈线性化将变量由时域空间投影至微分平坦空间，通过时间尺度变换使微分平坦空间中以弧长为自变量，并在微分平坦空间中利用线性二次型最优控制器将路径曲率变化率与可以加速误差衰减的衰减因子α建立联系。
7.一种全自动泊车系统路径跟随方法，其特征在于，包括以下步骤：
基于有效距离积分方法计算总参考距离sd，根据所述总参考距离sd与规划路径长度获得目标车速与参考点位置；
获取车辆的实际车速与实际位置，与所述目标车速与参考点位置进行相应比对，获得速度误差和位置误差；
获得所述速度误差和位置误差，产生相应的驱动力矩指令和转向力矩指令，实现对车辆的纵向控制和横向控制。
8.根据权利要求7所述的全自动泊车系统路径跟随方法，其特征在于，所述基于有效距离积分方法计算总参考距离sd具体包括：
获取车辆的实际位置，循环基于每个步长的参考点信息，近似计算出车辆行驶时的行驶路径长度向路径投影后的长度，以此作为该步长的有效距离，对每个步长的有效距离进行积分，获得所述总参考距离sd。
9.根据权利要求8所述的全自动泊车系统路径跟随方法，其特征在于，在每个步长内，获得所述有效距离后，通过比对参考点与车辆实际位置信息，近似计算出车辆沿路径的纵向误差，对该步长的有效距离进行修正，以修正后的有效距离参与所述积分。
10.根据权利要求7所述的全自动泊车系统路径跟随方法，其特征在于，产生所述转向力矩指令时，基于微分平坦理论，通过反馈线性化将变量由时域空间投影至微分平坦空间，通过时间尺度变换使微分平坦空间中以弧长为自变量，并在微分平坦空间中利用线性二次型最优控制器将路径曲率变化率与可以加速误差衰减的衰减因子α建立联系。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210489943.XA CN115092121A (zh) | 2022-05-06 | 2022-05-06 | 一种全自动泊车系统路径跟随装置及方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210489943.XA CN115092121A (zh) | 2022-05-06 | 2022-05-06 | 一种全自动泊车系统路径跟随装置及方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115092121A true CN115092121A (zh) | 2022-09-23 |
Family
ID=83286928
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210489943.XA Pending CN115092121A (zh) | 2022-05-06 | 2022-05-06 | 一种全自动泊车系统路径跟随装置及方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115092121A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115593408A (zh) * | 2022-11-01 | 2023-01-13 | 辉羲智能科技（上海）有限公司（Cn） | 一种车辆横向控制积分优化方法、pid控制方法及系统 |
-
2022
- 2022-05-06 CN CN202210489943.XA patent/CN115092121A/zh active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115593408A (zh) * | 2022-11-01 | 2023-01-13 | 辉羲智能科技（上海）有限公司（Cn） | 一种车辆横向控制积分优化方法、pid控制方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106372758B (zh) | 一种辅助泊车系统的路径跟随方法及装置 | |
| CN107415939B (zh) | 一种分布式驱动电动汽车转向稳定性控制方法 | |
| CN103552482B (zh) | 一种无轨车辆的电子差速控制方法及装置 | |
| CN112462760B (zh) | 一种双舵轮agv路径跟踪方法 | |
| CN112519882A (zh) | 一种车辆参考轨迹跟踪方法及系统 | |
| CN108646756A (zh) | 基于分段仿射模糊滑模的智能汽车横向控制方法及系统 | |
| CN111158379B (zh) | 一种方向盘零偏自学习的无人车轨迹跟踪方法 | |
| CN110320906B (zh) | 一种基于麦克纳姆轮的四驱agv小车差速直线行驶姿态调整方法 | |
| CN115092121A (zh) | 一种全自动泊车系统路径跟随装置及方法 | |
| CN113264049B (zh) | 一种智能网联车队协作换道控制方法 | |
| CN113335278A (zh) | 一种网联式智能车队自适应巡航控制方法及系统 | |
| CN108594637A (zh) | 两轮自平衡车平面点对点自主运动控制方法 | |
| CN113064344A (zh) | 一种多轴无人重载车辆的轨迹跟踪控制方法 | |
| CN113126623A (zh) | 一种考虑输入饱和的自适应动态滑模自动驾驶车辆路径跟踪控制方法 | |
| CN114942642A (zh) | 一种无人驾驶汽车轨迹规划方法 | |
| Zhou et al. | Modeling and simulation research of heavy-duty AGV tracking control system based on magnetic navigation | |
| CN111661048B (zh) | 多铰接式车辆及其轨迹跟随控制方法与系统 | |
| CN114906128A (zh) | 一种基于mcts算法的自动泊车运动规划方法 | |
| Yang et al. | A trajectory planning and fuzzy control for autonomous intelligent parking system | |
| CN117549908A (zh) | 一种基于前馈lqr的车辆横向控制方法 | |
| CN115717898A (zh) | 一种商用铰接车辆自动驾驶规划轨迹完美演绎方法 | |
| TWI770966B (zh) | 無人自走車之導引控制方法 | |
| CN115525054A (zh) | 大型工业园区无人清扫车沿边路径跟踪控制方法及系统 | |
| Qian et al. | Research on automatic parking system based on linear quadratic regulator | |
| CN114355941A (zh) | 基于改进Stanley控制的车辆路径跟踪方法 |
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 |