--- title: "Robot Car Path Planning with Motion Tracking and Lidar" url: "https://maker.wiznet.io/WIZnet/projects/robot-car-path-planning-with-motion-tracking-and-lidar/" markdown_url: "https://maker.wiznet.io/WIZnet/projects/robot-car-path-planning-with-motion-tracking-and-lidar/md" type: "UCC: User Created Content" author: "Yixiao Liu" author_url: "https://www.hackster.io/yixiaol2/robot-car-path-planning-with-motion-tracking-and-lidar-5d5ba3" editor: "WIZnet" editor_url: "https://maker.wiznet.io/" original_author: "Yixiao Liu" original_url: "https://www.hackster.io/yixiaol2/robot-car-path-planning-with-motion-tracking-and-lidar-5d5ba3" published: "2023-05-30" language: "en" hardware: ["Texas Instruments LAUNCHXL-F28069M C2000 Piccolo LaunchPad", "WIZnet W5500"] likes: 0 views: 148 comments: 0 source: "WIZnet Makers (https://maker.wiznet.io/)" --- # Robot Car Path Planning with Motion Tracking and Lidar > The robot detects obstacles with Lidar, plans the path with A* algorithm, then travels through the path with motion tracking. Original author: Yixiao Liu (source: https://www.hackster.io/yixiaol2/robot-car-path-planning-with-motion-tracking-and-lidar-5d5ba3) ## Components - **Texas Instruments LAUNCHXL-F28069M C2000 Piccolo LaunchPad** x 1 ([docs](https://www.ti.com/tool/LAUNCHXL-F28069M?HQS=corp-uni-null-hackster-cs-evm-LAUNCHXLF28069M-wwe)) - **WIZnet W5500** x 1 ([docs](https://docs.wiznet.io/Product/Chip/Ethernet/W5500)) WIZnet parts: W5500 ([Datasheet](https://docs.wiznet.io/Product/Chip/Ethernet/W5500/datasheet?utm_source=maker&utm_medium=project&utm_campaign=w5500), [product hub](https://maker.wiznet.io/products/w5500/)) ## Documents and links - [Final project codeC/C++ The code for both CPU in this project](https://hacksterio.s3.amazonaws.com/uploads/attachments/1300097/workspace.zip) (code) - [Circuit Board Schematic](https://hacksterio.s3.amazonaws.com/uploads/attachments/1299406/se423_f28379d_XwaB8oEJ79.sch) (schematics) ## Article ## Story This project is the final project of SE423 at UIUC. The goal of this project is to let the robot car navigate through a built track while avoiding the random obstacle in its way. There are several major components of this project. - Control the robot car and use dead reckoning to get the pose of the robot car - Process Lidar reading with F28379D Launchpad - Use Lidar to recognize the obstacle - Use A* algorithm to find the path - Communicate with the motion tracking system to navigate through the path ### Control the robot car We used the enhanced Quadrature Encoder Pulse (eQEP) peripheral of the F28379D launchpad to get the angles of the motors used in our robot car. With that, it is easy to get the velocity of the robot car with the sample time and the distance traveled. We then used a PI controller to steer the robot car around. Ideally, we can then calculate the pose of the robot car by dead reckoning assuming there’s no slip between the wheels and the floor. ### Process Lidar data For this project, a cheap YDLidar X2 model works well. It can sense a range of [0.10 to 8.0m with a 360° perspective.](https://github.com/YDLIDAR/YDLidar-SDK/blob/master/doc/Dataset.md) One great thing about this Lidar is that it will automatically start to collect data the instant it is powered on. ![YDLidar X2](https://hackster.imgix.net/uploads/attachments/1299423/image_5JcqUMWDEX.png?auto=compress%2Cformat&w=740&h=555&fit=max) YDLidar X2 The Lidar can transmit data through the serial port. In this project, SCID is used to accept the data, and the data of the Lidar comes in through pin 9 of the launchpad. ![Receiving data through pin 9 (SCID) of the launchpad](https://hackster.imgix.net/uploads/attachments/1299786/wechat_image_20210517214530_YsRZZ5PA7O.jpg?auto=compress%2Cformat&w=740&h=555&fit=max) Receiving data through pin 9 (SCID) of the launchpad I used an 8-state state machine to interpret and store the Lidar data. The resulting data is in the form of an array with 360 entries, each entry contains the distance and timestamp information of the points detected by the Lidar, and the index of this array is the angle at which the data is collected. **A Detailed explanation on reading the Lidar data can be found at **[**https://www.hackster.io/yixiaol2/using-ydlidar-with-ti-tms320f28379d-6b62c5**](https://www.hackster.io/yixiaol2/using-ydlidar-with-ti-tms320f28379d-6b62c5) ### Use Lidar to detect obstacles After getting the Lidar data from the last step, we get the information of what the robot sees: the robot is at the center, and there is a distance that corresponds to all integer angles from 0 to 359°. Therefore, a polar plot will be adequate for us to visualize the results. ![Polar plot of the data collected from my living room](https://hackster.imgix.net/uploads/attachments/1299801/image_ApaBdeR98q.png?auto=compress%2Cformat&w=740&h=555&fit=max) 1 / 2 • Polar plot of the data collected from my living room In our project, we built a 12*12 feet track in the mechatronics lab for the robot car to run within, and the obstacles are wood blocks that are 2*2 feet that can only be placed at the integer grid in the track. ![one possible configuration of the obstacles in the track](https://hackster.imgix.net/uploads/attachments/1299845/image_YHifGv9WHZ.png?auto=compress%2Cformat&w=740&h=555&fit=max) 1 / 3 • one possible configuration of the obstacles in the track This setup greatly simplifies our obstacle-finding procedure: based on the collected data, we can get the “global view” of the track by coordinate transformation given the initial conditions of the robot car (pose_x, pose_y, pose_theta). Then, round the readings of the obstacle within the range of the track should be adequate for us to get the edge of the obstacle since the obstacle can only appear on integer grids. The following initial condition corresponds to the second figure above, where `pose_x` is -4, `pose_y` is 6, and the `pose_theta` is 30°. ```plaintext //initial condition double pose_x = -4; //x_position of the robot car double pose_y = 6; //x_position of the robot car //need to put negative initial angle because the Lidar is rotating CW double pose_theta = 0; //angle of robot car double pose_rad; //angle in rad ``` The coordinate transformation is done in the while loop inside the main function. A ping-pong buffer is used to make sure there’s enough time to process all the data. In the following code, I first loop through all the points in the buffer array. If the obstacle within 0.1 m of the lidar, which is not detectable by the Lidar thus having a distance of 0, I put it at the origin, which corresponds to the origin globally. ```plaintext if (pingpts[i].distance ==0) { x_f[i].distance = 0; y_f[i].distance = 0; x_f[i].timestamp = pingpts[i].timestamp; y_f[i].timestamp = pingpts[i].timestamp; } ``` Otherwise, I first convert the polar coordinate to Cartesian coordinate, and used the change of basis equation to get the “global” position of the detected points. `x_ori` and `y_ori` are the direct results from changing polar coordinate to Cartesian coordinate, while `x_f` and `y_f` are the final results by change basis and translation. ```plaintext else { x_ori[i] = pingpts[i].distance*cos(i*0.01745329);//0.017453292519943 is pi/180 y_ori[i] = pingpts[i].distance*sin(i*0.01745329); x_f[i].distance = (x_ori[i])*cos(pose_rad)+(y_ori[i])*sin(pose_rad)+pose_x; //change basis y_f[i].distance = -(x_ori[i])*sin(pose_rad)+(y_ori[i])*cos(pose_rad)+pose_y; //change basis ``` Finally, with simple criteria of only considering the obstacle within the range of `x:(-6,6), y:(0,12),` we can mark the location with points detected as obstacle ‘x’ on the `mapCourseStart` array. The `mapCourseStart` is an array of characters with 176 (11*16) elements that store the map of the track’s grid crossing. It contains all ‘0’ during initialization, which means there are no obstacles yet. ```plaintext char mapCourseStart[176] = //16x11 { '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0' }; ``` Points that are within 1 tile of the robot are not considered as it may be some random data collection error. There’s a bijective correspondence between the global position(x, y) of the obstacle to the index of the `mapCourseStart` array that is given by `11*(11-y)+5-x.` Here (x, y) is the position of the lidar reading after coordinate transformation and translation. ```plaintext // code to process the obstacle if ((round(x_f[i].distance) >-6) && (round(x_f[i].distance) <6) ) { // within x range if ((round(y_f[i].distance) > 0)&& round(y_f[i].distance) < 12) { //within y range if ((fabs(x_f[i].distance-pose_x) > 1) && (fabs(y_f[i].distance-pose_y) > 1)) { //neglect points that are too close to the robot car mapCourseStart[(11*(11-(int)(round(y_f[i].distance))))+5+(int)(round(x_f[i].distance))] = 'x'; //set 'x' at correct location } } } x_f[i].timestamp = pingpts[i].timestamp; y_f[i].timestamp = pingpts[i].timestamp; } } ``` Below is a figure overlaying the `mapCourseStart` array with the grid. The points of the `mapCourseStart` are at the center of each tile. ![Overlaying the grid and the array that is used to keep track of the obstacle](https://hackster.imgix.net/uploads/attachments/1300064/untitled-3_feTNDcoW5y.jpg?auto=compress%2Cformat&w=740&h=555&fit=max) Overlaying the grid and the array that is used to keep track of the obstacle Therefore, seeing from the robot at the initial condition of `pose_x` = -4, `pose_y` = 6, and the `pose_theta` = 30°, the points with index 18, 29, 40, 41, 42 should change from ‘0’ to ‘x’ in the `mapCourseStart` array. ![Interpreted the setup onto the overlaying figure](https://hackster.imgix.net/uploads/attachments/1300065/image_THEnBTnJti.png?auto=compress%2Cformat&w=740&h=555&fit=max) 1 / 2 • Interpreted the setup onto the overlaying figure Checking the result from the expression window of Code Composer Studio for the `mapCourseStart` array, we get the expected result from our code, with index 18, 29, 40, 41, 42 being an obstacle (‘x’). ![](https://hackster.imgix.net/uploads/attachments/1300074/image_d6eEnrMwKd.png?auto=compress%2Cformat&w=740&h=555&fit=max) The next step is to use a path planning algorithm to find the path for the robot to go. ### Implement A* algorithm with diagonal steps A* is a path planning algorithm that takes both the traveled distance (g) and the distance from the current point to the destination (heuristic value, h) into account. Then it will choose to go to the point with the smallest g+h value. A more detailed explanation can be found at The code from previous semesters only considered moving in the horizontal and vertical direction, and the heuristic value here is the Manhattan distance given by `|x1-x2|+|y1-y2|`. However, it would be more efficient if we consider diagonal moves. Below is the code that I added to the `getNeighbors` function of the A* algorithm. To make a diagonal move, we need to make sure the adjacent vertical or horizontal neighbors are reachable. That is, if we want to go to the top-left neighbor, we need to make sure we can travel to both the top and left neighbor. ```plaintext //top left corner, only can travel when both top and left are reachable if ((canTravel(rowCurr - 1, colCurr) == 1) && (canTravel(rowCurr, colCurr - 1) == 1)) { if (canTravel(rowCurr - 1, colCurr - 1) == 1) { nodeToAdd.row = rowCurr - 1; nodeToAdd.col = colCurr - 1; neighbors[numNeighbors] = nodeToAdd; numNeighbors++; } } //top right, need top and right reachable if ((canTravel(rowCurr - 1, colCurr) == 1) && (canTravel(rowCurr, colCurr + 1) == 1)) { if (canTravel(rowCurr - 1, colCurr + 1) == 1) { nodeToAdd.row = rowCurr - 1; nodeToAdd.col = colCurr + 1; neighbors[numNeighbors] = nodeToAdd; numNeighbors++; } } //bottom left, need bottom and left reachable if ((canTravel(rowCurr + 1, colCurr) == 1) && (canTravel(rowCurr, colCurr - 1) == 1)) { if (canTravel(rowCurr + 1, colCurr - 1) == 1) { nodeToAdd.row = rowCurr + 1; nodeToAdd.col = colCurr - 1; neighbors[numNeighbors] = nodeToAdd; numNeighbors++; } } //bottom right, need bottom and right reachable if ((canTravel(rowCurr + 1, colCurr) == 1) && (canTravel(rowCurr, colCurr + 1) == 1)) { if (canTravel(rowCurr + 1, colCurr + 1) == 1) { nodeToAdd.row = rowCurr + 1; nodeToAdd.col = colCurr + 1; neighbors[numNeighbors] = nodeToAdd; numNeighbors++; } } ``` If we can travel diagonally, the heuristic value is closer to the Euclidean distance than the Manhattan distance. Therefore, I updated the heuristic value function and changed all the distance-related values from integer type to double type. ```plaintext double heuristic(int rowCurr, int colCurr, int rowGoal, int colGoal) { int rowDiff = rowCurr - rowGoal; int colDiff = colCurr - colGoal; return sqrt(rowDiff * rowDiff + colDiff * colDiff); } ``` Lastly, the distance traveled in diagonal is sqrt(2) instead of 1 in the previous vertical and horizontal movement. So I updated the traveled distance calculation. ```plaintext if (abs(next.row - minDistNode.row) + abs(next.col - minDistNode.col) == 2) { next.distTravelFromStart = currDist + sqrt(2); } else { next.distTravelFromStart = currDist + 1; } ``` Below are test cases for small, medium, and large map path planning. we can see that traveling diagonally can indeed save some steps (`pathLen`) and distance traveled (the `next.distTravelFromStart` value), thus more efficient. ![small map example](https://hackster.imgix.net/uploads/attachments/1300081/image_GXpu7VtA9q.png?auto=compress%2Cformat&w=740&h=555&fit=max) 1 / 3 • small map example The points from the A* algorithm are tracked in the `nodeTrack` array, after calling the `reconstructPath` function, we can get the path that is in `pathRow` and `pathCol` in the correct order from the starting point to the ending point. Navigating from point to point is done through the provided [`xy_control`](http://coecsl.ece.illinois.edu/ge423/spring19backup/user_xy.c)[function](http://coecsl.ece.illinois.edu/ge423/spring19backup/user_xy.c). We need to pass the `pathRow` and `pathCol` values with the correct index into the function. ### Communicate with the motion tracking system We can use dead reckoning to get the pose data of our robot car. However, since the wheel may slip during the operation, there will be some errors with the dead reckoning that may accumulate with time. Therefore, to make sure the robot car performs as expected, we use the Optitrack system set up at mechatronics lab to get a more precise pose of our robot car. For the Optitrack systems to function, we first need to set the robot car at an angle of 0°, with the robot’s coordinate’s direction lined up with the global coordinate. After selecting the reflective balls set on the top of the robot car in the Motive software and running the scripts that send the data through the router connected to the lab’s computer, we only need to find a way to receive the data. Here I used Wiznet W5500 chip to accomplish this. The W5500 chip is a Hardwired TCP/IP embedded Ethernet controller that enables easier internet connection for embedded systems using SPI (Serial Peripheral Interface). ![W5500 chip](https://hackster.imgix.net/uploads/attachments/1300086/image_qJAq3Y4Mih.png?auto=compress%2Cformat&w=740&h=555&fit=max) W5500 chip W5500 is connected to GPIO122, 123, 124, 125, which correspond to pin 12, 13, 17, 18 of the launchpad. For receiving the data, the UDP protocol is used, with the robot car as UDP server and the computer connecting with the Optitrack as UDP client. ![UDP protocol](https://hackster.imgix.net/uploads/attachments/1300087/image_3xeEkBcGaW.png?auto=compress%2Cformat&w=740&h=555&fit=max) UDP protocol The Ethernet converter module I used provided me with the capability of connecting a router to the robot car. Setting the router in client mode (the mode that usually use for devices without wifi function but with an ethernet port) and connecting it to the router that sends out the data in the configuration webpage, the robot car can get the data from the Optitrack system. To avoid CPU1 of the launchpad from being too busy and messing things up, I used CPU2 in this project to set up the UDP server, receive, and process the data from OptiTrack. Generally, to use CPU2, we first need to make sure the GPIOs used in CPU2 have their ownership set to CPU2 in the initialization of GPIO in CPU1. Here, we need to set GPIO 0, 122, 123, 124’s ownership to CPU2 in CPU1’s main function. ```plaintext //wiznet reset GPIO_SetupPinMux(0, GPIO_MUX_CPU2, 0); GPIO_SetupPinOptions(0, GPIO_OUTPUT, GPIO_PUSHPULL); GpioDataRegs.GPASET.bit.GPIO0 = 1; GPIO_SetupPinMux(125, GPIO_MUX_CPU2, 0); // wiznet cs GPIO_SetupPinOptions(125, GPIO_OUTPUT, GPIO_PUSHPULL); // Make GPIO2 an Output Pin GpioDataRegs.GPDSET.bit.GPIO125 = 1; //Initially Set GPIO2/SS High so wiznet is not selected GPIO_SetupPinMux(122, GPIO_MUX_CPU2, 6); //SPISIMOC/WIZNET GPIO_SetupPinMux(123, GPIO_MUX_CPU2, 6); //SPISOMIC/WIZNET GPIO_SetupPinMux(124, GPIO_MUX_CPU2, 6); //SPICLKC/WIZNET ``` Then we need to set the peripheral that we used to 1 in the `DevCfgRegs` register. Here we are using SPIC for communicating, therefore we are using `CPUSEL6.` ```plaintext EALLOW; DevCfgRegs.CPUSEL6.bit.SPI_C = 1; EDIS; ``` After the basic setup in CPU1, we can safely put all the UDP server code into CPU2’s function. I’m using a separate project for CPU2 here. For the Optitrack data, it is in the form of x, y, theta, rigid body number, and frame count. Those data are 2 bytes each, but we are only receiving 1 byte each time from the client. Therefore, we need to combine the data together for an entire reading. Here I used a union to store a single set of data received from Optitrack, which has a `rawData` array with the type of uint16_t and a `Data` array with the type of float. ```plaintext typedef union optiData_s { uint16_t rawData[10]; float Data[5]; } optiData_t; ``` Then by combining the received data, we have the raw data in 2 bytes each, and combining those 2 bytes data as a 4 bytes float, we restore the data from Optitrack. Finally, by setting the IPC register to 1 in CPU2, we interrupt CPU1 to receive the data. ```plaintext recvsize = recvfrom(0, (uint8_t *)buffer, len, recvfrom_ip, &recvfrom_port); mydata.rawData[0] = buffer[0] | (buffer[1] << 8); mydata.rawData[1] = buffer[2] | (buffer[3] << 8); mydata.rawData[2] = buffer[4] | (buffer[5] << 8); mydata.rawData[3] = buffer[6] | (buffer[7] << 8); mydata.rawData[4] = buffer[8] | (buffer[9] << 8); mydata.rawData[5] = buffer[10] | (buffer[11] << 8); mydata.rawData[6] = buffer[12] | (buffer[13] << 8); mydata.rawData[7] = buffer[14] | (buffer[15] << 8); mydata.rawData[8] = buffer[16] | (buffer[17] << 8); mydata.rawData[9] = buffer[18] | (buffer[19] << 8); recvcount += 1; cpu2tocpu1[0] = mydata.Data[0]; cpu2tocpu1[1] = mydata.Data[1]; cpu2tocpu1[2] = mydata.Data[2]; cpu2tocpu1[3] = mydata.Data[3]; cpu2tocpu1[4] = mydata.Data[4]; IpcRegs.IPCSET.bit.IPC0 = 1; ``` The below figure is the result reading from CPU2. ![we get the readings from Optitrack](https://hackster.imgix.net/uploads/attachments/1300095/image_W3WvdBXUHs.png?auto=compress%2Cformat&w=740&h=555&fit=max) we get the readings from Optitrack We then created a float array `cpu2tocpu1` for both CPUs to store the data from Optitrack. We then find the location of `cpu2tocpu1` ram in the cmd file, setting both to be at the same location in both CPU1 and CPU2, we can then write values to this array in CPU2 and retrieve it in CPU1. ![Memory location of CPU2 to CPU1 ram](https://hackster.imgix.net/uploads/attachments/1300091/image_EJKBSyUiCL.png?auto=compress%2Cformat&w=740&h=555&fit=max) Memory location of CPU2 to CPU1 ram ```plaintext //IPC __interrupt void CPU2toCPU1IPC0(void){ GpioDataRegs.GPBTOGGLE.bit.GPIO52 = 1; x = cpu2tocpu1[0]; y = cpu2tocpu1[1]; theta = cpu2tocpu1[2]; number = cpu2tocpu1[3]; framecount = cpu2tocpu1[4]; OPTITRACKps = UpdateOptitrackStates(ROBOTps); ROBOTps.x = OPTITRACKps.x; ROBOTps.y = OPTITRACKps.y; ROBOTps.theta = OPTITRACKps.theta; UARTPrint = 1; IpcRegs.IPCACK.bit.IPC0 = 1; PieCtrlRegs.PIEACK.all = PIEACK_GROUP1; } ``` To run this, we need to first `build `the project with CPU2, and `debug `CPU1 project. After CPU1 running, right click on cpu2 and `connect target`. Under the `run` menu, `load `the CPU2.out file. Finally, run CPU1 then CPU2. Having the Optitrack data, we can use it to command the `xy_control` function. below is a demo video of the robot car going to several preset points with the motion tracking system telling it its location. ### Future work Having all the components successfully working, the next step is to combine them together and generate a workflow of robot car detecting the obstacle using the Lidar, planning the path with A*, and traveling through the path with motion tracking. ### Credits Thanks to Professor Dan Block for helping me solve all the problems I had during this project and providing essential starter code. Thanks to TA Hang Cui for the help on UDP communication and the starter code for CPU2, Thanks to Scott Manhart for designing the Lidar stand. --- Source: https://maker.wiznet.io/WIZnet/projects/robot-car-path-planning-with-motion-tracking-and-lidar/