├── .catkin_workspace ├── .vscode ├── c_cpp_properties.json └── settings.json ├── LQR.png ├── MPC.png ├── README.md ├── pure_pursuit.png ├── src ├── CANBUS │ ├── CMakeLists.txt │ ├── README.md │ ├── package.xml │ └── src │ │ ├── receive.cpp │ │ └── send.cpp ├── CMakeLists.txt ├── LQR │ ├── CMakeLists.txt │ ├── include │ │ ├── KinematicModel.h │ │ ├── LQR.h │ │ ├── Reference_path.h │ │ ├── matplotlibcpp.h │ │ └── pid_controller.h │ ├── package.xml │ └── src │ │ ├── KinematicModel.cpp │ │ ├── LQR.cpp │ │ ├── Reference_path.cpp │ │ ├── main.cpp │ │ └── pid_controller.cpp ├── MPC │ ├── CMakeLists.txt │ ├── include │ │ ├── KinematicModel.h │ │ ├── MPC.h │ │ ├── Reference_path.h │ │ ├── matplotlibcpp.h │ │ └── pid_controller.h │ ├── package.xml │ └── src │ │ ├── KinematicModel.cpp │ │ ├── MPC.cpp │ │ ├── Reference_path.cpp │ │ ├── main.cpp │ │ └── pid_controller.cpp ├── Purepursuit │ ├── CMakeLists.txt │ ├── include │ │ ├── KinematicModel.h │ │ ├── matplotlibcpp.h │ │ ├── pid_controller.h │ │ └── pure_pursuit.h │ ├── package.xml │ └── src │ │ ├── KinematicModel.cpp │ │ ├── main.cpp │ │ ├── pid_controller.cpp │ │ └── pure_pursuit.cpp ├── QP_Smooth_referenceline │ ├── CMakeLists.txt │ ├── Figure_1.png │ ├── README.md │ ├── include │ │ ├── OSQP.h │ │ └── matplotlibcpp.h │ ├── package.xml │ └── src │ │ ├── OSQP.cpp │ │ ├── global_path_x.txt │ │ ├── global_path_y.txt │ │ └── main.cpp └── Stanley │ ├── CMakeLists.txt │ ├── include │ ├── KinematicModel.h │ ├── matplotlibcpp.h │ ├── pid_controller.h │ └── stanley.h │ ├── package.xml │ └── src │ ├── KinematicModel.cpp │ ├── main.cpp │ ├── pid_controller.cpp │ └── stanley.cpp └── stanley.png /.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "${default}", 6 | "limitSymbolsToIncludedHeaders": false 7 | }, 8 | "includePath": [ 9 | "/opt/ros/noetic/include/**", 10 | "/home/bigdavid/视频/Control_algorithm/src/LQR/include/**", 11 | "/home/bigdavid/视频/Control_algorithm/src/MPC/include/**", 12 | "/home/bigdavid/视频/Control_algorithm/src/Purepursuit/include/**", 13 | "/home/bigdavid/视频/Control_algorithm/src/Stanley/include/**", 14 | "/usr/include/**" 15 | ], 16 | "name": "ROS", 17 | "intelliSenseMode": "gcc-x64", 18 | "compilerPath": "/usr/bin/gcc", 19 | "cStandard": "gnu11", 20 | "cppStandard": "c++14" 21 | } 22 | ], 23 | "version": 4 24 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/opt/ros/noetic/lib/python3/dist-packages", 4 | "", 5 | "/opt/carla-0.9.11/PythonAPI/carla/dist/carla-0.9.11-py3.7-linux-x86_64.egg", 6 | "/opt/carla-0.9.11/PythonAPI/carla/dist/carla-0.9.11-py3.7-linux-x86_64.egg", 7 | "/opt/carla-0.9.11/PythonAPI/carla/agents", 8 | "/home/bigdavid/ocs2_ws/src/raisimLib/raisim/linux/lib" 9 | ], 10 | "python.analysis.extraPaths": [ 11 | "/opt/ros/noetic/lib/python3/dist-packages", 12 | "", 13 | "/opt/carla-0.9.11/PythonAPI/carla/dist/carla-0.9.11-py3.7-linux-x86_64.egg", 14 | "/opt/carla-0.9.11/PythonAPI/carla/dist/carla-0.9.11-py3.7-linux-x86_64.egg", 15 | "/opt/carla-0.9.11/PythonAPI/carla/agents", 16 | "/home/bigdavid/ocs2_ws/src/raisimLib/raisim/linux/lib" 17 | ], 18 | "files.associations": { 19 | "random": "cpp", 20 | "iostream": "cpp", 21 | "array": "cpp", 22 | "string": "cpp", 23 | "string_view": "cpp", 24 | "ranges": "cpp" 25 | } 26 | } -------------------------------------------------------------------------------- /LQR.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HardWorkingDavid/control_algorithm/def86dd2d5a2d9c66dd844ac714a5515115da38c/LQR.png -------------------------------------------------------------------------------- /MPC.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HardWorkingDavid/control_algorithm/def86dd2d5a2d9c66dd844ac714a5515115da38c/MPC.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ### ubuntu系统：20.04 2 | 3 | 纵向控制算法：PID 4 | 横向控制算法：Pure pursuit、Stanley、LQR、MPC 5 | 6 | CANBUS报文接受与发送 7 | -------------------------------------------------------------------------------- /pure_pursuit.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HardWorkingDavid/control_algorithm/def86dd2d5a2d9c66dd844ac714a5515115da38c/pure_pursuit.png -------------------------------------------------------------------------------- /src/CANBUS/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(CANBUS) 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 | std_msgs 13 | ) 14 | 15 | catkin_package( 16 | # INCLUDE_DIRS include 17 | # LIBRARIES CANBUS 18 | # CATKIN_DEPENDS roscpp std_msgs 19 | # DEPENDS system_lib 20 | ) 21 | 22 | include_directories( 23 | # include 24 | ${catkin_INCLUDE_DIRS} 25 | ) 26 | 27 | add_executable(send src/send.cpp) 28 | add_executable(receive src/receive.cpp) 29 | 30 | target_link_libraries(send 31 | ${catkin_LIBRARIES} 32 | ) 33 | 34 | target_link_libraries(receive 35 | ${catkin_LIBRARIES} 36 | ) 37 | -------------------------------------------------------------------------------- /src/CANBUS/README.md: -------------------------------------------------------------------------------- 1 | # CANBUS 2 | 3 | ## Linux CAN通信 4 | 本文旨在利用虚拟can接口实现简单的通信，为后续canbus通信学习打下基础。 5 | ![在这里插入图片描述](https://img-blog.csdnimg.cn/93d51224cd9b4e9f9fc64568616c0137.png) 6 | 7 | 1. 创建虚拟CAN接口 8 | 9 | **加载vcan内核模块:** `sudo modprobe vcan` 10 | 11 | **创建虚拟CAN接口:** `sudo ip link add dev vcan0 type vcan` 12 | 13 | **将虚拟CAN接口处于在线状态:** `sudo ip link set up vcan0` 14 | 15 | 通过命令`ip addr | grep "can"` 来**验证是否可用并处于在线状态** 16 | 17 | 2. 使用 can-utils 测试CAN通信 基于虚拟CAN接口，测试一下CAN通信情况 `sudo apt install can-utils` 18 | 3. 打开两个终端接口（一个用来查看、一个用来发送） 19 | 20 | ```markdown 21 | #查看 22 | candump -tz vcan0 23 | #发送 24 | cansend vcan0 123#00FFAA5501020304 25 | ``` 26 | 27 | #### CAN通信参考代码 28 | 29 | **CAN数据读取** 30 | 31 | ```c++ 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | int main(void) 43 | { 44 | struct ifreq ifr = {0}; 45 | struct sockaddr_can can_addr = {0}; 46 | struct can_frame frame = {0}; 47 | int sockfd = -1; 48 | int i; 49 | int ret; 50 | 51 | /* 打开套接字 */ 52 | sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW); 53 | if(0 > sockfd) { 54 | perror("socket error"); 55 | exit(EXIT_FAILURE); 56 | } 57 | 58 | /* 指定can0设备 */ 59 | strcpy(ifr.ifr_name, "vcan0"); 60 | ioctl(sockfd, SIOCGIFINDEX, &ifr); 61 | can_addr.can_family = AF_CAN; 62 | can_addr.can_ifindex = ifr.ifr_ifindex; 63 | 64 | /* 将can0与套接字进行绑定 */ 65 | ret = bind(sockfd, (struct sockaddr *)&can_addr, sizeof(can_addr)); 66 | if (0 > ret) { 67 | perror("bind error"); 68 | close(sockfd); 69 | exit(EXIT_FAILURE); 70 | } 71 | 72 | /* 设置过滤规则 */ 73 | //setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0); 74 | 75 | /* 接收数据 */ 76 | for ( ; ; ) { 77 | if (0 > read(sockfd, &frame, sizeof(struct can_frame))) { 78 | perror("read error"); 79 | break; 80 | } 81 | 82 | /* 校验是否接收到错误帧 */ 83 | if (frame.can_id & CAN_ERR_FLAG) { 84 | printf("Error frame!\n"); 85 | break; 86 | } 87 | 88 | /* 校验帧格式 */ 89 | if (frame.can_id & CAN_EFF_FLAG) //扩展帧 90 | printf("扩展帧 <0x%08x> ", frame.can_id & CAN_EFF_MASK); 91 | else //标准帧 92 | printf("标准帧 <0x%03x> ", frame.can_id & CAN_SFF_MASK); 93 | 94 | /* 校验帧类型：数据帧还是远程帧 */ 95 | if (frame.can_id & CAN_RTR_FLAG) { 96 | printf("remote request\n"); 97 | continue; 98 | } 99 | 100 | /* 打印数据长度 */ 101 | printf("[%d] ", frame.can_dlc); 102 | 103 | /* 打印数据 */ 104 | for (i = 0; i < frame.can_dlc; i++) 105 | printf("%02x ", frame.data[i]); 106 | printf("\n"); 107 | } 108 | 109 | /* 关闭套接字 */ 110 | close(sockfd); 111 | exit(EXIT_SUCCESS); 112 | } 113 | ``` 114 | 115 | **CAN数据发送** 116 | 117 | ```c++ 118 | #include 119 | #include 120 | #include 121 | #include 122 | #include 123 | #include 124 | #include 125 | #include 126 | #include 127 | 128 | int main(void) 129 | { 130 | struct ifreq ifr = {0}; 131 | struct sockaddr_can can_addr = {0}; 132 | struct can_frame frame = {0}; 133 | int sockfd = -1; 134 | int ret; 135 | 136 | /* 打开套接字 */ 137 | sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW); 138 | if(0 > sockfd) { 139 | perror("socket error"); 140 | exit(EXIT_FAILURE); 141 | } 142 | 143 | /* 指定can0设备 */ 144 | strcpy(ifr.ifr_name, "vcan0"); 145 | ioctl(sockfd, SIOCGIFINDEX, &ifr); 146 | can_addr.can_family = AF_CAN; 147 | can_addr.can_ifindex = ifr.ifr_ifindex; 148 | 149 | /* 将can0与套接字进行绑定 */ 150 | ret = bind(sockfd, (struct sockaddr *)&can_addr, sizeof(can_addr)); 151 | if (0 > ret) { 152 | perror("bind error"); 153 | close(sockfd); 154 | exit(EXIT_FAILURE); 155 | } 156 | 157 | /* 设置过滤规则：不接受任何报文、仅发送数据 */ 158 | setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0); 159 | 160 | /* 发送数据 */ 161 | frame.data[0] = 0xA0; 162 | frame.data[1] = 0xB0; 163 | frame.data[2] = 0xC0; 164 | frame.data[3] = 0xD0; 165 | frame.data[4] = 0xE0; 166 | frame.data[5] = 0xF0; 167 | frame.can_dlc = 6; //一次发送6个字节数据 168 | frame.can_id = 0x123;//帧ID为0x123,标准帧 169 | 170 | for ( ; ; ) { 171 | 172 | ret = write(sockfd, &frame, sizeof(frame)); //发送数据 173 | if(sizeof(frame) != ret) { //如果ret不等于帧长度，就说明发送失败 174 | perror("write error"); 175 | goto out; 176 | } 177 | 178 | sleep(1); //一秒钟发送一次 179 | } 180 | 181 | out: 182 | /* 关闭套接字 */ 183 | close(sockfd); 184 | exit(EXIT_SUCCESS); 185 | } 186 | ``` -------------------------------------------------------------------------------- /src/CANBUS/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | CANBUS 4 | 0.0.0 5 | The CANBUS package 6 | 7 | 8 | 9 | 10 | bigdavid 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 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_msgs 54 | roscpp 55 | std_msgs 56 | roscpp 57 | std_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/CANBUS/src/receive.cpp: -------------------------------------------------------------------------------- 1 | //导入头文件 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include //生成随机数用 12 | #include 13 | 14 | #define CAN_EFF_FLAG 0x80000000U // 扩展帧的标识 15 | #define CAN_RTR_FLAG 0x40000000U // 远程帧的标识 16 | #define CAN_ERR_FLAG 0x20000000U // 错误帧的标识，用于错误检查 17 | 18 | #define CAN_SFF_MASK 0x000007FFU /* 标准帧格式 (SFF) */ 19 | #define CAN_EFF_MASK 0x1FFFFFFFU /* 扩展帧格式 (EFF) */ 20 | #define CAN_ERR_MASK 0x1FFFFFFFU /* 忽略EFF, RTR, ERR标志 */ 21 | 22 | int main() 23 | { 24 | int s,nbytes1,nbytes2,nbytes3,nbytes4; 25 | srand(time(nullptr));//设置随机数种子 26 | rand();//产生一个随机数 27 | struct sockaddr_can addr; 28 | struct ifreq ifr; 29 | struct can_frame frame1 = {0}; 30 | struct can_frame frame2 = {0}; 31 | struct can_frame frame3 = {0}; 32 | struct can_frame frame4 = {0}; 33 | struct can_filter rfilter[4]; 34 | int count = 0; //故障保护 35 | s = socket(PF_CAN,SOCK_RAW,CAN_RAW); //创建套接字 36 | strcpy(ifr.ifr_name,"vcan0"); 37 | ioctl(s,SIOCGIFINDEX,&ifr); 38 | addr.can_family = AF_CAN; 39 | addr.can_ifindex = ifr.ifr_ifindex; 40 | bind(s,(struct sockaddr *)&addr,sizeof(addr)); 41 | //定义接收规则 42 | rfilter[0].can_id = CAN_EFF_FLAG | 0x18FEF121; 43 | rfilter[0].can_mask = CAN_EFF_MASK; 44 | 45 | rfilter[1].can_id = CAN_EFF_FLAG | 0x18FF0824; 46 | rfilter[1].can_mask = CAN_EFF_MASK; 47 | 48 | rfilter[2].can_id = CAN_EFF_FLAG | 0x0C02A224; 49 | rfilter[2].can_mask = CAN_EFF_MASK; 50 | 51 | rfilter[3].can_id = CAN_EFF_FLAG | 0x0c02a0a2; 52 | rfilter[3].can_mask = CAN_EFF_MASK; 53 | //设置过滤规则 54 | setsockopt(s,SOL_CAN_RAW,CAN_RAW_FILTER,&rfilter,sizeof(rfilter)); 55 | 56 | while(1) 57 | { 58 | double factor = 1/256.0;//车速的分辨率 59 | int Offset = 0;//车速的偏移量 60 | int num,num1; 61 | double zfactor = 0.1; //方向盘转向指令角度的分辨率 62 | int zOffset = -1575; // 方向盘转向指令角度的偏移量 63 | double nfactor = 0.1; //转向叠加扭矩信号的分辨率 64 | int nOffset = 0; //转向叠加扭矩信号的偏移量 65 | double ffactor = 10;//方向盘目标角速度的分辨率 66 | int fOffset = 0; //方向盘目标角速度的偏移量 67 | double dfactor = 0.000152; 68 | int dOffset = -5; 69 | nbytes1 = read(s,&frame1,sizeof(frame1)); //接收报文 70 | nbytes2 = read(s,&frame2,sizeof(frame2)); //接收报文 71 | 72 | if (nbytes1 > 0) 73 | { 74 | printf("ID = 0x%X dlc = %d data = 0x ",frame1.can_id,frame1.can_dlc); 75 | for (int i = 0; i < frame1.can_dlc; i++)printf("%02x ",frame1.data[i]); 76 | num = (frame1.data[2]<<8) | (frame1.data[1]); //将两个十六进制数字合并 77 | printf("v = %.2f km/h",factor*num+Offset); // 十六进制转为十进制输出 78 | printf("\n"); 79 | } 80 | if (nbytes2 > 0) 81 | { 82 | printf("ID = 0x%X dlc = %d data = 0x ",frame2.can_id,frame2.can_dlc); 83 | for (int i = 0; i < frame2.can_dlc; i++)printf("%02x ",frame2.data[i]); 84 | printf("\n"); 85 | if (frame2.data[4] == 0x01) 86 | { 87 | printf("Stop!\n"); 88 | } 89 | else if (frame2.data[4] == 0x02){ 90 | printf("Ready\n"); 91 | } 92 | else if (frame2.data[4] == 0x03){ 93 | nbytes3 = read(s,&frame3,sizeof(frame3)); //接收报文 94 | if (nbytes3 >0) 95 | { 96 | count++; 97 | if (count >5) //五个周期故障保护 98 | { 99 | if (frame3.data[7]>0xFA) //数据大于250 100 | { 101 | printf("此报文不可靠\n"); 102 | } 103 | else printf("此报文可靠\n"); 104 | } 105 | printf("ID = 0x%X dlc = %d data = 0x ",frame3.can_id,frame3.can_dlc); 106 | for (int i = 0; i < frame3.can_dlc; i++)printf("%02x ",frame3.data[i]); 107 | printf("\nGo转向可以启动!\n");//转向可以启动 108 | num1 = (frame3.data[1]<<8) | (frame3.data[0]); //将两个十六进制数字合并 109 | printf("方向盘转向指令角度 = %.1f度\n",num1*zfactor+zOffset); 110 | if (frame3.data[2] == 0x34)printf("收到EPS助力模式指令,进入EPS模式\n"); 111 | if (frame3.data[2] == 0x31)printf("收到自动驾驶模式指令,进入自动驾驶模式\n"); 112 | 113 | printf("转向叠加扭矩信号 = %.1f N.m\n",nfactor*frame3.data[3]+nOffset); 114 | printf("方向盘目标角速度 = %.1f 度/s\n",ffactor*frame3.data[4]+fOffset); 115 | printf("\n\n"); 116 | 117 | nbytes4 = read(s,&frame4,sizeof(frame4)); //接收报文 118 | if (nbytes4>0) 119 | { 120 | printf("ID = 0x%X dlc = %d data = 0x ",frame4.can_id,frame4.can_dlc); 121 | for (int i = 0; i < frame4.can_dlc; i++)printf("%02x ",frame4.data[i]); 122 | if (frame4.data[6] == 0x04) 123 | { 124 | printf("处于EPS模式状态\n"); 125 | } 126 | else if (frame4.data[6] == 0x01) 127 | { 128 | printf("处于自动驾驶模式状态\n"); 129 | } 130 | num1 = (frame4.data[1]<<8) | (frame4.data[0]); //将两个十六进制数字合并 131 | printf("方向盘转向角度 = %.1f度\n",num1*zfactor+zOffset); 132 | num1 = (frame4.data[3]<<8) | (frame4.data[2]); //将两个十六进制数字合并 133 | printf("电机助力扭矩 = %.2f Nm\n",num1*dfactor+dOffset); 134 | printf("方向盘反馈角速度 = %.1f 度/s \n",ffactor*frame4.data[4]+fOffset); 135 | printf("转向盘转矩信号 = %.1f Nm\n",nfactor*frame4.data[5]+nOffset); 136 | 137 | printf("\n"); 138 | } 139 | } 140 | 141 | } 142 | else if (frame2.data[4] == 0x04){ 143 | printf("远程升级\n"); 144 | } 145 | else if (frame2.data[4] == 0x06){ 146 | printf("跛行\n"); 147 | } 148 | else{ 149 | printf("失效\n"); 150 | } 151 | } 152 | } 153 | close(s); 154 | return 0; 155 | } 156 | 157 | -------------------------------------------------------------------------------- /src/CANBUS/src/send.cpp: -------------------------------------------------------------------------------- 1 | //导入头文件 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include //生成随机数用 12 | #include 13 | 14 | #define CAN_EFF_FLAG 0x80000000U // 扩展帧的标识 15 | #define CAN_RTR_FLAG 0x40000000U // 远程帧的标识 16 | #define CAN_ERR_FLAG 0x20000000U // 错误帧的标识，用于错误检查 17 | 18 | int main() 19 | { 20 | int s,nbytes1,nbytes2,nbytes3,nbytes4; 21 | int num; 22 | srand(time(nullptr));//设置随机数种子 23 | rand();//产生一个随机数 24 | struct sockaddr_can addr; 25 | struct ifreq ifr; 26 | struct can_frame frame1 = {0}; 27 | struct can_frame frame2 = {0}; 28 | struct can_frame frame3 = {0}; 29 | struct can_frame frame4 = {0}; 30 | 31 | s = socket(PF_CAN,SOCK_RAW,CAN_RAW); // 创建套接字 32 | strcpy(ifr.ifr_name,"vcan0"); 33 | ioctl(s,SIOCGIFINDEX,&ifr); // 指定can0设备 34 | addr.can_family = AF_CAN; 35 | addr.can_ifindex = ifr.ifr_ifindex; 36 | bind(s,(struct sockaddr *)&addr,sizeof(addr)); //将套接字与can0设备绑定 37 | 38 | //setsockopt(s,SOL_CAN_RAW,CAN_RAW_FILTER,NULL,0); // 禁用过滤规则 39 | //生成报文 40 | frame1.can_id = CAN_EFF_FLAG | 0x18FEF121;// ID为扩展帧 41 | //车速信号 42 | frame1.can_dlc = 8; 43 | frame1.data[0] = 0x00; 44 | frame1.data[3] = 0x00; 45 | frame1.data[4] = 0x00; 46 | frame1.data[5] = 0x00; 47 | frame1.data[6] = 0x00; 48 | frame1.data[7] = 0x00; 49 | 50 | frame2.can_id = CAN_EFF_FLAG | 0x18FF0824;// ID为扩展帧 51 | //发动机转速/电动泵信号 52 | frame2.can_dlc = 8; 53 | frame2.data[0] = 0x00; 54 | frame2.data[1] = 0x00; 55 | frame2.data[2] = 0x00; 56 | frame2.data[3] = 0x00; 57 | frame2.data[5] = 0x00; 58 | frame2.data[6] = 0x00; 59 | frame2.data[7] = 0x00; 60 | 61 | frame3.can_id = CAN_EFF_FLAG | 0x0C02A224;// ID为扩展帧 62 | //发动机转速/电动泵信号 63 | frame3.can_dlc = 8; 64 | frame3.data[5] = 0x80; 65 | frame3.data[6] = 0x80; 66 | 67 | // 转向控制器发给整车控制 68 | frame4.can_id = CAN_EFF_FLAG | 0x0c02a0a2;// ID为扩展帧 69 | frame4.can_dlc = 8; 70 | 71 | frame4.data[2] = rand() % 0xFF; 72 | frame4.data[3] = rand() % 0xFF; 73 | 74 | while(1) 75 | { 76 | frame1.data[1] = rand() % 0xFAFF; 77 | frame1.data[2] = rand() % 0xFAFF; 78 | if ((frame1.data[1]+frame1.data[2])>0xFAFF)break;//限制范围在0-250.996km/h 79 | nbytes1 = write(s,&frame1,sizeof(frame1)); // 发送frame1 80 | if (nbytes1 != sizeof(frame1)) 81 | { 82 | printf("Send Error frame1\n"); 83 | break; //发送错误，跳出循环 84 | } 85 | sleep(0.1); //发送周期0.1s 86 | 87 | frame2.data[4] = rand() % 0x07; // 0-0x07 88 | nbytes2 = write(s,&frame2,sizeof(frame2)); // 发送frame2 89 | if (nbytes2 != sizeof(frame2)) 90 | { 91 | printf("Send Error frame2\n"); 92 | break; //发送错误，跳出循环 93 | } 94 | sleep(0.1); //发送周期0.1s 95 | frame3.data[0] = rand() % 0xf4; 96 | frame3.data[1] = rand() % 0x7e;//限制范围在0-32500 97 | frame3.data[3] = rand() % 0x100; // 0-256 98 | frame3.data[4] = 0x14 + rand() % 0xa; //200-300 99 | frame3.data[7] = rand() % 0xFA; 100 | if (frame2.data[4] == 0x03) 101 | { 102 | while(1) 103 | { 104 | frame3.data[2] = 0x34; // 默认进入手动指令(EPS模式状态4) 105 | int a; 106 | if (std::cin >> a,a) //如需进入自动驾驶模式，输入1，否则输入0 107 | { 108 | frame3.data[2] = 0x31; 109 | } 110 | if (std::cin >> a,a) //如需再次进入自动驾驶模式输入5介入恢复指令，否则输入0 111 | { 112 | frame3.data[2] = 0x35; 113 | continue; 114 | } 115 | break; 116 | } 117 | 118 | nbytes3 = write(s,&frame3,sizeof(frame3)); // 发送frame3 119 | if (nbytes3 != sizeof(frame3)) 120 | { 121 | printf("Send Error frame3\n"); 122 | break; //发送错误，跳出循环 123 | } 124 | sleep(0.02); //发送周期20ms 125 | frame4.data[0] = frame3.data[0]; 126 | frame4.data[1] = frame3.data[1]; 127 | frame4.data[4] = frame3.data[4]; 128 | frame4.data[5] = frame3.data[3]; 129 | frame4.data[7] = frame3.data[7]; 130 | if (frame3.data[2] == 0x34) 131 | { 132 | frame4.data[6] = 0x04; //EPS助力模式 133 | } 134 | else if (frame3.data[2] == 0x31) 135 | { 136 | frame4.data[6] = 0x01; //自动驾驶模式 137 | } 138 | else 139 | { 140 | frame4.data[6] = 0x05; // 发送手动介入指令 141 | } 142 | nbytes4 = write(s,&frame4,sizeof(frame4)); // 发送frame4 143 | if (nbytes4 != sizeof(frame4)) 144 | { 145 | printf("Send Error frame4\n"); 146 | break; //发送错误，跳出循环 147 | } 148 | sleep(0.02); //发送周期20ms 149 | } 150 | } 151 | 152 | close(s); 153 | return 0; 154 | 155 | } 156 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /src/LQR/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(LQR) 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 | std_msgs 13 | ) 14 | 15 | set(CMAKE_CXX_STANDARD 11) 16 | 17 | file(GLOB_RECURSE PYTHON2.7_LIB "/usr/lib/python2.7/config-x86_64-linux-gnu/*.so") 18 | set(PYTHON2.7_INLCUDE_DIRS "/usr/include/python2.7") 19 | 20 | catkin_package( 21 | # INCLUDE_DIRS include 22 | # LIBRARIES huatu 23 | # CATKIN_DEPENDS roscpp std_msgs 24 | # DEPENDS system_lib 25 | ) 26 | 27 | include_directories(include 28 | ${PYTHON2.7_INLCUDE_DIRS} 29 | ) 30 | 31 | add_executable(lqr_controller src/LQR.cpp 32 | src/KinematicModel.cpp 33 | src/main.cpp 34 | src/pid_controller.cpp 35 | src/Reference_path.cpp) 36 | target_link_libraries(lqr_controller ${PYTHON2.7_LIB}) -------------------------------------------------------------------------------- /src/LQR/include/KinematicModel.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | using namespace std; 7 | using namespace Eigen; 8 | 9 | class KinematicModel { 10 | public: 11 | KinematicModel(); 12 | KinematicModel(double x, double y, double psi, double v, double L, double dt); 13 | 14 | vector getState(); 15 | void updateState(double a, double delta); 16 | vector stateSpace(double ref_delta, double ref_yaw); 17 | public: 18 | double x, y, psi, v, L, dt; 19 | }; -------------------------------------------------------------------------------- /src/LQR/include/LQR.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define EPS 1.0e-4 4 | #include 5 | #include 6 | #include 7 | using namespace std; 8 | using namespace Eigen; 9 | 10 | class LQR { 11 | private: 12 | int N; 13 | public: 14 | LQR(int n); 15 | 16 | MatrixXd calRicatti(MatrixXd A, MatrixXd B, MatrixXd Q, MatrixXd R); 17 | double LQRControl(vector robot_state, vector> ref_path, double s0, MatrixXd A, MatrixXd B, MatrixXd Q, MatrixXd R); 18 | }; -------------------------------------------------------------------------------- /src/LQR/include/Reference_path.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | #define PI 3.1415926 13 | 14 | struct refTraj 15 | { 16 | MatrixXd xref, dref; 17 | int ind; 18 | }; 19 | 20 | struct parameters 21 | { 22 | int L; 23 | int NX, NU, T; 24 | double dt; 25 | }; 26 | 27 | class ReferencePath 28 | { 29 | public: 30 | ReferencePath(); 31 | vector calcTrackError(vector robot_state); 32 | double normalizeAngle(double angle); 33 | 34 | // 计算参考轨迹点，统一化变量数组，便于后面MPC优化使用. 35 | refTraj calc_ref_trajectory(vector robot_state, parameters param, double dl = 1.0); 36 | 37 | public: 38 | vector> ref_path; // x, y, 切线方向, k 39 | vector ref_x, ref_y; 40 | }; -------------------------------------------------------------------------------- /src/LQR/include/pid_controller.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | using namespace std; 5 | 6 | class PID_controller { 7 | private: 8 | double Kp, Ki, Kd, target, upper, lower; 9 | double error = 0.0, pre_error = 0.0, sum_error = 0.0; 10 | public: 11 | PID_controller(double Kp, double Ki, double Kd, double target, double upper, double lower); 12 | 13 | void setTarget(double target); 14 | 15 | void setK(double Kp, double Ki, double Kd); 16 | 17 | void setBound(double upper, double lower); 18 | 19 | double calOutput(double state); 20 | 21 | void reset(); 22 | 23 | void setSumError(double sum_error); 24 | }; 25 | -------------------------------------------------------------------------------- /src/LQR/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | LQR 4 | 0.0.0 5 | The LQR package 6 | 7 | 8 | 9 | 10 | bigdavid 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 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_msgs 54 | roscpp 55 | std_msgs 56 | roscpp 57 | std_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/LQR/src/KinematicModel.cpp: -------------------------------------------------------------------------------- 1 | #include "KinematicModel.h" 2 | 3 | KinematicModel::KinematicModel(double x, double y, double psi, double v, double L, double dt) : x(x), y(y), psi(psi), 4 | v(v), L(L), dt(dt) {} 5 | 6 | void KinematicModel::updateState(double a, double delta) 7 | { 8 | x = x + v * cos(psi) * dt; 9 | y = y + v * sin(psi) * dt; 10 | psi = psi + v / L * tan(delta) * dt; 11 | v = v + a * dt; 12 | } 13 | 14 | vector KinematicModel::getState() 15 | { 16 | return {x, y, psi, v}; 17 | } 18 | 19 | // 将模型离散化后的状态空间表达 20 | vector KinematicModel::stateSpace(double ref_delta, double ref_yaw) 21 | { 22 | MatrixXd A(3, 3), B(3, 2); 23 | A << 1.0, 0.0, -v * sin(ref_yaw) * dt, 24 | 0.0, 1.0, v * cos(ref_yaw) * dt, 25 | 0.0, 0.0, 1.0; 26 | B << cos(ref_yaw) * dt, 0, 27 | sin(ref_yaw) * dt, 0, 28 | tan(ref_delta) * dt / L, v * dt / (L * cos(ref_delta) * cos(ref_delta)); 29 | return {A, B}; 30 | } 31 | -------------------------------------------------------------------------------- /src/LQR/src/LQR.cpp: -------------------------------------------------------------------------------- 1 | #include "LQR.h" 2 | 3 | LQR::LQR(int n) : N(n) {} 4 | // 解代数里卡提方程 5 | MatrixXd LQR::calRicatti(MatrixXd A, MatrixXd B, MatrixXd Q, MatrixXd R) 6 | { 7 | MatrixXd Qf = Q; 8 | MatrixXd P_old = Qf; 9 | MatrixXd P_new; 10 | // P _{new} =Q+A ^TPA−A ^TPB(R+B ^T PB) ^{−1}B ^TPA 11 | for (int i = 0; i < N; i++) 12 | { 13 | P_new = Q + A.transpose() * P_old * A - A.transpose() * P_old * B * (R + B.transpose() * P_old * B).inverse() * B.transpose() * P_old * A; 14 | if ((P_new - P_old).maxCoeff() < EPS && (P_old - P_new).maxCoeff() < EPS) break; 15 | P_old = P_new; 16 | } 17 | return P_new; 18 | } 19 | 20 | double LQR::LQRControl(vector robot_state, vector> ref_path, double s0, MatrixXd A, MatrixXd B, MatrixXd Q, MatrixXd R) 21 | { 22 | MatrixXd X(3, 1); 23 | X << robot_state[0] - ref_path[s0][0], 24 | robot_state[1] - ref_path[s0][1], 25 | robot_state[2] - ref_path[s0][2]; 26 | MatrixXd P = calRicatti(A, B, Q, R); 27 | // K=(R + B^TP_{new}B)^{-1}B^TP_{new}A 28 | MatrixXd K = (R + B.transpose() * P * B).inverse() * B.transpose() * P * A; 29 | MatrixXd u = -K * X; // [v - ref_v, delta - ref_delta] 30 | return u(1, 0); 31 | } -------------------------------------------------------------------------------- /src/LQR/src/Reference_path.cpp: -------------------------------------------------------------------------------- 1 | #include "Reference_path.h" 2 | 3 | ReferencePath::ReferencePath() 4 | { 5 | ref_path = vector>(1000, vector(4)); 6 | // 生成参考轨迹 7 | for (int i = 0; i < 1000; i++) 8 | { 9 | ref_path[i][0] = 0.1 * i; 10 | ref_path[i][1] = 2 * sin(ref_path[i][0] / 3.0); 11 | 12 | ref_x.push_back(ref_path[i][0]); 13 | ref_y.push_back(ref_path[i][1]); 14 | } 15 | double dx, dy, ddx, ddy; 16 | for (int i = 0; i < ref_path.size(); i++) 17 | { 18 | if (i == 0) { 19 | dx = ref_path[i + 1][0] - ref_path[i][0]; 20 | dy = ref_path[i + 1][1] - ref_path[i][1]; 21 | ddx = ref_path[2][0] + ref_path[0][0] - 2 * ref_path[1][0]; 22 | ddy = ref_path[2][1] + ref_path[0][1] - 2 * ref_path[1][1]; 23 | } else if (i == ref_path.size() - 1) { 24 | dx = ref_path[i][0] - ref_path[i- 1][0]; 25 | dy = ref_path[i][1] - ref_path[i- 1][1]; 26 | ddx = ref_path[i][0] + ref_path[i- 2][0] - 2 * ref_path[i - 1][0]; 27 | ddy = ref_path[i][1] + ref_path[i - 2][1] - 2 * ref_path[i - 1][1]; 28 | } else { 29 | dx = ref_path[i + 1][0] - ref_path[i][0]; 30 | dy = ref_path[i + 1][1] - ref_path[i][1]; 31 | ddx = ref_path[i + 1][0] + ref_path[i - 1][0] - 2 * ref_path[i][0]; 32 | ddy = ref_path[i + 1][1] + ref_path[i - 1][1] - 2 * ref_path[i][1]; 33 | } 34 | ref_path[i][2] = atan2(dy, dx); 35 | //计算曲率:设曲线r(t) =(x(t),y(t)),则曲率k=(x'y" - x"y')/((x')^2 + (y')^2)^(3/2). 36 | ref_path[i][3] = (ddy * dx - ddx * dy) / pow((dx * dx + dy * dy), 3.0 / 2); // k计算 37 | } 38 | } 39 | // 计算跟踪误差 40 | vector ReferencePath::calcTrackError(vector robot_state) 41 | { 42 | double x = robot_state[0], y = robot_state[1]; 43 | vector d_x(ref_path.size()), d_y(ref_path.size()), d(ref_path.size()); 44 | for (int i = 0; i < ref_path.size(); i++) 45 | { 46 | d_x[i]=ref_path[i][0]-x; 47 | d_y[i]=ref_path[i][1]-y; 48 | d[i] = sqrt(d_x[i]*d_x[i]+d_y[i]*d_y[i]); 49 | } 50 | double min_index = min_element(d.begin(), d.end()) - d.begin(); 51 | double yaw = ref_path[min_index][2]; 52 | double k = ref_path[min_index][3]; 53 | double angle = normalizeAngle(yaw - atan2(d_y[min_index], d_x[min_index])); 54 | double error = d[min_index]; 55 | if (angle < 0) error *= -1; 56 | return {error, k, yaw, min_index}; 57 | } 58 | 59 | double ReferencePath::normalizeAngle(double angle) 60 | { 61 | while (angle > PI) 62 | { 63 | angle -= 2 * PI; 64 | } 65 | while (angle < -PI) 66 | { 67 | angle += 2 * PI; 68 | } 69 | return angle; 70 | } 71 | 72 | // 计算参考轨迹点，统一化变量数组，只针对MPC优化使用 73 | // robot_state 车辆的状态(x,y,yaw,v) 74 | refTraj ReferencePath::calc_ref_trajectory(vector robot_state, parameters param, double dl) 75 | { 76 | vector track_error = calcTrackError(robot_state); 77 | double e = track_error[0], k = track_error[1], ref_yaw = track_error[2], ind = track_error[3]; 78 | refTraj ref_traj; 79 | ref_traj.xref = MatrixXd(param.NX, param.T + 1); 80 | ref_traj.dref = MatrixXd (param.NU,param.T); 81 | int ncourse = ref_path.size(); 82 | ref_traj.xref(0,0)=ref_path[ind][0]; 83 | ref_traj.xref(1,0)=ref_path[ind][1]; 84 | ref_traj.xref(2,0)=ref_path[ind][2]; 85 | //参考控制量[v,delta] 86 | double ref_delta = atan2(k * param.L, 1); 87 | for(int i=0;i x_, y_; 22 | ReferencePath referencePath; 23 | KinematicModel model(0, 1.0, 0, 0, 2.2, 0.1); 24 | PID_controller PID(3, 0.001, 30, Target_speed, 15.0 / 3.6, 0.0); 25 | LQR lqr(N); 26 | vector robot_state; 27 | 28 | for (int i = 0; i < 700; i++) 29 | { 30 | plt::clf(); 31 | robot_state = model.getState(); 32 | vector one_trial = referencePath.calcTrackError(robot_state); 33 | double k = one_trial[1], ref_yaw = one_trial[2], s0 = one_trial[3]; 34 | 35 | double ref_delta = atan2(k * model.L, 1); // L = 2.2 36 | vector state_space = model.stateSpace(ref_delta, ref_yaw); 37 | 38 | double delta = lqr.LQRControl(robot_state, referencePath.ref_path, s0, state_space[0], state_space[1], Q, R); 39 | delta = delta + ref_delta; 40 | double a = PID.calOutput(model.v); 41 | model.updateState(a, delta); 42 | cout << "Speed: " << model.v << " m/s" << endl; 43 | 44 | x_.push_back(model.x); 45 | y_.push_back(model.y); 46 | //画参考轨迹 47 | plt::plot(referencePath.ref_x, referencePath.ref_y, "b--"); 48 | plt::grid(true); 49 | plt::ylim(-5, 5); 50 | plt::plot(x_, y_, "r"); 51 | plt::pause(0.01); 52 | } 53 | const char* filename = "./LQR.png"; 54 | plt::save(filename); 55 | plt::show(); 56 | return 0; 57 | } 58 | -------------------------------------------------------------------------------- /src/LQR/src/pid_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "pid_controller.h" 2 | 3 | PID_controller::PID_controller(double Kp, double Ki, double Kd, double target, double upper, double lower) : Kp(Kp), 4 | Ki(Ki), 5 | Kd(Kd), 6 | target(target), 7 | upper(upper), 8 | lower(lower) {} 9 | void PID_controller::setTarget(double target) { 10 | PID_controller::target = target; 11 | } 12 | 13 | void PID_controller::setK(double Kp, double Ki, double Kd) { 14 | this->Kp = Kp; 15 | this->Ki = Ki; 16 | this->Kd = Kd; 17 | } 18 | 19 | void PID_controller::setBound(double upper, double lower) { 20 | this->upper = upper; 21 | this->lower = lower; 22 | } 23 | 24 | double PID_controller::calOutput(double state) { 25 | this->error = this->target - state; 26 | double u = this->error * this->Kp + this->sum_error * this->Ki + (this->error - this->pre_error) * this->Kd; 27 | if (u < this->lower) u = this->lower; 28 | else if (u > this->upper) u = this->upper; 29 | this->pre_error = this->error; 30 | this->sum_error = this->sum_error + this->error; 31 | return u; 32 | } 33 | 34 | void PID_controller::reset() { 35 | error = 0.0; 36 | pre_error = 0.0; 37 | sum_error = 0.0; 38 | } 39 | 40 | void PID_controller::setSumError(double sum_error) { 41 | this->sum_error = sum_error; 42 | } 43 | -------------------------------------------------------------------------------- /src/MPC/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(MPC) 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 | std_msgs 13 | ) 14 | 15 | set(CMAKE_CXX_STANDARD 11) 16 | 17 | file(GLOB_RECURSE PYTHON2.7_LIB "/usr/lib/python2.7/config-x86_64-linux-gnu/*.so") 18 | set(PYTHON2.7_INLCUDE_DIRS "/usr/include/python2.7") 19 | 20 | catkin_package( 21 | # INCLUDE_DIRS include 22 | # LIBRARIES huatu 23 | # CATKIN_DEPENDS roscpp std_msgs 24 | # DEPENDS system_lib 25 | ) 26 | link_directories( 27 | /usr/local/lib 28 | ) 29 | 30 | include_directories( 31 | include 32 | ${catkin_INCLUDE_DIRS} 33 | ${PYTHON2.7_INLCUDE_DIRS} 34 | ) 35 | 36 | add_executable(mpc_controller src/MPC.cpp 37 | src/KinematicModel.cpp 38 | src/main.cpp 39 | src/pid_controller.cpp 40 | src/Reference_path.cpp) 41 | target_link_libraries(mpc_controller ${PYTHON2.7_LIB} 42 | libosqp.so 43 | libOsqpEigen.so 44 | libqdldl.so) -------------------------------------------------------------------------------- /src/MPC/include/KinematicModel.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | using namespace std; 7 | using namespace Eigen; 8 | 9 | class KinematicModel { 10 | public: 11 | KinematicModel(); 12 | KinematicModel(double x, double y, double psi, double v, double L, double dt); 13 | 14 | vector getState(); 15 | void updateState(double a, double delta); 16 | vector stateSpace(double ref_delta, double ref_yaw); 17 | public: 18 | double x, y, psi, v, L, dt; 19 | }; -------------------------------------------------------------------------------- /src/MPC/include/MPC.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "KinematicModel.h" 7 | #include "OsqpEigen/OsqpEigen.h" 8 | #include 9 | #include "matplotlibcpp.h" 10 | 11 | namespace plt = matplotlibcpp; 12 | using namespace std; 13 | using namespace Eigen; 14 | using namespace OsqpEigen; 15 | 16 | namespace { 17 | const double MAX_STEER = M_PI / 6.0; 18 | const double MAX_VEL = 10.0; 19 | } 20 | class MPC { 21 | private: 22 | int NX, NU, T; 23 | MatrixXd R = MatrixXd::Identity(NU, NU); 24 | MatrixXd Rd = MatrixXd::Identity(NU, NU); 25 | MatrixXd Q = MatrixXd::Identity(NX, NX); 26 | MatrixXd Qf = Q; 27 | public: 28 | MPC(int nx, int nu, int t); 29 | vector LMPC(MatrixXd xref, Vector3d x0, MatrixXd ref_delta, KinematicModel model); 30 | }; -------------------------------------------------------------------------------- /src/MPC/include/Reference_path.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | #define PI 3.1415926 13 | 14 | struct refTraj 15 | { 16 | MatrixXd xref, dref; 17 | int ind; 18 | }; 19 | 20 | struct parameters 21 | { 22 | int L; 23 | int NX, NU, T; 24 | double dt; 25 | }; 26 | 27 | class ReferencePath 28 | { 29 | public: 30 | ReferencePath(); 31 | vector calcTrackError(vector robot_state); 32 | double normalizeAngle(double angle); 33 | 34 | // 计算参考轨迹点，统一化变量数组，便于后面MPC优化使用. 35 | refTraj calc_ref_trajectory(vector robot_state, parameters param, double dl = 1.0); 36 | 37 | public: 38 | vector> ref_path; // x, y, 切线方向, k 39 | vector ref_x, ref_y; 40 | }; -------------------------------------------------------------------------------- /src/MPC/include/pid_controller.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | using namespace std; 5 | 6 | class PID_controller { 7 | private: 8 | double Kp, Ki, Kd, target, upper, lower; 9 | double error = 0.0, pre_error = 0.0, sum_error = 0.0; 10 | public: 11 | PID_controller(double Kp, double Ki, double Kd, double target, double upper, double lower); 12 | 13 | void setTarget(double target); 14 | 15 | void setK(double Kp, double Ki, double Kd); 16 | 17 | void setBound(double upper, double lower); 18 | 19 | double calOutput(double state); 20 | 21 | void reset(); 22 | 23 | void setSumError(double sum_error); 24 | }; 25 | -------------------------------------------------------------------------------- /src/MPC/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | MPC 4 | 0.0.0 5 | The MPC package 6 | 7 | 8 | 9 | 10 | bigdavid 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 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_msgs 54 | roscpp 55 | std_msgs 56 | roscpp 57 | std_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/MPC/src/KinematicModel.cpp: -------------------------------------------------------------------------------- 1 | #include "KinematicModel.h" 2 | 3 | KinematicModel::KinematicModel(double x, double y, double psi, double v, double L, double dt) : x(x), y(y), psi(psi), 4 | v(v), L(L), dt(dt) {} 5 | 6 | void KinematicModel::updateState(double a, double delta) 7 | { 8 | x = x + v * cos(psi) * dt; 9 | y = y + v * sin(psi) * dt; 10 | psi = psi + v / L * tan(delta) * dt; 11 | v = v + a * dt; 12 | } 13 | 14 | vector KinematicModel::getState() 15 | { 16 | return {x, y, psi, v}; 17 | } 18 | 19 | // 将模型离散化后的状态空间表达 20 | vector KinematicModel::stateSpace(double ref_delta, double ref_yaw) 21 | { 22 | MatrixXd A(3, 3), B(3, 2); 23 | A << 1.0, 0.0, -v * sin(ref_yaw) * dt, 24 | 0.0, 1.0, v * cos(ref_yaw) * dt, 25 | 0.0, 0.0, 1.0; 26 | B << cos(ref_yaw) * dt, 0, 27 | sin(ref_yaw) * dt, 0, 28 | tan(ref_delta) * dt / L, v * dt / (L * cos(ref_delta) * cos(ref_delta)); 29 | return {A, B}; 30 | } 31 | -------------------------------------------------------------------------------- /src/MPC/src/MPC.cpp: -------------------------------------------------------------------------------- 1 | #include "MPC.h" 2 | 3 | MPC::MPC(int nx, int nu, int t) : NX(nx), NU(nu), T(t) {} 4 | 5 | vector MPC::LMPC(MatrixXd xref, Vector3d x0, MatrixXd ref_delta, KinematicModel model) 6 | { 7 | int NX = xref.rows(); // 3 8 | int NU = ref_delta.rows(); // 2 9 | int T = xref.cols() - 1; // Horizon length. // 2 10 | 11 | // Define optimization variables. 12 | MatrixXd x(NX, T + 1); // 3, 3 13 | MatrixXd u(NU, T); // 1, 2 14 | // Store A matrices. 15 | vector A_vec; 16 | // Store B matrices. 17 | vector B_vec; 18 | 19 | // Initialize A and B matrices. 20 | for (int t = 0; t < T; ++t) { 21 | auto state_space = model.stateSpace(ref_delta(1, t), xref(2, t)); 22 | A_vec.push_back(state_space[0]); 23 | B_vec.push_back(state_space[1]); 24 | } 25 | 26 | // Define the optimization problem. 27 | VectorXd cost(T + 1); // t + 1 28 | // List of constraint indices. 29 | vector> constraints; 30 | for (int t = 0; t < T; ++t) { 31 | cost(t) = (u.col(t) - ref_delta.col(t)).transpose() * R * (u.col(t) - ref_delta.col(t)); 32 | 33 | if (t != 0) { 34 | cost(t) += (x.col(t) - xref.col(t)).transpose() * Q * (x.col(t) - xref.col(t)); 35 | } 36 | 37 | MatrixXd A = A_vec[t]; 38 | MatrixXd B = B_vec[t]; 39 | // 3 6 6 9 40 | constraints.push_back({(t + 1) * NX, (t + 1) * NX + NX}); // State constraints. 41 | // 0 2 2 4 42 | constraints.push_back({t * NU, t * NU + NU}); // Input constraints. 43 | 44 | x.col(t + 1) = A * x.col(t) + B * (u.col(t) - ref_delta.col(t)); 45 | } 46 | 47 | // Final state cost. 48 | cost(T) = (x.col(T) - xref.col(T)).transpose() * Qf * (x.col(T) - xref.col(T)); 49 | 50 | // Set initial state. 51 | x.col(0) = x0; // 3 52 | 53 | VectorXd lower_bound(T * NU); // 4 54 | VectorXd upper_bound(T * NU); // 4 55 | 56 | for (int t = 0; t < T; t++) 57 | { 58 | lower_bound.segment(t * NU, NU) << -MAX_VEL, -MAX_STEER; // (1:2) (3:4) 59 | upper_bound.segment(t * NU, NU) << MAX_VEL, MAX_STEER; // (1:2) (3:4) 60 | } 61 | 62 | // Solve the optimization problem. 63 | OsqpEigen::Solver solver; 64 | 65 | // solver.settings()->setVerbosity(false); 66 | solver.settings()->setWarmStart(true); 67 | 68 | // A: T * (NU + NX) x NX * (T + 1) 69 | // solver.data()->setNumberOfVariables(NX * (T + 1)); // 9 70 | // solver.data()->setNumberOfConstraints(T * (NU + NX));// 10 71 | 72 | solver.data()->setNumberOfVariables(NX * (T + 1)); //变量数n 9 73 | solver.data()->setNumberOfConstraints(T * (NU + NX));// 约束数m 10 74 | 75 | // P:NX * (T + 1) x NX * (T + 1) 76 | // Define the Hessian matrix dimension. 77 | int N = NX * (T + 1); // 9 78 | 79 | // Define and set the Hessian matrix. 80 | Eigen::SparseMatrix P(N, N); // 9 x 9 81 | 82 | for (int t = 0; t < T; ++t) { 83 | for (int i = 0; i < NU; ++i) { 84 | P.coeffRef(t * NU + i, t * NU + i) = R(i, i); 85 | } 86 | } 87 | 88 | if (!solver.data()->setHessianMatrix(P)) return {}; 89 | 90 | // Define the gradient vector (cost vector). 91 | VectorXd q(N); // 9 x 1 92 | 93 | for (int t = 0; t < T; ++t) { 94 | q.segment(t * NU, NU) = cost(t) * VectorXd::Ones(NU); 95 | } 96 | 97 | if (!solver.data()->setGradient(q)) return {}; 98 | 99 | // Define the linear equality constraint matrix Aeq. 100 | int M = T * (NX + NU); // Number of equality constraints. // M = 10 101 | Eigen::SparseMatrix linearMatrix(M, N); // 10 x 9 102 | for (int i = 0; i < M; i++) 103 | { 104 | for (int j = 0; j < N; j++) 105 | { 106 | if (i == j) linearMatrix.insert(i, j) = 1; 107 | else linearMatrix.insert(i, j) = 0; 108 | } 109 | } 110 | 111 | if (!solver.data()->setLinearConstraintsMatrix(linearMatrix)) return {}; 112 | // Define the equality constraint vector beq. 113 | VectorXd beq(M); // 10 x 1 114 | // for (int i = 0; i < M; i++) 115 | // { 116 | // beq(i) = 0; 117 | // } 118 | 119 | // You should populate Aeq and beq based on your state dynamics. 120 | 121 | // Set lower and upper bounds for variables and constraints. 122 | if (!solver.data()->setLowerBound(lower_bound)) { 123 | cerr << "Error setting lower bound." << endl; 124 | return {}; 125 | } 126 | 127 | if (!solver.data()->setUpperBound(upper_bound)) { 128 | cerr << "Error setting upper bound." << endl; 129 | return {}; 130 | } 131 | 132 | // Initialize the solver. 133 | if (!solver.initSolver()) return {}; 134 | 135 | // Solve the problem. 136 | if (!solver.solve()) { 137 | cerr << "Error solving the optimization problem." << endl; 138 | return {}; 139 | } 140 | VectorXd optimal_solution = solver.getSolution(); 141 | 142 | // Extract optimal control inputs. 143 | vector optimal_input; 144 | 145 | for (int t = 0; t < T; ++t) { 146 | VectorXd u_t = optimal_solution.segment(t * NU, NU); // (1,2) (3,4) 147 | optimal_input.push_back(u_t(0)); // Extract the velocity input. 148 | } 149 | cout << optimal_input[1] << endl; 150 | return optimal_input; 151 | } -------------------------------------------------------------------------------- /src/MPC/src/Reference_path.cpp: -------------------------------------------------------------------------------- 1 | #include "Reference_path.h" 2 | 3 | ReferencePath::ReferencePath() 4 | { 5 | ref_path = vector>(1000, vector(4)); 6 | // 生成参考轨迹 7 | for (int i = 0; i < 1000; i++) 8 | { 9 | ref_path[i][0] = 0.1 * i; 10 | ref_path[i][1] = 2 * sin(ref_path[i][0] / 3.0); 11 | 12 | ref_x.push_back(ref_path[i][0]); 13 | ref_y.push_back(ref_path[i][1]); 14 | } 15 | double dx, dy, ddx, ddy; 16 | for (int i = 0; i < ref_path.size(); i++) 17 | { 18 | if (i == 0) { 19 | dx = ref_path[i + 1][0] - ref_path[i][0]; 20 | dy = ref_path[i + 1][1] - ref_path[i][1]; 21 | ddx = ref_path[2][0] + ref_path[0][0] - 2 * ref_path[1][0]; 22 | ddy = ref_path[2][1] + ref_path[0][1] - 2 * ref_path[1][1]; 23 | } else if (i == ref_path.size() - 1) { 24 | dx = ref_path[i][0] - ref_path[i- 1][0]; 25 | dy = ref_path[i][1] - ref_path[i- 1][1]; 26 | ddx = ref_path[i][0] + ref_path[i- 2][0] - 2 * ref_path[i - 1][0]; 27 | ddy = ref_path[i][1] + ref_path[i - 2][1] - 2 * ref_path[i - 1][1]; 28 | } else { 29 | dx = ref_path[i + 1][0] - ref_path[i][0]; 30 | dy = ref_path[i + 1][1] - ref_path[i][1]; 31 | ddx = ref_path[i + 1][0] + ref_path[i - 1][0] - 2 * ref_path[i][0]; 32 | ddy = ref_path[i + 1][1] + ref_path[i - 1][1] - 2 * ref_path[i][1]; 33 | } 34 | ref_path[i][2] = atan2(dy, dx); 35 | //计算曲率:设曲线r(t) =(x(t),y(t)),则曲率k=(x'y" - x"y')/((x')^2 + (y')^2)^(3/2). 36 | ref_path[i][3] = (ddy * dx - ddx * dy) / pow((dx * dx + dy * dy), 3.0 / 2); // k计算 37 | } 38 | } 39 | // 计算跟踪误差 40 | vector ReferencePath::calcTrackError(vector robot_state) 41 | { 42 | double x = robot_state[0], y = robot_state[1]; 43 | vector d_x(ref_path.size()), d_y(ref_path.size()), d(ref_path.size()); 44 | for (int i = 0; i < ref_path.size(); i++) 45 | { 46 | d_x[i]=ref_path[i][0]-x; 47 | d_y[i]=ref_path[i][1]-y; 48 | d[i] = sqrt(d_x[i]*d_x[i]+d_y[i]*d_y[i]); 49 | } 50 | double min_index = min_element(d.begin(), d.end()) - d.begin(); 51 | double yaw = ref_path[min_index][2]; 52 | double k = ref_path[min_index][3]; 53 | double angle = normalizeAngle(yaw - atan2(d_y[min_index], d_x[min_index])); 54 | double error = d[min_index]; 55 | if (angle < 0) error *= -1; 56 | return {error, k, yaw, min_index}; 57 | } 58 | 59 | double ReferencePath::normalizeAngle(double angle) 60 | { 61 | while (angle > PI) 62 | { 63 | angle -= 2 * PI; 64 | } 65 | while (angle < -PI) 66 | { 67 | angle += 2 * PI; 68 | } 69 | return angle; 70 | } 71 | 72 | // 计算参考轨迹点，统一化变量数组，只针对MPC优化使用 73 | // robot_state 车辆的状态(x,y,yaw,v) 74 | refTraj ReferencePath::calc_ref_trajectory(vector robot_state,parameters param, double dl) { 75 | vectortrack_error = calcTrackError(robot_state);//e,k,ref_yaw,ind 76 | double e = track_error[0],k=track_error[1],ref_yaw=track_error[2],ind = track_error[3]; 77 | refTraj ref_traj; 78 | ref_traj.xref=MatrixXd (param.NX,param.T+1); // 3 x 3 79 | ref_traj.dref = MatrixXd (param.NU,param.T); // 1 x 2 80 | int ncourse = ref_path.size(); 81 | ref_traj.xref(0,0)=ref_path[ind][0]; 82 | ref_traj.xref(1,0)=ref_path[ind][1]; 83 | ref_traj.xref(2,0)=ref_path[ind][2]; 84 | //参考控制量[v,delta] 85 | double ref_delta = atan2(param.L*k,1); 86 | for(int i=0;i robot_state = {0.0, -3.0, 0.0, 0.0}; 24 | double dt = 0.1; 25 | double L = 2.2; 26 | ReferencePath reference_path; 27 | auto reference_trajectory = reference_path.calc_ref_trajectory(robot_state, param, 1.0); 28 | KinematicModel model(x0(0), x0(1), x0(2), 0.0, L, dt); 29 | PID_controller PID(3, 0.001, 30, Target_speed, 15.0 / 3.6, 0.0); 30 | 31 | std::vector x_; 32 | std::vector y_; 33 | 34 | for (int i = 0; i < param.T; i++) 35 | { 36 | Eigen::MatrixXd xref = reference_trajectory.xref; 37 | Eigen::VectorXd xref_i = xref.col(i); // 3 x 1 38 | Eigen::VectorXd ref_delta = reference_trajectory.dref.col(i); // 2 x 1 39 | 40 | std::vector control_input = mpc.LMPC(xref_i, x0, ref_delta, model); 41 | cout << control_input[1] << endl; 42 | double a = PID.calOutput(model.v); 43 | 44 | model.updateState(a, control_input[1]); 45 | cout << "Speed: " << model.v << " m/s" << endl; 46 | 47 | x_.push_back(model.getState()[0]); 48 | y_.push_back(model.getState()[1]); 49 | 50 | const auto state = model.getState(); 51 | x0 << state[0], state[1], state[2]; 52 | 53 | //画参考轨迹 54 | plt::plot(reference_path.ref_x, reference_path.ref_y, "b--"); 55 | plt::grid(true); 56 | plt::ylim(-5, 5); 57 | plt::plot(x_, y_, "r"); 58 | plt::pause(0.01); 59 | } 60 | const char* filename = "./MPC.png"; 61 | plt::save(filename); 62 | plt::show(); 63 | return 0; 64 | } 65 | -------------------------------------------------------------------------------- /src/MPC/src/pid_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "pid_controller.h" 2 | 3 | PID_controller::PID_controller(double Kp, double Ki, double Kd, double target, double upper, double lower) : Kp(Kp), 4 | Ki(Ki), 5 | Kd(Kd), 6 | target(target), 7 | upper(upper), 8 | lower(lower) {} 9 | void PID_controller::setTarget(double target) { 10 | PID_controller::target = target; 11 | } 12 | 13 | void PID_controller::setK(double Kp, double Ki, double Kd) { 14 | this->Kp = Kp; 15 | this->Ki = Ki; 16 | this->Kd = Kd; 17 | } 18 | 19 | void PID_controller::setBound(double upper, double lower) { 20 | this->upper = upper; 21 | this->lower = lower; 22 | } 23 | 24 | double PID_controller::calOutput(double state) { 25 | this->error = this->target - state; 26 | double u = this->error * this->Kp + this->sum_error * this->Ki + (this->error - this->pre_error) * this->Kd; 27 | if (u < this->lower) u = this->lower; 28 | else if (u > this->upper) u = this->upper; 29 | this->pre_error = this->error; 30 | this->sum_error = this->sum_error + this->error; 31 | return u; 32 | } 33 | 34 | void PID_controller::reset() { 35 | error = 0.0; 36 | pre_error = 0.0; 37 | sum_error = 0.0; 38 | } 39 | 40 | void PID_controller::setSumError(double sum_error) { 41 | this->sum_error = sum_error; 42 | } 43 | -------------------------------------------------------------------------------- /src/Purepursuit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(Purepursuit) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | std_msgs 7 | ) 8 | 9 | 10 | set(CMAKE_CXX_STANDARD 11) 11 | 12 | file(GLOB_RECURSE PYTHON2.7_LIB "/usr/lib/python2.7/config-x86_64-linux-gnu/*.so") 13 | set(PYTHON2.7_INLCUDE_DIRS "/usr/include/python2.7") 14 | 15 | catkin_package( 16 | # INCLUDE_DIRS include 17 | # LIBRARIES huatu 18 | # CATKIN_DEPENDS roscpp std_msgs 19 | # DEPENDS system_lib 20 | ) 21 | 22 | include_directories(include 23 | ${PYTHON2.7_INLCUDE_DIRS} 24 | ) 25 | 26 | add_executable(pp_controller src/pure_pursuit.cpp 27 | src/KinematicModel.cpp 28 | src/main.cpp 29 | src/pid_controller.cpp) 30 | target_link_libraries(pp_controller ${PYTHON2.7_LIB}) 31 | -------------------------------------------------------------------------------- /src/Purepursuit/include/KinematicModel.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | using namespace std; 7 | using namespace Eigen; 8 | 9 | class KinematicModel { 10 | public: 11 | KinematicModel(); 12 | KinematicModel(double x, double y, double psi, double v, double L, double dt); 13 | 14 | vector getState(); 15 | void updateState(double a, double delta); 16 | vector stateSpace(double ref_delta, double ref_yaw); 17 | public: 18 | double x, y, psi, v, L, dt; 19 | }; -------------------------------------------------------------------------------- /src/Purepursuit/include/pid_controller.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | using namespace std; 5 | 6 | class PID_controller { 7 | private: 8 | double Kp, Ki, Kd, target, upper, lower; 9 | double error = 0.0, pre_error = 0.0, sum_error = 0.0; 10 | public: 11 | PID_controller(double Kp, double Ki, double Kd, double target, double upper, double lower); 12 | 13 | void setTarget(double target); 14 | 15 | void setK(double Kp, double Ki, double Kd); 16 | 17 | void setBound(double upper, double lower); 18 | 19 | double calOutput(double state); 20 | 21 | void reset(); 22 | 23 | void setSumError(double sum_error); 24 | }; 25 | -------------------------------------------------------------------------------- /src/Purepursuit/include/pure_pursuit.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | class PurePursuit { 9 | public: 10 | double calTargetIndex(vectorrobot_state, vector> ref_path, double l_d); 11 | 12 | double Pure_Pursuit_Control(vector robot_state, vector current_ref_point, double l_d, double psi, double L); 13 | }; 14 | -------------------------------------------------------------------------------- /src/Purepursuit/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Purepursuit 4 | 0.0.0 5 | The Purepursuit package 6 | 7 | 8 | 9 | 10 | bigdavid 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 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_msgs 54 | roscpp 55 | std_msgs 56 | roscpp 57 | std_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/Purepursuit/src/KinematicModel.cpp: -------------------------------------------------------------------------------- 1 | #include "KinematicModel.h" 2 | 3 | KinematicModel::KinematicModel(double x, double y, double psi, double v, double L, double dt) : x(x), y(y), psi(psi), 4 | v(v), L(L), dt(dt) {} 5 | 6 | void KinematicModel::updateState(double a, double delta) 7 | { 8 | x = x + v * cos(psi) * dt; 9 | y = y + v * sin(psi) * dt; 10 | psi = psi + v / L * tan(delta) * dt; 11 | v = v + a * dt; 12 | } 13 | 14 | vector KinematicModel::getState() 15 | { 16 | return {x, y, psi, v}; 17 | } 18 | 19 | // 将模型离散化后的状态空间表达 20 | vector KinematicModel::stateSpace(double ref_delta, double ref_yaw) 21 | { 22 | MatrixXd A(3, 3), B(3, 2); 23 | A << 1.0, 0.0, -v * sin(ref_yaw) * dt, 24 | 0.0, 1.0, v * cos(ref_yaw) * dt, 25 | 0.0, 0.0, 1.0; 26 | B << cos(ref_yaw) * dt, 0, 27 | sin(ref_yaw) * dt, 0, 28 | tan(ref_delta) * dt / L, v * dt / (L * cos(ref_delta) * cos(ref_delta)); 29 | return {A, B}; 30 | } 31 | -------------------------------------------------------------------------------- /src/Purepursuit/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "matplotlibcpp.h" 2 | #include 3 | #include 4 | #include "KinematicModel.h" 5 | #include "pid_controller.h" 6 | #include "pure_pursuit.h" 7 | 8 | namespace plt = matplotlibcpp; 9 | #define PI 3.1415926 10 | 11 | int main() 12 | { 13 | double lam = 0.1; 14 | double c = 2; 15 | KinematicModel model(0.0, 1.0, 0.74, 0.0, 2.2, 0.1); // x,y,psi,v,L,dt 16 | double Target_speed = 20.0 / 3.6; // m/s 17 | vector> ref_path(1000, vector(2)); 18 | vector ref_x, ref_y; 19 | // 生成参考轨迹 20 | for (int i = 0; i < 1000; i++) { 21 | ref_path[i][0] = 0.1 * i; 22 | ref_path[i][1] = 2 * sin(ref_path[i][0] / 3.0); 23 | ref_x.push_back(ref_path[i][0]); 24 | ref_y.push_back(ref_path[i][1]); 25 | } 26 | 27 | vector x_, y_; 28 | vector robot_state(2); 29 | PurePursuit pp; 30 | PID_controller PID(3, 0.001, 30, Target_speed, 22.0 / 3.6, 0.0); 31 | for (int i = 0; i < 600; i++) 32 | { 33 | plt::clf(); 34 | robot_state[0] = model.x; 35 | robot_state[1] = model.y; 36 | double a = PID.calOutput(model.v); 37 | 38 | double l_d = lam * model.v + c; 39 | double min_ind = pp.calTargetIndex(robot_state, ref_path, l_d); 40 | double delta = pp.Pure_Pursuit_Control(robot_state, ref_path[min_ind], l_d, model.psi, 2.2); 41 | model.updateState(a, delta); 42 | cout << "Speed: " << model.v << " m/s" << endl; 43 | x_.push_back(model.x); 44 | y_.push_back(model.y); 45 | 46 | plt::plot(ref_x, ref_y, "b--"); 47 | plt::plot(x_, y_, "r--"); 48 | plt::grid(true); 49 | plt::ylim(-5, 5); 50 | plt::pause(0.01); 51 | } 52 | const char* filename = "./pure_pursuit.png"; 53 | plt::save(filename); 54 | plt::show(); 55 | return 0; 56 | } -------------------------------------------------------------------------------- /src/Purepursuit/src/pid_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "pid_controller.h" 2 | 3 | PID_controller::PID_controller(double Kp, double Ki, double Kd, double target, double upper, double lower) : Kp(Kp), 4 | Ki(Ki), 5 | Kd(Kd), 6 | target(target), 7 | upper(upper), 8 | lower(lower) {} 9 | void PID_controller::setTarget(double target) { 10 | PID_controller::target = target; 11 | } 12 | 13 | void PID_controller::setK(double Kp, double Ki, double Kd) { 14 | this->Kp = Kp; 15 | this->Ki = Ki; 16 | this->Kd = Kd; 17 | } 18 | 19 | void PID_controller::setBound(double upper, double lower) { 20 | this->upper = upper; 21 | this->lower = lower; 22 | } 23 | 24 | double PID_controller::calOutput(double state) { 25 | this->error = this->target - state; 26 | double u = this->error * this->Kp + this->sum_error * this->Ki + (this->error - this->pre_error) * this->Kd; 27 | if (u < this->lower) u = this->lower; 28 | else if (u > this->upper) u = this->upper; 29 | this->pre_error = this->error; 30 | this->sum_error = this->sum_error + this->error; 31 | return u; 32 | } 33 | 34 | void PID_controller::reset() { 35 | error = 0.0; 36 | pre_error = 0.0; 37 | sum_error = 0.0; 38 | } 39 | 40 | void PID_controller::setSumError(double sum_error) { 41 | this->sum_error = sum_error; 42 | } 43 | -------------------------------------------------------------------------------- /src/Purepursuit/src/pure_pursuit.cpp: -------------------------------------------------------------------------------- 1 | #include "pure_pursuit.h" 2 | 3 | // 计算邻近路点 (G_x, G_y) 4 | // robot_state:当前机器人位置 ref_path:参考轨迹 l_d:前向距离 5 | double PurePursuit::calTargetIndex(vector robot_state, vector> ref_path, double l_d) 6 | { 7 | vector dists; 8 | for (vector xy : ref_path) 9 | { 10 | double dist = sqrt(pow(xy[0] - robot_state[0], 2) + pow(xy[1] - robot_state[1], 2)); 11 | dists.push_back(dist); 12 | } 13 | double min_index = min_element(dists.begin(), dists.end()) - dists.begin(); 14 | double delta_l = sqrt(pow(ref_path[min_index][0] - robot_state[0], 2) + pow(ref_path[min_index][1] - robot_state[1], 2)); 15 | 16 | while (l_d > delta_l && min_index < ref_path.size() - 1) 17 | { 18 | delta_l = sqrt(pow(ref_path[min_index + 1][0] - robot_state[0], 2) + pow(ref_path[min_index + 1][1] - robot_state[1], 2)); 19 | min_index += 1; 20 | } 21 | return min_index; 22 | } 23 | 24 | // Pure Pursuit Control 25 | // robot_state 当前机器人位置; current_ref_point 参考轨迹点 ; l_d 前向距离 ; psi 机器人航向角 ; L 轴距 ; return 转角控制量 26 | double PurePursuit::Pure_Pursuit_Control(vector robot_state, vector current_ref_point, double l_d, double psi, double L) 27 | { 28 | double alpha = atan2(current_ref_point[1] - robot_state[1], current_ref_point[0] - robot_state[0]) - psi; 29 | double delta = atan2(2 * L * sin(alpha), l_d); 30 | return delta; 31 | } 32 | -------------------------------------------------------------------------------- /src/QP_Smooth_referenceline/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(QP_Smooth_referenceline) 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 | std_msgs 13 | ) 14 | set(CMAKE_CXX_STANDARD 11) 15 | file(GLOB_RECURSE PYTHON2.7_LIB "/usr/lib/python2.7/config-x86_64-linux-gnu/*.so") 16 | set(PYTHON2.7_INLCUDE_DIRS "/usr/include/python2.7") 17 | 18 | catkin_package( 19 | # INCLUDE_DIRS include 20 | # LIBRARIES QP_Smooth_referenceline 21 | # CATKIN_DEPENDS roscpp std_msgs 22 | # DEPENDS system_lib 23 | ) 24 | include_directories(./eigen-3.4.0) 25 | 26 | link_directories( 27 | /usr/local/lib 28 | ) 29 | 30 | include_directories( 31 | include 32 | ${catkin_INCLUDE_DIRS} 33 | ${PYTHON2.7_INLCUDE_DIRS} 34 | ) 35 | add_executable(osqp1 src/OSQP.cpp 36 | src/main.cpp 37 | ) 38 | target_link_libraries(osqp1 ${PYTHON2.7_LIB} 39 | libosqp.so 40 | libOsqpEigen.so 41 | libqdldl.so) 42 | 43 | -------------------------------------------------------------------------------- /src/QP_Smooth_referenceline/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HardWorkingDavid/control_algorithm/def86dd2d5a2d9c66dd844ac714a5515115da38c/src/QP_Smooth_referenceline/Figure_1.png -------------------------------------------------------------------------------- /src/QP_Smooth_referenceline/README.md: -------------------------------------------------------------------------------- 1 | **二次规划平滑路径** 2 | 3 | 平滑结果： 4 | 5 | ![Alt text](Figure_1.png) -------------------------------------------------------------------------------- /src/QP_Smooth_referenceline/include/OSQP.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "matplotlibcpp.h" 4 | #include 5 | #include 6 | #include "OsqpEigen/OsqpEigen.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "math.h" 13 | using namespace std; 14 | extern std::vector global_path_x; 15 | extern std::vector global_path_y; 16 | 17 | namespace plt = matplotlibcpp; 18 | 19 | class Trajectory_Smoothing { 20 | public: 21 | double buff = 0.1; 22 | 23 | int Weight_smooth = 1; 24 | int Weight_length = 1; 25 | int Weight_ref = 1; 26 | int n; // 轨迹点 27 | 28 | //各个代价下的矩阵 29 | Eigen::MatrixXd A1; 30 | Eigen::MatrixXd A2; 31 | Eigen::MatrixXd A3; 32 | 33 | Eigen::VectorXd h; 34 | Eigen::MatrixXd H; 35 | Eigen::VectorXd f; 36 | 37 | //上下边界约束 38 | Eigen::VectorXd lb; 39 | Eigen::VectorXd ub; 40 | 41 | int Write(); // 用于读取txt文档中的轨迹X Y坐标点 42 | //为矩阵赋予相应维度零矩阵 43 | Eigen::MatrixXd wei_du_MatrixXd(int i, int j, Eigen::MatrixXd& MatrixXd_); 44 | 45 | //为向量赋予相应维度零矩阵 46 | Eigen::VectorXd wei_du_VectorXd(int i, Eigen::VectorXd& VectorXd_); 47 | 48 | //得到各个矩阵维度 49 | Eigen::MatrixXd get_MatrixXd(int rows, int cols); 50 | 51 | //得到各个向量维度 52 | Eigen::VectorXd get_VectorXd(int rows, int cols); 53 | 54 | //构建各A1矩阵乘相关权重用以计算H 55 | Eigen::MatrixXd create_A1(Eigen::MatrixXd& A1); 56 | 57 | //构建各A2矩阵乘相关权重用以计算H 58 | Eigen::MatrixXd create_A2(Eigen::MatrixXd& A2); 59 | 60 | //构建各A3矩阵乘相关权重用以计算H 61 | Eigen::MatrixXd create_A3(Eigen::MatrixXd& A3); 62 | 63 | //构建h乘相关权重用以计算f 64 | Eigen::VectorXd create_h(Eigen::VectorXd& h); 65 | 66 | //构建约束下限lb 67 | Eigen::VectorXd create_lb(Eigen::VectorXd& lb); 68 | 69 | //构建约束上限lu 70 | Eigen::VectorXd create_ub(Eigen::VectorXd& lu); 71 | }; -------------------------------------------------------------------------------- /src/QP_Smooth_referenceline/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | QP_Smooth_referenceline 4 | 0.0.0 5 | The QP_Smooth_referenceline package 6 | 7 | 8 | 9 | 10 | bigdavid 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 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_msgs 54 | roscpp 55 | std_msgs 56 | roscpp 57 | std_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/QP_Smooth_referenceline/src/OSQP.cpp: -------------------------------------------------------------------------------- 1 | #include "OSQP.h" 2 | 3 | vector global_path_x; 4 | vector global_path_y; 5 | 6 | int Trajectory_Smoothing::Write() { 7 | std::ifstream input_x("/home/bigdavid/视频/Control_algorithm/src/QP_Smooth_referenceline/src/global_path_x.txt"); 8 | if (!input_x.is_open()) { 9 | std::cout << "Failed to open the file." << std::endl; 10 | return 1; 11 | } 12 | 13 | std::string line_x; 14 | while (std::getline(input_x, line_x)) { 15 | double value = std::stod(line_x); 16 | global_path_x.push_back(value); 17 | if(global_path_x.size() > 500) break; 18 | } 19 | input_x.close(); 20 | 21 | std::ifstream input_y("/home/bigdavid/视频/Control_algorithm/src/QP_Smooth_referenceline/src/global_path_y.txt"); 22 | if (!input_y.is_open()) { 23 | std::cout << "Failed to open the file." << std::endl; 24 | return 1; 25 | } 26 | 27 | std::string line_y; 28 | while (std::getline(input_y, line_y)) { 29 | double value = std::stod(line_y); 30 | global_path_y.push_back(value); 31 | if(global_path_y.size() > 500) break; 32 | } 33 | input_y.close(); 34 | return 0; 35 | } 36 | 37 | //为矩阵赋予相应维度零矩阵 38 | Eigen::MatrixXd Trajectory_Smoothing::wei_du_MatrixXd(int i, int j, Eigen::MatrixXd& MatrixXd_){ 39 | 40 | MatrixXd_ = Eigen::MatrixXd::Zero(i, j); 41 | return MatrixXd_; 42 | } 43 | 44 | //为向量赋予相应维度零矩阵 45 | Eigen::VectorXd Trajectory_Smoothing::wei_du_VectorXd(int i, Eigen::VectorXd& VectorXd_){ 46 | 47 | VectorXd_ = Eigen::VectorXd::Zero(i); 48 | return VectorXd_; 49 | } 50 | 51 | //填充矩阵和向量 52 | Eigen::MatrixXd Trajectory_Smoothing::create_A1(Eigen::MatrixXd& A1){ 53 | for (int i=0; i 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "OSQP.h" 8 | 9 | using namespace std; 10 | 11 | int main() 12 | { 13 | Trajectory_Smoothing Smoothing; 14 | Smoothing.Write(); 15 | 16 | // 轨迹点 17 | unsigned int n; 18 | n = min(global_path_x.size(), global_path_y.size()); 19 | plt::clf(); 20 | 21 | plt::scatter(global_path_x, global_path_y); 22 | 23 | //计算H（hessian） 24 | Eigen::MatrixXd A1 = Smoothing.wei_du_MatrixXd(2*n, 2*n-4, Smoothing.A1); 25 | Eigen::MatrixXd A2 = Smoothing.wei_du_MatrixXd(2*n, 2*n-2, Smoothing.A2); 26 | Eigen::MatrixXd A3 = Smoothing.wei_du_MatrixXd(2*n, 2*n, Smoothing.A3); 27 | Eigen::MatrixXd H; 28 | 29 | A1 = Smoothing.create_A1(A1); 30 | A2 = Smoothing.create_A2(A2); 31 | A3 = Smoothing.create_A3(A3); 32 | H = (Smoothing.Weight_smooth * A1 * A1.transpose() + Smoothing.Weight_length * A2 * A2.transpose() + Smoothing.Weight_ref * A3 * A3.transpose()); 33 | 34 | //计算f（gradient） 35 | Eigen::VectorXd h = Smoothing.wei_du_VectorXd(2*n, Smoothing.h); 36 | Eigen::VectorXd f = Smoothing.wei_du_VectorXd(2*n, Smoothing.f); 37 | h = Smoothing.create_h(h); 38 | f = Smoothing.Weight_ref * h; 39 | 40 | //计算上下限 41 | Eigen::VectorXd lb = Smoothing.wei_du_VectorXd(2*n, Smoothing.lb); 42 | Eigen::VectorXd ub = Smoothing.wei_du_VectorXd(2*n, Smoothing.ub); 43 | lb = Smoothing.create_lb(lb); 44 | ub = Smoothing.create_ub(ub); 45 | 46 | //OSQP矩阵赋值 47 | Eigen::SparseMatrix hessian(2*n,2*n); //P: n*n正定矩阵,必须为稀疏矩阵SparseMatrix 48 | Eigen::SparseMatrix linearMatrix(2*n,2*n); //A: m*n矩阵,必须为稀疏矩阵SparseMatrix 49 | for (int i = 0; i < 2 * n; i++){ 50 | for (int j = 0; j < 2 * n; j++){ 51 | hessian.insert(i, j) = H(i,j); //注意稀疏矩阵的初始化方式,无法使用<<初始化 52 | } 53 | } 54 | 55 | for (int i = 0; i < 2 * n; i++){ //A（linearMatrix）矩阵 56 | linearMatrix.insert(i, i) = 1; //注意稀疏矩阵的初始化方式,无法使用<<初始化 57 | } 58 | 59 | Eigen::VectorXd gradient(2 * n); //Q: n*1向量 60 | Eigen::VectorXd lowerBound(2 * n); //L: m*1下限向量 61 | Eigen::VectorXd upperBound(2 * n); //U: m*1上限向量 62 | gradient << f; 63 | 64 | lowerBound << lb; 65 | upperBound << ub; 66 | 67 | OsqpEigen::Solver solver; 68 | 69 | // settings 70 | solver.settings()->setVerbosity(false); 71 | solver.settings()->setWarmStart(true); 72 | 73 | // set the initial data of the QP solver 74 | solver.data()->setNumberOfVariables(2 * n); //变量数n 75 | solver.data()->setNumberOfConstraints(2 * n); //约束数m 76 | if (!solver.data()->setHessianMatrix(hessian)) 77 | return 1; 78 | if (!solver.data()->setGradient(gradient)) 79 | return 1; 80 | if (!solver.data()->setLinearConstraintsMatrix(linearMatrix)) 81 | return 1; 82 | if (!solver.data()->setLowerBound(lowerBound)) 83 | return 1; 84 | if (!solver.data()->setUpperBound(upperBound)) 85 | return 1; 86 | 87 | // instantiate the solver 88 | if (!solver.initSolver()) 89 | return 1; 90 | Eigen::VectorXd QPSolution; 91 | 92 | // solve the QP problem 93 | if (!solver.solve()) 94 | { 95 | return 1; 96 | } 97 | QPSolution = solver.getSolution(); 98 | 99 | std::vector X_solve_; 100 | X_solve_.reserve(QPSolution.size() / 2); 101 | 102 | for (int i = 0; i < QPSolution.size(); i += 2) { 103 | X_solve_.push_back(QPSolution(i)); 104 | } 105 | 106 | std::vector Y_solve_; 107 | Y_solve_.reserve((QPSolution.size() + 1) / 2); 108 | 109 | for (int i = 1; i < QPSolution.size(); i += 2) { 110 | Y_solve_.push_back(QPSolution(i)); 111 | } 112 | 113 | plt::plot(X_solve_, Y_solve_, "r"); 114 | // 设置图形标题和轴标签 115 | plt::title("Lane Line"); 116 | plt::xlabel("X"); 117 | plt::ylabel("Y"); 118 | 119 | // 显示图形 120 | plt::show(); 121 | 122 | return 0; 123 | } -------------------------------------------------------------------------------- /src/Stanley/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(Stanley) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | std_msgs 7 | ) 8 | 9 | set(CMAKE_CXX_STANDARD 11) 10 | 11 | file(GLOB_RECURSE PYTHON2.7_LIB "/usr/lib/python2.7/config-x86_64-linux-gnu/*.so") 12 | set(PYTHON2.7_INLCUDE_DIRS "/usr/include/python2.7") 13 | 14 | catkin_package( 15 | # INCLUDE_DIRS include 16 | # LIBRARIES huatu 17 | # CATKIN_DEPENDS roscpp std_msgs 18 | # DEPENDS system_lib 19 | ) 20 | 21 | include_directories(include 22 | ${PYTHON2.7_INLCUDE_DIRS} 23 | ) 24 | 25 | add_executable(stanley_controller src/stanley.cpp 26 | src/KinematicModel.cpp 27 | src/main.cpp 28 | src/pid_controller.cpp) 29 | target_link_libraries(stanley_controller ${PYTHON2.7_LIB}) 30 | -------------------------------------------------------------------------------- /src/Stanley/include/KinematicModel.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | using namespace std; 7 | using namespace Eigen; 8 | 9 | class KinematicModel { 10 | public: 11 | KinematicModel(); 12 | KinematicModel(double x, double y, double psi, double v, double L, double dt); 13 | 14 | vector getState(); 15 | void updateState(double a, double delta); 16 | vector stateSpace(double ref_delta, double ref_yaw); 17 | public: 18 | double x, y, psi, v, L, dt; 19 | }; -------------------------------------------------------------------------------- /src/Stanley/include/pid_controller.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | using namespace std; 5 | 6 | class PID_controller { 7 | private: 8 | double Kp, Ki, Kd, target, upper, lower; 9 | double error = 0.0, pre_error = 0.0, sum_error = 0.0; 10 | public: 11 | PID_controller(double Kp, double Ki, double Kd, double target, double upper, double lower); 12 | 13 | void setTarget(double target); 14 | 15 | void setK(double Kp, double Ki, double Kd); 16 | 17 | void setBound(double upper, double lower); 18 | 19 | double calOutput(double state); 20 | 21 | void reset(); 22 | 23 | void setSumError(double sum_error); 24 | }; 25 | -------------------------------------------------------------------------------- /src/Stanley/include/stanley.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | using namespace std; 7 | 8 | #define PI 3.1415926 9 | class Stanley { 10 | private: 11 | double k; 12 | public: 13 | Stanley(double k); 14 | double calTargetIndex(vector robot_state, vector> ref_path); 15 | double normalizeAngle(double angle); 16 | vector stanleyControl(vector robot_state, vector> ref_path); 17 | }; -------------------------------------------------------------------------------- /src/Stanley/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Stanley 4 | 0.0.0 5 | The Stanley package 6 | 7 | 8 | 9 | 10 | bigdavid 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 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_msgs 54 | roscpp 55 | std_msgs 56 | roscpp 57 | std_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/Stanley/src/KinematicModel.cpp: -------------------------------------------------------------------------------- 1 | #include "KinematicModel.h" 2 | 3 | KinematicModel::KinematicModel(double x, double y, double psi, double v, double L, double dt) : x(x), y(y), psi(psi), 4 | v(v), L(L), dt(dt) {} 5 | 6 | void KinematicModel::updateState(double a, double delta) 7 | { 8 | x = x + v * cos(psi) * dt; 9 | y = y + v * sin(psi) * dt; 10 | psi = psi + v / L * tan(delta) * dt; 11 | v = v + a * dt; 12 | } 13 | 14 | vector KinematicModel::getState() 15 | { 16 | return {x, y, psi, v}; 17 | } 18 | 19 | // 将模型离散化后的状态空间表达 20 | vector KinematicModel::stateSpace(double ref_delta, double ref_yaw) 21 | { 22 | MatrixXd A(3, 3), B(3, 2); 23 | A << 1.0, 0.0, -v * sin(ref_yaw) * dt, 24 | 0.0, 1.0, v * cos(ref_yaw) * dt, 25 | 0.0, 0.0, 1.0; 26 | B << cos(ref_yaw) * dt, 0, 27 | sin(ref_yaw) * dt, 0, 28 | tan(ref_delta) * dt / L, v * dt / (L * cos(ref_delta) * cos(ref_delta)); 29 | return {A, B}; 30 | } 31 | -------------------------------------------------------------------------------- /src/Stanley/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "KinematicModel.h" 2 | #include "pid_controller.h" 3 | #include "matplotlibcpp.h" 4 | #include 5 | #include 6 | #include "stanley.h" 7 | 8 | namespace plt = matplotlibcpp; 9 | #define PI 3.1415926 10 | 11 | int main() 12 | { 13 | double Target_speed = 12.0 / 3.6; // m / s 14 | KinematicModel model(0.0, 1.0, 0.74, 0.0, 2.2, 0.1); // x,y,psi,v,L,dt 15 | vector> ref_path(1000, vector(3)); 16 | vector ref_x, ref_y; 17 | // 生成参考轨迹 18 | for (int i = 0; i < 1000; i++) 19 | { 20 | ref_path[i][0] = 0.1 * i; 21 | ref_path[i][1] = 2 * sin(ref_path[i][0] / 3.0); 22 | for (int i = 0; i < 999; i++) { 23 | ref_path[i][2]= atan2((ref_path[i+1][1]-ref_path[i][1]),(ref_path[i+1][0]-ref_path[i][0])); 24 | } 25 | ref_x.push_back(ref_path[i][0]); 26 | ref_y.push_back(ref_path[i][1]); 27 | } 28 | vector x_, y_; 29 | double k = 6.99; // 增益参数 30 | vector robot_state(4); 31 | Stanley stanley(k); 32 | PID_controller PID(2, 0.001, 30, Target_speed, 16.0 / 3.6, 0.0); 33 | for (int i = 0; i < 600; i++) 34 | { 35 | plt::clf(); 36 | robot_state = model.getState(); 37 | double a = PID.calOutput(model.v); 38 | vector delta_index = stanley.stanleyControl(robot_state, ref_path); 39 | 40 | model.updateState(a, delta_index[0]); 41 | cout << "Speed: " << model.v << " m/s" << endl; 42 | x_.push_back(model.x); 43 | y_.push_back(model.y); 44 | 45 | plt::plot(ref_x, ref_y, "b--"); 46 | plt::plot(x_, y_, "r--"); 47 | plt::grid(true); 48 | plt::ylim(-3, 3); 49 | plt::pause(0.01); 50 | if (delta_index[1] >= ref_path.size() - 1) break; 51 | } 52 | const char* filename = "./stanley.png"; 53 | plt::save(filename); 54 | plt::show(); 55 | return 0; 56 | } -------------------------------------------------------------------------------- /src/Stanley/src/pid_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "pid_controller.h" 2 | 3 | PID_controller::PID_controller(double Kp, double Ki, double Kd, double target, double upper, double lower) : Kp(Kp), 4 | Ki(Ki), 5 | Kd(Kd), 6 | target(target), 7 | upper(upper), 8 | lower(lower) {} 9 | void PID_controller::setTarget(double target) { 10 | PID_controller::target = target; 11 | } 12 | 13 | void PID_controller::setK(double Kp, double Ki, double Kd) { 14 | this->Kp = Kp; 15 | this->Ki = Ki; 16 | this->Kd = Kd; 17 | } 18 | 19 | void PID_controller::setBound(double upper, double lower) { 20 | this->upper = upper; 21 | this->lower = lower; 22 | } 23 | 24 | double PID_controller::calOutput(double state) { 25 | this->error = this->target - state; 26 | double u = this->error * this->Kp + this->sum_error * this->Ki + (this->error - this->pre_error) * this->Kd; 27 | if (u < this->lower) u = this->lower; 28 | else if (u > this->upper) u = this->upper; 29 | this->pre_error = this->error; 30 | this->sum_error = this->sum_error + this->error; 31 | return u; 32 | } 33 | 34 | void PID_controller::reset() { 35 | error = 0.0; 36 | pre_error = 0.0; 37 | sum_error = 0.0; 38 | } 39 | 40 | void PID_controller::setSumError(double sum_error) { 41 | this->sum_error = sum_error; 42 | } 43 | -------------------------------------------------------------------------------- /src/Stanley/src/stanley.cpp: -------------------------------------------------------------------------------- 1 | #include "stanley.h" 2 | 3 | Stanley::Stanley(double k) 4 | { 5 | this->k = k; 6 | } 7 | 8 | // 搜索目标邻近路点 9 | // robot_state 当前机器人位置 ref_path 参考轨迹(数组) 10 | double Stanley::calTargetIndex(vector robot_state, vector> ref_path) 11 | { 12 | vector dists; 13 | for (vector xy : ref_path) 14 | { 15 | double dist = sqrt(pow(xy[0] - robot_state[0], 2) + pow(xy[1] - robot_state[1], 2)); 16 | dists.push_back(dist); 17 | } 18 | return min_element(dists.begin(), dists.end()) - dists.begin(); 19 | } 20 | 21 | double Stanley::normalizeAngle(double angle) 22 | { 23 | while (angle > PI) 24 | { 25 | angle -= 2.0 * PI; 26 | } 27 | while (angle < -PI) 28 | { 29 | angle += 2.0 * PI; 30 | } 31 | return angle; 32 | } 33 | 34 | // stanley 控制 35 | // robot_state：x,y,yaw,v ref_path：x,y,theta 36 | // return 控制量 + 目标点索引 37 | vector Stanley::stanleyControl(vector robot_state, vector> ref_path) 38 | { 39 | double current_target_index = calTargetIndex(robot_state, ref_path); 40 | vector current_ref_point; 41 | 42 | if (current_target_index >= ref_path.size()) 43 | { 44 | current_target_index = ref_path.size() - 1; 45 | current_ref_point = ref_path[ref_path.size() - 1]; 46 | } else { 47 | current_ref_point = ref_path[current_target_index]; 48 | } 49 | double e_y; 50 | double psi_t = current_ref_point[2]; 51 | 52 | if ((robot_state[0] - current_ref_point[0]) * psi_t - (robot_state[1] - current_ref_point[1]) > 0) 53 | { 54 | e_y = sqrt(pow(current_ref_point[0]-robot_state[0],2)+pow(current_ref_point[1]-robot_state[1],2)); 55 | } 56 | else 57 | { 58 | e_y = -sqrt(pow(current_ref_point[0]-robot_state[0],2)+pow(current_ref_point[1]-robot_state[1],2)); 59 | } 60 | double psi = robot_state[2]; 61 | double v = robot_state[3]; 62 | double theta_e = psi_t - psi; 63 | double delta_e = atan2(k * e_y, v); 64 | double delta = normalizeAngle(delta_e + theta_e); 65 | 66 | // 限制车轮转角[-30, 30] 67 | if (delta > PI / 6.0) delta = PI / 6.0; 68 | else if (delta < -PI / 6.0) delta = -PI / 6.0; 69 | 70 | return {delta, current_target_index}; 71 | } 72 | -------------------------------------------------------------------------------- /stanley.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HardWorkingDavid/control_algorithm/def86dd2d5a2d9c66dd844ac714a5515115da38c/stanley.png --------------------------------------------------------------------------------