CN111912422A - 一种固定轨迹自动驾驶机器人路径规划方法 - Google Patents
一种固定轨迹自动驾驶机器人路径规划方法 Download PDFInfo
- Publication number
- CN111912422A CN111912422A CN202010701882.XA CN202010701882A CN111912422A CN 111912422 A CN111912422 A CN 111912422A CN 202010701882 A CN202010701882 A CN 202010701882A CN 111912422 A CN111912422 A CN 111912422A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- domain sequence
- time domain
- automatic driving
- coordinate
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 21
- 230000033001 locomotion Effects 0.000 claims abstract description 40
- 238000004458 analytical method Methods 0.000 claims abstract description 16
- 238000004364 calculation method Methods 0.000 claims abstract description 6
- 238000001228 spectrum Methods 0.000 claims description 17
- 238000012935 Averaging Methods 0.000 claims description 7
- 238000001914 filtration Methods 0.000 claims description 5
- 238000012217 deletion Methods 0.000 claims description 4
- 230000037430 deletion Effects 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 3
- 238000012937 correction Methods 0.000 abstract description 17
- 238000012545 processing Methods 0.000 abstract description 9
- 230000007774 longterm Effects 0.000 abstract description 7
- 238000005457 optimization Methods 0.000 abstract description 4
- 230000003068 static effect Effects 0.000 abstract description 3
- 230000002159 abnormal effect Effects 0.000 description 7
- 238000012544 monitoring process Methods 0.000 description 2
- 230000001502 supplementing effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000630 rising effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- 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/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
一种固定轨迹自动驾驶机器人路径规划方法，基于离散时间傅里叶变换分析及插值的固定轨迹自动驾驶机器人路径规划优化方法，实现对实时路径修正后进行重新规划，并不断更新静态规划路线，保证自动驾驶机器人平稳正确行驶。基于长期路径分析与修正，防止偶然运动状态直接修正带来的路径修正幅度过大，过于频繁，有效避免偶然错误修正，正确性较高，安全性强。采用离散时间傅里叶变换，处理效率较高，在频域进行分析处理精确度较高。由于均采用适合硬件计算的简单算法，算法较简单，运行速度快，实时性强，满足机器人嵌入式平台运行要求。
Description
技术领域
本发明涉及自动驾驶机器人技术领域，具体涉及一种固定轨迹自动驾驶机器人路径规划方法。
背景技术
固定轨迹自动驾驶机器人多采用SLAM等导航方式及DWA等路径规划器进行导航和自主路径规划，机器人根据路况、周围环境、自身状态以及实时算法结果会计算出不同的路线和运动方式，导致会出现不同的运动路线和状态，或出现运动反复异常等状态，与期望路线存在少量固定或出现概率较高的偏差。普通运动状态监控与重新规划、路线修正等方法仅能修正无规律的偶然偏差，且需要对车辆运动进行强制干预，会对机器人正常运算规划造成不必要的影响，无法从根本上解决问题。
发明内容
本发明为了克服以上技术的不足，提供了一种实时路径修正后进行重新规划，保证自动驾驶机器人平稳正确行使的方法。
本发明克服其技术问题所采用的技术方案是：
一种固定轨迹自动驾驶机器人路径规划方法，包括如下步骤：
a)对自动驾驶机器人当前次运动及当前次运动之前的m次运动的车辆运动方向角、车辆横坐标及车辆纵坐标分别按照时间排列形成时域序列x[n]；
b)对记录的车辆运动方向角、车辆横坐标及车辆纵坐标进行离散时间傅里叶变换后进行频域分析；
c)对傅里叶变换后的结果进行滤波，得到滤波后的频谱结果；
d)对滤波后的频谱结果进行离散时间傅里叶逆变换，得到车辆运动方向角、车辆横坐标及车辆纵坐标的新的时域序列y[n]；
e)当新的时域序列y[n]中的某一序列的值相对于时域序列x[n]中相同时间和相同位置出现连续缺失的大于s个值时，将时域序列x[n]中出现缺失的中心坐标点x3在新的时域序列y[n]中对应寻找出前后两个方向最近的与时域序列x[n]相同的点，前方向选取的点分别为y1、y2，后方向选取的点分别为y4、y5；
f)对y1和y2的x坐标和y坐标分别取均值得到均值坐标z1，对y4、y5的x坐标和y坐标分别取均值得到均值坐标z4；
g)将均值坐标z1作为导航规划点替代步骤e)中的y2，将均值坐标z4作为导航规划点替代步骤e)中的y4；
h)对点y1、z1进行线性拟合，从中心坐标点x3前方向选取一个距离大于a并小于b的拟合点z2，a与b均为小于0.5倍z1至x3之间距离的浮点数，其中a＜b，对点z4、y5进行线性拟合，从中心坐标点x3后方向选取一个距离大于c并小于d的拟合点z3，c与d均为小于0.5倍z4至x3之间距离的浮点数，其中c＜d；
i)将z2和z3的坐标取均值后得到z5，将z5替代步骤e)中的x3，得到优化后的新的时域序列y[n]，将优化后的新的时域序列y[n]对自动驾驶机器人重新进行路径规划。
进一步的，步骤b)中通过公式
计算得到x[n]的傅里叶变换X(ejω)，式中e为自然常数，n为车辆运动方向角、车辆横坐标及车辆纵坐标按时间序列排列的序号，ω为自动驾驶机器人的角频率，j为复数。进一步的，步骤c)中通过X(ejω)分别与理想低通滤波器和理想高通滤波器相乘得到滤波后的频谱结果Y(ejω)。
进一步的，步骤d)中通过公式
计算得到新的时域序列y[n]。
优选的，步骤e)中s取值为5。
本发明的有益效果是：基于离散时间傅里叶变换分析及插值的固定轨迹自动驾驶机器人路径规划优化方法，通过对当前次运动及之前一段时间的车辆运动方向角、横、纵坐标分别按照时间序列进行离散时间傅里叶变换，进行频域分析、低通、高通滤波。正常运动下的三个频谱均比较稳定，将处理结果结合相应时域点进行对比分析，对出现方向角频谱长期、密集出现异常高频并被滤除的方向角所在坐标点，或出现横、纵坐标频谱长期、密集出现异常低频并被滤除的坐标点，从附近前后两个方向进行线性拟合，并取均值后代替异常点对路径规划序列进行插值，对实时路径修正后进行重新规划，并不断更新静态规划路线，保证自动驾驶机器人平稳正确行驶。基于长期路径分析与修正，防止偶然运动状态直接修正带来的路径修正幅度过大，过于频繁，有效避免偶然错误修正，正确性较高，安全性强。采用离散时间傅里叶变换，处理效率较高，在频域进行分析处理精确度较高。由于均采用适合硬件计算的简单算法，算法较简单，运行速度快，实时性强，满足机器人嵌入式平台运行要求。
附图说明
图1为本发明的路径规划方法示意图。
具体实施方式
下面结合附图1对本发明做进一步说明。
一种固定轨迹自动驾驶机器人路径规划方法，包括如下步骤：
a)如附图1所示，通过计算和处理适合于高速ARM、FPGA等机器人嵌入式平台进行，对自动驾驶机器人当前次运动及当前次运动之前的m次运动的车辆运动方向角、车辆横坐标及车辆纵坐标分别按照时间排列形成时域序列x[n]。
b)对记录的车辆运动方向角、车辆横坐标及车辆纵坐标进行离散时间傅里叶变换后进行频域分析。
c)对傅里叶变换后的结果进行滤波，得到滤波后的频谱结果。
d)对滤波后的频谱结果进行离散时间傅里叶逆变换，得到车辆运动方向角、车辆横坐标及车辆纵坐标的新的时域序列y[n]。正常运动情况下时域序列x[n]与新的时域序列y[n]基本相同。
e)当新的时域序列y[n]中的某一序列的值相对于时域序列x[n]中相同时间和相同位置出现连续缺失的大于s个值时，表示此段时域序列x[n]所代表的实际运动位置出现了比较明显的运动状态偏差或反复。将时域序列x[n]中出现缺失的中心坐标点x3在新的时域序列y[n]中对应寻找出前后两个方向最近的与时域序列x[n]相同的点，前方向选取的点分别为y1、y2，后方向选取的点分别为y4、y5。f)对y1和y2的x坐标和y坐标分别取均值得到均值坐标z1，对y4、y5的x坐标和y坐标分别取均值得到均值坐标z4。
g)将均值坐标z1作为导航规划点替代步骤e)中的y2，即将z1作为导航规划点补充到新的时域序列y[n]的y1点的后面，将均值坐标z4作为导航规划点替代步骤e)中的y4，即将z4作为导航规划点补充到新的时域序列y[n]的y5点的前面。
h)对点y1、z1进行线性拟合，从中心坐标点x3前方向选取一个距离大于a并小于b的拟合点z2，a与b均为小于0.5倍z1至x3之间距离的浮点数，其中a＜b，对点z4、y5进行线性拟合，从中心坐标点x3后方向选取一个距离大于c并小于d的拟合点z3，c与d均为小于0.5倍z4至x3之间距离的浮点数，其中c＜d。
i)将z2和z3的坐标取均值后得到z5，将z5替代步骤e)中的x3，即将z5插入到新的时域序列y[n]序列的z1和z4之间，得到优化后的新的时域序列y[n]，将优化后的新的时域序列y[n]对自动驾驶机器人重新进行路径规划。
路径规划优化完成后可以根据需要进行2次以上的迭代修正，直到随运动次数上升观察车辆运动方向角、车辆横坐标及车辆纵坐标进行离散时间傅里叶变换的频谱曲线均不再出现明显的变化，即可获得稳定的规划路径。稳定运行的同时不断进行频谱监测与修正，保证机器人一直保持稳定规划和行驶。
基于离散时间傅里叶变换分析及插值的固定轨迹自动驾驶机器人路径规划优化方法，通过对当前次运动及之前一段时间的车辆运动方向角、横、纵坐标分别按照时间序列进行离散时间傅里叶变换，进行频域分析、低通、高通滤波。正常运动下的三个频谱均比较稳定，将处理结果结合相应时域点进行对比分析，对出现方向角频谱长期、密集出现异常高频并被滤除的方向角所在坐标点，或出现横、纵坐标频谱长期、密集出现异常低频并被滤除的坐标点，从附近前后两个方向进行线性拟合，并取均值后代替异常点对路径规划序列进行插值，对实时路径修正后进行重新规划，并不断更新静态规划路线，保证自动驾驶机器人平稳正确行驶。基于长期路径分析与修正，防止偶然运动状态直接修正带来的路径修正幅度过大，过于频繁，有效避免偶然错误修正，正确性较高，安全性强。采用离散时间傅里叶变换，处理效率较高，在频域进行分析处理精确度较高。由于均采用适合硬件计算的简单算法，算法较简单，运行速度快，实时性强，满足机器人嵌入式平台运行要求。
进一步的，步骤b)中通过公式
计算得到x[n]的傅里叶变换X(ejω)，式中e为自然常数，n为车辆运动方向角、车辆横坐标及车辆纵坐标按时间序列排列的序号，ω为自动驾驶机器人的角频率，j为复数。进一步的，步骤c)中通过X(ejω)分别与理想低通滤波器和理想高通滤波器相乘得到滤波后的频谱结果Y(ejω)。
进一步的，步骤d)中通过公式
计算得到新的时域序列y[n]。
优选的，步骤e)中s取值为5。
以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
Claims (5)
1.一种固定轨迹自动驾驶机器人路径规划方法，其特征在于，包括如下步骤：
a)对自动驾驶机器人当前次运动及当前次运动之前的m次运动的车辆运动方向角、车辆横坐标及车辆纵坐标分别按照时间排列形成时域序列x[n]；
b)对记录的车辆运动方向角、车辆横坐标及车辆纵坐标进行离散时间傅里叶变换后进行频域分析；
c)对傅里叶变换后的结果进行滤波，得到滤波后的频谱结果；
d)对滤波后的频谱结果进行离散时间傅里叶逆变换，得到车辆运动方向角、车辆横坐标及车辆纵坐标的新的时域序列y[n]；
e)当新的时域序列y[n]中的某一序列的值相对于时域序列x[n]中相同时间和相同位置出现连续缺失的大于s个值时，将时域序列x[n]中出现缺失的中心坐标点x3在新的时域序列y[n]中对应寻找出前后两个方向最近的与时域序列x[n]相同的点，前方向选取的点分别为y1、y2，后方向选取的点分别为y4、y5；
f)对y1和y2的x坐标和y坐标分别取均值得到均值坐标z1，对y4、y5的x坐标和y坐标分别取均值得到均值坐标z4；
g)将均值坐标z1作为导航规划点替代步骤e)中的y2，将均值坐标z4作为导航规划点替代步骤e)中的y4；
h)对点y1、z1进行线性拟合，从中心坐标点x3前方向选取一个距离大于a并小于b的拟合点z2，a与b均为小于0.5倍z1至x3之间距离的浮点数，其中a＜b，对点z4、y5进行线性拟合，从中心坐标点x3后方向选取一个距离大于c并小于d的拟合点z3，c与d均为小于0.5倍z4至x3之间距离的浮点数，其中c＜d；
i)将z2和z3的坐标取均值后得到z5，将z5替代步骤e)中的x3，得到优化后的新的时域序列y[n]，将优化后的新的时域序列y[n]对自动驾驶机器人重新进行路径规划。
2.根据权利要求1所述的固定轨迹自动驾驶机器人路径规划方法，其特征在于：步骤b)中通过公式
计算得到x[n]的傅里叶变换X(ejω)，式中e为自然常数，n为车辆运动方向角、车辆横坐标及车辆纵坐标按时间序列排列的序号，ω为自动驾驶机器人的角频率，j为复数。
3.根据权利要求2所述的固定轨迹自动驾驶机器人路径规划方法，其特征在于：步骤c)中通过X(ejω)分别与理想低通滤波器和理想高通滤波器相乘得到滤波后的频谱结果Y(ej ω)。
4.根据权利要求3所述的固定轨迹自动驾驶机器人路径规划方法，其特征在于：步骤d)中通过公式
计算得到新的时域序列y[n]。
5.根据权利要求1所述的固定轨迹自动驾驶机器人路径规划方法，其特征在于：步骤e)中s取值为5。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010701882.XA CN111912422B (zh) | 2020-07-20 | 2020-07-20 | 一种固定轨迹自动驾驶机器人路径规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010701882.XA CN111912422B (zh) | 2020-07-20 | 2020-07-20 | 一种固定轨迹自动驾驶机器人路径规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111912422A true CN111912422A (zh) | 2020-11-10 |
| CN111912422B CN111912422B (zh) | 2023-11-07 |
Family
ID=73280496
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010701882.XA Active CN111912422B (zh) | 2020-07-20 | 2020-07-20 | 一种固定轨迹自动驾驶机器人路径规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111912422B (zh) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101063302B1 (ko) * | 2010-10-05 | 2011-09-07 | 국방과학연구소 | 무인차량의 자율주행 제어 장치 및 방법 |
| CN102853842A (zh) * | 2012-05-15 | 2013-01-02 | 董路 | 导航路径的规划方法、装置及系统 |
| CN108827331A (zh) * | 2018-06-27 | 2018-11-16 | 西南交通大学 | 一种基于邻域系统的智能车辆轨迹规划方法 |
| CN111289008A (zh) * | 2020-04-28 | 2020-06-16 | 南京维思科汽车科技有限公司 | 一种用于无人驾驶车辆的局部路径规划算法 |
-
2020
- 2020-07-20 CN CN202010701882.XA patent/CN111912422B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101063302B1 (ko) * | 2010-10-05 | 2011-09-07 | 국방과학연구소 | 무인차량의 자율주행 제어 장치 및 방법 |
| CN102853842A (zh) * | 2012-05-15 | 2013-01-02 | 董路 | 导航路径的规划方法、装置及系统 |
| CN108827331A (zh) * | 2018-06-27 | 2018-11-16 | 西南交通大学 | 一种基于邻域系统的智能车辆轨迹规划方法 |
| CN111289008A (zh) * | 2020-04-28 | 2020-06-16 | 南京维思科汽车科技有限公司 | 一种用于无人驾驶车辆的局部路径规划算法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111912422B (zh) | 2023-11-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107618503B (zh) | 一种自动泊车控制方法及系统 | |
| CN107608344A (zh) | 基于轨迹规划的车辆运动控制方法、装置及相关设备 | |
| CN106017486B (zh) | 一种面向无人车导航的基于轨迹拐点滤波的地图定位方法 | |
| CN109154821A (zh) | 轨迹生成方法、装置和无人驾驶地面车辆 | |
| CN111158379B (zh) | 一种方向盘零偏自学习的无人车轨迹跟踪方法 | |
| CN110955262B (zh) | 光伏组件清洁机器人的路径规划与跟踪的控制方法及系统 | |
| WO2023093594A1 (zh) | 一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置 | |
| CN107045347A (zh) | 用于农机无人驾驶的自动掉头路径规划及其控制方法 | |
| CN107509443A (zh) | 一种智能割草机的坡地行驶控制方法及系统 | |
| CN111308884A (zh) | 一种全向移动agv多舵轮协同控制方法 | |
| CN111982091A (zh) | 一种基于同步imu的激光点云畸变纠正方法 | |
| CN111912422A (zh) | 一种固定轨迹自动驾驶机器人路径规划方法 | |
| CN114523477A (zh) | 关节位姿的校准方法、系统及存储介质 | |
| CN113467475B (zh) | 一种麦克纳姆轮全向移动机器人轨迹跟踪滑模控制方法 | |
| CN111595354A (zh) | 一种自适应动态观测域的ekf-slam算法 | |
| CN113985890A (zh) | 一种基于神经网络的轮式机器人自适应轨迹跟踪控制方法 | |
| CN110068832A (zh) | 一种激光导航agv的高精度定位方法 | |
| CN206373520U (zh) | 焊道偏差跟踪装置及焊接小车 | |
| CN111674406A (zh) | 自动驾驶系统车辆横向控制的方法 | |
| CN113390431B (zh) | 动态生成参考线的方法、装置、计算机设备和存储介质 | |
| CN115562304A (zh) | 一种基于深度点云的移动机器人视觉伺服控制方法和系统 | |
| CN115167429A (zh) | 一种移动机器人位置和姿态同时规划方法 | |
| Wu et al. | Trajectory tracking design of multi-robot formation based on leader-follower | |
| CN114227655A (zh) | 确定规划路径的方法、装置、scara机器人及介质 | |
| CN111609850A (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 | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20230926 Address after: 250000 building S02, No. 1036, Gaoxin Inspur Road, Jinan, Shandong Applicant after: Shandong Inspur Scientific Research Institute Co.,Ltd. Address before: 250104 1st floor, R & D building, No. 2877, Suncun Town, Licheng District, Jinan City, Shandong Province Applicant before: JINAN INSPUR HIGH-TECH TECHNOLOGY DEVELOPMENT Co.,Ltd. |
|
| TA01 | Transfer of patent application right | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |