├── src ├── LocalPathPlanning │ └── FSAE_PathPlanning │ │ ├── images │ │ ├── path.png │ │ ├── PanoExp.png │ │ ├── Flowchart.png │ │ ├── Bézier_Curve.png │ │ ├── after_filtering.png │ │ ├── paths_preview.png │ │ ├── traffic_cones.png │ │ ├── before_filtering.png │ │ ├── sensor_schematic.png │ │ └── real_time_planning.jpg │ │ ├── README.md │ │ ├── FSAE_local_path_planning.py │ │ └── test_data.py └── utilities │ ├── coordinate_transformation.py │ ├── numpy_additional_funcs.py │ └── discrete_curve_funcs.py ├── .pre-commit-config.yaml ├── pyproject.toml ├── .gitignore ├── README_en.md ├── README.md ├── environment.yml ├── requirements.txt └── LICENSE /src/LocalPathPlanning/FSAE_PathPlanning/images/path.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/muziing/PythonAutomatedDriving/HEAD/src/LocalPathPlanning/FSAE_PathPlanning/images/path.png -------------------------------------------------------------------------------- /src/LocalPathPlanning/FSAE_PathPlanning/images/PanoExp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/muziing/PythonAutomatedDriving/HEAD/src/LocalPathPlanning/FSAE_PathPlanning/images/PanoExp.png -------------------------------------------------------------------------------- /src/LocalPathPlanning/FSAE_PathPlanning/images/Flowchart.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/muziing/PythonAutomatedDriving/HEAD/src/LocalPathPlanning/FSAE_PathPlanning/images/Flowchart.png -------------------------------------------------------------------------------- /src/LocalPathPlanning/FSAE_PathPlanning/images/Bézier_Curve.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/muziing/PythonAutomatedDriving/HEAD/src/LocalPathPlanning/FSAE_PathPlanning/images/Bézier_Curve.png -------------------------------------------------------------------------------- /src/LocalPathPlanning/FSAE_PathPlanning/images/after_filtering.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/muziing/PythonAutomatedDriving/HEAD/src/LocalPathPlanning/FSAE_PathPlanning/images/after_filtering.png -------------------------------------------------------------------------------- /src/LocalPathPlanning/FSAE_PathPlanning/images/paths_preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/muziing/PythonAutomatedDriving/HEAD/src/LocalPathPlanning/FSAE_PathPlanning/images/paths_preview.png -------------------------------------------------------------------------------- /src/LocalPathPlanning/FSAE_PathPlanning/images/traffic_cones.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/muziing/PythonAutomatedDriving/HEAD/src/LocalPathPlanning/FSAE_PathPlanning/images/traffic_cones.png -------------------------------------------------------------------------------- /src/LocalPathPlanning/FSAE_PathPlanning/images/before_filtering.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/muziing/PythonAutomatedDriving/HEAD/src/LocalPathPlanning/FSAE_PathPlanning/images/before_filtering.png -------------------------------------------------------------------------------- /src/LocalPathPlanning/FSAE_PathPlanning/images/sensor_schematic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/muziing/PythonAutomatedDriving/HEAD/src/LocalPathPlanning/FSAE_PathPlanning/images/sensor_schematic.png -------------------------------------------------------------------------------- /src/LocalPathPlanning/FSAE_PathPlanning/images/real_time_planning.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/muziing/PythonAutomatedDriving/HEAD/src/LocalPathPlanning/FSAE_PathPlanning/images/real_time_planning.jpg -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | fail_fast: false 2 | 3 | repos: 4 | - repo: https://github.com/psf/black 5 | rev: 24.2.0 6 | hooks: 7 | - id: black 8 | args: [--config, "./pyproject.toml"] 9 | 10 | - repo: https://github.com/pycqa/isort 11 | rev: 5.13.2 12 | hooks: 13 | - id: isort 14 | args: [--settings-path, "./pyproject.toml"] 15 | 16 | - repo: https://github.com/astral-sh/ruff-pre-commit 17 | rev: v0.2.2 18 | hooks: 19 | - id: ruff 20 | 21 | - repo: https://github.com/pre-commit/pre-commit-hooks 22 | rev: v4.5.0 23 | hooks: 24 | - id: check-toml 25 | - id: check-yaml 26 | -------------------------------------------------------------------------------- /src/utilities/coordinate_transformation.py: -------------------------------------------------------------------------------- 1 | """ 2 | 坐标系变换算法 3 | """ 4 | 5 | import math 6 | 7 | import numpy as np 8 | 9 | 10 | def trans_2d_coordinate( 11 | raw_matrix: np.ndarray, 12 | rotation: float, 13 | translation_x: np.ndarray, 14 | translation_y: np.ndarray, 15 | ) -> np.ndarray: 16 | """ 17 | 二维坐标系变换，平移+旋转 \n 18 | :param raw_matrix: 原始坐标矩阵 19 | :param rotation: 旋转角 20 | :param translation_x: x方向平移 21 | :param translation_y: y方向平移 22 | :return: 变换后的坐标 23 | """ 24 | 25 | rotation_matrix = np.array( 26 | [ 27 | [math.cos(rotation), -math.sin(rotation)], 28 | [math.sin(rotation), math.cos(rotation)], 29 | ] 30 | ) 31 | translation_matrix = np.array([translation_x, translation_y]) 32 | 33 | return np.dot(raw_matrix, rotation_matrix) + translation_matrix 34 | -------------------------------------------------------------------------------- /src/utilities/numpy_additional_funcs.py: -------------------------------------------------------------------------------- 1 | """ 2 | 一些补充numpy功能的自实现函数 3 | """ 4 | 5 | import math 6 | 7 | import numpy as np 8 | 9 | 10 | def normalize_angle(angle: float) -> float: 11 | """ 12 | 将角度转换到 [-pi, pi] 范围中 \n 13 | :param angle: 弧度制角度 14 | :return: [-pi, pi] 范围内的弧度制角度 15 | """ 16 | 17 | return np.arctan2(np.sin(angle), np.cos(angle)) 18 | 19 | 20 | def find_array_nearest(array_1d, value, mode: int = 1, *, sorter=None) -> int: 21 | """ 22 | 在一维数组中搜索与给定值最接近的值的索引 \n 23 | ref: https://stackoverflow.com/questions/2566412 24 | :param array_1d: 一维数组 25 | :param value: 待搜索的值 26 | :param mode: 默认模式（mode!=0）可靠性高；快速模式（mode==0）要求数组有序或传入排序器 27 | :param sorter: 数组排序器（仅在对非有序数组使用快速模式时需要） 28 | :return: index 29 | """ 30 | 31 | array = np.array(array_1d) 32 | if mode: 33 | # 普通模式，速度慢，但数组无需排序 34 | index = np.argmin(abs(array - value)) 35 | else: 36 | # 快速模式，速度约为普通模式的3倍，要求数组有序，某些情况下可能会出错 37 | index = np.searchsorted(array, value, side="left", sorter=sorter) 38 | if index > 0 and ( 39 | index == len(array) 40 | or math.fabs(value - array[index - 1]) < math.fabs(value - array[index]) 41 | ): 42 | index -= 1 43 | 44 | return int(index) 45 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.poetry] 2 | name = "Python_Automated_Driving" 3 | version = "0.1.0" 4 | description = "Python codes for automated driving algorithms." 5 | keywords = ["automated-driving", "algorithms"] 6 | authors = ["muzing "] 7 | license = "Apache-2.0" 8 | readme = ["README.md", "README_zh.md"] 9 | repository = "https://github.com/muziing/PythonAutomatedDriving" 10 | 11 | [tool.poetry.urls] 12 | "Bug Tracker" = "https://github.com/muziing/PythonAutomatedDriving/issues" 13 | 14 | [tool.poetry.dependencies] 15 | python = ">=3.9,<3.12" 16 | numpy = "^1.25.0" 17 | scipy = "^1.11.0" 18 | matplotlib = "^3.7.0" 19 | jupyter = "^1.0.0" 20 | 21 | [tool.poetry.group.dev.dependencies] 22 | black = "^22.12.0" 23 | isort = "^5.11.0" 24 | mypy = "^1.4.1" 25 | ruff = "^0.0.275" 26 | line-profiler = "^4.0.0" 27 | pre-commit = "^3.3.3" 28 | jupyterlab = "^4.0.2" 29 | 30 | [[tool.poetry.source]] 31 | name = "tsinghua_mirror" 32 | url = "https://pypi.tuna.tsinghua.edu.cn/simple/" 33 | priority = "default" 34 | 35 | [[tool.poetry.source]] 36 | name = "PyPI" 37 | priority = "supplemental" 38 | 39 | [tool.black] 40 | line-length = 88 41 | target-version = ['py311'] 42 | 43 | [tool.isort] 44 | profile = "black" 45 | line_length = 88 46 | 47 | [tool.mypy] 48 | ignore_missing_imports = true 49 | check_untyped_defs = true 50 | 51 | [tool.ruff] 52 | # 目前版本的 Ruff 对中文字符支持不佳，对包含中文注释的代码会误认为过长， 53 | # 所以需要在配置文件中设置较大的行长度。真正的行长度控制由 Black 完成 54 | line-length = 130 55 | 56 | [build-system] 57 | requires = ["poetry-core"] 58 | build-backend = "poetry.core.masonry.api" 59 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Ruff linter 129 | .ruff_cache/ 130 | 131 | # Pyre type checker 132 | .pyre/ 133 | 134 | # PyCharm IDE 135 | .idea/ 136 | 137 | # pytype static type analyzer 138 | .pytype/ 139 | 140 | # Cython debug symbols 141 | cython_debug/ 142 | 143 | # Visual Studio Code Editor 144 | .vscode/ 145 | 146 | # line-profiler result files 147 | *.lprof 148 | -------------------------------------------------------------------------------- /README_en.md: -------------------------------------------------------------------------------- 1 |

