CN110307850A - 航迹推算定位方法及自动泊车*** - Google Patents
航迹推算定位方法及自动泊车*** Download PDFInfo
- Publication number
- CN110307850A CN110307850A CN201910711364.3A CN201910711364A CN110307850A CN 110307850 A CN110307850 A CN 110307850A CN 201910711364 A CN201910711364 A CN 201910711364A CN 110307850 A CN110307850 A CN 110307850A
- Authority
- CN
- China
- Prior art keywords
- parking
- wheel
- vehicle
- follows
- parking stall
- 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 60
- 230000004807 localization Effects 0.000 title claims abstract description 26
- 230000033001 locomotion Effects 0.000 claims abstract description 30
- 238000005259 measurement Methods 0.000 claims abstract description 21
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 6
- 238000001514 detection method Methods 0.000 claims description 5
- 230000005540 biological transmission Effects 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 4
- 241000153246 Anteros Species 0.000 claims description 3
- 230000001186 cumulative effect Effects 0.000 claims description 3
- 238000009434 installation Methods 0.000 claims description 3
- 230000002452 interceptive effect Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000000977 initiatory effect Effects 0.000 claims 1
- 238000002604 ultrasonography Methods 0.000 claims 1
- 238000005516 engineering process Methods 0.000 description 10
- 238000007796 conventional method Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 239000004568 cement Substances 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000005245 sintering Methods 0.000 description 2
- 230000003321 amplification Effects 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 239000000446 fuel Substances 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000003199 nucleic acid amplification method Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C1/00—Measuring angles
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
- G01S15/06—Systems determining the position data of a target
- G01S15/08—Systems for measuring distance only
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Acoustics & Sound (AREA)
- Computer Networks & Wireless Communication (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供了一种基于超声波传感器和轮速传感器的航迹推算定位方法及自动泊车***，以解决室内移动机器人、辅助泊车***在车辆低速运动状态下，采用惯性导航设备测量车辆行驶状态比较困难，利用超声波传感器确定泊车空间及泊车空间坐标系，使用轮速传感器的数据，采用航迹推算算法，定位车辆在泊车空间坐标系中的初始位姿和泊车过程中的车辆位姿；在移动机器人领域当中，可用于移动机器人自主对充电桩位置的探索。
Description
技术领域
本发明涉及室内移动机器人、辅助泊车***领域，特别地，涉及一种基于超声波传感器和轮速传感器的航迹推算定位方法及自动泊车***。
背景技术
随着工业飞速发展，汽车作为一种运输或代步工具逐渐成为人们工作与生活的重要组成部分。汽车技术已成为集成自动控制理论、人工智能理论、优化理论、传感器技术、通信技术、信号处理技术、图像处理技术、信息融合技术等多理论、多技术于一体的综合性技术。随着人民生活条件的同益改善，人们对汽车性能的要求也在不断提高，要求汽车不但具备较高的操纵稳定性、良好的乘坐舒适性、可观的燃油经济性，还要具有较高的驾驶安全性。人口数量的增加以及城市建筑设施的不断扩增，使有限的城市空间变得拥挤不堪，汽车数量的逐年增加，不仅导致交通同趋拥堵，每辆汽车平均分配到的停放空间也在逐渐缩小。
狭小的泊车空间及驾驶员有限的视野范围给汽车行驶，尤其是泊车入位操作增加了困难。泊车过程中驾驶员视野范围有限，仅能依靠后视镜获取车辆周围环境信息，不仅要时刻注意车辆与周围物体相对距离，同时还要通过离合器和制动器控制车辆平稳缓慢运动，并且要配合适当的方向盘转角，以保证车辆安全地进入目标车位。当目标车位过小或驾驶员泊车经验不足，方向盘转向操作和车速未能合理配合时，驾驶员需多次进出车位，调整车辆位姿，才能完成泊车。
自动泊车***成为近年来国内外研究热点，随着传感技术的快速发展，研究人员将多种传感器技术应用于泊车辅助***。其中车辆在泊车过程中，其自身的航迹和位姿定位技术是关键。
定位方法在机器人控制领域研究成果较多，所使用传感器不同，定位方法也不同，车辆定位或机器人定位主要分为：绝对定位、相对定位；根据定位原理及使用的传感器，相对定位方法主要为：基于视觉技术的相对定位方法和航迹推算法。
航迹推算法主要利用里程计、陀螺仪等车辆内部传感器测量车辆行驶状态，得到采样周期内车辆位置和方位角增量，分别对位姿增量累计，得到相对于初始状态的车辆位姿。根据传感器类型，航迹推算法又可分为：基于惯性导航仪的航迹推算法、基于里程计的航迹推算法。
由于视觉技术的定位方法，获取环境信息量大，但对信息处理器的数据处理能力要求高，且视觉传感器受环境光线强度影响大。光照过强或过暗都将影响基于视觉传感器定位方法的定位精度。采用惯性导航仪器测量车辆行驶状态较困难。
现有关于机器人定位技术主要有两种：GPS定位、IMU加视觉。这两种方法各有缺点：GPS定位的方法对环境要求较高，在室内或者桥洞下GPS信号不稳定，很难达到稳定的定位效果；IMU加视觉的方式，成本较高，且在恶劣环境下，图像质量交叉，视觉的用处将受到严重的制约。
发明内容
本发明目的在于提供一种基于超声波传感器和轮速传感器的航迹推算定位方法，以解决室内移动机器人、辅助泊车***在车辆低速运动状态下，采用惯性导航设备测量车辆行驶状态比较困难，利用超声波传感器确定泊车空间及泊车空间坐标系，使用轮速传感器的数据，采用航迹推算算法，定位车辆在泊车空间坐标系中的初始位姿和泊车过程中的车辆位姿；在移动机器人领域当中，可用于移动机器人自主对充电桩位置的探索。
为实现上述目的，本发明的一方面提供了一种基于超声波传感器和轮速传感器的航迹推算定位方法，包括步骤：
S1.启动泊车***，获取超声波传感器数据和轮速传感器数据，建立泊车坐标系；
S2.进行泊车位空间尺寸计算，并判断是否满足平行泊车位或垂直泊车位空间尺寸标准，倘若是，进入步骤S3，否则重新进行泊车位空间尺寸计算；
S3.进行泊车车辆初始位置计算，获取左右后轮轮速传感器数据，或者获取左右前轮轮速传感器及方向盘转角信息，并计算后轮轴中心点运动轨迹。
作为优选的技术方案之一，步骤S2中平行泊车位空间尺寸标准的确定方法如下：
根据自动泊车***的路径规划分析与推算，采用双圆弧叠加进行轨迹规划的平行泊车最小泊车位空间尺寸为：a×b(依据路径规划约束条件推算)，采用B样条进行轨迹规划的平行泊车最小泊车位空间尺寸为：c×d(依据路径规划约束条件推算)；为了提高道路两侧车位的利用率，当车位满足a×(b-0.2)或c×(d-0.2)时，认为是有效车位；设定目标车位长度为：lco，宽度为：lko，实测车位长度为：Ic；根据车位长度确定路径规划方法，如果实测车位长度lc≥a时，泊车***采用双圆弧叠加进行轨迹规划，即lco＝a，lko＝b；如果实测车位长度c≤lc≤a时，泊车***采用B样条进行轨迹规划，即lco＝c，lko＝d；以上两种泊车方式都采用以前车为车辆停放基准。
作为优选的技术方案之一，步骤S2中垂直泊车位空间尺寸标准的确定方法如下：
设定目标车位长度为：lco，宽度为：lko；根据自动泊车***的路径规划分析与推算，采用双圆弧叠加进行轨迹规划的垂直泊车最小泊车位空间尺寸为：e×f(依据路径规划约束条件推算)，采用B样条进行轨迹规划的平行泊车最小泊车位空间尺寸为：g×h(依据路径规划约束条件推算)；为了提高道路两侧车位的利用率，当车位满足(e-0.2)×f或(g-0.2)×h时，认为是有效车位。
作为优选的技术方案之一，步骤S3包括如下步骤：进行泊车车辆初始位置计算，根据数据异常检测算法检测前轮数据，若左右前轮数据正常，则用左右前轮数据计算后轮轴中心点运动轨迹；若左右前轮数据异常，则用左右后轮数据计算后轮轴中心点运动轨迹。
作为优选的技术方案之一，步骤S3中采用左右后轮数据计算后轮轴中心点运动轨迹的具体方法是：进行泊车车辆初始位置计算，获取左右后轮轮速传感器数据，推算后轮轴中心点速度vr，推算车辆航向角θ，计算后轮轴中心点运动轨迹(xr(t),yr(t))。
作为进一步优选的技术方案之一，由CAN总线得到后轮左右两轮的轮速传感器给出的车轮转动距离ΔsrL和ΔsrR，两者分别为左轮转动距离和右轮转动距离，对于后轮中心Mr，其瞬时速度为vr，满足如下公式：
式中，xr′(t)、yr′(t)分别为后轮轴中心在x，y轴方向的瞬时速度；Δxr、Δyr为x、y轴方向的瞬时位移；vr(t)为后轮轴中心瞬时速度；θ为后轮航向角。
其中航向角θ的瞬时改变量θ′(t)为：
为了简化计算，将左右后轮的瞬时速度的平均值作为后轮中心点的瞬时速度，设后轮中心处Mr的速度vr为左右后轮瞬时速度的平均值，即：
将(3)式代入(1)式，合并(2)式，整理得：
式中w为后轮轴轮距。
作为优选的技术方案之一，步骤S3中采用左右前轮数据计算后轮轴中心点运动轨迹的具体方法是：进行泊车车辆初始位置计算，获取左右前轮轮速传感器及方向盘转角信息，计算前轮中心转向角计算前轮轴中心点速度vf，推算后轮轴中心点速度vr，推算车辆航向角θ，计算后轮轴中心点运动轨迹(xr(t),yr(t))。
作为进一步优选的技术方案之一，由CAN总线得到前轮左右两轮的轮速传感器给出的车轮转动距离ΔsfL和ΔsfR，方向盘转向角为φ可知，由方向盘转向角为φ得前轮中心转向角
其中k为方向盘转角φ与前轮中心转向角之间的传动比。
在坐标系XOY中，前后轴中心点位置Mr和Mf坐标关系为：
变换为：
对上式进行微分：
一般情况下，泊车速度比较低，可以假定无滑轮现象，即后轮的轨迹在车辆垂直方向速度为0，其方程式表示为：
y′rcosθ-x′rsinθ＝0 (9)
将(8)式代入(9)中得：
x′f·sinθ-y′f·cosθ+θ′·l＝0 (10)
前轮轴中心点Mf处的x,y方向速度为：
将(11)式代入(10)中可得到车辆回转角速度：
将(11)式和(12)代入(7)得到后轮轴中间点Mr在x,y方向速度为：
其中vf为前轮中心处Mf的速度，为了通过左右前轮瞬时位移ΔsfL、ΔsfR，求出vf，即可得到后轮轴中间点Mr的运动轨迹。
设前轮中心转向角在瞬时不变，车辆瞬时沿作圆轨道运动，瞬时前进的距离分别为：ΔsfL、ΔsfR；
为了简化计算，设前轮中心处Mf的速度vf为左右前轮瞬时速度的平均值，即：
在Δt时间内，由：
将(14)式代入(15)(16)式，整理得到：
通过单位时间内左右前轮运动行程ΔsfL、ΔsfR和方向盘转角φ，计算车身方位角增量和后轴中点位置增量，分别对车身方位角增量和位置增量累加，可得出车辆相对起始状态的位姿。式中由公式(5)得到，l为车辆前后轴距。
作为优选的技术方案之一，步骤S3中泊车车辆初始位置的计算方法如下：
泊车***根据实测车位长度，确定泊车方式和目标车位尺寸；根据实测车位宽度，采用以前车为车辆停放基准；坐标系的原点以开始启动自动泊车时为基准，建立坐标系并计算车辆初始位置，其车辆初始坐标为：
在(18)式中，Δs＝(xs2-xs1-lco)/2，Lrad为测位超声波安装点到后轴距离，yrd1为超声波测得最小侧向距离，Lk为车辆宽度，x0、y0为车辆以泊车位外侧边界线为基准停车时，车辆泊车起点坐标。
作为进一步优选的技术方案之一，车位长度和车位宽度的检测方法如下：
泊车***开启后，泊车***通过超声波传感器检测右侧车位，根据超声波测距的变化，判断车位起始点与终止点，并记录对应车轮里程xs1、xs2；车辆里程位xs1～xs2时，记超声波测距最小值为yrd2，车轮里程大于xs2时，记超声波测距最小值为yrd1；
若车位满足xs2-xs1≥a，且yrd2-yrd1≥b，或者车位满足xs2-xs1≥c，且yrd2-yrd1≥d，泊车***可以通过人机交互***，确定泊车方位(即左侧或右侧泊车)，车辆进入泊车起始区域后，根据超声波传感器测距更新yrd1；车辆停止运动后，泊车***记录停车时车辆里程xs3；泊车***根据超声波测距及车辆里程，得到车位长度lc＝xs2-xs1，车位宽度lk＝yrd2-yrd1。
本发明的另一方面提供一种自动泊车***，所述泊车***采用上述所述的航迹推算定位方法。
本发明的有益效果在于：
1、本发明提供一种基于超声波传感器和轮速传感器的航迹推算定位方法，以解决室内移动机器人、辅助泊车***在车辆低速运动状态下，采用惯性导航设备测量车辆行驶状态比较困难，利用超声波传感器确定泊车空间及泊车空间坐标系，使用轮速传感器的数据，采用航迹推算算法，定位车辆在泊车空间坐标系中的初始位姿和泊车过程中的车辆位姿；在移动机器人领域当中，可用于移动机器人自主对充电桩位置的探索。
2、本发明的主要特点是受环境影响较小，且成本低；传感器静态漂移测量误差小；在轮速传感器传输数据出现跳变时，通过计算动态阈值，可有效滤掉非正常数据，对车辆或者机器人进行有效定位。
3、图3-6为仿真结果图，从图中，可以得知：传统方法单次定位误差经常会发生跳变，定位误差变得很大，而本发明单次定位误差曲线较为稳定，且误差都比较小，说明传统方法误差发生跳变的原因是前轮传感器数据出错，而利用本发明方法可以有效避免前轮传感器数据出错给小车定位带来误差。同理，传统方法各组实验平均定位误差比本发明方法更大，且不稳定，这同样是前轮传感器数据出错导致的。
附图说明
构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的限定。在附图中：
图1是本发明实施例中车辆运动位姿航迹推算定位图；
图2是本发明实施例提供的一种基于超声波传感器和轮速传感器的航迹推算定位方法的流程图；
图3是传统方法定位单次误差曲线图；
图4是本发明实施例提供的一种基于超声波传感器和轮速传感器的航迹推算定位方法定位单次误差曲线图；
图5是传统方法各组实验平均定位误差曲线图；
图6是本发明实施例发明实施例提供的一种基于超声波传感器和轮速传感器的航迹推算定位方法各组实验平均定位误差曲线图。
具体实施方式
以下将结合实施例对本发明的构思、具体结构及产生的技术效果进行清楚、完整地描述，以充分地理解本发明的目的、特征和效果。显然，所描述的实施例只是本发明的一部分实施例，而不是全部实施例，基于本发明的实施例，本领域的技术人员在不付出创造性劳动的前提下所获得的其他实施例，均属于本发明保护的范围。
实施例1：
本发明实施例1提供一种基于超声波传感器和轮速传感器的航迹推算定位方法，，该方法使用左右前轮码盘脉冲计数和方向盘转角信息，测量车轮位移，根据采样周期内车辆的变化，计算车辆方位角增量和位置增量，得到相对初始状态的位姿，定位车辆在泊车空间坐标系中的位姿。该算法的具体步骤为：
1、泊车位标准的确定
(1)、平行泊车位空间尺寸标准
根据自动泊车***的路径规划分析与推算，采用双圆弧叠加进行轨迹规划的平行泊车最小泊车位空间尺寸为：a×b(依据路径规划约束条件推算)，采用B样条进行轨迹规划的平行泊车最小泊车位空间尺寸为：c×d(依据路径规划约束条件推算)。为了提高道路两侧车位的利用率，当车位满足a×(b-0.2)或c×(d-0.2)时，认为是有效车位。记目标车位长度为：lco，宽度为：lko。
根据车位长度确定路径规划方法，如果实测车位长度lc≥a时，泊车***采用双圆弧叠加进行轨迹规划，即lco＝a，lko＝b；如果实测车位长度c≤lc≤a时，泊车***采用B样条进行轨迹规划，即lco＝c，lko＝d；以上两种泊车方式都采用以前车为车辆停放基准。
(2)垂直泊车位空间尺寸标准
根据自动泊车***的路径规划分析与推算，采用双圆弧叠加进行轨迹规划的垂直泊车最小泊车位空间尺寸为：e×f(依据路径规划约束条件推算)，采用B样条进行轨迹规划的平行泊车最小泊车位空间尺寸为：g×h(依据路径规划约束条件推算)。为了提高道路两侧车位的利用率，当车位满足(e-0.2)×f或(g-0.2)×h时，认为是有效车位。记目标车位长度为：lco，宽度为：lko。
2、泊车初始位置计算
(1)、泊车位检测方法及确定
泊车***开启后，泊车***通过超声波传感器检测右侧车位，根据超声波测距的变化，判断车位起始点与终止点，并记录对应车轮里程xs1、xs2。车辆里程位xs1～xs2时，记超声波测距最小值为yrd2，车轮里程大于xs2时，记超声波测距最小值为yrd1。
若车位满足xs2-xs1≥a，且yrd2-yrd1≥b，或者车位满足xs2-xs1≥c，且yrd2-yrd1≥d，泊车***可以通过人机交互***，确定泊车方位(即左侧或右侧泊车)，车辆进入泊车起始区域后，根据超声波传感器测距更新yrd1。车辆停止运动后，泊车***记录停车时车辆里程xs3。泊车***根据超声波测距及车辆里程，得到车位长度lc＝xs2-xs1，车位宽度lk＝yrd2-yrd1。
(2)车辆初始位计算
泊车***根据实测车位长度，确定泊车方式和目标车位尺寸。根据实测车位宽度，采用以前车为车辆停放基准。
坐标系的原点以开始启动自动泊车时为基准，建立坐标系并计算车辆初始位置，如1所示，其车辆初始坐标为：
在(1)式中，Δs＝(xs2-xs1-lco)/2，Lrad为测位超声波安装点到后轴距离，yrd1为超声波测得最小侧向距离，Lk为车辆宽度，x0、y0为车辆以泊车位外侧边界线为基准停车时，车辆泊车起点坐标。
3、基于航迹推算方法的车辆运动位姿计算方法
航迹推算方法主要使用左右后轮码盘脉冲计数和方向盘转角信息，测量车轮位移，根据采样周期内车辆的变化，计算车辆方位角增量和位置增量，得到相对初始状态的位姿，定位车辆在泊车空间坐标系中的位姿，根据CAN总线获取的左右轮码盘脉冲计数和方向盘转角信息的种类，进行下列两种基于航迹推算方法的车辆运动位姿计算方法：
(1)已知左右前轮轮速传感器数据和方向盘信息推算车辆运动位姿
由CAN总线得到前轮左右两轮的轮速传感器给出的车轮转动距离ΔsfL和ΔsfR，方向盘转向角为φ可知，由方向盘转向角为φ得前轮中心转向角
其中k为方向盘转角φ与前轮中心转向角之间的传动比。
在坐标系XOY中，前后轴中心点位置Mr和Mf坐标关系为：
变换为：
对上式进行微分：
一般情况下，泊车速度比较低，可以假定无滑轮现象，即后轮的轨迹在车辆垂直方向速度为0，其方程式表示为：
y′rcosθ-x′rsinθ＝0 (6)
将(5)式代入(6)中得：
x′f·sinθ-y′f·cosθ+θ′·l＝0 (7)
由图2可知，前轮轴中心点Mf处的x,y方向速度为：
将(8)式代入(7)中可得到车辆回转角速度：
将(8)式和(9)代入(5)得到后轮轴中间点Mr在x,y方向速度为：
其中vf为前轮中心处Mf的速度。为了通过左右前轮瞬时位移ΔsfL、ΔsfR，求出vf，即可得到后轮轴中间点Mr的运动轨迹。
设前轮中心转向角在瞬时不变，车辆瞬时沿作圆轨道运动，瞬时前进的距离分别为：ΔsfL、ΔsfR。
为了简化计算，设前轮中心处Mf的速度vf为左右前轮瞬时速度的平均值，即：
在Δt时间内，由：
将(11)式代入(12)(13)式，整理得到：
通过单位时间内左右前轮运动行程ΔsfL、ΔsfR和方向盘转角φ，计算车身方位角增量和后轴中点位置增量，分别对车身方位角增量和位置增量累加，可得出车辆相对起始状态的位姿。式中由公式(2)得到，l为车辆前后轴距。
(2)已知左右后轮轮速传感器数据推算车辆运动位姿
由CAN总线得到后轮左右两轮的轮速传感器给出的车轮转动距离ΔsrL和ΔsrR。如图3所示，对于后轮中心Mr，其瞬时速度为vr，满足如下公式：
其中航向角θ为：
为了简化计算，设后轮中心处Mr的速度vr为左右后轮瞬时速度的平均值，即：
将(17)式代入(15)式，合并(16)式，整理得：
式中w为后轮轴轮距。
实施例2
本发明的实施例2提供一种自动泊车***，所述泊车***采用实施例1所述的航迹推算定位方法。
需要说明的是，在整个申请文件中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实例的说明只是用于帮助理解本发明的方法及其核心思想。以上仅是本发明的优选实施方式，应当指出，由于文字表达的有限性，而客观上存在无限的试试方式，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进、润饰或变化，也可以将上述技术特征以适当的方式进行组合；这些改进润饰、变化或组合，或未经改进将发明的构思和技术方案直接应用于其它场合的，均应视为本发明的保护范围。
Claims (10)
1.基于超声波传感器和轮速传感器的航迹推算定位方法，其特征在于，包括步骤：
S1.启动泊车***，获取超声波传感器数据和轮速传感器数据，建立泊车坐标系；
S2.进行泊车位空间尺寸计算，并判断是否满足平行泊车位或垂直泊车位空间尺寸标准，倘若是，进入步骤S3，否则重新进行泊车位空间尺寸计算；
S3.进行泊车车辆初始位置计算，获取左右后轮轮速传感器数据，或者获取左右前轮轮速传感器及方向盘转角信息，并计算后轮轴中心点运动轨迹。
2.根据权利要求1所述的航迹推算定位方法，其特征在于，步骤S2中平行泊车位空间尺寸标准的确定方法如下：
根据自动泊车***的路径规划分析与推算，采用双圆弧叠加进行轨迹规划的平行泊车最小泊车位空间尺寸为：a×b，采用B样条进行轨迹规划的平行泊车最小泊车位空间尺寸为：c×d；为了提高道路两侧车位的利用率，当车位满足a×(b-0.2)或c×(d-0.2)时，认为是有效车位；设定目标车位长度为：lco，宽度为：lko，实测车位长度为：Ic；根据车位长度确定路径规划方法，如果实测车位长度lc≥a时，泊车***采用双圆弧叠加进行轨迹规划，即lco＝a，lko＝b；如果实测车位长度c≤lc≤a时，泊车***采用B样条进行轨迹规划，即lco＝c，lko＝d；以上两种泊车方式都采用以前车为车辆停放基准；
步骤S2中垂直泊车位空间尺寸标准的确定方法如下：
设定目标车位长度为：lco，宽度为：lko；根据自动泊车***的路径规划分析与推算，采用双圆弧叠加进行轨迹规划的垂直泊车最小泊车位空间尺寸为：e×f，采用B样条进行轨迹规划的平行泊车最小泊车位空间尺寸为：g×h；为了提高道路两侧车位的利用率，当车位满足(e-0.2)×f或(g-0.2)×h时，认为是有效车位。
3.根据权利要求1所述的航迹推算定位方法，其特征在于，步骤S3包括如下步骤：进行泊车车辆初始位置计算，根据数据异常检测算法检测前轮数据，若前轮数据正常，则用前轮数据计算后轮轴中心点运动轨迹；若前轮数据异常，则用后轮数据计算后轮轴中心点运动轨迹。
4.根据权利要求3所述的航迹推算定位方法，其特征在于，步骤S3中采用后轮数据计算后轮轴中心点运动轨迹的具体方法是：进行泊车车辆初始位置计算，获取左右后轮轮速传感器数据，推算后轮轴中心点速度vr，推算车辆航向角θ，计算后轮轴中心点运动轨迹(xr(t),yr(t))。
5.根据权利要求4所述的航迹推算定位方法，其特征在于，由CAN总线得到后轮左右两轮的轮速传感器给出的车轮转动距离ΔsrL和ΔsrR，对于后轮中心Mr，其瞬时速度为vr，满足如下公式：
其中航向角θ为：
为了简化计算，设后轮中心处Mr的速度vr为左右后轮瞬时速度的平均值，即：
将(3)式代入(1)式，合并(2)式，整理得：
式中w为后轮轴轮距。
6.根据权利要求3所述的航迹推算定位方法，其特征在于，步骤S3中采用前轮数据计算后轮轴中心点运动轨迹的具体方法是：进行泊车车辆初始位置计算，获取左右前轮轮速传感器及方向盘转角信息，计算前轮中心转向角计算前轮轴中心点速度vf，推算后轮轴中心点速度vr，推算车辆航向角θ，计算后轮轴中心点运动轨迹(xr(t),yr(t))。
7.根据权利要求6所述的航迹推算定位方法，其特征在于，由CAN总线得到前轮左右两轮的轮速传感器给出的车轮转动距离ΔsfL和ΔsfR，方向盘转向角为φ可知，由方向盘转向角为φ得前轮中心转向角
其中k为方向盘转角φ与前轮中心转向角之间的传动比；
在坐标系XOY中，前后轴中心点位置Mr和Mf坐标关系为：
变换为：
对上式进行微分：
假定无滑轮现象，即后轮的轨迹在车辆垂直方向速度为0，其方程式表示为：
y′rcosθ-x′rsinθ＝0 (9)
将(8)式代入(9)中得：
x′f·sinθ-y′f·cosθ+θ′·l＝0 (10)
前轮轴中心点Mf处的x,y方向速度为：
将(11)式代入(10)中可得到车辆回转角速度：
将(11)式和(12)代入(7)得到后轮轴中间点Mr在x,y方向速度为：
其中vf为前轮中心处Mf的速度；为了通过左右前轮瞬时位移ΔsfL、ΔsfR，求出vf，即得到后轮轴中间点Mr的运动轨迹；
设前轮中心转向角在瞬时不变，车辆瞬时沿规划轨迹作圆轨道运动，瞬时前进的距离分别为：ΔsfL、ΔsfR。
为了简化计算，设前轮中心处Mf的速度vf为左右前轮瞬时速度的平均值，即：
在Δt时间内，由：
将(14)式代入(15)(16)式，整理得到：
通过单位时间内左右前轮运动行程ΔsfL、ΔsfR和方向盘转角φ，计算车身方位角增量和后轴中点位置增量，分别对车身方位角增量和位置增量累加，得出车辆相对起始状态的位姿；式中由公式(5)得到，l为车辆前后轴距。
8.根据权利要求1所述的航迹推算定位方法，其特征在于，步骤S3中泊车车辆初始位置的计算方法如下：
泊车***根据实测车位长度，确定泊车方式和目标车位尺寸；根据实测车位宽度，采用以前车为车辆停放基准；坐标系的原点以开始启动自动泊车时为基准，建立坐标系并计算车辆初始位置，其车辆初始坐标为：
在(18)式中，Δs＝(xs2-xs1-lco)/2，Lrad为测位超声波安装点到后轴距离，yrd1为超声波测得最小侧向距离，Lk为车辆宽度，x0、y0为车辆以泊车位外侧边界线为基准停车时，车辆泊车起点坐标。
9.根据权利要求8所述的航迹推算定位方法，其特征在于，车位长度和车位宽度的检测方法如下：
泊车***开启后，泊车***通过超声波传感器检测右侧车位，根据超声波测距的变化，判断车位起始点与终止点，并记录对应车轮里程xs1、xs2；车辆里程位xs1～xs2时，记超声波测距最小值为yrd2，车轮里程大于xs2时，记超声波测距最小值为yrd1；
若车位满足xs2-xs1≥a，且yrd2-yrd1≥b，或者车位满足xs2-xs1≥c，且yrd2-yrd1≥d，泊车***能通过人机交互***，确定泊车方位，车辆进入泊车起始区域后，根据超声波传感器测距更新yrd1；车辆停止运动后，泊车***记录停车时车辆里程xs3；泊车***根据超声波测距及车辆里程，得到车位长度lc＝xs2-xs1，车位宽度lk＝yrd2-yrd1。
10.一种自动泊车***，其特征在于，所述泊车***采用权利要求1-9任意一项所述的航迹推算定位方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910711364.3A CN110307850A (zh) | 2019-08-02 | 2019-08-02 | 航迹推算定位方法及自动泊车*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910711364.3A CN110307850A (zh) | 2019-08-02 | 2019-08-02 | 航迹推算定位方法及自动泊车*** |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110307850A true CN110307850A (zh) | 2019-10-08 |
Family
ID=68082946
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910711364.3A Pending CN110307850A (zh) | 2019-08-02 | 2019-08-02 | 航迹推算定位方法及自动泊车*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110307850A (zh) |
Cited By (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110940345A (zh) * | 2019-12-19 | 2020-03-31 | 深圳南方德尔汽车电子有限公司 | 车位定位、装置、计算机设备和存储介质 |
| CN110962844A (zh) * | 2019-10-28 | 2020-04-07 | 纵目科技（上海）股份有限公司 | 一种车辆航向角修正方法及***、存储介质及终端 |
| CN111089598A (zh) * | 2019-11-25 | 2020-05-01 | 首都师范大学 | 一种基于icciu的车道级车载实时地图匹配方法 |
| CN111497828A (zh) * | 2020-02-17 | 2020-08-07 | 湖北亿咖通科技有限公司 | 一种自动泊车方法、装置、介质及设备 |
| CN111578925A (zh) * | 2019-12-25 | 2020-08-25 | 重庆自行者科技有限公司 | 一种基于imu和码盘融合的车辆定位方法 |
| CN111623767A (zh) * | 2020-04-10 | 2020-09-04 | 北京百度网讯科技有限公司 | Imu伪数据的生成方法、装置、电子设备及存储介质 |
| CN111762153A (zh) * | 2020-06-02 | 2020-10-13 | 武汉理工大学 | 基于5g通讯模式下的汽车自动泊车方法 |
| CN111854731A (zh) * | 2020-07-22 | 2020-10-30 | 中国第一汽车股份有限公司 | 一种位姿确定方法、装置、车辆及存储介质 |
| CN112193239A (zh) * | 2020-08-27 | 2021-01-08 | 武汉乐庭软件技术有限公司 | 自动泊车控制***中的轨迹计算方法、设备及存储设备 |
| CN112339748A (zh) * | 2020-11-09 | 2021-02-09 | 东风汽车集团有限公司 | 自动泊车中通过环境扫描修正车辆位姿信息的方法及装置 |
| CN112509375A (zh) * | 2020-10-20 | 2021-03-16 | 东风汽车集团有限公司 | 一种泊车动态显示方法及*** |
| CN112649013A (zh) * | 2020-12-29 | 2021-04-13 | 天津天瞳威势电子科技有限公司 | 一种可通行区域的确定方法、装置及电子设备 |
| WO2021093410A1 (zh) * | 2019-11-12 | 2021-05-20 | 深圳创维数字技术有限公司 | 车辆的控制方法、装置及计算机可读存储介质 |
| CN113887060A (zh) * | 2021-10-13 | 2022-01-04 | 英博超算(南京)科技有限公司 | 一种新型的自动泊车***车辆定位算法 |
| CN114623834A (zh) * | 2022-05-12 | 2022-06-14 | 华航导控(天津)科技有限公司 | 一种组合导航定位测量方法及测量*** |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP1908666A2 (de) * | 2006-10-07 | 2008-04-09 | Hella KG Hueck & Co. | Verfahren zur Unterstützung des Fahrers eines Kraftfahrzeuges bei einem Einparkvorgang |
| CN108423067A (zh) * | 2017-02-15 | 2018-08-21 | 联创汽车电子有限公司 | 水平泊车控制方法及控制*** |
| CN109017765A (zh) * | 2018-08-03 | 2018-12-18 | 湖北汽车工业学院 | 用于自动驾驶车辆的平行车位全自动泊车***的控制方法 |
| CN109766757A (zh) * | 2018-12-11 | 2019-05-17 | 惠州市德赛西威汽车电子股份有限公司 | 一种融合车辆和视觉信息的泊车位高精度定位方法及*** |
-
2019
- 2019-08-02 CN CN201910711364.3A patent/CN110307850A/zh active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP1908666A2 (de) * | 2006-10-07 | 2008-04-09 | Hella KG Hueck & Co. | Verfahren zur Unterstützung des Fahrers eines Kraftfahrzeuges bei einem Einparkvorgang |
| CN108423067A (zh) * | 2017-02-15 | 2018-08-21 | 联创汽车电子有限公司 | 水平泊车控制方法及控制*** |
| CN109017765A (zh) * | 2018-08-03 | 2018-12-18 | 湖北汽车工业学院 | 用于自动驾驶车辆的平行车位全自动泊车***的控制方法 |
| CN109766757A (zh) * | 2018-12-11 | 2019-05-17 | 惠州市德赛西威汽车电子股份有限公司 | 一种融合车辆和视觉信息的泊车位高精度定位方法及*** |
Cited By (20)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110962844A (zh) * | 2019-10-28 | 2020-04-07 | 纵目科技（上海）股份有限公司 | 一种车辆航向角修正方法及***、存储介质及终端 |
| WO2021093410A1 (zh) * | 2019-11-12 | 2021-05-20 | 深圳创维数字技术有限公司 | 车辆的控制方法、装置及计算机可读存储介质 |
| CN111089598A (zh) * | 2019-11-25 | 2020-05-01 | 首都师范大学 | 一种基于icciu的车道级车载实时地图匹配方法 |
| CN110940345A (zh) * | 2019-12-19 | 2020-03-31 | 深圳南方德尔汽车电子有限公司 | 车位定位、装置、计算机设备和存储介质 |
| CN110940345B (zh) * | 2019-12-19 | 2022-03-18 | 深圳南方德尔汽车电子有限公司 | 车身定位、装置、计算机设备和存储介质 |
| CN111578925A (zh) * | 2019-12-25 | 2020-08-25 | 重庆自行者科技有限公司 | 一种基于imu和码盘融合的车辆定位方法 |
| CN111497828A (zh) * | 2020-02-17 | 2020-08-07 | 湖北亿咖通科技有限公司 | 一种自动泊车方法、装置、介质及设备 |
| CN111623767A (zh) * | 2020-04-10 | 2020-09-04 | 北京百度网讯科技有限公司 | Imu伪数据的生成方法、装置、电子设备及存储介质 |
| CN111623767B (zh) * | 2020-04-10 | 2022-08-23 | 北京百度网讯科技有限公司 | 用于定位的imu伪数据的生成方法、装置、电子设备及介质 |
| CN111762153A (zh) * | 2020-06-02 | 2020-10-13 | 武汉理工大学 | 基于5g通讯模式下的汽车自动泊车方法 |
| CN111854731A (zh) * | 2020-07-22 | 2020-10-30 | 中国第一汽车股份有限公司 | 一种位姿确定方法、装置、车辆及存储介质 |
| CN111854731B (zh) * | 2020-07-22 | 2022-04-29 | 中国第一汽车股份有限公司 | 一种位姿确定方法、装置、车辆及存储介质 |
| CN112193239A (zh) * | 2020-08-27 | 2021-01-08 | 武汉乐庭软件技术有限公司 | 自动泊车控制***中的轨迹计算方法、设备及存储设备 |
| CN112509375A (zh) * | 2020-10-20 | 2021-03-16 | 东风汽车集团有限公司 | 一种泊车动态显示方法及*** |
| CN112509375B (zh) * | 2020-10-20 | 2022-03-08 | 东风汽车集团有限公司 | 一种泊车动态显示方法及*** |
| CN112339748A (zh) * | 2020-11-09 | 2021-02-09 | 东风汽车集团有限公司 | 自动泊车中通过环境扫描修正车辆位姿信息的方法及装置 |
| CN112649013A (zh) * | 2020-12-29 | 2021-04-13 | 天津天瞳威势电子科技有限公司 | 一种可通行区域的确定方法、装置及电子设备 |
| CN113887060A (zh) * | 2021-10-13 | 2022-01-04 | 英博超算(南京)科技有限公司 | 一种新型的自动泊车***车辆定位算法 |
| CN113887060B (zh) * | 2021-10-13 | 2024-05-10 | 英博超算(南京)科技有限公司 | 一种新型的自动泊车***车辆定位方法 |
| CN114623834A (zh) * | 2022-05-12 | 2022-06-14 | 华航导控(天津)科技有限公司 | 一种组合导航定位测量方法及测量*** |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110307850A (zh) | 航迹推算定位方法及自动泊车*** | |
| US11327493B1 (en) | Change detection using curve alignment | |
| CN106080744B (zh) | 自动驾驶车辆*** | |
| CN105928531B (zh) | 一种适用于无人驾驶汽车的行进路线精准生成方法 | |
| CN104537829B (zh) | 一种智能交通物理仿真平台及用于该智能交通物理仿真平台的定位方法 | |
| CN107783535A (zh) | 车辆控制装置 | |
| CN111907516B (zh) | 一种全自动泊车方法和*** | |
| CN108628324A (zh) | 基于矢量地图的无人车导航方法、装置、设备及存储介质 | |
| CN105675006B (zh) | 一种道路偏离检测方法 | |
| CN105159291B (zh) | 一种基于信息物理网的车队智能避障装置及避障方法 | |
| CN107615011A (zh) | 停车位置设定装置及方法 | |
| CN105936276A (zh) | 行驶控制装置 | |
| CN205880660U (zh) | 一种采用轮毂电机驱动的agv小车 | |
| CN105892464A (zh) | 基于固定路线的专用车自动驾驶***及其驾驶方法 | |
| CN107615201A (zh) | 自身位置估计装置及自身位置估计方法 | |
| CN105955257A (zh) | 基于固定路线的公交车自动驾驶***及其驾驶方法 | |
| CN108919802A (zh) | 无人驾驶的车辆行驶和装置 | |
| CN108961811A (zh) | 停车场车辆定位方法、***、移动终端及存储介质 | |
| CN211956223U (zh) | 一种车道变更轨迹规划*** | |
| CN113051765B (zh) | 一种基于虚拟场景变换的智能驾驶车辆车路在环测试方法 | |
| CN212965862U (zh) | 一种无人驾驶公交车智能控制***结构 | |
| CN105182971A (zh) | 一种室内环境下服务机器人的避障方法 | |
| CN111176298A (zh) | 一种无人车轨迹录制与轨迹跟踪方法 | |
| CN111413990A (zh) | 一种车道变更轨迹规划*** | |
| CN106203341A (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 | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20191008 |
|
| WD01 | Invention patent application deemed withdrawn after publication |