Homework 5: LiDAR PID Wall Following

Deliverables

This assignment introduces a wall following algorithm to stay a set distance away from the wall. The controller used here is a PID controller, think cruise control on your car. Additionally, you will need to check to see if the start or stop button was pressed from a joystick.

Expected Behavior:

• Subscriber 1: The node will monitor Joy to check if the joystick’s ‘start’ and ‘stop’ button was pressed.

• Subscriber 2: Everytime a scan topic arrives:

  • Calculate current distance from wall

  • Calculate error at time t

  • Calculate change in sterring angle

  • publish ackermann_msgs with new sterring angle

You should have two subscriber topics: scan for the Lidar and joy for the joystick and one publisher topic vechile_command_ackermann.

General Overview

• Due Date: TBH

• Points: 40

• ROS 2 Topics: scan and joy (sub) and vehicle_command_ackermann (pub)

• ROS 2 Messages: LaserScan and Joy in sensor_msgs (sub) and AckermannDriveStamped in ackermann_msgs (pub)

LiDAR and You

LiDAR Data

First you must understand how the LiDAR data is being published. The LiDAR data is published in the ROS2 topic scan with the message LaserScan. You realistically only need the ranges part of the message and maybe the header for consistencies sake.

The ranges are stored in the form of distances in meters. To parse through the range data, it is important to understand Figure 1.

Figure 1: Ranges defined from 0 degrees. Red is \(+x\) and Blue is \(+y\).

Here on our Lidar, distances are given from the \(-x\) or \(180^{\circ}\) from the red arrow and move in a counter clock-wise direction. Therefore the first distance will point straight back on your car. There are 720 data points, therefore the angular resolution of the LiDAR is half a degree. Therefore to parse through the data if you want to get \(90^{\circ}\) degrees or \(180^{\circ}\) from the blue line you need to look for the data point \([180]\).

Warning

Remember that the data is 180 degrees out of phase, so you will have to account for that.

Running The LiDAR

To launch the lidar on the car you can run the following command:

ros2 launch vehicle_launch vehicle_lidar.launch.py

Visualizing the LaserScan

To visualize the LaserScan, you can view it using RViz. To run it do the following:

• Launch the LiDAR topic.

• In a new terminal, run rviz2.

• Change Fixed Frame to lidar.

• add by topic scane.

• Change view too TopDownOrtho and zoom in.

Finding The Perpendicular Distance

Dr. Crane’s method

Warning

Double check this section, they were taken from the LiDAR PID powerpoint. If there is a mistake, please let the TA know.

Figure 2: Calculating the distance between two points

First, you need to get \(d_{1}\) which is the LiDAR value along \(-y\) axis.

Second, you need to get \(d_{2}\) which is the LiDAR value at angle \(\theta\), the offset angle you chose from the \(-y\) axis.

Now, you can calculate \(d_{3}\) with the following equation:

\[d_3 = d_1^2 + d_2^2 - 2 d_1 d_2 \cos \theta\]

Figure 3: Calculating the distance from the wall

Now we have to the unit direction vector that is pointing from \(P_{1}\) to \(P_{2}\):

\[v = \frac{(x_2 - x_1) \hat{i} + (y_2 + y_1) \hat{j}}{d_3}\]

Note

\((x_{1}, y_{1})\) and \((x_{2}, y_{2})\) are the x and y values from \(d_{1}\) and \(d_{2}\), respectively.

Afterwards, we can \(\phi\) using the following equations:

\[\cos \phi = \frac{x_2 - x_1}{d3}\]

With \(\phi\), you can find the distance from the wall using the following equation:

\[d_{wall} = d_1 * \cos \phi\]

You can find the error, which is the desired set distance from the wall minus the distance you calculated.

\[e = d_{setpoint} - d_{wall}\]

Aditya and Patrick’s Method

To find the perpendicular distance, first the angle alpha as shown in the figure below has to be found.

Figure 4: Geometry for Perpendicular Distance

\(\alpha\) can be found using the following:

\[\alpha = \arctan \left( \frac{d_{offset} \cos \theta - d}{d_{offset} \sin \theta} \right)\]

where \(\theta\) is the offset angle you chose from the right side of the car, with \(d_{offset}\) being the offset distance and \(d\) as the perpendicular distance from the vehicle coordinate system.

Next you can simply find the perpendicular distance \(D_{perp}\) by using the following

\[D_{perp} = d \cos \alpha\]

Now, you cannot simply use the perpendicular distance as with this the car will not be able to react quickly enough causing overshoots. To counteract this, a look ahead distance \(L\) is established. You can use this \(L\) to find a perpendicular distance to add as such

\[D_{perp+L} = L \sin \alpha + D_{perp}\]

Hint

You \(L\) should be between 10 and to 30 cm in front of the car and \(\theta\) should be between 15 and 30 degrees from the cars perpendicular.

Warning