2 | Automated driving algorithms 3 |

4 |

5 | Python codes for automated driving algorithms. 6 |

7 |

8 | 简体中文 | English 9 |

10 | 11 | [![GitHub Repo stars](https://img.shields.io/github/stars/muziing/PythonAutomatedDriving)](https://github.com/muziing/PythonAutomatedDriving) 12 | ![License](https://img.shields.io/github/license/muziing/PythonAutomatedDriving) 13 | 14 | [![Poetry](https://img.shields.io/endpoint?url=https://python-poetry.org/badge/v0.json)](https://python-poetry.org/) 15 | [![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black) 16 | [![Ruff](https://img.shields.io/endpoint?url=https://raw.githubusercontent.com/charliermarsh/ruff/main/assets/badge/v2.json)](https://github.com/astral-sh/ruff) 17 | [![Checked with mypy](https://www.mypy-lang.org/static/mypy_badge.svg)](https://mypy-lang.org/) 18 | 19 | ## How to use 20 | 21 | ### 1. Get source code 22 | 23 | 1. Add star to this repo if you like it :smiley: 24 | 2. Get source code by one of the following methods 25 | - [Download code zip](https://github.com/muziing/PythonAutomatedDriving/archive/refs/heads/main.zip) 26 | - (For development only) clone repo `git clone https://github.com/muziing/PythonAutomatedDriving.git` 27 | 3. Go to the project directory `cd /your/path/to/PythonAutomatedDriving` 28 | 29 | ### 2. Creating Virtual Environment and Install requirements 30 | 31 | **Option A**: venv & pip 32 | 33 | 1. Ensure that the Python version matches the one required in [pyproject.toml](./pyproject.toml). 34 | 2. Create virtual environment 35 | - Windows: `python -m venv --upgrade-deps venv` 36 | - Linux/macOS: `python3 -m venv --upgrade-deps venv` 37 | 3. Activate virtual environment 38 | - Windows: `venv\Scripts\activate` 39 | - Linux/macOS: `. venv/bin/activate` 40 | 4. Install the requirements `pip install -r requirements.txt` 41 | 42 | **Option B**: [Poetry](https://python-poetry.org) 43 | 44 | 1. Make sure the Python version is the same as required in [pyproject.toml](./pyproject.toml). 45 | 2. Install Poetry as instructed in the [official documentation](https://python-poetry.org/docs/#installation). 46 | 3. Create a virtual environment: `poetry env use /full/path/to/python` 47 | 4. Install dependencies: `poetry install --no-dev` 48 | 5. Use the virtual environment: `poetry shell` (or configure it in an IDE like PyCharm). 49 | 50 | **Option C**: [Anaconda](https://www.anaconda.com/) 51 | 52 | ````shell 53 | conda env create -f environment.yml 54 | ```` 55 | 56 | ### 3. Run! 57 | 58 | Execute Python script in each directory. 59 | 60 | ## Table of Contents 61 | 62 | ### [AuxiliaryFunctions](AuxiliaryFunctions) 63 | 64 | - [curvature](AuxiliaryFunctions/curvature.py) calculate curvature 65 | 66 | ### [LocalPathPlanning](LocalPathPlanning) 67 | 68 | - [FSAE Path Planning](LocalPathPlanning/FSAE_PathPlanning) 适用于 FSAE 无人赛车高速循迹项目的路径规划算法 69 | 70 | ## License 71 | 72 | ````text 73 | Copyright 2022-2023 muzing 74 | 75 | Licensed under the Apache License, Version 2.0 (the "License"); 76 | you may not use this file except in compliance with the License. 77 | You may obtain a copy of the License at 78 | 79 | https://www.apache.org/licenses/LICENSE-2.0 80 | 81 | Unless required by applicable law or agreed to in writing, software 82 | distributed under the License is distributed on an "AS IS" BASIS, 83 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 84 | See the License for the specific language governing permissions and 85 | limitations under the License. 86 | ```` 87 | -------------------------------------------------------------------------------- /src/LocalPathPlanning/FSAE_PathPlanning/README.md: -------------------------------------------------------------------------------- 1 | # FSAE 无人赛车路径规划算法 2 | 3 | 参加 CIAC2022 算法挑战赛的过程中，在[冀速车队](https://gitee.com/jscd_1)的小伙伴们的帮助下，开发了此用于 FSAE 无人赛车高速循迹项目的局部路径规划算法。由于时间仓促、缺乏经验，此算法仍有诸多不足之处。现将代码全部[开源](https://github.com/muziing/PythonAutomatedDriving/tree/main/LocalPathPlanning/FSAE_PathPlanning)，旨在抛砖引玉、增加交流。 4 | 5 | ![实时仿真效果](images/real_time_planning.jpg) 6 | 7 | ## 背景 8 | 9 | ### 赛道布局 10 | 11 | - 直线：不超过80m 12 | - 定半径弯：直径最大50m 13 | - 发夹弯：最小外径9m（弯折处） 14 | - 最小车道宽度为3m 15 | - 复杂赛道：连续弯道、减小半径转弯等 16 | - 赛道左侧边界标记为红色锥桶 17 | - 赛道右侧边界标记为蓝色锥桶 18 | - 赛道两侧不存在边界线 19 | - 赛道终点计时线后安全停车区标记为两侧黄色锥桶 20 | - 同侧锥桶间最大距离为5m 21 | - 在转角处，同侧锥桶间的距离会略有缩小，以提供更好的识别效果 22 | - 赛道标识锥桶根据具体情况会进行间距与宽度调整，且不保证一一对应的状态 23 | 24 | ### 仿真平台 25 | 26 | 基于 [PanoSim](http://www.panosim.com/) 仿真平台进行测试。 27 | 28 | ![PanoSim](images/PanoExp.png) 29 | 30 | ### 传感器 31 | 32 | 传感器为赛事官方为比赛设计的「目标级雷达」，仅限于在 PanoSim 仿真平台中使用，与真实物理传感器差别较大。按此处使用方式，可将其简单理解为前视摄像头与激光雷达组合：能识别车辆行驶方向前方一定距离一定宽度内的所有锥桶、并返回每个锥桶相对车辆坐标系的位置坐标。 33 | 34 | ![传感器示意图](images/sensor_schematic.png) 35 | 36 | ## 算法详解 37 | 38 | ### 总体设计思路 39 | 40 | > 本节对应算法中的主函数 `local_path_fitting()` 41 | 42 | 算法主要流程如下图： 43 | 44 | ![流程图](images/Flowchart.png) 45 | 46 | 将离散的锥桶坐标作为控制点，可以拟合出连续平滑的贝塞尔曲线（车道边界线）。考虑到进入弯道后内侧锥桶可见情况极差、内外侧锥桶数量不保证一一对应，舍弃「分别拟合两侧车道边界线、再取中心为期望路径」的方案，而使用「将一侧车道线进行适当平移作为期望路径」方案。为便于表述，下文将用于拟合的这一侧锥桶称为“主要侧”。 47 | 48 | ### 传感器数据处理 49 | 50 | > 本节对应算法中的 `get_traffic_cone_matrix()` 函数 51 | 52 | 传感器返回的数据示例如下： 53 | 54 | ```python 55 | test_data = [ 56 | (1017, 2, 3.343387449437274, 1.8235284646302428), 57 | (1018, 2, 5.867827908680232, 0.944779519389953), 58 | (1019, 2, 7.6091855208348225, -1.1205953728195666), 59 | (1020, 2, 7.913549020138137, -3.727770636814051), 60 | (1117, 11, -0.12286178543346227, -1.6824113908721865), 61 | (1118, 11, 3.030101167823757, -1.6460322572791233), 62 | (1119, 11, 4.45810309689678, -2.812570004534515), 63 | (1218, 2, 0.2487513787190473, 1.8585494303263963), 64 | ] 65 | ``` 66 | 67 | - 格式为二维矩阵，每行含义为 `[锥桶序号, 锥桶颜色, x坐标, y坐标]` 68 | - 序号以 1000 起始，与相对位置无必然联系，原始数据严格按序号排列； 69 | - 颜色：2-左侧红锥桶、11-右侧蓝锥筒、13-起终点黄锥桶； 70 | - 坐标为车辆坐标系，单位`[m]`，感知范围：`[-1:12,-4.5:4.5]`； 71 | 72 | 使用函数 `get_traffic_cone_matrix()` 对原始数据进行处理：每次按颜色筛选出单侧锥桶，按 x 坐标重新排序，形成单侧坐标矩阵。 73 | 74 | ![锥桶坐标](images/traffic_cones.png) 75 | 76 | ### 边界线拟合 77 | 78 | > 本节对应算法中的 `get_bezier_curve()` 函数 79 | 80 | 由高速循迹赛道的复杂性（变半径弯道、连续转弯、发卡弯等），不宜使用最小二乘法拟合直线与定半径圆。故先以折线法连接锥桶坐标，再用贝塞尔曲线进行平滑处理。 81 | 82 | ![贝塞尔曲线](images/Bézier_Curve.png) 83 | 84 | 贝塞尔曲线连接点平滑、描述曲线特性能力强，如图所示，基本反映了车道边界线情况。 85 | 86 | ### 车道宽度计算 87 | 88 | > 本节对应算法中的 `get_road_width()` 函数 89 | 90 | 按[赛道布局](#赛道布局)要求，车道宽度最小值为3m，非固定值，需要计算当前路段的车道宽度，为计算平移距离提供依据。对于可同时感知到车道两侧锥桶的情况，以车道线间最小距离为车道宽度；对于只能感知到一侧锥桶的情况，则近似认为车道宽度为 3m。 91 | 92 | [`scipy.spatial.distance`](https://docs.scipy.org/doc/scipy/reference/spatial.distance.html) 模块中提供了多种用于计算距离的函数。其中 `cdist()` 函数用于计算两个输入集合的每一对之间的距离。 93 | 94 | 将主要侧拟合出的曲线、次要侧的锥桶两个二维向量作为参数传入 `cdist()`，返回值为次要侧锥桶坐标到曲线上各点距离，简单地认为其中的最小值即为车道宽度。 95 | 96 | ### 平移车道线 97 | 98 | > 本节对应算法中的 `get_translation()` 函数 99 | 100 | 记[车道宽度](#车道宽度计算)为 d，将主要侧车道线向内平移 d/2、向后平移 d/4 作为规划路径。 101 | 102 | ![路径规划效果](images/path.png) 103 | 104 | ### 排除异常锥桶干扰 105 | 106 | > 本节对应算法中的 `filter_traffic_cone()` 函数 107 | 108 | 由于传感器探测范围较宽，在急弯、特别是掉头弯处可能会出现非本侧车道边界锥桶被检测到，而严重影响路径规划的情况。 109 | 110 | ![过滤前的异常锥桶情况](images/before_filtering.png) 111 | 112 | 提出一种简易过滤算法：首先根据与车辆最近锥桶 y 坐标变化方向判断弯道方向（左转、右转或直行），然后反复遍历所有锥桶计算 Δy，每次剔除首个 y 坐标变化方向相反的锥桶。 113 | 114 | 经尝试，此过滤算法可一定程度上解决识别到异常的问题，但效果仍不理想，有待进一步改进。 115 | 116 | ![过滤后效果](images/after_filtering.png) 117 | 118 | ## 总结 119 | 120 | 如图，这种方式获得的期望路径在前半段效果较好，基本处于车道中心，且光滑平顺，达到设计要求；后半段效果稍差，会有偏向主要侧的趋势，但考虑到车辆在行驶到后半段期望轨迹之前已经进行下一次局部路径规划，故可以接受。 121 | 122 | ![规划路径质量展示](images/paths_preview.png) 123 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |

2 | 自动驾驶算法 3 |

4 |

5 | 自动驾驶相关算法代码仓库，Python 语言版。 6 |

7 |

8 | 简体中文 | English 9 |

10 | 11 | [![GitHub Repo stars](https://img.shields.io/github/stars/muziing/PythonAutomatedDriving)](https://github.com/muziing/PythonAutomatedDriving) 12 | ![License](https://img.shields.io/github/license/muziing/PythonAutomatedDriving) 13 | 14 | [![Poetry](https://img.shields.io/endpoint?url=https://python-poetry.org/badge/v0.json)](https://python-poetry.org/) 15 | [![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black) 16 | [![Ruff](https://img.shields.io/endpoint?url=https://raw.githubusercontent.com/charliermarsh/ruff/main/assets/badge/v2.json)](https://github.com/astral-sh/ruff) 17 | [![Checked with mypy](https://www.mypy-lang.org/static/mypy_badge.svg)](https://mypy-lang.org/) 18 | [![pre-commit.ci status](https://results.pre-commit.ci/badge/github/muziing/PythonAutomatedDriving/main.svg)](https://results.pre-commit.ci/latest/github/muziing/PythonAutomatedDriving/main) 19 | 20 | ## 简介 21 | 22 | 本代码仓库收录若干关于「自动驾驶-规划与控制算法」相关的代码，全部由 Python 语言实现。希望能为其他 PnC 算法学习者提供一批接口清晰统一、可直接复用的代码段，避免减少重复造轮子。 23 | 24 | ## 如何使用 25 | 26 | ### 一、获取源码 27 | 28 | 1. Star 本仓库 :smiley: 29 | 2. 通过以下方式之一获取源码： 30 | - 下载代码压缩包： 31 | - （仅需要参与开发时）克隆仓库 `git clone https://github.com/muziing/PythonAutomatedDriving.git` 32 | 3. 进入项目目录 `cd /your/path/to/PythonAutomatedDriving` 33 | 34 | ### 二、配置虚拟环境与安装依赖 35 | 36 | **方式 A**： venv 与 pip 37 | 38 | 1. 确保 Python 版本与 [pyproject.toml](./pyproject.toml) 中要求的一致 39 | 2. 创建虚拟环境 40 | - Windows: `python -m venv --upgrade-deps venv` 41 | - Linux/macOS: `python3 -m venv --upgrade-deps venv` 42 | 3. 激活虚拟环境 43 | - Windows: `venv\Scripts\activate` 44 | - Linux/macOS: `. venv/bin/activate` 45 | 4. 安装依赖 `pip install -r requirements.txt` 46 | 47 | **方式 B**：[Poetry](https://python-poetry.org) 48 | 49 | 1. 确保 Python 版本与 [pyproject.toml](./pyproject.toml) 中要求的一致 50 | 2. 按[官方文档](https://python-poetry.org/docs/#installation)指示安装 Poetry 51 | 3. 创建虚拟环境：`poetry env use /full/path/to/python`（注意替换路径，使用符合版本要求的解释器） 52 | 4. 安装依赖：`poetry install --no-dev` 53 | 5. 使用该虚拟环境： `poetry shell`（或在 PyCharm 等 IDE 中配置） 54 | 55 | **方式 C**：[Anaconda](https://www.anaconda.com/) 56 | 57 | 由于主要开发与测试工作仅在 Poetry 环境中进行，conda 环境可能出现意料之外的错误。请[提交 issue](https://github.com/muziing/PythonAutomatedDriving/issues) 帮助我发现和解决这些问题。 58 | 59 | ````shell 60 | conda env create -f environment.yml 61 | ```` 62 | 63 | ### 三、运行！ 64 | 65 | 在每个子项目目录中运行 Python 脚本。 66 | 67 | ## 目录 68 | 69 | ### [utilities](src/utilities) 辅助函数 70 | 71 | - [coordinate_transformation](src/utilities/coordinate_transformation.py) - 坐标变换 72 | - [discrete_curve_funcs](src/utilities/discrete_curve_funcs.py) - 处理二维离散曲线的函数 73 | - [numpy_additional_funcs](src/utilities/numpy_additional_funcs.py) - 一些补充numpy功能的自实现函数 74 | 75 | ### [LocalPathPlanning](src/LocalPathPlanning) - 局部路径规划 76 | 77 | - [FSAE Path Planning](src/LocalPathPlanning/FSAE_PathPlanning) - 一种适用于 FSAE 无人赛车高速循迹项目的路径规划算法 78 | 79 | ## 许可协议 80 | 81 | ````text 82 | Copyright 2022-2023 muzing 83 | 84 | Licensed under the Apache License, Version 2.0 (the "License"); 85 | you may not use this file except in compliance with the License. 86 | You may obtain a copy of the License at 87 | 88 | https://www.apache.org/licenses/LICENSE-2.0 89 | 90 | Unless required by applicable law or agreed to in writing, software 91 | distributed under the License is distributed on an "AS IS" BASIS, 92 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 93 | See the License for the specific language governing permissions and 94 | limitations under the License. 95 | ```` 96 | -------------------------------------------------------------------------------- /environment.yml: -------------------------------------------------------------------------------- 1 | name: PythonAutomatedDriving 2 | channels: 3 | - defaults 4 | dependencies: 5 | - _libgcc_mutex=0.1=main 6 | - _openmp_mutex=5.1=1_gnu 7 | - appdirs=1.4.4=pyhd3eb1b0_0 8 | - blas=1.0=mkl 9 | - brotli=1.0.9=h5eee18b_7 10 | - brotli-bin=1.0.9=h5eee18b_7 11 | - brotlipy=0.7.0=py310h7f8727e_1002 12 | - bzip2=1.0.8=h7b6447c_0 13 | - ca-certificates=2023.05.30=h06a4308_0 14 | - certifi=2023.5.7=py310h06a4308_0 15 | - cffi=1.15.1=py310h5eee18b_3 16 | - charset-normalizer=2.0.4=pyhd3eb1b0_0 17 | - contourpy=1.0.5=py310hdb19cb5_0 18 | - cryptography=39.0.1=py310h9ce1e76_2 19 | - cycler=0.11.0=pyhd3eb1b0_0 20 | - dbus=1.13.18=hb2f20db_0 21 | - expat=2.4.9=h6a678d5_0 22 | - fontconfig=2.14.1=h52c9d5c_1 23 | - fonttools=4.25.0=pyhd3eb1b0_0 24 | - freetype=2.12.1=h4a9f257_0 25 | - giflib=5.2.1=h5eee18b_3 26 | - glib=2.69.1=he621ea3_2 27 | - gst-plugins-base=1.14.1=h6a678d5_1 28 | - gstreamer=1.14.1=h5eee18b_1 29 | - icu=58.2=he6710b0_3 30 | - idna=3.4=py310h06a4308_0 31 | - intel-openmp=2023.1.0=hdb19cb5_46305 32 | - jpeg=9e=h5eee18b_1 33 | - kiwisolver=1.4.4=py310h6a678d5_0 34 | - krb5=1.20.1=h143b758_1 35 | - lcms2=2.12=h3be6417_0 36 | - ld_impl_linux-64=2.38=h1181459_1 37 | - lerc=3.0=h295c915_0 38 | - libbrotlicommon=1.0.9=h5eee18b_7 39 | - libbrotlidec=1.0.9=h5eee18b_7 40 | - libbrotlienc=1.0.9=h5eee18b_7 41 | - libclang=10.0.1=default_hb85057a_2 42 | - libdeflate=1.17=h5eee18b_0 43 | - libedit=3.1.20221030=h5eee18b_0 44 | - libevent=2.1.12=hdbd6064_1 45 | - libffi=3.4.4=h6a678d5_0 46 | - libgcc-ng=11.2.0=h1234567_1 47 | - libgfortran-ng=11.2.0=h00389a5_1 48 | - libgfortran5=11.2.0=h1234567_1 49 | - libgomp=11.2.0=h1234567_1 50 | - libllvm10=10.0.1=hbcb73fb_5 51 | - libpng=1.6.39=h5eee18b_0 52 | - libpq=12.15=hdbd6064_1 53 | - libstdcxx-ng=11.2.0=h1234567_1 54 | - libtiff=4.5.0=h6a678d5_2 55 | - libuuid=1.41.5=h5eee18b_0 56 | - libwebp=1.2.4=h11a3e52_1 57 | - libwebp-base=1.2.4=h5eee18b_1 58 | - libxcb=1.15=h7f8727e_0 59 | - libxkbcommon=1.0.1=hfa300c1_0 60 | - libxml2=2.9.14=h74e7548_0 61 | - libxslt=1.1.35=h4e12654_0 62 | - lz4-c=1.9.4=h6a678d5_0 63 | - matplotlib=3.7.1=py310h06a4308_1 64 | - matplotlib-base=3.7.1=py310h1128e8f_1 65 | - mkl=2023.1.0=h6d00ec8_46342 66 | - mkl-service=2.4.0=py310h5eee18b_1 67 | - mkl_fft=1.3.6=py310h1128e8f_1 68 | - mkl_random=1.2.2=py310h1128e8f_1 69 | - munkres=1.1.4=py_0 70 | - ncurses=6.4=h6a678d5_0 71 | - nspr=4.35=h6a678d5_0 72 | - nss=3.89.1=h6a678d5_0 73 | - numpy=1.25.0=py310h5f9d8c6_0 74 | - numpy-base=1.25.0=py310hb5e798b_0 75 | - openssl=3.0.9=h7f8727e_0 76 | - packaging=23.0=py310h06a4308_0 77 | - pcre=8.45=h295c915_0 78 | - pillow=9.4.0=py310h6a678d5_0 79 | - pip=23.1.2=py310h06a4308_0 80 | - ply=3.11=py310h06a4308_0 81 | - pooch=1.4.0=pyhd3eb1b0_0 82 | - pycparser=2.21=pyhd3eb1b0_0 83 | - pyopenssl=23.0.0=py310h06a4308_0 84 | - pyparsing=3.0.9=py310h06a4308_0 85 | - pyqt=5.15.7=py310h6a678d5_1 86 | - pysocks=1.7.1=py310h06a4308_0 87 | - python=3.10.12=h955ad1f_0 88 | - python-dateutil=2.8.2=pyhd3eb1b0_0 89 | - qt-main=5.15.2=h327a75a_7 90 | - qt-webengine=5.15.9=hd2b0992_4 91 | - qtwebkit=5.212=h4eab89a_4 92 | - readline=8.2=h5eee18b_0 93 | - requests=2.29.0=py310h06a4308_0 94 | - scipy=1.10.1=py310h5f9d8c6_1 95 | - setuptools=67.8.0=py310h06a4308_0 96 | - sip=6.6.2=py310h6a678d5_0 97 | - six=1.16.0=pyhd3eb1b0_1 98 | - sqlite=3.41.2=h5eee18b_0 99 | - tbb=2021.8.0=hdb19cb5_0 100 | - tk=8.6.12=h1ccaba5_0 101 | - toml=0.10.2=pyhd3eb1b0_0 102 | - tornado=6.2=py310h5eee18b_0 103 | - tzdata=2023c=h04d1e81_0 104 | - urllib3=1.26.16=py310h06a4308_0 105 | - wheel=0.38.4=py310h06a4308_0 106 | - xz=5.4.2=h5eee18b_0 107 | - zlib=1.2.13=h5eee18b_0 108 | - zstd=1.5.5=hc292b87_0 109 | - pip: 110 | - pyqt5-sip==12.11.0 111 | prefix: /home/muzing/anaconda3/envs/PythonAutomatedDriving 112 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | anyio==3.7.1 ; python_version >= "3.9" and python_version < "3.12" 2 | appnope==0.1.3 ; python_version >= "3.9" and python_version < "3.12" and (platform_system == "Darwin" or sys_platform == "darwin") 3 | argon2-cffi-bindings==21.2.0 ; python_version >= "3.9" and python_version < "3.12" 4 | argon2-cffi==21.3.0 ; python_version >= "3.9" and python_version < "3.12" 5 | arrow==1.2.3 ; python_version >= "3.9" and python_version < "3.12" 6 | asttokens==2.2.1 ; python_version >= "3.9" and python_version < "3.12" 7 | attrs==23.1.0 ; python_version >= "3.9" and python_version < "3.12" 8 | backcall==0.2.0 ; python_version >= "3.9" and python_version < "3.12" 9 | beautifulsoup4==4.12.2 ; python_version >= "3.9" and python_version < "3.12" 10 | bleach==6.0.0 ; python_version >= "3.9" and python_version < "3.12" 11 | cffi==1.15.1 ; python_version >= "3.9" and python_version < "3.12" 12 | colorama==0.4.6 ; python_version >= "3.9" and python_version < "3.12" and sys_platform == "win32" 13 | comm==0.1.3 ; python_version >= "3.9" and python_version < "3.12" 14 | contourpy==1.1.0 ; python_version >= "3.9" and python_version < "3.12" 15 | cycler==0.11.0 ; python_version >= "3.9" and python_version < "3.12" 16 | debugpy==1.6.7 ; python_version >= "3.9" and python_version < "3.12" 17 | decorator==5.1.1 ; python_version >= "3.9" and python_version < "3.12" 18 | defusedxml==0.7.1 ; python_version >= "3.9" and python_version < "3.12" 19 | exceptiongroup==1.1.2 ; python_version >= "3.9" and python_version < "3.11" 20 | executing==1.2.0 ; python_version >= "3.9" and python_version < "3.12" 21 | fastjsonschema==2.17.1 ; python_version >= "3.9" and python_version < "3.12" 22 | fonttools==4.40.0 ; python_version >= "3.9" and python_version < "3.12" 23 | fqdn==1.5.1 ; python_version >= "3.9" and python_version < "3.12" 24 | idna==3.4 ; python_version >= "3.9" and python_version < "3.12" 25 | importlib-metadata==6.8.0 ; python_version >= "3.9" and python_version < "3.10" 26 | importlib-resources==6.0.0 ; python_version >= "3.9" and python_version < "3.10" 27 | ipykernel==6.24.0 ; python_version >= "3.9" and python_version < "3.12" 28 | ipython-genutils==0.2.0 ; python_version >= "3.9" and python_version < "3.12" 29 | ipython==8.14.0 ; python_version >= "3.9" and python_version < "3.12" 30 | ipywidgets==8.0.7 ; python_version >= "3.9" and python_version < "3.12" 31 | isoduration==20.11.0 ; python_version >= "3.9" and python_version < "3.12" 32 | jedi==0.18.2 ; python_version >= "3.9" and python_version < "3.12" 33 | jinja2==3.1.2 ; python_version >= "3.9" and python_version < "3.12" 34 | jsonpointer==2.4 ; python_version >= "3.9" and python_version < "3.12" 35 | jsonschema-specifications==2023.6.1 ; python_version >= "3.9" and python_version < "3.12" 36 | jsonschema==4.18.0 ; python_version >= "3.9" and python_version < "3.12" 37 | jsonschema[format-nongpl]==4.18.0 ; python_version >= "3.9" and python_version < "3.12" 38 | jupyter-client==8.3.0 ; python_version >= "3.9" and python_version < "3.12" 39 | jupyter-console==6.6.3 ; python_version >= "3.9" and python_version < "3.12" 40 | jupyter-core==5.3.1 ; python_version >= "3.9" and python_version < "3.12" 41 | jupyter-events==0.6.3 ; python_version >= "3.9" and python_version < "3.12" 42 | jupyter-server-terminals==0.4.4 ; python_version >= "3.9" and python_version < "3.12" 43 | jupyter-server==2.7.0 ; python_version >= "3.9" and python_version < "3.12" 44 | jupyter==1.0.0 ; python_version >= "3.9" and python_version < "3.12" 45 | jupyterlab-pygments==0.2.2 ; python_version >= "3.9" and python_version < "3.12" 46 | jupyterlab-widgets==3.0.8 ; python_version >= "3.9" and python_version < "3.12" 47 | kiwisolver==1.4.4 ; python_version >= "3.9" and python_version < "3.12" 48 | markupsafe==2.1.3 ; python_version >= "3.9" and python_version < "3.12" 49 | matplotlib-inline==0.1.6 ; python_version >= "3.9" and python_version < "3.12" 50 | matplotlib==3.7.2 ; python_version >= "3.9" and python_version < "3.12" 51 | mistune==3.0.1 ; python_version >= "3.9" and python_version < "3.12" 52 | nbclassic==1.0.0 ; python_version >= "3.9" and python_version < "3.12" 53 | nbclient==0.8.0 ; python_version >= "3.9" and python_version < "3.12" 54 | nbconvert==7.6.0 ; python_version >= "3.9" and python_version < "3.12" 55 | nbformat==5.9.0 ; python_version >= "3.9" and python_version < "3.12" 56 | nest-asyncio==1.5.6 ; python_version >= "3.9" and python_version < "3.12" 57 | notebook-shim==0.2.3 ; python_version >= "3.9" and python_version < "3.12" 58 | notebook==6.5.4 ; python_version >= "3.9" and python_version < "3.12" 59 | numpy==1.25.1 ; python_version >= "3.9" and python_version < "3.12" 60 | overrides==7.3.1 ; python_version >= "3.9" and python_version < "3.12" 61 | packaging==23.1 ; python_version >= "3.9" and python_version < "3.12" 62 | pandocfilters==1.5.0 ; python_version >= "3.9" and python_version < "3.12" 63 | parso==0.8.3 ; python_version >= "3.9" and python_version < "3.12" 64 | pexpect==4.8.0 ; python_version >= "3.9" and python_version < "3.12" and sys_platform != "win32" 65 | pickleshare==0.7.5 ; python_version >= "3.9" and python_version < "3.12" 66 | pillow==10.0.0 ; python_version >= "3.9" and python_version < "3.12" 67 | platformdirs==3.8.1 ; python_version >= "3.9" and python_version < "3.12" 68 | prometheus-client==0.17.0 ; python_version >= "3.9" and python_version < "3.12" 69 | prompt-toolkit==3.0.39 ; python_version >= "3.9" and python_version < "3.12" 70 | psutil==5.9.5 ; python_version >= "3.9" and python_version < "3.12" 71 | ptyprocess==0.7.0 ; python_version >= "3.9" and python_version < "3.12" and (os_name != "nt" or sys_platform != "win32") 72 | pure-eval==0.2.2 ; python_version >= "3.9" and python_version < "3.12" 73 | pycparser==2.21 ; python_version >= "3.9" and python_version < "3.12" 74 | pygments==2.15.1 ; python_version >= "3.9" and python_version < "3.12" 75 | pyparsing==3.0.9 ; python_version >= "3.9" and python_version < "3.12" 76 | python-dateutil==2.8.2 ; python_version >= "3.9" and python_version < "3.12" 77 | python-json-logger==2.0.7 ; python_version >= "3.9" and python_version < "3.12" 78 | pywin32==306 ; sys_platform == "win32" and platform_python_implementation != "PyPy" and python_version >= "3.9" and python_version < "3.12" 79 | pywinpty==2.0.10 ; python_version >= "3.9" and python_version < "3.12" and os_name == "nt" 80 | pyyaml==6.0 ; python_version >= "3.9" and python_version < "3.12" 81 | pyzmq==25.1.0 ; python_version >= "3.9" and python_version < "3.12" 82 | qtconsole==5.4.3 ; python_version >= "3.9" and python_version < "3.12" 83 | qtpy==2.3.1 ; python_version >= "3.9" and python_version < "3.12" 84 | referencing==0.29.1 ; python_version >= "3.9" and python_version < "3.12" 85 | rfc3339-validator==0.1.4 ; python_version >= "3.9" and python_version < "3.12" 86 | rfc3986-validator==0.1.1 ; python_version >= "3.9" and python_version < "3.12" 87 | rpds-py==0.8.10 ; python_version >= "3.9" and python_version < "3.12" 88 | scipy==1.11.1 ; python_version >= "3.9" and python_version < "3.12" 89 | send2trash==1.8.2 ; python_version >= "3.9" and python_version < "3.12" 90 | six==1.16.0 ; python_version >= "3.9" and python_version < "3.12" 91 | sniffio==1.3.0 ; python_version >= "3.9" and python_version < "3.12" 92 | soupsieve==2.4.1 ; python_version >= "3.9" and python_version < "3.12" 93 | stack-data==0.6.2 ; python_version >= "3.9" and python_version < "3.12" 94 | terminado==0.17.1 ; python_version >= "3.9" and python_version < "3.12" 95 | tinycss2==1.2.1 ; python_version >= "3.9" and python_version < "3.12" 96 | tornado==6.3.2 ; python_version >= "3.9" and python_version < "3.12" 97 | traitlets==5.9.0 ; python_version >= "3.9" and python_version < "3.12" 98 | typing-extensions==4.7.1 ; python_version >= "3.9" and python_version < "3.10" 99 | uri-template==1.3.0 ; python_version >= "3.9" and python_version < "3.12" 100 | wcwidth==0.2.6 ; python_version >= "3.9" and python_version < "3.12" 101 | webcolors==1.13 ; python_version >= "3.9" and python_version < "3.12" 102 | webencodings==0.5.1 ; python_version >= "3.9" and python_version < "3.12" 103 | websocket-client==1.6.1 ; python_version >= "3.9" and python_version < "3.12" 104 | widgetsnbextension==4.0.8 ; python_version >= "3.9" and python_version < "3.12" 105 | zipp==3.16.0 ; python_version >= "3.9" and python_version < "3.10" 106 | -------------------------------------------------------------------------------- /src/utilities/discrete_curve_funcs.py: -------------------------------------------------------------------------------- 1 | """ 2 | 离散曲线相关函数 3 | """ 4 | 5 | import math 6 | from typing import Optional, Tuple 7 | 8 | import numpy as np 9 | import numpy.typing as npt 10 | from numpy_additional_funcs import normalize_angle 11 | 12 | 13 | def find_match_point( 14 | xy_coordinate: tuple[float, float], 15 | reference_line_nodes: npt.NDArray[np.float_], 16 | *, 17 | start_index: int = 0, 18 | ) -> Tuple[int, float]: 19 | """ 20 | 曲线由若干离散点组成的点集表达，遍历曲线外某一点到该点集中每一点的距离，取距离最小的为匹配点，返回该点在点集中的索引和最小距离 21 | :param xy_coordinate: (x, y) 点的坐标 22 | :param reference_line_nodes: [[x0, y0], [x1, y1], ...] 构成参考线曲线的点集 23 | :param start_index: 从点集中的该索引位置开始匹配 24 | :return: (match_point_index, min_distance) 匹配点在曲线点集中的索引、到匹配点的距离 25 | """ 26 | 27 | x, y = xy_coordinate 28 | reference_line_length = reference_line_nodes.shape[0] 29 | increase_count = ( 30 | 0 # 用 increase_count 记录 distance 连续增大的次数，避免多个局部最小值的干扰 31 | ) 32 | min_distance_square = float("inf") 33 | match_point_index = ( 34 | start_index # 若最后仍没有找到匹配索引，则说明起始索引已经是最佳匹配，直接返回 35 | ) 36 | 37 | if start_index == 0: 38 | # 首次运行情况 39 | increase_count_limit = reference_line_length // 3 40 | direction_flag = 1 # 正向遍历 41 | elif start_index < 0: 42 | raise ValueError("index < 0") 43 | else: 44 | # 非首次运行情况 45 | increase_count_limit = 5 46 | 47 | # 上个周期匹配点坐标 48 | pre_match_point = [ 49 | reference_line_nodes[start_index, 0], 50 | reference_line_nodes[start_index, 1], 51 | ] 52 | # 以上个周期匹配点、上个周期匹配点的前一个点之间的连线的方向，近似表示切向 53 | d_x = pre_match_point[0] - reference_line_nodes[start_index - 1, 0] 54 | d_y = pre_match_point[1] - reference_line_nodes[start_index - 1, 1] 55 | pre_match_point_theta = np.arctan2(d_y, d_x) 56 | 57 | # 上个匹配点在曲线上的切向向量 58 | pre_match_point_direction = np.array( 59 | [ 60 | normalize_angle(np.cos(pre_match_point_theta)), 61 | normalize_angle(np.sin(pre_match_point_theta)), 62 | ] 63 | ) 64 | 65 | # 计算上个匹配点指向当前 (x, y) 的向量 66 | pre_match_to_xy_v = np.array([x - pre_match_point[0], y - pre_match_point[1]]) 67 | 68 | # 计算 pre_match_to_xy_v 在 pre_match_point_direction 上的投影，用于判断遍历方向 69 | direction_flag = np.dot( 70 | pre_match_to_xy_v, pre_match_point_direction 71 | ) # 大于零正反向遍历，反之，反方向遍历 72 | 73 | if direction_flag > 0: # 正向遍历 74 | search_range = (start_index, reference_line_length, 1) 75 | else: # 反向遍历 76 | search_range = (start_index, -1, -1) 77 | 78 | # 确定匹配点 79 | for i in range(*search_range): 80 | reference_line_node_x = reference_line_nodes[i][0] 81 | reference_line_node_y = reference_line_nodes[i][1] 82 | # 计算 (x, y) 与 (reference_line_node_x, reference_line_node_y) 之间的距离 83 | distance_square = (reference_line_node_x - x) ** 2 + ( 84 | reference_line_node_y - y 85 | ) ** 2 86 | if distance_square < min_distance_square: 87 | min_distance_square = distance_square # 保留最小值 88 | match_point_index = i 89 | increase_count = 0 90 | else: 91 | increase_count += 1 92 | if increase_count >= increase_count_limit: 93 | break 94 | 95 | return match_point_index, math.sqrt(min_distance_square) 96 | 97 | 98 | def get_projection_point( 99 | xy_coordinate: tuple[float, float], 100 | reference_line_nodes: npt.NDArray[np.float_], 101 | match_point_index: Optional[int] = None, 102 | ) -> tuple[float, float, float, float]: 103 | """ 104 | TODO 此函数待验证 105 | 获取某点在一条由离散点表示的参考线上的投影点信息 \n 106 | ref: https://www.bilibili.com/video/BV1EM4y137Jv 107 | :param xy_coordinate: (x, y) 笛卡尔坐标系下点的坐标 108 | :param reference_line_nodes: [[x0, y0, heading0, kappa0], ...] 参考线上的若干离散点 109 | :param match_point_index: [可选参数] 匹配点在参考线点集中的索引 110 | :return: 匹配点的 (x, y, theta, kappa) 111 | """ 112 | 113 | x, y = xy_coordinate 114 | 115 | # d_v 是匹配点（x_match, y_match）指向待投影点（x,y）的向量（x-x_match, y-y_match） 116 | # tau 是匹配点的单位切向量(cos(theta_match), sin(theta_match))' 117 | # (x_r, y_r)' 约等于 (x_match, y_match)' + (d_v . tau) * tau 118 | # kappa_r 约等于 kappa_match，投影点曲率 119 | # theta_r 约等于 theta_match + k_m * (d_v . tau)，投影点切线与坐标轴夹角 120 | 121 | if match_point_index is None: 122 | match_point_index, _ = find_match_point( 123 | xy_coordinate, 124 | reference_line_nodes[:, 0:2], 125 | start_index=0, 126 | ) 127 | 128 | # 通过匹配点确定投影点 129 | x_match, y_match, theta_match, kappa_match = reference_line_nodes[match_point_index] 130 | d_v = np.array([x - x_match, y - y_match]) # 匹配点指向待投影点的向量 131 | tau = np.array([np.cos(theta_match), np.sin(theta_match)]) # 匹配点的单位切向量 132 | ds = np.dot(d_v, tau) 133 | r_m_v = np.array([x_match, y_match]) 134 | 135 | # 计算投影点的位置信息 136 | x_r, y_r = r_m_v + ds * tau # 计算投影点坐标 137 | theta_r = normalize_angle( 138 | theta_match + kappa_match * ds 139 | ) # 计算投影点在参考线上切线与 x 轴的夹角 140 | kappa_r = kappa_match # 投影点在参考线处的曲率 141 | 142 | return x_r, y_r, theta_r, kappa_r 143 | 144 | 145 | def calculate_heading_kappa(path_points: npt.NDArray[np.float_]): 146 | """ 147 | 计算曲线上每个点的切向角 theta（与直角坐标轴x轴之间的角度）和曲率 kappa \n 148 | ref: https://github.com/6Lackiu/EMplanner_Carla/blob/4cb40d5ca04af8c49f3f7dd6b6966fa70bb7dc2d/planner/planning_utils.py#L185 149 | :param path_points: 曲线上每一点的坐标 [(x0, y0), (x1, y1), ...] 150 | :return: [[theta0, kappa0], [theta1, kappa1], ...] 151 | """ 152 | # 原理: 153 | # theta = arctan(d_y/d_x) 154 | # kappa = d_theta / d_s 155 | # d_s = (d_x^2 + d_y^2)^0.5 156 | 157 | # 用割线斜率近似表示切线斜率，参考数值微分知识 158 | # 采用中点欧拉法来计算每个点处的切线方向角： 159 | # 当前点与前一个点连成的线段的方向角、和当前点与下一点连成线段的方向角求平均值，作为该点的切线方向 160 | 161 | # TODO 将填补差分的功能提取至单独的函数中 162 | 163 | points_array = np.array(path_points) 164 | d_xy_ = ( 165 | points_array[1:, :] - points_array[:-1, :] 166 | ) # 一阶差分，此种写法比 np.diff() 性能高得多 167 | d_xy = np.empty_like(points_array) # 定义变量，预分配内存 168 | # 由于 n 个点差分得到的只有 n-1 个差分结果，所以要在首尾添加重复单元来近似求每个节点的 dx、dy 169 | d_xy[0, :] = d_xy_[:, :][0] 170 | d_xy[-1, :] = d_xy_[:, :][-1] 171 | d_xy[1:-1, :] = (d_xy_[1:, :] + d_xy_[:-1, :]) / 2 172 | 173 | # 计算切线方向角 theta 174 | theta = np.arctan2( 175 | d_xy[:, 1], d_xy[:, 0] 176 | ) # np.arctan2 会将角度限制在 (-pi, pi)之间 177 | 178 | d_theta_ = theta[1:] - theta[:-1] # 差分，这种写法比np.diff()性能高得多 179 | d_theta = np.empty_like(theta) 180 | d_theta[0] = d_theta_[0] 181 | d_theta[-1] = d_theta_[-1] 182 | # d_theta[1:-1] = (d_theta_[1:] + d_theta_[:-1]) / 2 # 准确值，但有多值性风险 183 | d_theta[1:-1] = np.sin( 184 | (d_theta_[1:] + d_theta_[:-1]) / 2 185 | ) # 认为 d_theta 是个小量，用 sin(d_theta) 代替 d_theta，避免多值性 186 | 187 | # 计算曲率 kappa 188 | d_s = np.sqrt(d_xy[:, 0] ** 2 + d_xy[:, 1] ** 2) 189 | kappa = d_theta / d_s 190 | 191 | result = np.vstack((theta, kappa)).T 192 | return result 193 | 194 | 195 | def enhance_reference_line( 196 | reference_line_node_list: npt.NDArray[np.float_], 197 | ) -> npt.NDArray[np.float_]: 198 | """ 199 | 将仅包含坐标信息的参考线增强为包含坐标、切向角、曲率的参考线 \n 200 | :param reference_line_node_list: [[x0, y0], [x1, y1], ...] 参考线上的若干离散点 201 | :return: [[x0, y0, heading0, kappa0], ...] 202 | """ 203 | 204 | heading_kappa = calculate_heading_kappa(reference_line_node_list) 205 | return np.hstack((reference_line_node_list, heading_kappa)) 206 | 207 | 208 | def calculate_curvature(points: np.ndarray) -> float: 209 | """ 210 | 曲率半径计算函数 \n 211 | ref: https://github.com/Pjer-zhang/PJCurvature \n 212 | :param points: 三个点的坐标 213 | :return: 曲率半径 214 | """ 215 | 216 | x = points[:, 0] 217 | y = points[:, 1] 218 | 219 | t_a = np.linalg.norm([x[1] - x[0], y[1] - y[0]]) 220 | t_b = np.linalg.norm([x[2] - x[1], y[2] - y[1]]) 221 | 222 | m = np.array([[1, -t_a, t_a**2], [1, 0, 0], [1, t_b, t_b**2]]) 223 | 224 | a = np.matmul(np.linalg.inv(m), x) 225 | b = np.matmul(np.linalg.inv(m), y) 226 | 227 | kappa = 2 * (a[2] * b[1] - b[2] * a[1]) / (a[1] ** 2.0 + b[1] ** 2.0) ** 1.5 228 | 229 | return kappa 230 | -------------------------------------------------------------------------------- /src/LocalPathPlanning/FSAE_PathPlanning/FSAE_local_path_planning.py: -------------------------------------------------------------------------------- 1 | """ 2 | Local path planning algorithm for FSAE race car. 3 | author: muzing 4 | doc: https://github.com/muziing/PythonAutomatedDriving 5 | 6 | 原始数据处理：对原始锥桶数据进行整理处理； 7 | 边界线拟合：由离散的锥桶位置拟合出连续平滑的车道边界线； 8 | 局部路径规划：以外侧车道线向内平移适当距离作为期望路径； 9 | """ 10 | 11 | import math 12 | import sys 13 | 14 | import numpy as np 15 | from scipy.spatial.distance import cdist, euclidean 16 | from scipy.special import comb 17 | 18 | 19 | def get_traffic_cone_matrix(raw_data_matrix: np.ndarray, color: int) -> np.ndarray: 20 | """ 21 | 处理原始数据，返回特定类型的锥桶位置矩阵 \n 22 | :param raw_data_matrix: 由传感器返回的原始数据矩阵 23 | :param color: 锥桶颜色类型，2-红 11-蓝 13-黄 24 | :return: 锥桶位置矩阵 25 | """ 26 | 27 | traffic_cone = raw_data_matrix[np.where(raw_data_matrix[:, 1] == color), 2:][0, :] 28 | 29 | if traffic_cone.size < 4: # 若只有单个锥桶，则直接返回 30 | return traffic_cone 31 | 32 | sorted_traffic_cone = sort_traffic_cone(traffic_cone, mode=0) # 使用排序算法排序 33 | # sorted_traffic_cone = sorted_traffic_cone[0:5, :] # 出于求解性能之考虑，抛弃第5个点之外的点（最高为4阶贝塞尔曲线） 34 | clean_traffic_cone = filter_traffic_cone(sorted_traffic_cone) # 剔除异常的锥桶 35 | 36 | return clean_traffic_cone 37 | 38 | 39 | def sort_traffic_cone(traffic_cone: np.ndarray, mode: int = 0) -> np.ndarray: 40 | """ 41 | 对锥桶坐标列表进行排序 \n 42 | :param traffic_cone: 待排序的锥桶位置矩阵 43 | :param mode: 选用的排序方式 44 | :return: 排序后的锥桶位置矩阵 45 | """ 46 | 47 | sorted_cones = np.zeros_like(traffic_cone) # 最终排序好的坐标矩阵 48 | 49 | if mode == 0: 50 | # 方式一：按x坐标排序 51 | sorted_cones = traffic_cone[np.lexsort(traffic_cone[:, ::-1].T)] 52 | elif mode == 1: 53 | # 方式二：距离第1个锥桶最近的为第2个锥桶，距离第2个锥桶最近的为第3个锥桶……以此类推 54 | # 创建变量 55 | sorted_index = list() # 存储所有已排序锥桶的索引 56 | unsorted_index = list(range(traffic_cone.shape[0])) # 还未排序的所有锥桶的索引 57 | min_distance = sys.maxsize # 以极大的数初始化最小距离，避免干扰 58 | index_flag = -1 59 | counter = 0 60 | 61 | # 找到第1个锥桶 62 | fist_cone_index = int(np.argmin(traffic_cone[:, 0])) # x坐标最小的作为首个锥桶 63 | sorted_index.append(fist_cone_index) 64 | unsorted_index.remove(fist_cone_index) 65 | 66 | # 距离第1个锥桶最近的为第2个锥桶，距离第2个锥桶最近的为第3个锥桶……以此类推 67 | while unsorted_index: 68 | for index in unsorted_index: 69 | # 计算欧几里德距离 70 | distance = euclidean( 71 | traffic_cone[index, :], traffic_cone[sorted_index[-1], :] 72 | ) 73 | if distance < min_distance: 74 | min_distance = distance 75 | index_flag = index 76 | sorted_index.append(index_flag) 77 | unsorted_index.remove(index_flag) 78 | min_distance = sys.maxsize 79 | 80 | # 按排序好的索引顺序拷贝数据，获得最终排序好的坐标矩阵 81 | for cone in sorted_index: 82 | sorted_cones[counter] = traffic_cone[cone, :] 83 | counter += 1 84 | 85 | return sorted_cones 86 | 87 | 88 | def filter_traffic_cone(traffic_cone: np.ndarray) -> np.ndarray: 89 | """ 90 | 过滤剔除异常的锥桶 \n 91 | :param traffic_cone: 按序排列的锥桶位置矩阵 92 | :return: 剔除异常锥桶后的位置矩阵 93 | """ 94 | 95 | delta_y = traffic_cone[1:, 1] - traffic_cone[:-1, 1] 96 | # 判断弯道方向 97 | for y in delta_y: 98 | if abs(y) < 0.25: 99 | continue 100 | else: 101 | direction = math.copysign(1.0, y) 102 | break 103 | else: 104 | direction = 0 # 如果循环结束而没有中途被break，则方向为直行 105 | 106 | # 剔除Δy异常的锥桶 107 | while direction != 0: 108 | delta_y = traffic_cone[1:, 1] - traffic_cone[:-1, 1] 109 | error_index = np.where(delta_y * direction < -0.4)[0] # 出现异常的锥桶的索引 110 | if error_index.size > 0: 111 | traffic_cone = np.delete( 112 | traffic_cone, error_index[0] + 1, axis=0 113 | ) # 移除首个异常锥桶 114 | else: 115 | break 116 | 117 | return traffic_cone 118 | 119 | 120 | def local_path_fitting( 121 | traffic_cone_l: np.ndarray, 122 | traffic_cone_r: np.ndarray, 123 | interpolation_num: int = 25, 124 | translation_dis: float = 1.5, 125 | ) -> np.ndarray: 126 | """ 127 | 局部路径规划算法 \n 128 | :param traffic_cone_l: 左侧锥桶坐标 129 | :param traffic_cone_r: 右侧锥桶坐标 130 | :param interpolation_num: 贝塞尔曲线插值点的数量（用于平滑曲线） 131 | :param translation_dis: 只有单侧锥桶时，向内平移的距离，[m] 132 | :return: 期望路径 133 | """ 134 | 135 | # 只能看到一侧锥桶时，直接对该侧进行平滑拟合、平移固定距离 136 | if traffic_cone_l.size == 0: 137 | bezier_points = get_bezier_curve(traffic_cone_r, interpolation_num) 138 | translation_points = get_translation(bezier_points, "l", translation_dis) 139 | elif traffic_cone_r.size == 0: 140 | bezier_points = get_bezier_curve(traffic_cone_l, interpolation_num) 141 | translation_points = get_translation(bezier_points, "r", translation_dis) 142 | else: 143 | # 能看到两侧锥桶时，以x方向延伸更远的一侧为主，进行平滑拟合、平移距离由计算出的路宽决定 144 | if traffic_cone_l[-1, 0] > traffic_cone_r[-1, 0]: 145 | bezier_points = get_bezier_curve(traffic_cone_l, interpolation_num) 146 | road_width = get_road_width(bezier_points, traffic_cone_r) 147 | translation_points = get_translation(bezier_points, "r", road_width / 2) 148 | else: 149 | bezier_points = get_bezier_curve(traffic_cone_r, interpolation_num) 150 | road_width = get_road_width(bezier_points, traffic_cone_l) 151 | translation_points = get_translation(bezier_points, "l", road_width / 2) 152 | 153 | path_points = bezier_points + translation_points 154 | 155 | return path_points 156 | 157 | 158 | def get_bezier_curve(points: np.ndarray, interpolation_num: int = 25) -> np.ndarray: 159 | """ 160 | 计算贝塞尔曲线 \n 161 | ref: https://zhuanlan.zhihu.com/p/409585038 \n 162 | :param points: 控制点 163 | :param interpolation_num: 控制点间的插值个数 164 | :return: 贝塞尔曲线 165 | """ 166 | 167 | # 生成对应二项式系数 168 | n = points.shape[0] - 1 169 | k = np.arange(0, n + 1, 1) 170 | b = comb(n, k) 171 | 172 | # 计算权重 173 | n = points.shape[0] - 1 174 | sep = 1 / interpolation_num 175 | t = np.arange(0, 1 + sep, sep) 176 | t = t.reshape(-1, 1) 177 | k = np.arange(0, n + 1, 1) 178 | weight = np.power(t, k) * np.power(1 - t, n - k) * b 179 | 180 | # 计算贝塞尔曲线 181 | x = np.sum(weight * points[:, 0], axis=1) 182 | y = np.sum(weight * points[:, 1], axis=1) 183 | line = np.concatenate([x.reshape(-1, 1), y.reshape(-1, 1)], axis=1) 184 | 185 | return line 186 | 187 | 188 | def get_road_width(smooth_curve: np.ndarray, traffic_cone_matrix: np.ndarray) -> float: 189 | """ 190 | 计算道路宽度 \n 191 | :param smooth_curve: 车道一侧拟合出的平滑曲线坐标 192 | :param traffic_cone_matrix: 车道另一侧离散的锥桶坐标 193 | :return: 道路宽度，[m] 194 | """ 195 | 196 | distance_matrix = cdist(smooth_curve, traffic_cone_matrix, "euclidean") 197 | road_width = distance_matrix.min(initial=None) 198 | 199 | return road_width 200 | 201 | 202 | def get_translation( 203 | original_points: np.ndarray, direction: str, distance: float = 1.5 204 | ) -> np.ndarray: 205 | """ 206 | 辅助函数，将曲线上的所有点按一定方向平移 \n 207 | :param original_points: 原始曲线（二维矩阵） 208 | :param direction: 平移方向，right(r)或left(l) 209 | :param distance: 平移距离，[m] 210 | :return: 平移矩阵，需要与原始曲线矩阵相加 211 | """ 212 | 213 | y_dis = distance 214 | x_dis = -0.5 * y_dis 215 | 216 | if direction == "r" or direction in {"right", "R"}: 217 | translation = np.tile(np.array([x_dis, -y_dis]), (original_points.shape[0], 1)) 218 | elif direction == "l" or direction in {"left", "L"}: 219 | translation = np.tile(np.array([x_dis, y_dis]), (original_points.shape[0], 1)) 220 | else: 221 | translation = np.zeros_like(original_points) 222 | 223 | return translation 224 | 225 | 226 | if __name__ == "__main__": 227 | import matplotlib.pyplot as plt 228 | from test_data import test_data # 加载测试数据 229 | 230 | # 绘图 231 | fig, axs = plt.subplots(3, 4) 232 | fig.suptitle("Local Path Planning") 233 | fig.subplots_adjust(wspace=0.1, hspace=0.5) 234 | 235 | for i in range(len(test_data)): 236 | red = get_traffic_cone_matrix(np.array(test_data[i]), 2) # 左侧锥桶位置矩阵 237 | blue = get_traffic_cone_matrix(np.array(test_data[i]), 11) # 右侧锥桶位置矩阵 238 | local_path = local_path_fitting( 239 | red, blue, translation_dis=1.5 240 | ) # 规划出的局部路径矩阵 241 | 242 | row = i // 4 # 绘图布局用 243 | col = i % 4 244 | 245 | # 出于观察方便之考虑，屏幕竖直向上为x正方向 246 | axs[row, col].plot(red[:, 1] * -1, red[:, 0], "o-", color="r") 247 | axs[row, col].plot(blue[:, 1] * -1, blue[:, 0], "o-", color="b") 248 | axs[row, col].plot( 249 | local_path[:, 1] * -1, local_path[:, 0], color="k", linestyle="--" 250 | ) 251 | axs[row, col].set_aspect("equal") 252 | axs[row, col].set_title(f"{i}") 253 | 254 | plt.show() 255 | -------------------------------------------------------------------------------- /src/LocalPathPlanning/FSAE_PathPlanning/test_data.py: -------------------------------------------------------------------------------- 1 | """ 2 | 传感器返回的锥桶位置坐标数据，用于测试路径规划相关算法的性能 3 | 4 | 格式为二维矩阵，每行定义为 [锥桶序号, 锥桶颜色, x坐标, y坐标] 5 | 序号以1000起始，与相对位置无必然联系，原始数据严格按序号排列； 6 | 颜色：2-左侧红锥桶、11-右侧蓝锥筒、13-起终点黄锥桶； 7 | 坐标为车辆坐标系，单位[m]，感知范围：[-1:12,-5:5]； 8 | """ 9 | 10 | test_data = [ 11 | [ 12 | (1028, 2, 0.5198568803717668, 1.7300003443652543), 13 | (1029, 2, 5.519694100421943, 1.7300003966270006), 14 | (1030, 2, 10.519531320472119, 1.7300004488887468), 15 | (1130, 11, 5.519694136169957, -1.6899996033730011), 16 | (1131, 11, 10.519531356220131, -1.6899995511112549), 17 | (1220, 11, 0.5198569161197811, -1.6899996556347474), 18 | ], 19 | [ 20 | (1254, 2, 0.14480948141390498, 1.7696306144045828), 21 | (1099, 2, 2.859179404712742, 1.6902837846199898), 22 | (1100, 2, 5.13707964163392, 1.6772258140972323), 23 | (1255, 2, 7.548079919962241, 2.015571285288534), 24 | (1000, 2, 9.982251589267294, 2.9575417114300855), 25 | (1202, 11, 1.5204094887822874, -1.7240529152462962), 26 | (1201, 11, 3.9004688742748463, -1.8896346817309273), 27 | (1256, 11, 6.3608022224497285, -1.798202653166403), 28 | (1200, 11, 9.20788044510444, -1.1349940933080962), 29 | ], 30 | [ 31 | (1016, 2, 0.42012055083200484, 1.4933996216668495), 32 | (1017, 2, 6.614956638639475, 1.4948017112610235), 33 | (1018, 2, 9.12002792664868, 0.5622689881383982), 34 | (1116, 11, 0.033277292651222146, -1.9078431229527557), 35 | (1117, 11, 3.0745261014930523, -1.9362112484171177), 36 | (1118, 11, 6.227546018943245, -1.9672660125246542), 37 | (1119, 11, 7.630275187077222, -3.164074573539731), 38 | (1120, 11, 6.911739586705433, -4.582773449769418), 39 | (1218, 2, 3.521777145176589, 1.5959929362822562), 40 | ], 41 | [ 42 | (1039, 2, 1.5719844863476329, 1.3452631698408124), 43 | (1040, 2, 5.471146464329374, 2.2192398404864857), 44 | (1041, 2, 7.173426120730759, 3.4437985033604983), 45 | (1140, 11, 1.1395663052627842, -2.123744520660087), 46 | (1141, 11, 7.5684376804787785, -0.5507184153002762), 47 | (1142, 11, 4.873666455657334, -1.6759774141645938), 48 | (1143, 11, 9.772187208726246, 1.1532440075683486), 49 | (1144, 11, 11.408631466763312, 3.2716514715137155), 50 | (1226, 2, 3.6069463444451566, 1.5374350350016928), 51 | ], 52 | [ 53 | (1017, 2, 3.343387449437274, 1.8235284646302428), 54 | (1018, 2, 5.867827908680232, 0.944779519389953), 55 | (1019, 2, 7.6091855208348225, -1.1205953728195666), 56 | (1020, 2, 7.913549020138137, -3.727770636814051), 57 | (1117, 11, -0.12286178543346227, -1.6824113908721865), 58 | (1118, 11, 3.030101167823757, -1.6460322572791233), 59 | (1119, 11, 4.45810309689678, -2.812570004534515), 60 | (1218, 2, 0.2487513787190473, 1.8585494303263963), 61 | ], 62 | [ 63 | (1030, 2, 3.855235818424428, 1.5423620453234432), 64 | (1031, 2, 6.802416903909067, 1.2431585793293864), 65 | (1032, 2, 9.921365356765948, -0.8978363970224887), 66 | (1131, 11, 3.821364242935853, -1.8774702190389383), 67 | (1132, 11, 5.928685611478859, -2.1583548416046026), 68 | (1133, 11, 7.22941059989056, -3.1012834285527457), 69 | (1134, 11, 7.904025568779316, -4.6580411465623595), 70 | ], 71 | [ 72 | (1069, 2, -0.36148326173253903, 2.099907018625206), 73 | (1070, 2, 4.902509208637504, -0.651616784190163), 74 | (1071, 2, 8.842008859635454, -3.1141444436037977), 75 | (1175, 11, 0.8268523619948722, -1.6719702850548483), 76 | (1176, 11, 2.3372658876536523, -2.9338145602949948), 77 | (1177, 11, 4.0995415583234465, -4.682552519030701), 78 | (1235, 2, 2.29508948290152, 1.5338493798590191), 79 | (1236, 2, 6.582265949758745, -2.213832288143355), 80 | ], 81 | [ 82 | (1082, 2, 1.3614711631559742, 2.39482984502546), 83 | (1248, 2, -0.8395530615027752, 1.6630565968354079), 84 | (1249, 2, 3.0967314349510704, 3.4233587307924735), 85 | (1192, 11, 1.6401075472967785, -1.294134965057924), 86 | (1193, 11, 4.583666256806346, 0.13466233340862432), 87 | (1194, 11, 6.620033092621611, 1.9643371709554591), 88 | (1195, 11, 8.316246507153558, 4.464829130300567), 89 | ], 90 | [ 91 | (1003, 2, 1.6244383786482377, 2.2361230815193496), 92 | (1004, 2, 4.495378060668235, 1.0500770214530075), 93 | (1005, 2, 6.232240263376512, -0.7618030548189494), 94 | (1006, 2, 7.997610825008419, -3.6511742703273162), 95 | (1104, 11, 1.5556034392533136, -1.3533839078037486), 96 | (1105, 11, 3.4682040002815637, -2.9006585772683775), 97 | (1258, 11, -0.30406979303716725, -1.3680626465900734), 98 | ], 99 | [ 100 | (1018, 2, -0.10327455696259846, 2.121826124938412), 101 | (1019, 2, 2.25874565672506, 0.8107365994690188), 102 | (1021, 2, 3.383840634611671, -3.99595237941175), 103 | (1020, 2, 3.4703873866817023, -1.5177656204758398), 104 | (1119, 11, -0.08463894666596705, -1.8912341131430028), 105 | (1120, 11, -0.21782110014428646, -3.475930459048841), 106 | ], 107 | [ 108 | (1014, 2, 0.7150756031350525, 1.5625977103500328), 109 | (1015, 2, 2.7404605669236592, 2.8730995950395704), 110 | (1016, 2, 4.936308731615277, 4.508474200727712), 111 | (1114, 11, 1.8318083973593622, -1.68563375315944), 112 | (1115, 11, 4.3809453180425155, -0.16421989506332668), 113 | (1116, 11, 6.619351606113854, 1.5276242213616196), 114 | (1117, 11, 9.098338331843745, 3.28961852068921), 115 | (1122, 11, 8.881594246235908, -1.7232769136185775), 116 | (1259, 11, -0.9488109833904796, -2.4074753164458884), 117 | ], 118 | [ 119 | (1012, 2, -0.8576028235585439, 1.577359811133982), 120 | (1013, 2, 1.24015368581702, 2.4630333494540584), 121 | (1113, 11, 1.6975405773835923, -1.364772419878486), 122 | (1114, 11, 5.4924067318938885, 2.0126512584850627), 123 | (1123, 11, 10.394647655296563, 1.0283068516200227), 124 | (1124, 11, 9.439751061381783, -3.879663414332267), 125 | (1259, 11, 3.5954666334519683, -0.1447819836991685), 126 | ], 127 | # [ 128 | # (1038, 2, 1.4108042597457042, 1.7743796750218634), 129 | # (1039, 2, 4.971689118521692, 1.6907256011075877), 130 | # (1040, 2, 8.84837819987129, 2.659529453434246), 131 | # (1041, 2, 10.52028884381998, 3.9252366584190015), 132 | # (1139, 11, 1.2020327661415302, -1.7031646041644342), 133 | # (1140, 11, 4.6239966253000535, -1.7877955958345062), 134 | # (1141, 11, 11.012595359495268, -0.058459325316497335), 135 | # (1142, 11, 8.346066744654475, -1.2490998414625407), 136 | # (1226, 2, 7.001359361464683, 1.932466010035617), 137 | # ], 138 | # [ 139 | # (1039, 2, 2.348804880371027, 1.7869402606255744), 140 | # (1040, 2, 6.193305547205714, 2.8764876470282075), 141 | # # (1041, 2, 7.824822232581027, 4.193853661055201), 142 | # (1140, 11, 2.1100496021182025, -1.7007517616276973), 143 | # (1141, 11, 8.441451139600414, 0.22749916228635936), 144 | # (1142, 11, 5.8134556316835555, -1.0459368245235776), 145 | # (1143, 11, 10.546990107321779, 2.051425016294885), 146 | # (1226, 2, 4.369923855416798, 2.092026686073652), 147 | # ], 148 | # [ 149 | # (1077, 2, 2.1706222067413314, 1.642849160694925), 150 | # (1078, 2, 7.041147280416473, 3.8517593922180877), 151 | # (1185, 11, 2.50982739558705, -1.7841376659199133), 152 | # (1186, 11, 7.531344434391471, -0.09068975418433634), 153 | # (1187, 11, 9.429703319024918, 1.338972163508752), 154 | # (1241, 2, -0.10834121929478607, 1.5794009956527184), 155 | # (1242, 11, -0.18249107423163857, -1.9087217892977257), 156 | # (1243, 2, 4.235519328637718, 2.123161336819051), 157 | # (1244, 11, 5.251705770826409, -1.2095048847575405), 158 | # (1245, 2, 5.861603153533731, 2.886215244832027), 159 | # ], 160 | # [ 161 | # (1038, 2, 1.423079842977017, 1.7593361276928536), 162 | # (1039, 2, 4.984944835776931, 1.7634296476457458), 163 | # (1040, 2, 8.836590964689249, 2.827441507222289), 164 | # (1139, 11, 1.3000408243233927, -1.7222958388841827), 165 | # (1140, 11, 4.7230510444345075, -1.722601251521696), 166 | # (1142, 11, 8.4307208258648, -1.092375987752217), 167 | # (1226, 2, 7.00804383979026, 2.0550975083847565), 168 | # ], 169 | # [ 170 | # (1017, 2, 3.9372580636061483, 1.6639379332519466), 171 | # (1018, 2, 6.430249595206017, 0.6995766622472099), 172 | # (1019, 2, 8.100131162789506, -1.4240041253517999), 173 | # # (1020, 2, 8.315371035020126, -4.04004537020066), 174 | # (1117, 11, 0.35341784733174775, -1.7217064104271242), 175 | # (1118, 11, 3.505786533618373, -1.7929150041949014), 176 | # (1119, 11, 4.893159575729504, -3.007491429480535), 177 | # # (1120, 11, 4.156613933609854, -4.416924091044431), 178 | # (1218, 2, 0.8456181945522163, 1.8045151816841152), 179 | # ], 180 | # [ 181 | # (1015, 2, 0.9580219229873854, 1.5034681459028105), 182 | # (1016, 2, 3.671936643823763, 1.8652224139522784), 183 | # (1218, 2, 6.739184164219195, 2.3372305663180746), 184 | # (1017, 2, 9.822334569208715, 2.6058988600704405), 185 | # (1115, 11, 0.9152019289846853, -1.9482946431185315), 186 | # (1116, 11, 3.69375819085667, -1.5578790652519086), 187 | # (1117, 11, 6.716658407084179, -1.223105441960686), 188 | # (1118, 11, 9.85085159001001, -0.8776606262211146), 189 | # # (1121, 11, 8.901992793389207, -4.1386522942111), 190 | # ], 191 | ] 192 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | --------------------------------------------------------------------------------