├── .gitignore ├── .vscode ├── c_cpp_properties.json ├── launch.json ├── settings.json └── tasks.json ├── CMakeLists.txt ├── README.md ├── README_zh_cn.md ├── README_zh_cn.pdf ├── img ├── demo.mp4 ├── demo.png ├── model │ ├── coordinate_system.png │ ├── kinBkMdl_eqns.png │ └── nomenclature.png ├── mpc │ └── mpc.png └── qp │ └── qp.png ├── include ├── controller_mpc │ ├── mpc.h │ ├── path_planner.h │ └── quintic_solver.hpp └── simulator │ ├── simulator.h │ └── vec2.h ├── launch ├── nocontrol.launch └── sim.launch ├── msg └── car_state.msg ├── package.xml ├── param └── simulator_racetrack01.yaml ├── rviz └── sim.rviz ├── src ├── controller_mpc │ ├── controller_mpc.cpp │ ├── mpc.cpp │ └── path_planner.cpp ├── simulator │ ├── main.cpp │ └── simulator.cpp ├── test │ ├── test.cpp │ ├── test_planner.hpp │ ├── test_qp.hpp │ └── unittest.hpp └── track_publisher │ └── track_publisher.cpp ├── srv └── simulator_reset.srv └── tracks └── racetrack01.txt /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${workspaceFolder}/**", 7 | "/usr/include/**", 8 | "/usr/local/include/**", 9 | "/opt/ros/melodic/include/**", 10 | "/usr/lib/gcc/x86_64-linux-gnu/**", 11 | "${workspaceFolder}/../../devel/include/**" 12 | ], 13 | "defines": [], 14 | "compilerPath": "/usr/bin/gcc", 15 | "cStandard": "c11", 16 | "cppStandard": "c++17", 17 | "intelliSenseMode": "clang-x64" 18 | } 19 | ], 20 | "version": 4 21 | } -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // Use IntelliSense to learn about possible attributes. 3 | // Hover to view descriptions of existing attributes. 4 | // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | { 8 | "name": "(gdb) Launch", 9 | "type": "cppdbg", 10 | "request": "launch", 11 | "preLaunchTask": "build", 12 | 13 | "program": "${workspaceFolder}/../../devel/lib/${workspaceFolderBasename}/${fileBasenameNoExtension}", 14 | "args": [], 15 | "stopAtEntry": false, 16 | "cwd": "${workspaceFolder}", 17 | "environment": [], 18 | "externalConsole": false, 19 | "MIMode": "gdb", 20 | "miDebuggerPath": "/usr/bin/gdb", 21 | 22 | "setupCommands": [ 23 | { 24 | "description": "Enable pretty-printing for gdb", 25 | "text": "-enable-pretty-printing", 26 | "ignoreFailures": true 27 | } 28 | ] 29 | } 30 | ] 31 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "cctype": "cpp", 4 | "cmath": "cpp", 5 | "cstdarg": "cpp", 6 | "cstddef": "cpp", 7 | "cstdio": "cpp", 8 | "cstdlib": "cpp", 9 | "cstring": "cpp", 10 | "ctime": "cpp", 11 | "cwchar": "cpp", 12 | "array": "cpp", 13 | "atomic": "cpp", 14 | "strstream": "cpp", 15 | "*.tcc": "cpp", 16 | "bitset": "cpp", 17 | "chrono": "cpp", 18 | "clocale": "cpp", 19 | "complex": "cpp", 20 | "cstdint": "cpp", 21 | "cwctype": "cpp", 22 | "deque": "cpp", 23 | "list": "cpp", 24 | "unordered_map": "cpp", 25 | "vector": "cpp", 26 | "exception": "cpp", 27 | "fstream": "cpp", 28 | "functional": "cpp", 29 | "initializer_list": "cpp", 30 | "iomanip": "cpp", 31 | "iosfwd": "cpp", 32 | "iostream": "cpp", 33 | "istream": "cpp", 34 | "limits": "cpp", 35 | "memory": "cpp", 36 | "new": "cpp", 37 | "ostream": "cpp", 38 | "numeric": "cpp", 39 | "ratio": "cpp", 40 | "sstream": "cpp", 41 | "stdexcept": "cpp", 42 | "streambuf": "cpp", 43 | "system_error": "cpp", 44 | "thread": "cpp", 45 | "cinttypes": "cpp", 46 | "type_traits": "cpp", 47 | "tuple": "cpp", 48 | "typeindex": "cpp", 49 | "typeinfo": "cpp", 50 | "utility": "cpp", 51 | "algorithm": "cpp", 52 | "csignal": "cpp", 53 | "condition_variable": "cpp", 54 | "unordered_set": "cpp", 55 | "iterator": "cpp", 56 | "map": "cpp", 57 | "memory_resource": "cpp", 58 | "optional": "cpp", 59 | "set": "cpp", 60 | "string": "cpp", 61 | "string_view": "cpp", 62 | "hash_map": "cpp", 63 | "hash_set": "cpp", 64 | "mutex": "cpp", 65 | "cfenv": "cpp", 66 | "random": "cpp", 67 | "valarray": "cpp", 68 | "*.ipp": "cpp" 69 | }, 70 | "python.autoComplete.extraPaths": [ 71 | "/opt/ros/melodic/lib/python2.7/dist-packages" 72 | ] 73 | } -------------------------------------------------------------------------------- /.vscode/tasks.json: -------------------------------------------------------------------------------- 1 | { 2 | // See https://go.microsoft.com/fwlink/?LinkId=733558 3 | // for the documentation about the tasks.json format 4 | "version": "2.0.0", 5 | "tasks": [ 6 | { 7 | "label": "build", 8 | "type": "shell", 9 | "command": "source /home/one/Scripts/scripts_init.sh && catkin_make --only-pkg-with-deps car_model -DCMAKE_BUILD_TYPE=Debug", 10 | "args": [], 11 | "options": { 12 | "cwd": "${workspaceFolder}/../../" 13 | }, 14 | "group": "build", 15 | "presentation": { 16 | // Reveal the output only if unrecognized errors occur. 17 | "reveal": "silent" 18 | }, 19 | // Use the standard MS compiler pattern to detect errors, warnings and infos 20 | "problemMatcher": { 21 | "base": "$gcc", 22 | "fileLocation": "absolute" 23 | } 24 | } 25 | ] 26 | } 27 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(car_model) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | tf 15 | ecl_geometry 16 | message_generation 17 | dynamic_reconfigure 18 | ) 19 | 20 | find_package(Eigen3 REQUIRED) 21 | 22 | ## System dependencies are found with CMake's conventions 23 | # find_package(Boost REQUIRED COMPONENTS system) 24 | 25 | 26 | ## Uncomment this if the package has a setup.py. This macro ensures 27 | ## modules and global scripts declared therein get installed 28 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 29 | # catkin_python_setup() 30 | 31 | ################################################ 32 | ## Declare ROS messages, services and actions ## 33 | ################################################ 34 | 35 | ## To declare and build messages, services or actions from within this 36 | ## package, follow these steps: 37 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 38 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 39 | ## * In the file package.xml: 40 | ## * add a build_depend tag for "message_generation" 41 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 42 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 43 | ## but can be declared for certainty nonetheless: 44 | ## * add a exec_depend tag for "message_runtime" 45 | ## * In this file (CMakeLists.txt): 46 | ## * add "message_generation" and every package in MSG_DEP_SET to 47 | ## find_package(catkin REQUIRED COMPONENTS ...) 48 | ## * add "message_runtime" and every package in MSG_DEP_SET to 49 | ## catkin_package(CATKIN_DEPENDS ...) 50 | ## * uncomment the add_*_files sections below as needed 51 | ## and list every .msg/.srv/.action file to be processed 52 | ## * uncomment the generate_messages entry below 53 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 54 | 55 | ## Generate messages in the 'msg' folder 56 | add_message_files( 57 | FILES 58 | car_state.msg 59 | ) 60 | 61 | ## Generate services in the 'srv' folder 62 | add_service_files( 63 | FILES 64 | simulator_reset.srv 65 | ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | generate_messages( 76 | DEPENDENCIES 77 | std_msgs 78 | ) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | generate_dynamic_reconfigure_options( 96 | ) 97 | 98 | ################################### 99 | ## catkin specific configuration ## 100 | ################################### 101 | ## The catkin_package macro generates cmake config files for your package 102 | ## Declare things to be passed to dependent projects 103 | ## INCLUDE_DIRS: uncomment this if your package contains header files 104 | ## LIBRARIES: libraries you create in this project that dependent projects also need 105 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 106 | ## DEPENDS: system dependencies of this project that dependent projects also need 107 | catkin_package( 108 | INCLUDE_DIRS include 109 | # LIBRARIES car_model 110 | CATKIN_DEPENDS roscpp rospy std_msgs message_runtime 111 | # DEPENDS system_lib 112 | ) 113 | 114 | ########### 115 | ## Build ## 116 | ########### 117 | 118 | ## Specify additional locations of header files 119 | ## Your package locations should be listed before other locations 120 | include_directories( 121 | include 122 | ${catkin_INCLUDE_DIRS} 123 | ${EIGEN3_INCLUDE_DIRS} 124 | ) 125 | 126 | ## Declare a C++ library 127 | # add_library(${PROJECT_NAME} 128 | # src/${PROJECT_NAME}/car_model.cpp 129 | # ) 130 | 131 | ## Add cmake target dependencies of the library 132 | ## as an example, code may need to be generated before libraries 133 | ## either from message generation or dynamic reconfigure 134 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Declare a C++ executable 137 | ## With catkin_make all packages are built within a single CMake context 138 | ## The recommended prefix ensures that target names across packages don't collide 139 | # add_executable(${PROJECT_NAME}_node src/car_model_node.cpp) 140 | 141 | ## Rename C++ executable without prefix 142 | ## The above recommended prefix causes long target names, the following renames the 143 | ## target back to the shorter version for ease of user use 144 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 145 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 146 | 147 | ## Add cmake target dependencies of the executable 148 | ## same as for the library above 149 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | # target_link_libraries(${PROJECT_NAME}_node 153 | # ${catkin_LIBRARIES} 154 | # ) 155 | 156 | add_compile_options(-Wall -Wextra -Wconversion -g) 157 | #add_compile_options(-Ofast) 158 | 159 | aux_source_directory(src/simulator simulator_SOURCE_FILES) 160 | add_executable(simulator ${simulator_SOURCE_FILES}) 161 | target_link_libraries(simulator ${catkin_LIBRARIES}) 162 | add_dependencies(simulator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 163 | 164 | aux_source_directory(src/track_publisher track_publisher_SOURCE_FILES) 165 | add_executable(track_publisher ${track_publisher_SOURCE_FILES}) 166 | target_link_libraries(track_publisher ${catkin_LIBRARIES}) 167 | add_dependencies(track_publisher ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 168 | 169 | #MPC controller 170 | aux_source_directory(src/controller_mpc controller_mpc_SOURCE_FILES) 171 | add_executable(controller_mpc ${controller_mpc_SOURCE_FILES}) 172 | target_link_libraries(controller_mpc ${catkin_LIBRARIES}) 173 | add_dependencies(controller_mpc ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 174 | 175 | #Unit Test 176 | add_executable(test src/test/test.cpp src/controller_mpc/mpc.cpp src/controller_mpc/path_planner.cpp) 177 | target_link_libraries(test ${catkin_LIBRARIES}) 178 | add_dependencies(test ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 179 | 180 | #OSQP for MPC controller 181 | target_include_directories(controller_mpc PUBLIC osqp/include) 182 | find_library(OSQP_LIBRARY libosqp.a HINTS osqp/build/out) 183 | target_link_libraries(controller_mpc ${OSQP_LIBRARY} dl) 184 | 185 | target_include_directories(test PUBLIC osqp/include) 186 | target_link_libraries(test ${OSQP_LIBRARY} dl) 187 | 188 | ############# 189 | ## Install ## 190 | ############# 191 | 192 | # all install targets should use catkin DESTINATION variables 193 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 194 | 195 | ## Mark executable scripts (Python etc.) for installation 196 | ## in contrast to setup.py, you can choose the destination 197 | # install(PROGRAMS 198 | # scripts/my_python_script 199 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 200 | # ) 201 | 202 | ## Mark executables for installation 203 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 204 | # install(TARGETS ${PROJECT_NAME}_node 205 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 206 | # ) 207 | 208 | ## Mark libraries for installation 209 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 210 | # install(TARGETS ${PROJECT_NAME} 211 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 212 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 213 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 214 | # ) 215 | 216 | ## Mark cpp header files for installation 217 | # install(DIRECTORY include/${PROJECT_NAME}/ 218 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 219 | # FILES_MATCHING PATTERN "*.h" 220 | # PATTERN ".svn" EXCLUDE 221 | # ) 222 | 223 | ## Mark other files for installation (e.g. launch and bag files, etc.) 224 | # install(FILES 225 | # # myfile1 226 | # # myfile2 227 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 228 | # ) 229 | 230 | ############# 231 | ## Testing ## 232 | ############# 233 | 234 | ## Add gtest based cpp test target and link libraries 235 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_car_model.cpp) 236 | # if(TARGET ${PROJECT_NAME}-test) 237 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 238 | # endif() 239 | 240 | ## Add folders to be run by python nosetests 241 | # catkin_add_nosetests(test) 242 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Model Predictive Controller for Autonomous Driving 2 | 3 | Autonomous driving simulator and model predictive algorithm implemented with C++ and ROS framework 4 | 5 | ## 1. Build Instructions 6 | 7 | Step #1. Install ROS Melodic 8 | 9 | Hint: ROS Melodic only supports Ubuntu 18.04 and Debian Stretch 10 | > -------------------------------------------------------------------------------- /README_zh_cn.md: -------------------------------------------------------------------------------- 1 | # 自动驾驶车辆模型预测控制算法 2 | 3 | ## Demo及摘要 4 | 5 | 点击下面播放视频Demo： 6 | 7 | [![Demo Video](./img/demo.png)](img/demo.mp4) 8 | 9 | 坐标轴为车辆中心，红色箭头为预测车辆运行轨迹，蓝色箭头为规划目标轨迹，绿色曲线为车道中心。 10 | 11 | ## 研究内容 12 | 13 | 业界自动驾驶车辆的控制（Control）大多数使用模型预测控制（MPC）算法，即接受行驶轨迹，控制方向盘，油门及刹车，使得车辆跟随轨迹行驶（Lane Following）。 14 | 15 | 本项目基于ROS框架开发，实现了模型预测控制（MPC）算法，以及一个实时仿真环境。可以实现在仿真环境中稳定地控制自动驾驶车辆跟随目标轨迹的功能。 16 | 17 | ## 应用 18 | 19 | 本项目是一个通用的模型预测控制（MPC）控制算法，并且实现了标准的ROS接口，可以直接通过ROS通讯框架对接硬件，只需稍加修改，即可实现自动驾驶车辆，机器人，无人机等系统的控制部分。 20 | 21 | # 编译方法 22 | 23 | ## Step 1 24 | 1. 本项目在```Ubuntu 18.04 LTS```系统下编译及测试，推荐使用该系统作为运行环境。 25 | 2. 安装```ROS Melodic```版本，**此步骤较为繁琐**，请参考ROS Wiki完成安装，具体操作方式详见[ROS Wiki: Installation](http://wiki.ros.org/melodic/Installation)，需要desktop-full安装，在此不在赘述。 26 | 3. 安装```ecl_geometry```包 27 | 28 | ```bash 29 | sudo apt install ros-melodic-ecl-geometry 30 | ``` 31 | 32 | ## Step 2 33 | 下载并构建OSQP 34 | 35 | 请在本项目根目录输入如下指令 36 | 37 | ```bash 38 | git clone --recursive https://github.com/oxfordcontrol/osqp 39 | cd osqp 40 | mkdir build 41 | cd build 42 | cmake -G "Unix Makefiles" .. 43 | cmake --build . 44 | ``` 45 | 46 | 如果此步骤出现问题，请参考[OSQP 官方文档](https://osqp.org/docs/get_started/sources.html) 47 | 48 | ## Step 3 49 | 编译本项目 50 | 51 | 请将本项目放置于ROS工作区中，项目文件夹名字为```car_model```，关于如何建立ROS工作区请参考[ROS Wiki: Create a Workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace)，并且在ROS工作区目录输入如下指令 52 | 53 | ```bash 54 | catkin_make 55 | ``` 56 | 57 | 至此，大功告成！ 58 | 59 | # 使用及测试 60 | 61 | 打开终端输入 62 | 63 | ```bash 64 | roslaunch car_model sim.launch 65 | ``` 66 | 67 | 即启动控制器和模拟器，并且显示RViz可视化界面。 68 | 69 | 可视化界面中，坐标轴为车辆中心，红色箭头为预测车辆运行轨迹，蓝色箭头为规划目标轨迹，绿色曲线为车道中心。 70 | 71 | 可以看到车辆很稳定地跟随车道 72 | 73 | 如果想控制硬件系统，需要启动对应系统的姿态估计程序，并且发布对应的 ```car_state``` 消息来确定系统状态，然后接收 ```geometry_twist``` 消息作为硬件控制指令，参考 ```tracks/racetrack01.txt``` 编写轨迹文件，然后启动控制器 74 | 75 | ```bash 76 | rosrun car_model controller_mpc track_filename:=<轨迹文件名> track_scale:=1.0 77 | ``` 78 | 79 | # 文件结构 80 | 81 | - include ... 头文件 82 | - controller_mpc -- MPC控制器头文件 83 | - mpc.h -- MPC控制器 84 | - path_planner.h -- 轨迹规划程序 85 | - quintic_solver.h -- 五次方程数值求解，使用Descartes Rule of Signs及二分法 86 | 87 | - simulator -- 模拟器 88 | - simulator.h -- 模拟器 89 | - vec2.h -- 二维向量库 90 | 91 | - src 92 | - controller_mpc -- MPC控制器 93 | - controller_mpc.cpp -- MPC控制器 ROS 接口部分 94 | - mpc.cpp -- MPC控制器 主体实现 95 | - path_planner.cpp -- 轨迹规划程序 96 | 97 | - simulator -- 模拟器 98 | - main.cpp -- 主程序 99 | - simulator.cpp -- 模拟器算法及车辆仿真模型 100 | 101 | - tracks 102 | > 车道轨迹，每行两个数x,y，表示路径点坐标，程序会自动插值成三次样条。 103 | 104 | - launch 105 | > roslaunch 启动文件 106 | 107 | - msg 108 | > ROS 消息定义 109 | 110 | - osqp 111 | > OSQP 二次规划求解器 112 | 113 | # 算法 114 | 115 | ## 模型预测控制 116 | 117 | ![MPC](img/mpc/mpc.png) 118 | 119 | MPC 通过模型预测未来的系统状态，转化为QP问题，求出最优的控制方案 120 | 121 | ## 车辆物理模型 122 | 123 | ![Coordinate](img/model/coordinate_system.png) 124 | 125 | ![Nomenclature](img/model/nomenclature.png) 126 | 127 | ![Model](img/model/kinBkMdl_eqns.png) 128 | 129 | *参考文献: 物理模型部分 - Car Model* 130 | 131 | ## 二次规划 132 | 133 | 二次规划解决这样一个优化问题 134 | 135 | ![QP](img/qp/qp.png) 136 | 137 | 这里使用的是OSQP求解器 138 | 139 | ## 模拟器 140 | 141 | 模拟器通过ROS框架与控制器实时交互，根据车辆动力学数值求解微分方程进行仿真，动力学模型在车辆物理模型部分有详细描述。 142 | 143 | 数值求解微分方程使用前向欧拉法 (Forward Euler method) 144 | 145 | # 验证 146 | 147 | 软件验证采用单元测试，蒙特卡洛测试和综合测试相结合的方式，遵循从部分到整体的严谨测试方式。 148 | 149 | ## 单元测试和蒙特卡洛测试 150 | 151 | 轨迹规划和MPC部分采用单元测试+蒙特卡洛测试的方式。 152 | 153 | 单元测试通过不同等价类的数据测试算法正确性。 154 | 155 | 蒙特卡洛测试通过生成大量随机数据测试算法正确性，运行效率与稳定性。 156 | 157 | 轨迹规划了蒙特卡洛测试的方式，加载固定轨迹，并且随机取点测试规划正确性及效率。 158 | 159 | QP部分采用了几个等价类的sample进行测试。 160 | 161 | 运行测试方式： 162 | 163 | ```bash 164 | rosrun car_model tests 165 | ``` 166 | 167 | ## 单元测试结果 168 | 169 | 全部通过 170 | 171 | ## 综合测试 172 | 173 | 综合测试控制器性能需要通过观察预测轨迹和Cross-Track-Error，预测轨迹越接近实际轨迹，则物理模型性能越好。Cross-Track-Error，即车辆偏离车道中心的距离，是量化指标，越低表示控制器性能越好。 174 | 175 | 通过在模拟环境运行控制器 176 | ```bash 177 | roslaunch car_model sim.launch 178 | ``` 179 | 180 | 若红色轨迹与实际轨迹偏差较大，即**车辆物理模型**存在问题 181 | 182 | 若蓝色轨迹与绿色目标轨迹偏差较大，即**车辆物理模型**存在问题 183 | 184 | 若控制器性能较差，即**控制器**或者**控制器参数**存在问题 185 | 186 | ## 综合测试结果 187 | 188 | 控制器稳定运行，各轨迹均不存在较大偏差，测试通过。 189 | 190 | # 参考文献 191 | 192 | *链接均有效，如无法打开，请检查网络连接* 193 | 194 | **物理模型部分** 195 | 1. [Robust Model Predictive Control for Autonomous Vehicle/Self-Driving Cars](https://arxiv.org/pdf/1805.08551.pdf) 196 | 2. [Car Model](https://github.com/MPC-Berkeley/barc/wiki/Car-Model) 197 | 3. [PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics) 198 | 199 | **控制算法** 200 | 1. [Lane keeping in autonomous driving with Model Predictive Control & PID](https://medium.com/@jonathan_hui/lane-keeping-in-autonomous-driving-with-model-predictive-control-50f06e989bc9) 201 | 202 | **QP solver** 203 | 1. [OSQP solver documentation](https://osqp.org/docs/index.html) 204 | 205 | **ROS 框架** 206 | 1. [ROS Wiki](http://wiki.ros.org/) -------------------------------------------------------------------------------- /README_zh_cn.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/imoneoi/autonomous_driving_mpc/51d7e01bcb46e97bdbe10e53bab9688373b4088a/README_zh_cn.pdf -------------------------------------------------------------------------------- /img/demo.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/imoneoi/autonomous_driving_mpc/51d7e01bcb46e97bdbe10e53bab9688373b4088a/img/demo.mp4 -------------------------------------------------------------------------------- /img/demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/imoneoi/autonomous_driving_mpc/51d7e01bcb46e97bdbe10e53bab9688373b4088a/img/demo.png -------------------------------------------------------------------------------- /img/model/coordinate_system.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/imoneoi/autonomous_driving_mpc/51d7e01bcb46e97bdbe10e53bab9688373b4088a/img/model/coordinate_system.png -------------------------------------------------------------------------------- /img/model/kinBkMdl_eqns.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/imoneoi/autonomous_driving_mpc/51d7e01bcb46e97bdbe10e53bab9688373b4088a/img/model/kinBkMdl_eqns.png -------------------------------------------------------------------------------- /img/model/nomenclature.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/imoneoi/autonomous_driving_mpc/51d7e01bcb46e97bdbe10e53bab9688373b4088a/img/model/nomenclature.png -------------------------------------------------------------------------------- /img/mpc/mpc.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/imoneoi/autonomous_driving_mpc/51d7e01bcb46e97bdbe10e53bab9688373b4088a/img/mpc/mpc.png -------------------------------------------------------------------------------- /img/qp/qp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/imoneoi/autonomous_driving_mpc/51d7e01bcb46e97bdbe10e53bab9688373b4088a/img/qp/qp.png -------------------------------------------------------------------------------- /include/controller_mpc/mpc.h: -------------------------------------------------------------------------------- 1 | #ifndef __CAR_MODEL_CONTROLLER_MPC_MPC_H__ 2 | #define __CAR_MODEL_CONTROLLER_MPC_MPC_H__ 3 | 4 | #include "path_planner.h" 5 | #include "osqp.h" 6 | #include 7 | 8 | namespace MPC { 9 | struct HardConstraint { 10 | double max_steer; 11 | 12 | double max_accel; 13 | double min_accel; 14 | 15 | double max_steer_rate; 16 | double max_jerk; 17 | }; 18 | 19 | struct CostFunctionWeights { 20 | double w_position; 21 | double w_angle; 22 | 23 | double w_velocity; 24 | 25 | double w_accel; 26 | double w_jerk; 27 | 28 | double w_steer; 29 | double w_dsteer; 30 | }; 31 | 32 | struct State { 33 | double x; 34 | double y; 35 | double yaw; 36 | double v; 37 | 38 | double steer_angle; 39 | double accel; 40 | }; 41 | 42 | struct Parameters { 43 | int pred_horizon; 44 | int control_horizon; 45 | 46 | double dt; 47 | }; 48 | 49 | struct Model { 50 | double l_f; 51 | double l_r; 52 | double m; 53 | }; 54 | 55 | struct ControlOutput { 56 | int error_code; 57 | double steer; 58 | double accel; 59 | }; 60 | 61 | template 62 | struct SparseMatrixElement { 63 | int r, c; 64 | T v; 65 | 66 | inline bool operator<(const SparseMatrixElement &rhs) { 67 | return (c == rhs.c) ? (r < rhs.r) : (c < rhs.c); 68 | } 69 | }; 70 | 71 | template 72 | class SparseMatrix { 73 | private: 74 | int m_, n_; 75 | 76 | std::vector< SparseMatrixElement > elements_; 77 | 78 | std::vector osqp_csc_data_; 79 | std::vector osqp_csc_row_idx_; 80 | std::vector osqp_csc_col_start_; 81 | csc *osqp_csc_instance = nullptr; 82 | 83 | void freeOSQPCSCInstance(); 84 | 85 | public: 86 | SparseMatrix(); 87 | ~SparseMatrix(); 88 | 89 | void initialize(int m, int n); 90 | void addElement(int r, int c, T v); 91 | csc *toOSQPCSC(); 92 | }; 93 | 94 | template 95 | class QPProblem { 96 | private: 97 | OSQPWorkspace *osqp_workspace_ = nullptr; 98 | OSQPSettings *osqp_settings_= nullptr; 99 | OSQPData *osqp_data_ = nullptr; 100 | 101 | public: 102 | //number of variables and constraints 103 | int n_, m_; 104 | 105 | //constraints 106 | SparseMatrix A_; 107 | std::vector l_, u_; 108 | 109 | //cost function 110 | SparseMatrix P_; 111 | std::vector q_; 112 | 113 | ~QPProblem(); 114 | void initialize(int n, int m); 115 | OSQPSolution* solve(int *error_code); 116 | }; 117 | 118 | class Controller { 119 | private: 120 | QPProblem qp_; 121 | 122 | public: 123 | /* controller parameters */ 124 | HardConstraint constraint_; 125 | CostFunctionWeights weights_; 126 | Parameters parameters_; 127 | Model model_; 128 | 129 | Controller(); 130 | 131 | void initialize(const Parameters ¶meters, const Model &model, const HardConstraint &constraint, const CostFunctionWeights &weights); 132 | void update(const State &state, const State &linearize_point, const std::vector &track_input, ControlOutput *out, std::vector *pred_out); 133 | }; 134 | 135 | class IterativeController : public Controller { 136 | public: 137 | void simulate(const State &state, State *next); 138 | void update(const State &state, const std::vector &track_input, int iterations, double threshold, ControlOutput *out, std::vector *pred_out); 139 | }; 140 | } 141 | 142 | #endif -------------------------------------------------------------------------------- /include/controller_mpc/path_planner.h: -------------------------------------------------------------------------------- 1 | #ifndef __CAR_MODEL_CONTROLLER_MPC_PATH_PLANNER_H__ 2 | #define __CAR_MODEL_CONTROLLER_MPC_PATH_PLANNER_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "ros/ros.h" 9 | #include 10 | #include 11 | #include 12 | 13 | using ecl::CubicSpline; 14 | 15 | namespace PathPlanner { 16 | struct PoseStamped { 17 | double x, y, yaw; 18 | double v; 19 | double t; 20 | }; 21 | 22 | class Planner { 23 | private: 24 | std::vector waypoints_; 25 | CubicSpline spline_x_; 26 | CubicSpline spline_y_; 27 | double spline_max_t_; 28 | 29 | void computeSplineWithPoints(); 30 | double nearestPointOnSpline(double x, double y, double eps = 1e-8); 31 | double lengthOfSpline(double a, double b); 32 | double pointWithFixedDistance(double t, double d, double eps = 1e-8); 33 | double maxCurvature(double l, double r, double numerical_eps = 1e-8); 34 | 35 | double randomFloat(); 36 | public: 37 | Planner(); 38 | 39 | bool loadPath(std::string filename, double scale = 1); 40 | void getPath(const PathPlanner::PoseStamped &cur_pose, double dt, double v_ref, double v_min, double k, double max_brake_accel, int n, std::vector *path); 41 | 42 | void runTests(); 43 | }; 44 | } 45 | 46 | #endif -------------------------------------------------------------------------------- /include/controller_mpc/quintic_solver.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __CAR_MODEL_CONTROLLER_MPC_QUINTIC_SOLVER_H__ 2 | #define __CAR_MODEL_CONTROLLER_MPC_QUINTIC_SOLVER_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace QuinticSolver { 9 | //a[0] + a[1]*x + a[2]*x^2 + a[3]*x^3 + a[4]*x^4 + a[5]*x^5 10 | typedef double QuinticPolynomial[6]; 11 | 12 | inline double evaluatePolynomial(QuinticPolynomial a, double x) { 13 | return a[0] + x * (a[1] + x * (a[2] + x * (a[3] + x * (a[4] + x * a[5])))); 14 | } 15 | 16 | //descartes sign changes on f(x + t) 17 | /* 18 | Calculated using sagemath 19 | 20 | a, b, c, d, e, f, x, t = var("a, b, c, d, e, f, x, t") 21 | 22 | (a+b*(x+t)+c*(x+t)^2+d*(x+t)^3+e*(x+t)^4+f*(x+t)^5).expand().collect(x) 23 | */ 24 | inline int descartesSignChanges(QuinticPolynomial a, double t, double eps = 1e-12) { 25 | double t2 = t * t; 26 | double t3 = t2 * t; 27 | double t4 = t3 * t; 28 | double t5 = t4 * t; 29 | 30 | QuinticPolynomial g = { 31 | a[5]*t5 + a[4]*t4 + a[3]*t3 + a[2]*t2 + a[1]*t + a[0], //0 32 | 5*a[5]*t4 + 4*a[4]*t3 + 3*a[3]*t2 + 2*a[2]*t + a[1], //1 33 | 10*a[5]*t3 + 6*a[4]*t2 + 3*a[3]*t + a[2], //2 34 | 10*a[5]*t2 + 4*a[4]*t + a[3], //3 35 | 5*a[5]*t + a[4], //4 36 | a[5] //5 37 | }; 38 | 39 | int result = 0; 40 | int last_sign = 0; 41 | for(int i = 5; i >= 0; i--) if(std::fabs(g[i]) > eps) { 42 | int sign = (g[i] > 0) ? 1 : -1; 43 | 44 | if((last_sign != 0) && (sign != last_sign)) result++; 45 | last_sign = sign; 46 | } 47 | return result; 48 | } 49 | 50 | 51 | // max number of real roots in (l, r] 52 | /* 53 | Budan's theorem 54 | https://en.wikipedia.org/wiki/Budan%27s_theorem 55 | */ 56 | inline int maxNumberOfRealRoots(QuinticPolynomial a, double l, double r, double eps = 1e-12) { 57 | int result = descartesSignChanges(a, l, eps) - descartesSignChanges(a, r, eps); 58 | 59 | assert(result >= 0); 60 | return result; 61 | } 62 | 63 | // solves all real roots in (l, r] 64 | /* 65 | Budan's theorem for isolation 66 | Bisection for solve roots 67 | 68 | Special: checks whether (f(l) < 0) && (f(r) > 0) 69 | 70 | Reference: Improved Algebraic Algorithm On Point Projection For Bézier Curves https://hal.inria.fr/file/index/docid/518379/filename/Xiao-DiaoChen2007c.pdf 71 | */ 72 | inline void solveRealRootsSpecialForCubicSplineDerivative(QuinticPolynomial a, double l, double r, std::vector *roots, double eps = 1e-8, double numerical_eps = 1e-12) { 73 | roots->clear(); 74 | 75 | struct rootInterval 76 | { 77 | double l, r; 78 | int max_roots; 79 | }; 80 | 81 | std::vector intervals; 82 | intervals.reserve(10); 83 | intervals.push_back({l, r, maxNumberOfRealRoots(a, l, r, numerical_eps)}); 84 | while (intervals.size()) { 85 | rootInterval interval = *intervals.rbegin(); 86 | intervals.pop_back(); 87 | 88 | //end condition 89 | if((interval.max_roots <= 1) || ((interval.r - interval.l) < eps)) { 90 | if(interval.max_roots > 0) { 91 | //special here 92 | if((evaluatePolynomial(a, l) < 0) && (evaluatePolynomial(a, r) > 0)) { 93 | //bisection root solver 94 | double bi_l = interval.l, bi_r = interval.r; 95 | while((bi_r - bi_l) > eps) { 96 | double bi_m = (bi_l + bi_r) / 2; 97 | 98 | if( evaluatePolynomial(a, bi_m) > 0 ) bi_r = bi_m; 99 | else bi_l = bi_m; 100 | } 101 | 102 | roots->push_back( (bi_l + bi_r) / 2 ); 103 | } 104 | } 105 | } 106 | else { 107 | //bisection 108 | double mid = (interval.l + interval.r) / 2; 109 | intervals.push_back({interval.l, mid, maxNumberOfRealRoots(a, interval.l, mid, numerical_eps)}); 110 | intervals.push_back({mid, interval.r, maxNumberOfRealRoots(a, mid, interval.r, numerical_eps)}); 111 | } 112 | } 113 | } 114 | } 115 | 116 | #endif -------------------------------------------------------------------------------- /include/simulator/simulator.h: -------------------------------------------------------------------------------- 1 | #ifndef __CAR_MODEL_SIMULATOR_SIMULATOR_H__ 2 | #define __CAR_MODEL_SIMULATOR_SIMULATOR_H__ 3 | 4 | #include "vec2.h" 5 | 6 | template 7 | //Parameters are SI 8 | struct VehicleParameters { 9 | T a; 10 | T b; 11 | T m; 12 | T I_z; 13 | 14 | T max_steer; 15 | T max_accel; 16 | }; 17 | 18 | template 19 | struct TireParameters { 20 | T B; 21 | T C; 22 | T mu; 23 | }; 24 | 25 | template 26 | struct ExtForceParameters { 27 | T a0; //Air drag coefficient Fd = a0 * v ^ 2 28 | T Ff; //Ground friction coefficient 29 | }; 30 | 31 | template 32 | struct SimulatorParameters { 33 | T dt_; 34 | T display_dt_; 35 | 36 | VehicleParameters vehicle_parameters; 37 | TireParameters tire_parameters; 38 | ExtForceParameters extforce_parameters; 39 | }; 40 | 41 | template 42 | struct SimulatorInput { 43 | T force_; 44 | T steer_; 45 | }; 46 | 47 | template 48 | struct SimulatorState { 49 | T x; 50 | T y; 51 | T v_x; 52 | T v_y; 53 | T phi; 54 | T r; 55 | 56 | T t; 57 | }; 58 | 59 | template 60 | class Simulator { 61 | private: 62 | T f_pajecka(T b, T c, T d, T alpha); 63 | public: 64 | Simulator(); 65 | 66 | void initializeStateAndInput(SimulatorState *state, SimulatorInput *input); 67 | void step(SimulatorState *state, const SimulatorInput &input, const SimulatorParameters ¶m, int num_steps); 68 | }; 69 | 70 | #endif -------------------------------------------------------------------------------- /include/simulator/vec2.h: -------------------------------------------------------------------------------- 1 | #ifndef __CAR_MODEL_SIMULATOR_VEC2_H__ 2 | #define __CAR_MODEL_SIMULATOR_VEC2_H__ 3 | 4 | #include 5 | #include 6 | 7 | template 8 | struct Vec2 { 9 | T x, y; 10 | 11 | inline Vec2 operator*(const T& rhs) const { return {x * rhs, y * rhs}; } 12 | inline Vec2 operator/(const T& rhs) const { return {x / rhs, y / rhs}; } 13 | 14 | inline Vec2 operator+(const Vec2& rhs) const { return {x + rhs.x, y + rhs.y}; } 15 | inline Vec2 operator+=(const Vec2& rhs) { (*this) = (*this) + rhs; return (*this); } 16 | 17 | inline T len() const { return std::sqrt(x * x + y * y); } 18 | }; 19 | 20 | #endif -------------------------------------------------------------------------------- /launch/nocontrol.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /launch/sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /msg/car_state.msg: -------------------------------------------------------------------------------- 1 | # 6DoF Car Model 2 | 3 | float64 x 4 | float64 y 5 | float64 v_x 6 | float64 v_y 7 | float64 phi 8 | float64 r -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | car_model 4 | 0.0.0 5 | The car_model package 6 | 7 | 8 | 9 | 10 | one 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | message_generation 41 | dynamic_reconfigure 42 | 43 | message_generation 44 | dynamic_reconfigure 45 | 46 | 47 | 48 | message_runtime 49 | dynamic_reconfigure 50 | 51 | 52 | 53 | 54 | catkin 55 | roscpp 56 | rospy 57 | std_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | roscpp 62 | rospy 63 | std_msgs 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /param/simulator_racetrack01.yaml: -------------------------------------------------------------------------------- 1 | simulation: 2 | initial_position_x: -0.432000 3 | initial_position_y: 25.105000 -------------------------------------------------------------------------------- /rviz/sim.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Grid1 8 | - /TF1 9 | - /TF1/Frames1 10 | - /PoseArray1 11 | - /PoseArray2 12 | Splitter Ratio: 0.5 13 | Tree Height: 708 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: true 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 220 56 | Reference Frame: 57 | Value: true 58 | - Class: rviz/TF 59 | Enabled: true 60 | Frame Timeout: 15 61 | Frames: 62 | All Enabled: true 63 | base_link: 64 | Value: true 65 | map: 66 | Value: true 67 | wheel_back: 68 | Value: true 69 | wheel_front: 70 | Value: true 71 | Marker Scale: 1 72 | Name: TF 73 | Show Arrows: false 74 | Show Axes: true 75 | Show Names: false 76 | Tree: 77 | map: 78 | base_link: 79 | {} 80 | wheel_back: 81 | {} 82 | wheel_front: 83 | {} 84 | Update Interval: 0 85 | Value: true 86 | - Class: rviz/Axes 87 | Enabled: true 88 | Length: 1 89 | Name: Axes 90 | Radius: 0.10000000149011612 91 | Reference Frame: 92 | Value: true 93 | - Alpha: 1 94 | Buffer Length: 1 95 | Class: rviz/Path 96 | Color: 25; 255; 0 97 | Enabled: true 98 | Head Diameter: 0.30000001192092896 99 | Head Length: 0.20000000298023224 100 | Length: 0.10000000149011612 101 | Line Style: Lines 102 | Line Width: 0.029999999329447746 103 | Name: Path 104 | Offset: 105 | X: 0 106 | Y: 0 107 | Z: 0 108 | Pose Color: 255; 85; 255 109 | Pose Style: None 110 | Radius: 0.009999999776482582 111 | Shaft Diameter: 0.10000000149011612 112 | Shaft Length: 0.10000000149011612 113 | Topic: /track 114 | Unreliable: false 115 | Value: true 116 | - Alpha: 1 117 | Arrow Length: 0.30000001192092896 118 | Axes Length: 0.30000001192092896 119 | Axes Radius: 0.009999999776482582 120 | Class: rviz/PoseArray 121 | Color: 255; 25; 0 122 | Enabled: true 123 | Head Length: 0.07000000029802322 124 | Head Radius: 0.029999999329447746 125 | Name: PoseArray 126 | Shaft Length: 0.23000000417232513 127 | Shaft Radius: 0.009999999776482582 128 | Shape: Arrow (Flat) 129 | Topic: /predicted_trajectory 130 | Unreliable: false 131 | Value: true 132 | - Alpha: 1 133 | Arrow Length: 0.05000000074505806 134 | Axes Length: 0.30000001192092896 135 | Axes Radius: 0.009999999776482582 136 | Class: rviz/PoseArray 137 | Color: 0; 0; 255 138 | Enabled: true 139 | Head Length: 0.07000000029802322 140 | Head Radius: 0.029999999329447746 141 | Name: PoseArray 142 | Shaft Length: 0.23000000417232513 143 | Shaft Radius: 0.009999999776482582 144 | Shape: Arrow (Flat) 145 | Topic: /planned_trajectory 146 | Unreliable: false 147 | Value: true 148 | Enabled: true 149 | Global Options: 150 | Background Color: 48; 48; 48 151 | Default Light: true 152 | Fixed Frame: map 153 | Frame Rate: 30 154 | Name: root 155 | Tools: 156 | - Class: rviz/Interact 157 | Hide Inactive Objects: true 158 | - Class: rviz/MoveCamera 159 | - Class: rviz/Select 160 | - Class: rviz/FocusCamera 161 | - Class: rviz/Measure 162 | - Class: rviz/SetInitialPose 163 | Theta std deviation: 0.2617993950843811 164 | Topic: /initialpose 165 | X std deviation: 0.5 166 | Y std deviation: 0.5 167 | - Class: rviz/SetGoal 168 | Topic: /move_base_simple/goal 169 | - Class: rviz/PublishPoint 170 | Single click: true 171 | Topic: /clicked_point 172 | Value: true 173 | Views: 174 | Current: 175 | Angle: 0 176 | Class: rviz/TopDownOrtho 177 | Enable Stereo Rendering: 178 | Stereo Eye Separation: 0.05999999865889549 179 | Stereo Focal Distance: 1 180 | Swap Stereo Eyes: false 181 | Value: false 182 | Invert Z Axis: false 183 | Name: Current View 184 | Near Clip Distance: 0.009999999776482582 185 | Scale: 76.76531982421875 186 | Target Frame: base_link 187 | Value: TopDownOrtho (rviz) 188 | X: 0 189 | Y: 0 190 | Saved: ~ 191 | Window Geometry: 192 | Displays: 193 | collapsed: false 194 | Height: 1016 195 | Hide Left Dock: false 196 | Hide Right Dock: false 197 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000354fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004000000354000000ce00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000354fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004000000354000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025700fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000035400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 198 | Selection: 199 | collapsed: false 200 | Time: 201 | collapsed: false 202 | Tool Properties: 203 | collapsed: false 204 | Views: 205 | collapsed: false 206 | Width: 1920 207 | X: 0 208 | Y: 27 209 | -------------------------------------------------------------------------------- /src/controller_mpc/controller_mpc.cpp: -------------------------------------------------------------------------------- 1 | #include "controller_mpc/path_planner.h" 2 | #include "controller_mpc/mpc.h" 3 | 4 | #include "ros/ros.h" 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | double gVelocity = 2.0; 13 | double gMinVelocity = 0.5; 14 | double gCurvatureK = 150.0; 15 | 16 | car_model::car_state gState; 17 | PathPlanner::Planner gPlanner; 18 | MPC::IterativeController gController; 19 | 20 | void initializeMPC() { 21 | MPC::Parameters parameters; 22 | parameters.dt = 0.01; 23 | parameters.pred_horizon = 40; 24 | parameters.control_horizon = 20; 25 | 26 | MPC::Model model; 27 | model.l_f = 0.125; 28 | model.l_r = 0.125; 29 | model.m = 1.98; 30 | 31 | MPC::HardConstraint constraint; 32 | constraint.max_accel = 2.0; 33 | constraint.min_accel = -2.0; 34 | constraint.max_steer = 30.0 / 180.0 * M_PI; 35 | 36 | constraint.max_steer_rate = 60.0 / 180.0 * M_PI; 37 | constraint.max_jerk = 10.0; 38 | 39 | MPC::CostFunctionWeights weights; 40 | weights.w_position = 1.0; 41 | weights.w_angle = 0.1; 42 | 43 | weights.w_velocity = 0.025; 44 | 45 | weights.w_steer = 0.01; 46 | weights.w_accel = 0.01; 47 | 48 | weights.w_dsteer = 0; 49 | weights.w_jerk = 0; 50 | 51 | gController.initialize(parameters, model, constraint, weights); 52 | } 53 | 54 | void stateCallback(const car_model::car_state::ConstPtr &state) { 55 | gState = *state; 56 | } 57 | 58 | void resetSimulator() { 59 | while(ros::ok()) { 60 | car_model::simulator_reset simulator_reset_srv; 61 | std::vector poses; 62 | PathPlanner::PoseStamped initial_state; 63 | 64 | initial_state.x = 0; 65 | initial_state.y = 0; 66 | initial_state.yaw = 0; 67 | initial_state.v = 0; 68 | initial_state.t = 0; 69 | gPlanner.getPath(initial_state, 0, 0, 0, 0, 1, 1, &poses); 70 | 71 | simulator_reset_srv.request.x = poses[0].x; 72 | simulator_reset_srv.request.y = poses[0].y; 73 | simulator_reset_srv.request.yaw = poses[0].yaw; 74 | 75 | if(ros::service::call("simulator_reset", simulator_reset_srv)) { 76 | break; 77 | } 78 | 79 | ros::Duration(0.1).sleep(); 80 | ros::spinOnce(); 81 | } 82 | 83 | ros::Duration(0.1).sleep(); 84 | } 85 | 86 | double normalizeAngle(double a) { 87 | return fmod(fmod(a + M_PI, 2 * M_PI) + 2 * M_PI, 2 * M_PI) - M_PI; 88 | } 89 | 90 | int main(int argc, char **argv) { 91 | ros::init(argc, argv, "controller_mpc"); 92 | 93 | //setup communication 94 | ros::NodeHandle nh; 95 | ros::Publisher cmd_vel_pub = nh.advertise("cmd_vel", 10); 96 | ros::Publisher trajectory_pub = nh.advertise("planned_trajectory", 10); 97 | ros::Publisher prediction_pub = nh.advertise("predicted_trajectory", 10); 98 | ros::Subscriber state_sub = nh.subscribe("car_state", 10, stateCallback); 99 | 100 | //load tracks 101 | ros::NodeHandle nh_priv("~"); 102 | std::string track_filename = nh_priv.param("track_filename", "/home/one/Documents/ROS_Workspace/src/car_model/tracks/racetrack01.txt"); 103 | double track_scale = nh_priv.param("track_scale", 0.0025); 104 | gPlanner.loadPath(track_filename, track_scale); 105 | 106 | //reset simulator 107 | resetSimulator(); 108 | 109 | //initialize controller 110 | initializeMPC(); 111 | 112 | ROS_INFO("MPC controller online."); 113 | 114 | double last_steer_angle = 0.0; 115 | double last_accel = 0; 116 | 117 | ros::Rate rate(50); 118 | while (ros::ok()) { 119 | //get state 120 | MPC::State state; 121 | state.x = gState.x; 122 | state.y = gState.y; 123 | state.yaw = normalizeAngle(gState.phi); 124 | state.v = sqrt(gState.v_x * gState.v_x + gState.v_y * gState.v_y); 125 | state.steer_angle = last_steer_angle; 126 | state.accel = last_accel; 127 | 128 | //plan track 129 | PathPlanner::PoseStamped start_pose; 130 | std::vector track; 131 | 132 | start_pose.x = gState.x; 133 | start_pose.y = gState.y; 134 | start_pose.yaw = gState.phi; 135 | start_pose.v = gState.v_x; 136 | start_pose.t = 0; 137 | 138 | gPlanner.getPath(start_pose, gController.parameters_.dt, gVelocity, gMinVelocity, gCurvatureK, 0.25, gController.parameters_.pred_horizon - 1, &track); 139 | 140 | //calculate mpc 141 | MPC::ControlOutput control_output; 142 | std::vector prediction_output; 143 | gController.update(state, track, 3, 0.01, &control_output, &prediction_output); 144 | 145 | //send control commands 146 | geometry_msgs::Twist cmd_vel; 147 | 148 | cmd_vel.linear.x = control_output.accel; 149 | cmd_vel.angular.z = control_output.steer; 150 | 151 | cmd_vel_pub.publish(cmd_vel); 152 | 153 | last_steer_angle = control_output.steer; 154 | last_accel = control_output.accel; 155 | 156 | //visualize track 157 | geometry_msgs::PoseArray planned_trajectory; 158 | planned_trajectory.header.frame_id = "map"; 159 | 160 | for(int i = 0; i < track.size(); i++) { 161 | geometry_msgs::Pose pose; 162 | pose.position.x = track[i].x; 163 | pose.position.y = track[i].y; 164 | pose.position.z = 0; 165 | pose.orientation = tf::createQuaternionMsgFromYaw(track[i].yaw); 166 | 167 | planned_trajectory.poses.push_back(pose); 168 | } 169 | trajectory_pub.publish(planned_trajectory); 170 | 171 | //visualize predicted track 172 | geometry_msgs::PoseArray predicted_trajectory; 173 | predicted_trajectory.header.frame_id = "map"; 174 | 175 | for(int i = 0; i < prediction_output.size(); i++) { 176 | geometry_msgs::Pose pose; 177 | pose.position.x = prediction_output[i].x; 178 | pose.position.y = prediction_output[i].y; 179 | pose.position.z = 0; 180 | pose.orientation = tf::createQuaternionMsgFromYaw(prediction_output[i].yaw); 181 | 182 | predicted_trajectory.poses.push_back(pose); 183 | } 184 | prediction_pub.publish(predicted_trajectory); 185 | 186 | //sleep and spin 187 | rate.sleep(); 188 | ros::spinOnce(); 189 | } 190 | } -------------------------------------------------------------------------------- /src/controller_mpc/mpc.cpp: -------------------------------------------------------------------------------- 1 | #include "controller_mpc/mpc.h" 2 | 3 | template 4 | class MPC::SparseMatrix; 5 | 6 | template 7 | class MPC::QPProblem; 8 | 9 | template 10 | MPC::SparseMatrix::SparseMatrix() { 11 | m_ = n_ = 0; 12 | } 13 | 14 | template 15 | MPC::SparseMatrix::~SparseMatrix() { 16 | elements_.clear(); 17 | freeOSQPCSCInstance(); 18 | } 19 | 20 | template 21 | void MPC::SparseMatrix::freeOSQPCSCInstance() { 22 | osqp_csc_data_.clear(); 23 | osqp_csc_row_idx_.clear(); 24 | osqp_csc_col_start_.clear(); 25 | 26 | if(osqp_csc_instance != nullptr) { 27 | c_free(osqp_csc_instance); 28 | osqp_csc_instance = nullptr; 29 | } 30 | } 31 | 32 | template 33 | void MPC::SparseMatrix::initialize(int m, int n) { 34 | m_ = m; 35 | n_ = n; 36 | elements_.clear(); 37 | } 38 | 39 | template 40 | void MPC::SparseMatrix::addElement(int r, int c, T v) { 41 | elements_.push_back({r, c, v}); 42 | } 43 | 44 | template 45 | csc* MPC::SparseMatrix::toOSQPCSC() { 46 | freeOSQPCSCInstance(); 47 | 48 | sort(elements_.begin(), elements_.end()); 49 | 50 | int idx = 0; 51 | int n_elem = elements_.size(); 52 | 53 | osqp_csc_col_start_.push_back(0); 54 | for(int c = 0; c < n_; c++) { 55 | while((idx < n_elem) && elements_[idx].c == c) { 56 | osqp_csc_data_.push_back(elements_[idx].v); 57 | osqp_csc_row_idx_.push_back(elements_[idx].r); 58 | idx++; 59 | } 60 | 61 | osqp_csc_col_start_.push_back(osqp_csc_data_.size()); 62 | } 63 | 64 | osqp_csc_instance = csc_matrix(m_, n_, osqp_csc_data_.size(), osqp_csc_data_.data(), osqp_csc_row_idx_.data(), osqp_csc_col_start_.data()); 65 | return osqp_csc_instance; 66 | } 67 | 68 | template 69 | MPC::QPProblem::~QPProblem() { 70 | if(osqp_workspace_ != nullptr) { 71 | osqp_workspace_->data->P = nullptr; 72 | osqp_workspace_->data->q = nullptr; 73 | 74 | osqp_workspace_->data->A = nullptr; 75 | osqp_workspace_->data->l = nullptr; 76 | osqp_workspace_->data->u = nullptr; 77 | 78 | //cleanup workspace 79 | osqp_cleanup(osqp_workspace_); 80 | } 81 | } 82 | 83 | template 84 | void MPC::QPProblem::initialize(int n, int m) { 85 | n_ = n; 86 | m_ = m; 87 | 88 | A_.initialize(m_, n_); 89 | l_.resize(m_); 90 | u_.resize(m_); 91 | 92 | std::fill(l_.begin(), l_.end(), 0); 93 | std::fill(u_.begin(), u_.end(), 0); 94 | 95 | P_.initialize(n_, n_); 96 | q_.resize(n_); 97 | 98 | std::fill(q_.begin(), q_.end(), 0); 99 | } 100 | 101 | template 102 | OSQPSolution* MPC::QPProblem::solve(int *error_code) { 103 | //set up workspace 104 | if(osqp_workspace_ == nullptr) { 105 | osqp_settings_ = (OSQPSettings *)c_malloc(sizeof(OSQPSettings)); 106 | osqp_data_ = (OSQPData *) c_malloc(sizeof(OSQPData)); 107 | 108 | //populate data 109 | osqp_data_->n = n_; 110 | osqp_data_->m = m_; 111 | 112 | osqp_data_->A = A_.toOSQPCSC(); 113 | osqp_data_->l = l_.data(); 114 | osqp_data_->u = u_.data(); 115 | 116 | osqp_data_->P = P_.toOSQPCSC(); 117 | osqp_data_->q = q_.data(); 118 | 119 | osqp_set_default_settings(osqp_settings_); 120 | osqp_setup(&osqp_workspace_, osqp_data_, osqp_settings_); 121 | } 122 | else { 123 | csc *A_csc = A_.toOSQPCSC(); 124 | osqp_update_A(osqp_workspace_, A_csc->x, NULL, A_csc->nzmax); 125 | osqp_update_bounds(osqp_workspace_, l_.data(), u_.data()); 126 | 127 | csc *P_csc = P_.toOSQPCSC(); 128 | osqp_update_P(osqp_workspace_, P_csc->x, NULL, P_csc->nzmax); 129 | osqp_update_lin_cost(osqp_workspace_, q_.data()); 130 | } 131 | 132 | *error_code = osqp_solve(osqp_workspace_); 133 | 134 | return osqp_workspace_->solution; 135 | } 136 | 137 | MPC::Controller::Controller() { 138 | 139 | } 140 | 141 | void MPC::Controller::initialize(const Parameters ¶meters, const Model &model, const HardConstraint &constraint, const CostFunctionWeights &weights) { 142 | parameters_ = parameters; 143 | model_ = model; 144 | 145 | constraint_ = constraint; 146 | weights_ = weights; 147 | } 148 | 149 | void MPC::Controller::update(const State &state, const State &linearize_point, const std::vector &track_input, ControlOutput *out, std::vector *pred_out) { 150 | /* 151 | Variable: 152 | [s0 s1 s2 s3 ...](t=0) [s0 s1 s2 s3 ...](t=1) ... [c0 c1 ...](t=0) 153 | */ 154 | const int dim_state = 4; 155 | const int dim_control = 2; 156 | 157 | /* 158 | Sub-functions 159 | */ 160 | auto state_var_idx = [&] (int t, int id) { 161 | return t * dim_state + id; 162 | }; 163 | auto control_var_idx = [&] (int t, int id) { 164 | return parameters_.pred_horizon * dim_state + std::min(t, parameters_.control_horizon - 1) * dim_control + id; 165 | }; 166 | auto control_derivative_row_idx = [&] (int t, int id) { 167 | return dim_state * parameters_.pred_horizon + dim_control * parameters_.control_horizon + std::min(t, parameters_.control_horizon - 1) * dim_control + id; 168 | }; 169 | /*auto normalize_angle = [&] (double a) { 170 | return fmod(fmod(a + M_PI, 2 * M_PI) + 2 * M_PI, 2 * M_PI) - M_PI; 171 | };*/ 172 | auto get_beta_from_steer = [&] (double steer) { 173 | return std::atan(model_.l_r / (model_.l_f + model_.l_r) * std::tan(steer)); 174 | }; 175 | auto get_steer_from_beta = [&] (double beta) { 176 | return std::atan((model_.l_f + model_.l_r) / model_.l_r * std::tan(beta)); 177 | }; 178 | 179 | /* 180 | Setup and solve QP 181 | */ 182 | 183 | int n = dim_state * parameters_.pred_horizon + dim_control * parameters_.control_horizon; //number of variables 184 | int m = n + dim_control * parameters_.control_horizon; //number of constraints 185 | 186 | c_float dt = parameters_.dt; 187 | 188 | qp_.initialize(n, m); 189 | 190 | /* 191 | initialize constraint matrix 192 | */ 193 | 194 | //create initial state constraints 195 | for(int i = 0; i < dim_state; i++) qp_.A_.addElement(state_var_idx(0, i), state_var_idx(0, i), 1); 196 | qp_.l_[0] = qp_.u_[0] = state.x; 197 | qp_.l_[1] = qp_.u_[1] = state.y; 198 | qp_.l_[2] = qp_.u_[2] = state.v; 199 | qp_.l_[3] = qp_.u_[3] = state.yaw; 200 | 201 | c_float linearize_beta = get_beta_from_steer(linearize_point.steer_angle); 202 | 203 | //create state model constraints 204 | for(int t = 1; t < parameters_.pred_horizon; t++) { 205 | //adaptive MPC 206 | 207 | //x 208 | qp_.A_.addElement(state_var_idx(t, 0), state_var_idx(t, 0), -1); 209 | qp_.A_.addElement(state_var_idx(t, 0), state_var_idx(t - 1, 0), 1); 210 | 211 | qp_.A_.addElement(state_var_idx(t, 0), state_var_idx(t - 1, 2), dt * std::cos(linearize_point.yaw + linearize_beta)); 212 | qp_.A_.addElement(state_var_idx(t, 0), state_var_idx(t - 1, 3), dt * -linearize_point.v * std::sin(linearize_point.yaw + linearize_beta)); 213 | qp_.l_[state_var_idx(t, 0)] = qp_.u_[state_var_idx(t, 0)] = -(dt * linearize_point.v * std::sin(linearize_point.yaw + linearize_beta) * (linearize_point.yaw + linearize_beta)); 214 | 215 | //y 216 | qp_.A_.addElement(state_var_idx(t, 1), state_var_idx(t, 1), -1); 217 | qp_.A_.addElement(state_var_idx(t, 1), state_var_idx(t - 1, 1), 1); 218 | 219 | qp_.A_.addElement(state_var_idx(t, 1), state_var_idx(t - 1, 2), dt * std::sin(linearize_point.yaw + linearize_beta)); 220 | qp_.A_.addElement(state_var_idx(t, 1), state_var_idx(t - 1, 3), dt * linearize_point.v * std::cos(linearize_point.yaw + linearize_beta)); 221 | qp_.l_[state_var_idx(t, 1)] = qp_.u_[state_var_idx(t, 1)] = -(-dt * linearize_point.v * std::cos(linearize_point.yaw + linearize_beta) * (linearize_point.yaw + linearize_beta)); 222 | 223 | //v 224 | qp_.A_.addElement(state_var_idx(t, 2), state_var_idx(t, 2), -1); 225 | qp_.A_.addElement(state_var_idx(t, 2), state_var_idx(t - 1, 2), 1); 226 | qp_.A_.addElement(state_var_idx(t, 2), control_var_idx(t - 1, 1), dt); 227 | qp_.l_[state_var_idx(t, 2)] = qp_.u_[state_var_idx(t, 2)] = 0; 228 | 229 | //yaw 230 | qp_.A_.addElement(state_var_idx(t, 3), state_var_idx(t, 3), -1); 231 | qp_.A_.addElement(state_var_idx(t, 3), state_var_idx(t - 1, 3), 1); 232 | qp_.A_.addElement(state_var_idx(t, 3), state_var_idx(t - 1, 2), dt * std::sin(linearize_beta) / model_.l_r); 233 | qp_.A_.addElement(state_var_idx(t, 3), control_var_idx(t - 1, 0), dt * linearize_point.v / model_.l_r * std::cos(linearize_beta)); 234 | qp_.l_[state_var_idx(t, 3)] = qp_.u_[state_var_idx(t, 3)] = -(-dt * linearize_point.v / model_.l_r * std::cos(linearize_beta) * linearize_beta); 235 | } 236 | 237 | //create control output constraints 238 | for(int t = 0; t < parameters_.control_horizon; t++) { 239 | for(int i = 0; i < dim_control; i++) qp_.A_.addElement(control_var_idx(t, i), control_var_idx(t, i), 1); 240 | 241 | qp_.l_[control_var_idx(t, 0)] = get_beta_from_steer(-constraint_.max_steer); 242 | qp_.u_[control_var_idx(t, 0)] = get_beta_from_steer( constraint_.max_steer); 243 | 244 | qp_.l_[control_var_idx(t, 1)] = constraint_.min_accel; 245 | qp_.u_[control_var_idx(t, 1)] = constraint_.max_accel; 246 | } 247 | 248 | //create control derivative constraints 249 | //integral average approximation 250 | double max_delta_beta = dt * constraint_.max_steer_rate * ((get_beta_from_steer(constraint_.max_steer) - get_beta_from_steer(0)) / (constraint_.max_steer - 0)); 251 | double max_delta_accel = dt * constraint_.max_jerk; 252 | 253 | for(int i = 0; i < dim_control; i++) { 254 | qp_.A_.addElement(control_derivative_row_idx(0, i), control_var_idx(0, i), 1); 255 | } 256 | 257 | c_float state_beta = get_beta_from_steer(state.steer_angle); 258 | qp_.l_[control_derivative_row_idx(0, 0)] = state_beta - max_delta_beta; 259 | qp_.u_[control_derivative_row_idx(0, 0)] = state_beta + max_delta_beta; 260 | 261 | qp_.l_[control_derivative_row_idx(0, 1)] = state.accel - max_delta_accel; 262 | qp_.u_[control_derivative_row_idx(0, 1)] = state.accel + max_delta_accel; 263 | 264 | for(int t = 1; t < parameters_.control_horizon; t++) { 265 | for(int i = 0; i < dim_control; i++) { 266 | qp_.A_.addElement(control_derivative_row_idx(t, i), control_var_idx(t, i), 1); 267 | qp_.A_.addElement(control_derivative_row_idx(t, i), control_var_idx(t - 1, i), -1); 268 | } 269 | 270 | qp_.l_[control_derivative_row_idx(t, 0)] = -max_delta_beta; 271 | qp_.u_[control_derivative_row_idx(t, 0)] = max_delta_beta; 272 | 273 | qp_.l_[control_derivative_row_idx(t, 1)] = -max_delta_accel; 274 | qp_.u_[control_derivative_row_idx(t, 1)] = max_delta_accel; 275 | } 276 | 277 | //create cost function 278 | //pred horizon 279 | for(int t = 1; t < parameters_.pred_horizon; t++) { 280 | PathPlanner::PoseStamped pose = track_input[t - 1]; 281 | 282 | /* 283 | Fix yaw +-2pi for QP 284 | */ 285 | if(std::fabs((pose.yaw + M_PI * 2.0) - state.yaw) < std::fabs(pose.yaw - state.yaw)) 286 | pose.yaw += M_PI * 2.0; 287 | 288 | if(std::fabs((pose.yaw - M_PI * 2.0) - state.yaw) < std::fabs(pose.yaw - state.yaw)) 289 | pose.yaw -= M_PI * 2.0; 290 | 291 | //position 292 | qp_.P_.addElement(state_var_idx(t, 0), state_var_idx(t, 0), weights_.w_position); 293 | qp_.q_[state_var_idx(t, 0)] = -weights_.w_position * pose.x; 294 | 295 | qp_.P_.addElement(state_var_idx(t, 1), state_var_idx(t, 1), weights_.w_position); 296 | qp_.q_[state_var_idx(t, 1)] = -weights_.w_position * pose.y; 297 | 298 | //angle 299 | qp_.P_.addElement(state_var_idx(t, 3), state_var_idx(t, 3), weights_.w_angle); 300 | qp_.q_[state_var_idx(t, 3)] = -weights_.w_angle * pose.yaw; 301 | 302 | //velocity 303 | qp_.P_.addElement(state_var_idx(t, 2), state_var_idx(t, 2), weights_.w_velocity); 304 | qp_.q_[state_var_idx(t, 2)] = -weights_.w_velocity * pose.v; 305 | } 306 | 307 | //control horizon 308 | for(int t = 0; t < parameters_.control_horizon; t++) { 309 | //accel 310 | qp_.P_.addElement(control_var_idx(t, 0), control_var_idx(t, 0), weights_.w_accel); 311 | 312 | //steer 313 | qp_.P_.addElement(control_var_idx(t, 1), control_var_idx(t, 1), weights_.w_steer); 314 | } 315 | 316 | //solve qp 317 | OSQPSolution *solution = qp_.solve(&out->error_code); 318 | 319 | out->steer = get_steer_from_beta(solution->x[control_var_idx(0, 0)]); 320 | out->accel = solution->x[control_var_idx(0, 1)]; 321 | 322 | //output predictive result 323 | if(pred_out != nullptr) { 324 | pred_out->clear(); 325 | 326 | for(int t = 0; t < parameters_.pred_horizon; t++) { 327 | State pred_state; 328 | pred_state.x = solution->x[state_var_idx(t, 0)]; 329 | pred_state.y = solution->x[state_var_idx(t, 1)]; 330 | pred_state.v = solution->x[state_var_idx(t, 2)]; 331 | pred_state.yaw = solution->x[state_var_idx(t, 3)]; 332 | 333 | pred_out->push_back(pred_state); 334 | } 335 | } 336 | } 337 | 338 | void MPC::IterativeController::simulate(const State &state, State *next) { 339 | double dt = parameters_.dt; 340 | double beta = std::atan(model_.l_r / (model_.l_f + model_.l_r) * std::tan(state.steer_angle)); 341 | 342 | next->x = state.x + dt * state.v * std::cos(state.yaw + beta); 343 | next->y = state.y + dt * state.v * std::sin(state.yaw + beta); 344 | next->yaw = state.yaw + dt * state.v / model_.l_r * std::sin(beta); 345 | next->v = state.v + dt * state.accel; 346 | 347 | next->accel = state.accel; 348 | next->steer_angle = state.steer_angle; 349 | } 350 | 351 | void MPC::IterativeController::update(const State &state, const std::vector &track_input, int iterations, double threshold, ControlOutput *out, std::vector *pred_out) { 352 | State state_with_new_control_input = state; 353 | 354 | int num_iterations = 0; 355 | for(int i = 0; i < iterations; i++) { 356 | num_iterations++; 357 | 358 | State linearize_point; 359 | simulate(state_with_new_control_input, &linearize_point); 360 | 361 | Controller::update(state, linearize_point, track_input, out, pred_out); 362 | 363 | double delta_output = 364 | std::fabs(state_with_new_control_input.accel - out->accel) + 365 | std::fabs(state_with_new_control_input.steer_angle - out->steer); 366 | 367 | if(delta_output < threshold) break; 368 | 369 | state_with_new_control_input.accel = out->accel; 370 | state_with_new_control_input.steer_angle = out->steer; 371 | } 372 | 373 | printf("%d iterations\n", num_iterations); 374 | } -------------------------------------------------------------------------------- /src/controller_mpc/path_planner.cpp: -------------------------------------------------------------------------------- 1 | #include "controller_mpc/path_planner.h" 2 | #include "controller_mpc/quintic_solver.hpp" 3 | 4 | PathPlanner::Planner::Planner() { 5 | 6 | } 7 | 8 | bool PathPlanner::Planner::loadPath(std::string filename, double scale) { 9 | waypoints_.clear(); 10 | 11 | FILE *file = fopen(filename.c_str(), "rt"); 12 | if(file != nullptr) { 13 | double x, y; 14 | while(fscanf(file, "%lf,%lf", &x, &y) != EOF) { 15 | geometry_msgs::Point point; 16 | 17 | point.x = scale * x; 18 | point.y = scale * y; 19 | point.z = 0; 20 | 21 | waypoints_.push_back(point); 22 | } 23 | fclose(file); 24 | 25 | computeSplineWithPoints(); 26 | 27 | return true; 28 | } 29 | 30 | return false; 31 | } 32 | 33 | void PathPlanner::Planner::computeSplineWithPoints() { 34 | int num_waypoints = int(waypoints_.size()); 35 | if(num_waypoints <= 0) { 36 | return; 37 | } 38 | 39 | //create x and y line array 40 | static ecl::Array x_list, y_list, t_list; 41 | x_list.resize(num_waypoints); 42 | y_list.resize(num_waypoints); 43 | t_list.resize(num_waypoints); 44 | 45 | double t = 0; 46 | for(int i = 0; i < num_waypoints; i++) { 47 | x_list[i] = waypoints_[i].x; 48 | y_list[i] = waypoints_[i].y; 49 | t_list[i] = t; 50 | 51 | t += 1; 52 | } 53 | 54 | spline_max_t_ = t_list[num_waypoints - 1]; 55 | 56 | //interpolate parametric cubic spline 57 | spline_x_ = CubicSpline::Natural(t_list, x_list); 58 | spline_y_ = CubicSpline::Natural(t_list, y_list); 59 | } 60 | 61 | /* 62 | Assume that a, b >= 0 63 | */ 64 | double PathPlanner::Planner::lengthOfSpline(double a, double b) { 65 | const double gauss_quadrature_coeff[5][2] = { 66 | { 0.0, 0.5688888888888889 }, 67 | { -0.5384693101056831, 0.47862867049936647 }, 68 | { 0.5384693101056831, 0.47862867049936647 }, 69 | { -0.906179845938664, 0.23692688505618908 }, 70 | { 0.906179845938664, 0.23692688505618908 } 71 | }; 72 | 73 | double length = 0; 74 | int max_segments = int(spline_x_.polynomials().size()) - 1; 75 | for(int segment = int(a); segment <= std::min(int(b), max_segments); segment++) { 76 | double start = std::max(a, double(segment)); 77 | double end = std::min(b, double(segment + 1)); 78 | 79 | //get coefficients 80 | const ecl::CubicPolynomial::Coefficients& coeff_x = spline_x_.polynomials()[segment].coefficients(); 81 | const ecl::CubicPolynomial::Coefficients& coeff_y = spline_y_.polynomials()[segment].coefficients(); 82 | 83 | //integrate this segment 84 | double integral = 0; 85 | for(int k = 0; k < 5; k++) { 86 | double x = ((end - start) / 2) * gauss_quadrature_coeff[k][0] + ((start + end) / 2); 87 | 88 | double A_x = coeff_x[1] + 2 * coeff_x[2] * x + 3 * coeff_x[3] * x * x; 89 | double B_x = coeff_y[1] + 2 * coeff_y[2] * x + 3 * coeff_y[3] * x * x; 90 | double F_x = std::sqrt( A_x * A_x + B_x * B_x ); 91 | 92 | integral += gauss_quadrature_coeff[k][1] * F_x; 93 | } 94 | integral *= (end - start) / 2; 95 | 96 | length += integral; 97 | } 98 | 99 | return length; 100 | } 101 | 102 | double PathPlanner::Planner::nearestPointOnSpline(double x, double y, double eps) { 103 | double min_dist_2 = 1.0 / 0.0; 104 | double min_t = 0; 105 | 106 | for(int segment = 0; segment < spline_max_t_; segment++) { 107 | //get 5th polynomial 108 | /* 109 | evaluated using sagemath 110 | 111 | a, b, c, d = var("a, b, c, d") 112 | e, f, g, h = var("e, f, g, h") 113 | x, y = var("x, y") 114 | 115 | f_x(t) = a + b * t + c * t^2 + d * t^3 116 | f_y(t) = e + f * t + g * t^2 + h * t^3 117 | 118 | df_x(t) = b + 2*c*t + 3*d*t^2 119 | df_y(t) = f + 2*g*t + 3*h*t^2 120 | 121 | -( 122 | (x - f_x(t)) * df_x(t) + (y - f_y(t)) * df_y(t) 123 | ).expand().collect(t) 124 | */ 125 | 126 | const ecl::CubicPolynomial::Coefficients& coeff_x = spline_x_.polynomials()[segment].coefficients(); 127 | const ecl::CubicPolynomial::Coefficients& coeff_y = spline_y_.polynomials()[segment].coefficients(); 128 | 129 | double a = coeff_x[0]; 130 | double b = coeff_x[1]; 131 | double c = coeff_x[2]; 132 | double d = coeff_x[3]; 133 | 134 | double e = coeff_y[0]; 135 | double f = coeff_y[1]; 136 | double g = coeff_y[2]; 137 | double h = coeff_y[3]; 138 | 139 | QuinticSolver::QuinticPolynomial poly = { 140 | a*b + e*f - b*x - f*y, //0 141 | (b*b + 2*a*c + f*f + 2*e*g - 2*c*x - 2*g*y), //1 142 | 3*(b*c + a*d + f*g + e*h - d*x - h*y), //2 143 | 2*(c*c + 2*b*d + g*g + 2*f*h), //3 144 | 5*(c*d + g*h), //4 145 | 3*(d*d + h*h) //5 146 | }; 147 | 148 | //solve 5th polynomial 149 | std::vector sol_t; 150 | QuinticSolver::solveRealRootsSpecialForCubicSplineDerivative(poly, double(segment), double(segment + 1), &sol_t, eps); 151 | 152 | //get nearest 153 | for(auto it = sol_t.begin(); it != sol_t.end(); it++) { 154 | double t = *it; 155 | double dx = x - spline_x_(t); 156 | double dy = y - spline_y_(t); 157 | double dist_2 = dx * dx + dy * dy; 158 | 159 | if(dist_2 < min_dist_2) { 160 | min_dist_2 = dist_2; 161 | min_t = t; 162 | } 163 | } 164 | } 165 | 166 | return min_t; 167 | } 168 | 169 | double PathPlanner::Planner::pointWithFixedDistance(double t, double d, double eps) { 170 | if(d < eps) return t; 171 | 172 | //bi-section 173 | double l = t; 174 | double r = spline_max_t_; 175 | 176 | while((r - l) > eps) { 177 | double m = (l + r) / 2; 178 | 179 | if(lengthOfSpline(t, m) <= d) l = m; 180 | else r = m; 181 | } 182 | 183 | return (l + r) / 2; 184 | } 185 | 186 | double PathPlanner::Planner::maxCurvature(double l, double r, double numerical_eps) { 187 | double max_curvature = 0; 188 | 189 | int max_segments = int(spline_x_.polynomials().size()) - 1; 190 | for(int segment = int(l); segment <= std::min(int(r), max_segments); segment++) { 191 | double start = std::max(l, double(segment)); 192 | double end = std::min(r, double(segment + 1)); 193 | 194 | const ecl::CubicPolynomial::Coefficients& coeff_x = spline_x_.polynomials()[segment].coefficients(); 195 | const ecl::CubicPolynomial::Coefficients& coeff_y = spline_y_.polynomials()[segment].coefficients(); 196 | 197 | //quadratic equation of curvature = a * x^2 + b * x + c 198 | double a = 36 * (coeff_x[3] * coeff_x[3] + coeff_y[3] * coeff_y[3]); 199 | double b = 24 * (coeff_x[2] * coeff_x[3] + coeff_y[2] * coeff_y[3]); 200 | double c = 4 * (coeff_x[2] * coeff_x[2] + coeff_y[2] * coeff_y[2]); 201 | 202 | if(std::fabs(b) > numerical_eps) { 203 | double x = -a / b; 204 | if((x >= start) && (x <= end)) max_curvature = std::max(max_curvature, c + x * (b + x * a)); 205 | } 206 | 207 | max_curvature = std::max(max_curvature, c + start * (b + start * a)); 208 | max_curvature = std::max(max_curvature, c + end * (b + end * a)); 209 | } 210 | 211 | return max_curvature; 212 | } 213 | 214 | void PathPlanner::Planner::getPath(const PathPlanner::PoseStamped &cur_pose, double dt, double v_ref, double v_min, double k, double max_brake_accel, int n, std::vector *path) { 215 | //initial pose 216 | path->clear(); 217 | 218 | //find nearest point on trajectory 219 | double t_start = nearestPointOnSpline(cur_pose.x, cur_pose.y); 220 | 221 | //calculate max curvature radius 222 | /*double brake_time = std::max(v_min, cur_pose.v) / max_brake_accel; 223 | double horizon_length = cur_pose.v * brake_time - 0.5 * max_brake_accel * brake_time * brake_time; 224 | double max_curvature = maxCurvature(t_start, pointWithFixedDistance(t_start, horizon_length)); 225 | 226 | //calculate speed 227 | double eps = 1e-8; 228 | double v = v_ref; 229 | if(max_curvature > eps) { 230 | //max velocity at certain radius 231 | double max_v = k * (1 / max_curvature); 232 | 233 | v = std::min(v, std::max(v_min, max_v)); 234 | } 235 | 236 | printf("h: %lf, v: %lf\n", horizon_length, v);*/ 237 | 238 | double v = v_ref; 239 | 240 | double time = dt; 241 | 242 | for(int i = 0; i < n; i++) { 243 | double t = pointWithFixedDistance(t_start, v * time); 244 | 245 | path->push_back(PathPlanner::PoseStamped({ 246 | spline_x_(t), spline_y_(t), std::atan2(spline_y_.derivative(t), spline_x_.derivative(t)), 247 | v, 248 | time 249 | })); 250 | 251 | time += dt; 252 | } 253 | } 254 | 255 | double PathPlanner::Planner::randomFloat() { 256 | static std::random_device rd; 257 | static std::mt19937 e(rd()); 258 | static std::uniform_real_distribution<> dist(0, 1); 259 | 260 | return dist(e); 261 | } 262 | 263 | void PathPlanner::Planner::runTests() { 264 | fprintf(stderr, "PathPlanner Tests begin.\n"); 265 | 266 | clock_t t_start, t_end; 267 | double t_total; 268 | int n_compute; 269 | 270 | //spline length 271 | t_start = clock(); 272 | n_compute = 0; 273 | 274 | for(int i = 0; i < 100; i++) { 275 | double start_point = randomFloat() * spline_max_t_; 276 | 277 | double dt = spline_max_t_ / 10000; 278 | double last_len = -1; 279 | 280 | for(double t = start_point; t < spline_max_t_; t += dt) { 281 | double len = lengthOfSpline(0, t); 282 | n_compute++; 283 | 284 | assert(len > last_len); 285 | 286 | last_len = len; 287 | } 288 | } 289 | 290 | t_end = clock(); 291 | t_total = double(t_end - t_start) * 1000 / CLOCKS_PER_SEC / n_compute; 292 | 293 | fprintf(stderr, "[OK] Spline length, %lf msec/call.\n", t_total); 294 | 295 | //point on spline 296 | t_start = clock(); 297 | n_compute = 0; 298 | 299 | double max_error = 0; 300 | for(int i = 0; i < 10000; i++) { 301 | double t = randomFloat() * spline_max_t_; 302 | double x = spline_x_(t); 303 | double y = spline_y_(t); 304 | 305 | double t_estimated = nearestPointOnSpline(x, y); 306 | double x_estimated = spline_x_(t_estimated); 307 | double y_estimated = spline_y_(t_estimated); 308 | 309 | double error = std::sqrt((x - x_estimated) * (x - x_estimated) + (y - y_estimated) * (y - y_estimated)); 310 | max_error = std::max(max_error, error); 311 | 312 | n_compute++; 313 | } 314 | 315 | t_end = clock(); 316 | t_total = double(t_end - t_start) * 1000 / CLOCKS_PER_SEC / n_compute; 317 | 318 | fprintf(stderr, "[OK] Spline point, max error = %lf, %lf msec/call.\n", max_error, t_total); 319 | 320 | //point with fixed distance 321 | t_start = clock(); 322 | n_compute = 0; 323 | 324 | max_error = 0; 325 | for(int i = 0; i < 10000; i++) { 326 | double t = randomFloat() * spline_max_t_; 327 | double d = randomFloat() * lengthOfSpline(t, spline_max_t_); 328 | 329 | double t_next = pointWithFixedDistance(t, d); 330 | assert(t_next >= t); 331 | 332 | double error = std::fabs(lengthOfSpline(t, t_next) - d); 333 | 334 | max_error = std::max(max_error, error); 335 | 336 | n_compute++; 337 | } 338 | 339 | t_end = clock(); 340 | t_total = double(t_end - t_start) * 1000 / CLOCKS_PER_SEC / n_compute; 341 | 342 | fprintf(stderr, "[OK] Point with distance, max error = %lf, %lf msec/call.\n", max_error, t_total); 343 | } -------------------------------------------------------------------------------- /src/simulator/main.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "simulator/simulator.h" 11 | #define SIMULATOR_DATA_TYPE double 12 | 13 | Simulator gSimulator; 14 | SimulatorInput gInput; 15 | SimulatorState gState; 16 | SimulatorParameters gParam; 17 | std::mutex gSimulateMutex; 18 | 19 | car_model::simulator_reset::Request gResetRequest; 20 | bool gResetRequestFlag = false; 21 | 22 | void loadParameters() { 23 | ros::NodeHandle nh_priv("~"); 24 | 25 | nh_priv.param("simulation/dt", gParam.dt_, 0.001); 26 | nh_priv.param("simulation/display_dt", gParam.display_dt_, 0.01); 27 | 28 | //default parameter setting from BARC https://github.com/MPC-Berkeley/barc 29 | nh_priv.param("vehicle/m", gParam.vehicle_parameters.m, 1.98); 30 | nh_priv.param("vehicle/I_z", gParam.vehicle_parameters.I_z, 0.24); 31 | nh_priv.param("vehicle/a", gParam.vehicle_parameters.a, 0.125); 32 | nh_priv.param("vehicle/b", gParam.vehicle_parameters.b, 0.125); 33 | 34 | nh_priv.param("vehicle/max_steer", gParam.vehicle_parameters.max_steer, M_PI / 2); 35 | nh_priv.param("vehicle/max_accel", gParam.vehicle_parameters.max_accel, 2.0); 36 | 37 | nh_priv.param("tire/B", gParam.tire_parameters.B, 0.3); 38 | nh_priv.param("tire/C", gParam.tire_parameters.C, 1.25); 39 | nh_priv.param("tire/mu", gParam.tire_parameters.mu, 0.234); 40 | 41 | nh_priv.param("extforce/a0", gParam.extforce_parameters.a0, 0.1308); 42 | nh_priv.param("extforce/Kf", gParam.extforce_parameters.Ff, 0.1711); 43 | } 44 | 45 | void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg) { 46 | gSimulateMutex.lock(); 47 | 48 | gInput.force_ = msg->linear.x; 49 | gInput.steer_ = msg->angular.z; 50 | 51 | gSimulateMutex.unlock(); 52 | } 53 | 54 | bool simulatorResetServiceCallback(car_model::simulator_reset::Request &request, car_model::simulator_reset::Response &response) { 55 | gSimulateMutex.lock(); 56 | gResetRequest = request; 57 | gResetRequestFlag = true; 58 | gSimulateMutex.unlock(); 59 | 60 | return true; 61 | } 62 | 63 | void simulatorThread() { 64 | ros::Rate loop_rate(1.0 / gParam.display_dt_); 65 | 66 | //time synchorization 67 | gSimulateMutex.lock(); 68 | gState.t = ros::Time::now().toSec(); 69 | gSimulateMutex.unlock(); 70 | 71 | SimulatorInput input; 72 | SimulatorState state; 73 | while (ros::ok()) { 74 | gSimulateMutex.lock(); 75 | 76 | //Reset state request 77 | if(gResetRequestFlag) { 78 | gResetRequestFlag = false; 79 | 80 | gSimulator.initializeStateAndInput(&gState, &gInput); 81 | 82 | gState.x = gResetRequest.x; 83 | gState.y = gResetRequest.y; 84 | gState.phi = gResetRequest.yaw; 85 | 86 | gState.t = ros::Time::now().toSec(); 87 | } 88 | 89 | input = gInput; 90 | state = gState; 91 | 92 | gSimulateMutex.unlock(); 93 | 94 | int num_steps = int(std::lround((ros::Time::now().toSec() - state.t) / gParam.dt_)); 95 | gSimulator.step(&state, input, gParam, num_steps); 96 | 97 | gSimulateMutex.lock(); 98 | gState = state; 99 | gSimulateMutex.unlock(); 100 | 101 | loop_rate.sleep(); 102 | } 103 | } 104 | 105 | int main(int argc, char **argv) { 106 | ros::init(argc, argv, "car_simulator"); 107 | 108 | ros::NodeHandle nh; 109 | ros::Subscriber cmd_vel_sub = nh.subscribe("cmd_vel", 10, cmdVelCallback); 110 | ros::ServiceServer service = nh.advertiseService("simulator_reset", simulatorResetServiceCallback); 111 | ros::Publisher state_pub = nh.advertise("car_state", 10); 112 | tf::TransformBroadcaster pos_pub_tf; 113 | 114 | loadParameters(); 115 | gSimulator.initializeStateAndInput(&gState, &gInput); 116 | 117 | //create simulator thread 118 | std::thread simulator_thread(simulatorThread); 119 | 120 | //main loop 121 | ros::Rate loop_rate(1.0 / gParam.display_dt_); 122 | while (ros::ok()) { 123 | tf::Transform transform; 124 | tf::Quaternion q; 125 | 126 | //read state 127 | SimulatorState state; 128 | gSimulateMutex.lock(); 129 | state = gState; 130 | gSimulateMutex.unlock(); 131 | 132 | //broadcast state 133 | car_model::car_state state_msg; 134 | state_msg.x = state.x; 135 | state_msg.y = state.y; 136 | state_msg.v_x = state.v_x; 137 | state_msg.v_y = state.v_y; 138 | state_msg.phi = state.phi; 139 | state_msg.r = state.r; 140 | state_pub.publish(state_msg); 141 | 142 | //broadcast CoM tf 143 | ros::Time state_stamp(state.t); 144 | 145 | transform.setOrigin( tf::Vector3(state.x, state.y, 0.0) ); 146 | q.setRPY(0, 0, state.phi); 147 | transform.setRotation(q); 148 | 149 | pos_pub_tf.sendTransform(tf::StampedTransform(transform, state_stamp, "map", "base_link")); 150 | 151 | //broadcast wheel tf 152 | transform.setOrigin( tf::Vector3(state.x + gParam.vehicle_parameters.a * std::cos(state.phi), state.y + gParam.vehicle_parameters.a * std::sin(state.phi), 0.0) ); 153 | q.setRPY(0, 0, state.phi + gInput.steer_); 154 | transform.setRotation(q); 155 | 156 | pos_pub_tf.sendTransform(tf::StampedTransform(transform, state_stamp, "map", "wheel_front")); 157 | 158 | transform.setOrigin( tf::Vector3(state.x - gParam.vehicle_parameters.a * std::cos(state.phi), state.y - gParam.vehicle_parameters.a * std::sin(state.phi), 0.0) ); 159 | q.setRPY(0, 0, state.phi); 160 | transform.setRotation(q); 161 | 162 | pos_pub_tf.sendTransform(tf::StampedTransform(transform, state_stamp, "map", "wheel_back")); 163 | 164 | //sleep and spin 165 | loop_rate.sleep(); 166 | ros::spinOnce(); 167 | } 168 | 169 | //wait for simulator thread 170 | simulator_thread.join(); 171 | 172 | return 0; 173 | } -------------------------------------------------------------------------------- /src/simulator/simulator.cpp: -------------------------------------------------------------------------------- 1 | #include "simulator/simulator.h" 2 | 3 | template 4 | class Simulator; 5 | template 6 | class Simulator; 7 | 8 | template 9 | Simulator::Simulator() { 10 | 11 | } 12 | 13 | template 14 | void Simulator::initializeStateAndInput(SimulatorState *state, SimulatorInput *input) { 15 | state->x = 0; 16 | state->y = 0; 17 | 18 | state->v_x = 0; 19 | state->v_y = 0; 20 | 21 | state->phi = 0; 22 | state->r = 0; 23 | 24 | state->t = 0; 25 | 26 | input->steer_ = 0; 27 | input->force_ = 0; 28 | } 29 | 30 | template 31 | T Simulator::f_pajecka(T b, T c, T d, T alpha) { 32 | /* 33 | f_pajecka = d*sin(c*atan(b*alpha)) 34 | 35 | inputs : 36 | * trMdl := tire model, a list or tuple of parameters (b,c,d) 37 | * alpha := tire slip angle [radians] 38 | outputs : 39 | * Fy := lateral force from tire [Newtons] 40 | */ 41 | 42 | return d*std::sin(c*std::atan(b*alpha)); 43 | } 44 | 45 | template 46 | void Simulator::step(SimulatorState *state, const SimulatorInput &input, const SimulatorParameters ¶m, int num_steps) { 47 | T dt = param.dt_; 48 | 49 | //Naive model 50 | T max_delta = param.vehicle_parameters.max_steer; 51 | T delta = std::min(max_delta, std::max(-max_delta, input.steer_)); 52 | T a = std::min(std::max(-param.vehicle_parameters.max_accel, input.force_), param.vehicle_parameters.max_accel); 53 | 54 | for (int step = 0; step < num_steps; step++) { 55 | T beta = std::atan(param.vehicle_parameters.b / (param.vehicle_parameters.a + param.vehicle_parameters.b) * std::tan(delta)); 56 | 57 | T x_next = state->x + dt * state->v_x * std::cos(state->phi + beta); 58 | T y_next = state->y + dt * state->v_x * std::sin(state->phi + beta); 59 | T phi_next = state->phi + dt * state->v_x / param.vehicle_parameters.b * sin(beta); 60 | T v_next = state->v_x + a * dt; 61 | 62 | state->x = x_next; 63 | state->y = y_next; 64 | state->phi = phi_next; 65 | state->v_x = v_next; 66 | state->v_y = 0; 67 | state->r = 0; 68 | 69 | state->t += dt; 70 | } 71 | 72 | //Reference: https://github.com/MPC-Berkeley/barc/blob/master/workspace/src/barc/src/estimation/system_models.py 73 | 74 | //T eps = 1e-8; 75 | 76 | //extract parameters 77 | /*T max_d_f = param.vehicle_parameters.max_steer; 78 | T d_f = std::min(max_d_f, std::max(-max_d_f, input.steer_)); 79 | T FxR = std::min(std::max(-param.vehicle_parameters.max_accel, input.force_), param.vehicle_parameters.max_accel); 80 | 81 | //T d_f = input.steer_; 82 | //T FxR = input.force_; 83 | 84 | T a = param.vehicle_parameters.a; 85 | T b = param.vehicle_parameters.b; 86 | T m = param.vehicle_parameters.m; 87 | T I_z = param.vehicle_parameters.I_z; 88 | 89 | T a0 = param.extforce_parameters.a0; 90 | T Ff = param.extforce_parameters.Ff; 91 | 92 | T B = param.tire_parameters.B; 93 | T C = param.tire_parameters.C; 94 | T mu = param.tire_parameters.mu; 95 | 96 | T g = 9.81; 97 | T Fn = m * g / 2.0; 98 | 99 | for (int step = 0; step < num_steps; step++) { 100 | // limit force to tire friction circle 101 | if(FxR >= mu * Fn) { 102 | FxR = mu * Fn; 103 | } 104 | 105 | // compute the front/rear slip [rad/s] 106 | // ref: Hindiyeh Thesis, p58 107 | T a_F = std::atan2(state->v_y + a * state->r, state->v_x) - d_f; 108 | T a_R = std::atan2(state->v_y - b * state->r, state->v_x); 109 | 110 | // compute lateral tire force at the front 111 | T FyF = -f_pajecka(B, C, mu*Fn, a_F); 112 | 113 | // compute lateral tire force at the rear 114 | // ensure that magnitude of longitudinal/lateral force lie within friction circle 115 | //FIXME: Sign error? 116 | T FyR_paj = -f_pajecka(B, C, mu*Fn, a_R); 117 | T FyR_max = std::sqrt((mu*Fn)*(mu*Fn) - FxR * FxR); 118 | T FyR = std::max(-FyR_max, std::min(FyR_max, FyR_paj)); 119 | 120 | //compute force 121 | T x_force = FxR - FyF*std::sin(d_f); 122 | T y_force = FyF * std::cos(d_f) + FyR; 123 | 124 | //compute friction 125 | x_force += ( -((state->v_x > 0) ? 1 : -1) * (a0 * state->v_x * state->v_x ) ) + (-((state->v_x > 0) ? 1 : -1) * Ff); 126 | 127 | // compute next state 128 | T x_next = state->x + dt * (state->v_x*std::cos(state->phi) - state->v_y * std::sin(state->phi)); 129 | T y_next = state->y + dt * (state->v_x*std::sin(state->phi) + state->v_y * std::cos(state->phi)); 130 | T phi_next = state->phi + dt * state->r; 131 | T v_x_next = state->v_x + dt * (state->r * state->v_y + (1/m) * x_force); 132 | T v_y_next = state->v_y + dt * (-state->r * state->v_x + (1/m) * y_force); 133 | T r_next = state->r + dt / I_z * (a*FyF*std::cos(d_f) - b * FyR); 134 | 135 | state->x = x_next; 136 | state->y = y_next; 137 | state->phi = phi_next; 138 | state->v_x = v_x_next; 139 | state->v_y = v_y_next; 140 | state->r = r_next; 141 | 142 | state->t += dt; 143 | }*/ 144 | } -------------------------------------------------------------------------------- /src/test/test.cpp: -------------------------------------------------------------------------------- 1 | #include "unittest.hpp" 2 | #include "test_planner.hpp" 3 | #include "test_qp.hpp" 4 | 5 | int main() { 6 | Test_Planner testPlanner; 7 | testPlanner.run(); 8 | 9 | Test_QP testQP; 10 | testQP.run(); 11 | } -------------------------------------------------------------------------------- /src/test/test_planner.hpp: -------------------------------------------------------------------------------- 1 | #include "unittest.hpp" 2 | #include "controller_mpc/path_planner.h" 3 | 4 | const double test_scale = 0.0025; 5 | const std::string test_data = "-432.000,25105.000\n369.000,21978.000\n-2634.000,20113.000\n-5571.000,22050.000\n-8951.000,21458.000\n-9051.000,17624.000\n-4456.000,15356.000\n-3422.000,13139.000\n-1326.000,12162.000\n-2004.000,8018.000\n-4686.000,5332.000\n-7984.000,5762.000\n-6360.000,10108.000\n-5777.000,13501.000\n-10103.000,14267.000\n-12399.000,9573.000\n-14768.000,8884.000\n-15975.000,10085.000\n-13981.000,12918.000\n-13378.000,15714.000\n-13934.000,18535.000\n-17565.000,20527.000\n-19545.000,22525.000\n-18134.000,24751.000\n-12959.000,23080.000\n-10419.000,24898.000\n-11654.000,27816.000\n-13287.000,30324.000\n-15080.000,33211.000\n-16563.000,36746.000\n-16174.000,39635.000\n-13398.000,40445.000\n-10227.000,40000.000\n-6835.000,39460.000\n-6643.000,33503.000\n-2985.000,33003.000\n-2239.000,37010.000\n234.000,37397.000\n3143.000,36732.000\n5083.000,36062.000\n7314.000,35321.000\n9885.000,34234.000\n12010.000,33204.000\n14905.000,30727.000\n15267.000,28862.000\n14634.000,27093.000\n13534.000,25188.000\n11781.000,24865.000\n10574.000,26700.000\n10443.000,29648.000\n7243.000,30504.000\n5773.000,33551.000\n2374.000,33081.000\n1438.000,28193.000\n-33.000,26699.000\n-202.000,25805.000\n-432.000,25105.000"; 6 | 7 | class Test_Planner : public UnitTest { 8 | public: 9 | void run() { 10 | testInfo("Testing planner..."); 11 | 12 | std::string testdata_filename = "__test_path_data.txt"; 13 | testInfo("Writing test data to " + testdata_filename); 14 | 15 | FILE *f = fopen(testdata_filename.c_str(), "wt"); 16 | fprintf(f, "%s", test_data.c_str()); 17 | fclose(f); 18 | 19 | PathPlanner::Planner planner; 20 | planner.loadPath(testdata_filename, test_scale); 21 | planner.runTests(); 22 | 23 | testPass("Planner"); 24 | } 25 | }; -------------------------------------------------------------------------------- /src/test/test_qp.hpp: -------------------------------------------------------------------------------- 1 | #include "unittest.hpp" 2 | #include "controller_mpc/mpc.h" 3 | 4 | class Test_QP : public UnitTest { 5 | public: 6 | void run() { 7 | testInfo("Testing QP..."); 8 | MPC::QPProblem qp; 9 | 10 | double inf = 1e8; 11 | 12 | qp.initialize(2, 3); 13 | qp.A_.addElement(0, 0, 2); 14 | qp.A_.addElement(0, 1, 3); 15 | qp.A_.addElement(1, 0, 1); 16 | qp.A_.addElement(2, 1, 1); 17 | qp.l_ = {4, 0, 0}; 18 | qp.u_ = {inf, inf, inf}; 19 | 20 | qp.P_.addElement(0, 0, 3); 21 | qp.P_.addElement(1, 1, 1); 22 | qp.P_.addElement(0, 1, 2); 23 | 24 | qp.q_ = {1, 6}; 25 | 26 | int errcode; 27 | OSQPSolution *sol = qp.solve(&errcode); 28 | 29 | testAssert(errcode == 0, "QP solver fail"); 30 | 31 | testPass("QP"); 32 | } 33 | }; -------------------------------------------------------------------------------- /src/test/unittest.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __CAR_MODEL_SRC_TEST_TEST_H__ 2 | #define __CAR_MODEL_SRC_TEST_TEST_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | class UnitTest { 10 | protected: 11 | void testAssert(bool x, std::string error_msg) { 12 | if(!x) { 13 | std::cerr << "!!![TEST ERROR]!!! " << error_msg << std::endl; 14 | 15 | exit(-1); 16 | } 17 | } 18 | 19 | void testInfo(std::string info_msg) { 20 | std::cerr << "[INFO] " << info_msg << std::endl; 21 | } 22 | 23 | void testPass(std::string name) { 24 | std::cerr << "[PASS] " << name << std::endl; 25 | } 26 | 27 | double randomFloat(double l = 0., double r = 1.) { 28 | static std::random_device rd; 29 | static std::mt19937 e(rd()); 30 | static std::uniform_real_distribution<> dist(0, 1); 31 | 32 | assert(l <= r); 33 | 34 | return l + dist(e) * (r - l); 35 | } 36 | public: 37 | void run() {} 38 | }; 39 | 40 | #endif -------------------------------------------------------------------------------- /src/track_publisher/track_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include 3 | #include 4 | #include 5 | 6 | using ecl::CubicSpline; 7 | 8 | std::vector gWaypoints; 9 | nav_msgs::Path gPath; 10 | 11 | void loadWaypoints(std::string filename, double scale, std::vector *waypoints) { 12 | FILE *file = fopen(filename.c_str(), "rt"); 13 | 14 | double x, y; 15 | while(fscanf(file, "%lf,%lf", &x, &y) != EOF) { 16 | geometry_msgs::Point point; 17 | 18 | point.x = scale * x; 19 | point.y = scale * y; 20 | point.z = 0; 21 | 22 | waypoints->push_back(point); 23 | } 24 | fclose(file); 25 | } 26 | 27 | void interpolatePathNaive(const std::vector &waypoints, nav_msgs::Path *path) { 28 | path->header.frame_id = "map"; 29 | 30 | path->poses.clear(); 31 | for(int i = 0; i < waypoints.size(); i++) { 32 | geometry_msgs::PoseStamped pose; 33 | 34 | pose.header.stamp = ros::Time(0); 35 | //pose.header.frame_id = "base_link"; 36 | 37 | pose.pose.position = waypoints[i]; 38 | 39 | pose.pose.orientation.w = 1; 40 | pose.pose.orientation.x = 0; 41 | pose.pose.orientation.y = 0; 42 | pose.pose.orientation.z = 0; 43 | 44 | path->poses.push_back(pose); 45 | } 46 | } 47 | 48 | void interpolatePathCubic(const std::vector &waypoints, double density, nav_msgs::Path *path) { 49 | int num_waypoints = waypoints.size(); 50 | 51 | //create x and y splines 52 | static ecl::Array x_list, y_list, t_list; 53 | x_list.resize(num_waypoints); 54 | y_list.resize(num_waypoints); 55 | t_list.resize(num_waypoints); 56 | 57 | double t = 0; 58 | for(int i = 0; i < num_waypoints; i++) { 59 | x_list[i] = waypoints[i].x; 60 | y_list[i] = waypoints[i].y; 61 | t_list[i] = t; 62 | 63 | t += 1; 64 | } 65 | 66 | CubicSpline spline_x = CubicSpline::Natural(t_list, x_list); 67 | CubicSpline spline_y = CubicSpline::Natural(t_list, y_list); 68 | 69 | //generate path 70 | path->header.frame_id = "map"; 71 | 72 | path->poses.clear(); 73 | 74 | //for every segment of spline 75 | ecl::Array segments_x = spline_x.polynomials(); 76 | ecl::Array segments_y = spline_y.polynomials(); 77 | 78 | double gauss_quadrature_coeff[5][2] = { 79 | { 0.0, 0.5688888888888889 }, 80 | { -0.5384693101056831, 0.47862867049936647 }, 81 | { 0.5384693101056831, 0.47862867049936647 }, 82 | { -0.906179845938664, 0.23692688505618908 }, 83 | { 0.906179845938664, 0.23692688505618908 } 84 | }; 85 | 86 | double last_remain_len = 0.0; 87 | for(int i = 0; i < segments_x.size(); i++) { 88 | //intergrate arc length using gauss quadraute 89 | double arc_len = 0; 90 | for(int k = 0; k < 5; k++) { 91 | double x = i + ((gauss_quadrature_coeff[k][0] + 1) / 2); 92 | double A_x = segments_x[i].coefficients()[1] + 2 * segments_x[i].coefficients()[2] * x + 3 * segments_x[i].coefficients()[3] * x * x; 93 | double B_x = segments_y[i].coefficients()[1] + 2 * segments_y[i].coefficients()[2] * x + 3 * segments_y[i].coefficients()[3] * x * x; 94 | double F_x = std::sqrt( A_x * A_x + B_x * B_x ); 95 | 96 | arc_len += gauss_quadrature_coeff[k][1] * F_x; 97 | } 98 | arc_len /= 2; 99 | 100 | /*printf("x: %lf ", segments_x[i](0 + i)); 101 | printf("y: %lf ", segments_x[i](0 + i)); 102 | printf("len: %lf\n", arc_len);*/ 103 | 104 | //calculate points on segment 105 | double delta = density / arc_len; 106 | double v_t; 107 | for(v_t = (density - last_remain_len) / arc_len; v_t < 1; v_t += delta) { 108 | geometry_msgs::PoseStamped pose; 109 | 110 | pose.header.stamp = ros::Time(0); 111 | 112 | pose.pose.position.x = segments_x[i](i + v_t); 113 | pose.pose.position.y = segments_y[i](i + v_t); 114 | pose.pose.position.z = 0; 115 | 116 | tf::Quaternion q; 117 | q.setRPY(0, 0, atan2(segments_y[i].derivative(i + v_t), segments_x[i].derivative(i + v_t))); 118 | tf::quaternionTFToMsg(q, pose.pose.orientation); 119 | 120 | path->poses.push_back(pose); 121 | } 122 | 123 | v_t -= delta; 124 | last_remain_len = (1 - v_t) * arc_len; 125 | } 126 | } 127 | 128 | int main(int argc, char **argv) { 129 | ros::init(argc, argv, "track_publisher"); 130 | 131 | ros::NodeHandle nh; 132 | ros::Publisher path_pub = nh.advertise("track", 1); 133 | 134 | std::string param_waypoints_filename; 135 | double param_scale; 136 | double param_density; 137 | 138 | ros::NodeHandle npriv("~"); 139 | npriv.param("waypoints_file", param_waypoints_filename, ""); 140 | npriv.param("scale", param_scale, 0.001); 141 | npriv.param("density", param_density, 0.2); 142 | 143 | loadWaypoints(param_waypoints_filename, param_scale, &gWaypoints); 144 | interpolatePathCubic(gWaypoints, param_density, &gPath); 145 | 146 | ros::Rate loop_rate(1.0 / 5.0); 147 | while (ros::ok()) { 148 | path_pub.publish(gPath); 149 | 150 | //sleep and spin 151 | loop_rate.sleep(); 152 | ros::spinOnce(); 153 | } 154 | 155 | return 0; 156 | } -------------------------------------------------------------------------------- /srv/simulator_reset.srv: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | 4 | float64 yaw 5 | 6 | --- -------------------------------------------------------------------------------- /tracks/racetrack01.txt: -------------------------------------------------------------------------------- 1 | -432.000,25105.000 2 | 369.000,21978.000 3 | -2634.000,20113.000 4 | -5571.000,22050.000 5 | -8951.000,21458.000 6 | -9051.000,17624.000 7 | -4456.000,15356.000 8 | -3422.000,13139.000 9 | -1326.000,12162.000 10 | -2004.000,8018.000 11 | -4686.000,5332.000 12 | -7984.000,5762.000 13 | -6360.000,10108.000 14 | -5777.000,13501.000 15 | -10103.000,14267.000 16 | -12399.000,9573.000 17 | -14768.000,8884.000 18 | -15975.000,10085.000 19 | -13981.000,12918.000 20 | -13378.000,15714.000 21 | -13934.000,18535.000 22 | -17565.000,20527.000 23 | -19545.000,22525.000 24 | -18134.000,24751.000 25 | -12959.000,23080.000 26 | -10419.000,24898.000 27 | -11654.000,27816.000 28 | -13287.000,30324.000 29 | -15080.000,33211.000 30 | -16563.000,36746.000 31 | -16174.000,39635.000 32 | -13398.000,40445.000 33 | -10227.000,40000.000 34 | -6835.000,39460.000 35 | -6643.000,33503.000 36 | -2985.000,33003.000 37 | -2239.000,37010.000 38 | 234.000,37397.000 39 | 3143.000,36732.000 40 | 5083.000,36062.000 41 | 7314.000,35321.000 42 | 9885.000,34234.000 43 | 12010.000,33204.000 44 | 14905.000,30727.000 45 | 15267.000,28862.000 46 | 14634.000,27093.000 47 | 13534.000,25188.000 48 | 11781.000,24865.000 49 | 10574.000,26700.000 50 | 10443.000,29648.000 51 | 7243.000,30504.000 52 | 5773.000,33551.000 53 | 2374.000,33081.000 54 | 1438.000,28193.000 55 | -33.000,26699.000 56 | -202.000,25805.000 57 | -432.000,25105.000 --------------------------------------------------------------------------------