Set your values for speed to be relatively slow, around 1m/s. Any faster it could possibly crash into the wall.

Now with the look ahead distance, you can find the error, which is the desired set distance from the wall minus the distance you calculated.

\[e = d_{setpoint} - D_{perp+L}\]

Optional Test: Bang-Bang Controller

Originally, we would do a Bang-Bang Controller before moving onto PID. So this part is optional, and can be skipped.

However, you are still welcome to use this controller to test if your perpendicular distance program works as intended.

A Bang-Bang controller is a 2 state controller that abruptly changes from a state to another, in our case from steering left to right. To determine which state you are in, providing feedback to your controller, you will find the perpendicular distance from the wall to your car (covered in the next section). If it is too far away from the wall, steer in the opposite direction that you are currently and vice versa.

For our problem here, we will be following the right wall. If it is too far from the right wall you want to max turn right, and if it is too close, max turn left. You can model this by using two states of -1 and 1. If an error you take from a set distance and the true distance, depending on how you structure the math, each sign will correspond to a turning right or left. The error can be calculated as such

\[e = d_{setpoint} - D_{perp}\]

Which then can give you the equation for a Bang-Bang as

\[\begin{split}\phi = \begin{matrix} -\beta & \text{if } e > 0\\ \beta & \text{if } e < 0\\ \end{matrix}\end{split}\]

where \(\phi\) is the steering angle, and \(\beta\) is a set steering angle that your controller will oscillate between.

Warning

Remember that the car has actuation limits on the steering to be between -45 and 45.

Note

You can change the order as needed to get a certain positive or negative value.

Hint

Try to get the right turn to be positive and the left turn as negative. Using the numpy.sign() function should make this trivial. Just ensure you filter out NaN values with np.isnan().

PID Controller

More info on PID Controllers can be found here. PID controllers are simple controllers that employ feedback and continuously controls as system based on an error. The idea is to drive that error to 0 based on the Proportional (P), Integral (I) and Derivate (D) controller terms. The error can be calculated as follows,

\[e(t) = r - y,\]

where \(e(t)\) is the error w.r.t time, \(r\) is the desired set point (value you want your system to be at) and \(y\) is the system output (\(\dot{x}\)).

The simplest form of a PID is multiplying your controller by a fixed or proportional gain. A form that is often given in theoretical controls classes (EML4312)

\[u = K_p e(t),\]

where \(u\) is the control command given to the system and \(K_p\) is the proportional gain.

The next term, the Integral (I) controller has the following equation,

\[u = \int_{0}^{t} K_i \, e(t) \, dt,\]

where \(K_i\) is the integral gain. The integral controller in this form is not very useful to us. An alternative form is,

\[u = K_i \sum_{k=1}^{k} e_k \Delta t.\]

The idea is you take your old values of \(e_k\) (the error) and you keep adding to it’s self and multiplying by a fixed integral gain \(K_i\). \(\Delta t\) is just taking your current \(t_k\) and subtracting the old one (previous iteration) \(t_{k-1}\), where \(k^th\) is the current iteration.

Note

\(\Delta t\) Can and probably should be set to a constant value of 10Hz or 0.1s.

You will need to window your integral controller, i.e. only sum up the last certain amount of error values. Usually for our case it could be around 100 to 200 values.

Hint

Storing the errors as a list and then summing them up will be the easiest method to achieve this. You can also use the pop() function in python to remove a certain value from a list.

The final term is the derivative (D) controller which multiplies a gain by the derivative or slope of your error over time. The equation of this controller would be

\[u = \dfrac{d}{dt} e(t) K_d.\]

where \(K_d\) is the derivative gain. A more useful form of this controller is,

\[u = K_d \dfrac {e_k - e_{k-1} } {t_k - (t_{k-1})}\]

A full Proportional, Integral and Derivate (PID) controller is essentially just mashing all three controllers together and has the following equation,

\[u = K_p e(t) + \int_{0}^{t} K_i \, e(t) \, dt + \dfrac{d}{dt} e(t) K_d\]

or

\[u = K_p e_k + K_i \sum_{k=1}^{k} e_k \Delta t + K_d \dfrac {e_k - e_{k-1} } {t_k - (t_{k-1})}.\]

You do not need to use the full PID controller you can use PI or PD controllers as well or other formats. See which one works best and use that for your controller.

You will need to assign a self.var to store your old values of integral addition errors time and error. You will then use the \(u\) message as the vehicle_command_angle value.

Note

The steering angle must be sent in radians, not degrees. The \(u\) message should already be in radians. You do not need to convert to degrees.

Warning

Remember that the car has actuation limits on the steering to be between -45 and 45 degrees.

Note

You can change the order as needed to get a certain positive or negative value.

Hint

Try to get the right turn to be positive and the left turn as negative. Using the numpy.sign() function should make this trivial. Just ensure you filter out NaN values with np.isnan().