Fast Robot Lab Page
Author : Yiqi Sun
The detailed information is shown as follows
Lab One
Lab 1: The Artemis board and Bluetooth.
In the first experiment, I set up a basic environment for subsequent experiments and used examples to test the environment. In addition, I also used BLE to establish Bluetooth communication between the Artemis board and the computer, and wrote multiple examples to test the transmission and reception of data in different ways.
Lab One Information
Setting Up Computer for Python Development with the Artemis Board
- Ensure Python 3.9+ and pip 21.0+ are installed. Check versions with
python3 --versionandpip --version. - Install Python and pip using my system's package manager for Linux/FreeBSD users.
- Create a virtual environment named "FastRobots_ble" in my project directory using
python3 -m venv FastRobots_ble. - Activate the virtual environment with
.\FastRobots_ble\Scripts\activate(Windows) or its Linux/macOS equivalent. - Install necessary Python packages (
numpy,pyyaml,colorama,nest_asyncio,bleak,jupyterlab) using pip within the virtual environment. - Set up the Lab 2 codebase by downloading, unzipping, and ensuring the
ble_pythondirectory is in my project directory. - Start the Jupyter server with
jupyter labto work on Python code in Jupyter notebooks.
This streamlined process prepares my computer for developing Bluetooth communication projects with the Artemis board using Python.
Prelab 1 Blink test
In the first lab I ran the blink example and got the following results
Prelab 2 Serial test
The prelab 2 is to run an Artemis example sketch that operates serial communication between the board and PC.
The MCU contains two UART peripherals to perform serial communication. In this sketch, the baud rate is set to be 115200. Upon running the program, the MCU counts from 0 to 9 then enters a loop where it listens and echoes messages received. The result is shown as follows.
Prelab 3 Analog read test
The prelab 3 is to run another Artemis example analogRead to test the temperature sensor of the MCU.
Upon running the example program, the MCU continuously reads data from the temperature sensor and prints the information to the serial monitor. The result shows as the demo video.
Prelab 4 Microphone test
In this task, an example sketch is run to test the onboard pulse density microphone (PDM). The one with the largest magnitude among the sensed frequencies is printed to the serial monitor.
In the demo video below, I tried to sing. The resultant data range was 100-400, indicating higher frequencies.
Prelab Additiional for 5000-level
An extra assignment involves creating a detector for the musical note "A."
Artemis Board Setup Summary
- Obtain Artemis MAC Address: Get the BLE MAC address from the Artemis board.
- Update
connections.yaml: Replaceartemis_addresswith the obtained MAC address. - Generate UUID: Use a Python script with
from uuid import uuid4; print(uuid4())to generate a UUID. - Update Files with UUID:
- For
ble_arduino.ino: Replace the BLEService UUID. - For
connections.yaml: Updateble_servicewith the new UUID.
- For
- Save Changes: Ensure all files are saved with updates.
- Test BLE Connection: Re-test to confirm everything is set up correctly.
Note: Adjust base_ble.py for platform-specific configurations if necessary.
Task 1: ECHO
Issue an ECHO command to transmit a specific string from the computer to the Artemis board. For instance, when the computer dispatches the string "1" utilizing the ECHO functionality, it should anticipate receiving a modified version of the string, such as "Robot says -> 1 :)", retrieved through a GATT characteristic read operation.
The picture below is the result returned on the Serial Monitor after Artemis accepts the ECHO command.
Task 2： GET_TIME
The second task is to complete a command GET_TIME_MILLIS where the MCU sends the time in the format of "T:123456".
I added GET_TIME_MILLIS to the enumeration of both PC and Artemis program. I completed the command as:
The result shows as:
Task 3： Handler
The second task is to complete a command GET_TIME_MILLIS where the MCU sends the time in the format of "T:123456".
I added GET_TIME_MILLIS to the enumeration of both PC and Artemis program. I completed the command as:
Task 4： Loop_send
The second task is to complete a command GET_TIME_MILLIS where the MCU sends the time in the format of "T:123456".
I added GET_TIME_MILLIS to the enumeration of both PC and Artemis program. I completed the command as:
The result shows as:
Task 5： Send Time Data
The second task is to complete a command GET_TIME_MILLIS where the MCU sends the time in the format of "T:123456".
I added GET_TIME_MILLIS to the enumeration of both PC and Artemis program. I completed the command as:
The result shows as:
Task 6： Send Temp Data
The second task is to complete a command GET_TIME_MILLIS where the MCU sends the time in the format of "T:123456".
I added GET_TIME_MILLIS to the enumeration of both PC and Artemis program. I completed the command as:
The result shows as:
Task 7： Limitations
The Artemis board is equipped with 384 kB of RAM, and assuming this entire memory capacity is available for storing sampled data, it's possible to calculate the storage capacity for 16-bit data samples. By converting the board's RAM size into bits (384 * 1000 * 8) and then dividing by 16 bits (the size of each data sample), we find that a total of 192,000 16-bit samples can be accommodated. If these samples are collected at a rate of 150 Hz, the board can record data continuously for 1280 seconds before its memory is full, as 192,000 samples divided by 150 samples per second equals 1280 seconds. Consequently, this means the board can handle 256 "5-second flows," where each flow consists of 16-bit values sampled at 150 Hz. "T:123456".
5000-level 1 ： Data Rate
In order to generate messages of various lengths, I implemented a DATARATE command that prompts the MCU to dispatch a string of a specific length as determined by the PC. For instance, when the PC executes the command ble.send_command(CMD.DATARATE, 20), the MCU responds by sending back a string composed of 20 period characters ('.'), constituting a total of 20 bytes.
To track the data rate in real-time, I configured a notification handler on the PC that captures the current time recv_time each time a message is received. Following this, the PC commands the Artemis board to transmit a reply that includes an extra byte. The time of dispatch send_time is noted, and the data rate is determined by dividing the message length by the time interval (recv_time - send_time), with this process repeating for each message exchange.
The plot shows as:
The graph suggests that there is a general trend where the data rate rises with the increase in message payload size. Consequently, we can infer that larger message sizes contribute to minimizing overhead proportionally and enhance the actual data transfer rate. Conversely, transmitting data in several smaller packets incurs more overhead.
5000-level 2 ： Reliability
To tally the quantity of messages dispatched and acknowledged by each party, I executed a command on the Artemis board that persistently sends messages, each tagged with a sequential index. Concurrently, the PC maintains a count of the messages it receives through a dedicated handler:
With the MCU transmitting data incessantly, it achieves the maximal possible data rate. During the examination, the computer successfully captured every message emanating from Artemis, with no data loss occurring. From this observation, it's reasonable to deduce that the communication channel is dependable.
Lab Two
Lab two is about IMU model and the filters applied for the data.
Lab Two Information
Task 1: IMU Setup
The IMU is interfaced with the Artemis microcontroller via the Qwiic breakout board. Once the IMU example code is uploaded to the Artemis, it retrieves and prints the readings from the accelerometer, gyroscope, and magnetometer sensors.
In the example code, a constant AD0_VAL is defined for .begin(WIRE_PORT, AD0_VAL);. It should be set to 1, which is the default value for the SparkFun 9DoF IMU breakout, but should be 0 if the ADR jumper is closed.
I added codes to blink the LED on Artemis when IMU is connected and started as a visual indication:
\
Task 2: Accelerometer
(a) Pitch and Roll
Accelerometer readings can be turned into pitch and roll through mathematical calculations. Pitch is given by pitch_a = atan2(myICM.accX(),myICM.accZ())*180/M_PI, and roll is given by roll_a = atan2(myICM.accY(),myICM.accZ())*180/M_PI.
Example outputs for {-90, 0, 90} degrees pitch and roll are:
(b) Accuracy Discussion
While noise significantly impacts the precision of accelerometer readings, resulting in an error of around ±2 degrees, the accuracy remains relatively high, with readings typically centered around the true values. Consequently, I believe that a two-point calibration is unnecessary in this scenario.
(c) Frequency Spectrum Analysis
FFT is used to analyze noise in accelerometer readings. I sampled roll data with a sample rate of 64 for 5 seconds and got 320 points to perform FFT. During the sampling period, I slowly turned IMU around its x-axis.
With the sampled data from Artemis, a python script was run to generate FFT result:
The frequency domain plot reveals that the primary component, representing the pertinent data, is situated in the low-frequency region near 0 Hz. Notably, there are no significant spikes evident in the frequency domain plot, likely due to the default selection of a low-pass filter for the accelerometer in the IMU chip. However, if I were to opt for another cut-off frequency to further attenuate residual noise, I would select 5 Hz. This choice would retain essential low-frequency data while effectively filtering out the majority of the noise.
Task 3: Gyroscope
(a) Pitch, Roll, and Yaw
Gyroscope data are collected to calculate pitch, roll and yaw:
Below are some example outputs at different gestures:
(b) Complementary Filter
The accelerometer, being sensitive to noise but resistant to drifting, complements the gyroscope, which produces smooth but drifting results. By combining the two sensors with a complementary filter, their respective drawbacks can be mitigated. The filter equation is represented as: theta = (theta + gyr_data * dt) * (1 - alpha) + acc_data * alpha.
After several tests, I chose alpha to be 0.3 to maximize performance. Here's the video demonstrating results from accelerometer only, gyroscope only, and combined, which is close to the accurate accelerometer result but without noise:
Task 4: Sample Data
To transmit IMU data to a PC, I introduced a new command called GET_SAMPLE_IMU. This command collects IMU data into arrays, then transmits the data to the PC via Bluetooth. The data is formatted as follows: time|accx,accy,accz|gyrx,gyry,gyrz.
Received data are processed by a notification handler in Python and then plotted:
Considering that an integer occupies 2 bytes and a float occupies 4 bytes, each sample will require 3 integers (6 bytes) and 6 floats (24 bytes), totaling 30 bytes. With a sampling rate of 10 samples per second, each second will generate 300 bytes of data. With 384 kB of RAM available on the Artemis, it can support approximately 1310 seconds of continuous sampling when the entire memory is allocated to the arrays.
Task 5: Record a Stunt
With the remote control, I managed to make the RC car perform stunts like flipping and spinning in place. To do a flip, the motors should first dash in one direction then motors abruptly inverse direction.
Lab Three
Lab two is about arming the Artemis board with two time-of-flight distance sensors. The range, accuracy, repeatability, and reading time of the sensors are tested.
Lab Three Information
Prelab
(a)I2C
The sensors use I2C to communicate with the Artemis board. As is specified on VL53L1X's datasheet, the default I2C address is 0x52.
(b)2 Sensors
When using multiple sensors with the same default I2C address on a single I2C bus, you need to change the address of at least one of the sensors to avoid conflicts. Here's a description of how you can achieve this using a microcontroller like the SparkFun Artemis, which has programmable GPIO pins:Initially, both Time-of-Flight (ToF) sensors are connected to the Artemis, sharing the same I2C address. To differentiate them, the XSHUT (shutdown) pin of one sensor is connected to a GPIO pin on the Artemis.
(c) Placement on Robot
Each sensor is able to detect obstacles on its corresponding side. If they detect an obstacle of the same distance simultaneously, it is probable that there is an obstacle in the front. However, this requires both of the sensors function correctly with high precision. Also, small obstacles in the front could be missed if it is closer to one sensor. More accurate measurement could be achieved by adding more sensors.
(d) Wiring
Qwiic cables are arranged inConsidering the robot's operational needs in diverse environments, robustness to ambient light is essential. Additionally, for a compact vehicle, a range of 1.3 meters is sufficient. Therefore, Short mode appears to be the most suitable for this application.
Lab Tasks & Results
Task 1: Connection
The two ToF sensors are connected to a QWIIC break-out board then to the Artemis board.
Upon establishing a physical connection and conducting an I2C scan, the Artemis detected a device at address 0x29. This address differs from the expected 0x52 listed in the VL53L1X datasheet. The discrepancy is due to the I2C protocol transmitting the most significant bit (MSB) first. The binary representation of 0x29 is '101001', and for 0x52 it is '1010010'. The first seven bits are identical for both values. I2C addresses are 7 bits long, so during transmission, these seven bits are sent, followed by a zero bit to complete the byte, translating 0x29 into 0x52 in an 8-bit context.
Task 2: Sensor Data of Chosen Mode
The ToF sensor operates in three distinct modes. Long mode, the default setting, allows for measurements up to 4 meters, offering the greatest range but with heightened sensitivity to variations in ambient light. Short mode limits the detection distance to 1.3 meters, yet it provides the strongest resilience to ambient light fluctuations. Medium mode presents a balance, detecting obstacles up to 3 meters while offering moderate ambient light sensitivity.
Considering the robot's operational needs in diverse environments, robustness to ambient light is essential. Additionally, for a compact vehicle, a range of 1.3 meters is sufficient. Therefore, Short mode appears to be the most suitable for this application.
Task 3: Two sensor working in parallel
During the preliminary lab session, I altered the I2C address of one sensor to 0x33 to enable simultaneous distance measurements from both sensors. The code used to modify the address is provided below:
The result of running the measure distance with two sensor shown in the image below:
Task 4: ToF Sensor Speed
In order to enhance the readout velocity, I eliminated the stopRanging() and clearInterrupt() instructions and relocated the startRanging() method to the setup section of the code. Now, sensor readings are outputted exclusively when new data is available, which is when .checkForDataReady() evaluates to True.
Despite the presence of redundant code segments (e.g., time retrieval and serial printing), the loop consistently executes within the range of 10 to 11 milliseconds. This marks a significant reduction from the original ranging time of 50 milliseconds. Within this 10-11 millisecond window, the predominant portion of time is still consumed by the ToF (Time-of-Flight) reading operations, accounting for approximately 8 milliseconds. As a result, it is evident that the primary bottleneck in reducing processing time lies in the I2C communication conducted between the sensors and the Artemis microcontroller.
Task 5: Time v Distance
A GET_ToF_10s command is written to send ToF data sampled in 5 s to PC through BLE:
The PC utilizes a notification handler to interpret the received message. Subsequently, the data samples from the two sensors are graphed against their respective time stamps.
Task 6: (5160) Discussion on infrared transmission based sensors
The sensor utilized in this laboratory relies on time-of-flight (ToF) infrared transmission. ToF infrared sensors function by emitting a pulse-modulated signal, measuring the time taken (t) for the signal to travel to the target and return, and then calculating the distance (d) using the formula d = t * c / 2, where c represents the speed of light.ToF IR sensors offer several advantages, including a high sample rate, compact form factor, and immunity to variations in colors, textures, and ambient lighting conditions. However, they also have drawbacks such as high cost and complex processing requirements.
Certain infrared distance sensors utilize amplitude-based measurement methods, where the amplitude of the reflected wave is compared to that of the original emitted wave to calculate distance. These sensors offer advantages such as low cost and straightforward circuitry, and they generally perform adequately across various conditions. However, they exhibit high sensitivity to target reflectivity and may experience diminished performance in environments with high ambient light levels.
Triangulation-based sensors emit a wave at an angle towards the target and measure the reflected wave using an array of diodes. Distance calculation involves analyzing the angle of incidence and the distance between the diode with the highest signal magnitude. These sensors offer advantages such as simple circuitry and reduced sensitivity to factors like color, texture, and ambient light. However, they tend to be expensive and bulky, and may not function well in environments with high ambient light levels. Additionally, they often suffer from a low sample rate.
Task 7: (5160) Sensitivity of sensors to colors and textures
In this part, I used cardboard of different colors to stick on the wall for testing to explore the impact of color on the sensor. I tested black, white and yellow respectively, and the relevant results are shown below.
It turned out that there was no notable difference among these materials in terms of reading accuracy and speed. Therefore, the conclusion that the ToF IR sensor is not sensitive to colors and textures is implied.
Lab Four
In this laboratory session, two motor drivers are initially examined for functionality before being employed to operate the vehicle's motors. Finally, an open-loop control is executed without any physical connection to the system, allowing for autonomous operation.
Lab Four Information
Prelab
The intended connections between the motor drivers, Artemis, and battery are shown as:
I chose 11, 12, 6 and 7 on Artemis as the motor drivers' input because these pins support PWM, which are implied in the schematic plot by a ~ sign.
Separate batteries are used to energize the Artemis and the motor drivers, enabling independent troubleshooting and preventing electrical noise from the motors from affecting the Artemis. Moreover, since the motors consume energy at a much higher rate compared to the Artemis, they require an independent power source to ensure the microcontroller unit (MCU) operates efficiently without rapid power depletion.
Lab Tasks
Task 1: PWM Signals Generation
To visualize the PWM signal, the probe tip of the oscilloscope is connected to the output of a motor driver, while the ground clip is secured to the motor driver's ground terminal.
As for power supply of the driver, I simply attach the 3.7 V power supply to it, which is within the operating voltage range of 2.7 V to 10.8 V stated in the motor driver's datasheet.
I first tested the driver with analogWrite(11,63); analogWrite(12,0);, which generates a 25% duty cycle PWM signal as 63 is approximately 25% of 255. The corresponding oscilloscope display is:
Then I tested on analogWrite(11,127); analogWrite(12,0);, which generates a 50% duty cycle PWM signal as 127 is approximately half of 255. The corresponding oscilloscope display is:
Task 2: Wheels Spinning
By running the following codes, I managed to spin the right-side wheels forward.
With battery attached to the motor drivers, I put the car on the Large electrical tape and let wheels on both sides spin：
Task 3: Secure Components
All the components are secured as follows:
Task 4: Lower Limit PWM Value
The minimum PWM values required to propel the car forward on the laboratory floor and to execute turns are determined by programming various speed values into the Artemis and observing which values are effective in achieving movement.
To make the car run, the lower limit PWM value I obtained is 60:
The lower limit to do in-place turns I obtained is 110:
But these results are susceptible to change in lab settings, e.g. different textures of the floor and smoothness of the tires, etc..
Task 5: Go a Straight Line
Initially, the car tended to drift towards its left side, so I added a calibration factor 1.2 which gave the left wheels more power so that the car can follow a fairly straight line. However, such static calibration method is susceptible to environment changes, so it is necessary to use PID control in future labs.
By applying the calibration stated above, the car can go a fairly straight line in the video
Task 6: Open Loop Control
I write up all the actions in functions to make the code more readable:
For the untethered open loop control, the car is programmed to move forth and back and make a clockwise turn repeatedly with the following codes:
Additional 5160 level Task 1: AnalogWrite Frequency Discussion
The Artemis board, by default, generates a PWM signal at 500 Hz. To produce a PWM signal with a significantly higher or lower frequency, it's necessary to alter the counter/timer configuration and utilize the repeated pulse mode. For instance, to create a PWM signal at approximately 3kHz with a 75% duty cycle, one would start by setting up the clock to output at the target frequency. To achieve the 75% duty cycle, the CMPR0 register is set to 75% of the maximum count value, causing the output to toggle each time the counter hits this value. Once the counter reaches its maximum and resets, the cycle starts anew.
The motor driver's datasheet indicates an acceptable input PWM frequency range from 0 Hz to 200 kHz, with a stipulation that input pulses should maintain a minimum width of 800 ns for reliable detection. With a base frequency of 500 Hz, there is a theoretical duty cycle range from nearly 0% to just under 100%. At higher PWM frequencies, the effective duty cycle range diminishes because of the shorter period of the PWM signal. Nonetheless, demonstrations in the provided videos show that the 500 Hz frequency is adequate, negating the necessity to increase the frequency for this application.
Additional 5160 level Task 2: Lower Limit PWM Value in Motion
As static friction is greater than kinetic friction, it takes higher voltage to start the car from still than to keep it moving while in motion. To explore the lower limit PWM value in motion, I set the initial speed as 60 to start the car, then deduct 2 from the value every 3 seconds to see which value stops the car.
As demonstrated in the video, the car stopped after 9 seconds from the time it started to move, which corresponded to PWM value of 54. So the lowest limit is 54 to keep the car running in motion, which is lower than the limit to start (60).
Lab Five
Lab Five is about Linear PID control and Linear interpolation
Lab Five Information
Prelab
While observing our car's behavior is crucial for PID tuning, accessing key data from sensors and Artemis is essential. Hence, I developed a new string processing function, along with data storage and plotting capabilities.
On Artemis, data comprising time, distance, and PWM is packaged and sent. By eliminating symbols and using "|" for separation, I've minimized the data transmission time, allowing for a shorter control period. This adjustment is crucial because we're working with a single core for both PID control and data transmission. The implementation of this strategy, alongside PID control, will be detailed in the subsequent section.
P/I/D Discussion
PID stands for Proportional-Integral-Derivative control, with the formula for correction being correction = Kperror + Kiintegral of error + Kd*derivative of error.
The proportional gain, Kp, addresses the current error (the discrepancy between the desired and actual values) proportionately. Increasing Kp intensifies the control response, though it may lead to overshoot or instability.
The integral gain, Ki, compensates for accumulated errors over time. It enhances control action for prolonged errors, but overly high values can introduce overshoot or oscillation due to the delay in counteracting accumulated errors.
The derivative gain, Kd, anticipates future errors by considering the error's rate of change. It serves as a dampener to minimize overshoot and slow down the response speed, but it may exacerbate noise in the system.
Range/Sampling Time Discussion
Initially, my PID loop included a delay to wait for the distance sensor data to be ready, using while (!distanceSensor1.checkForDataReady()) delay(1). This setup resulted in each cycle averaging 90-100 ms. To accelerate the loop times, I removed the code that waited for sensor readiness. Consequently, the revised PID loop now operates at 8-9 ms per cycle, with the majority of this time, approximately 7 ms, being attributed to reading from the Time-of-Flight (TOF) sensor.
PID Implementation
The PID control was implemented using the code provided. It's important to highlight that the car requires a minimum PWM value to overcome friction Therefore, any PWM value below these thresholds is practically ineffective. To address this, I incorporate these threshold values directly when configuring the GPIO. Additionally, I've imposed a limit of 30 on the PWM for the left side to simplify control of the car.
The code is as below, together with BLE data-sending functions. I finally tuned my proportional parameter to 0.7 and my integral parameter to 0.02.
Result
In the short-distance startup test, the car's response was as follows, and I have plotted the data accordingly. I believe the car performed well.
Wind-up protection for integrator (5000+ level)
Windup, a challenge with the integral term, leads to overshoot by allowing the integral component's accumulation to exceed the control signal's saturation limits. To combat this, I utilize two strategies: implementing bounds and adopting the Clegg Integrator. The Clegg Integrator resets the accumulated integral value whenever the error changes sign, effectively clearing any built-up error that could contribute to windup in the opposite direction.
Lab Six
Lab six focus on developing PID control systems that enable robotic cars to autonomously fix their orientation.
Lab Six Information
Prelab
The lab focuses on developing PID control systems that enable robotic cars to autonomously fix their orientation. The orientation measurements required for this operation are obtained via an IMU sensor integrated into the robot.
Debugging data was send over bluetooth from the Artemis board to the computer, where a Python handler function is used to store the data in various arrays. Time, error (Angle to the set oriantation), PID value (sent seperately) and pwm were all logged.
P/I/D Discussion
PID stands for Proportional-Integral-Derivative control, with the formula for correction being correction = Kperror + Kiintegral of error + Kd*derivative of error.
The proportional gain, Kp, addresses the current error (the discrepancy between the desired and actual values) proportionately. Increasing Kp intensifies the control response, though it may lead to overshoot or instability.
The integral gain, Ki, compensates for accumulated errors over time. It enhances control action for prolonged errors, but overly high values can introduce overshoot or oscillation due to the delay in counteracting accumulated errors.
The derivative gain, Kd, anticipates future errors by considering the error's rate of change. It serves as a dampener to minimize overshoot and slow down the response speed, but it may exacerbate noise in the system.
Range/Sampling Time Discussion
Initially, my PID loop included a delay to wait for the distance sensor data to be ready, using while (!distanceSensor1.checkForDataReady()) delay(1). This setup resulted in each cycle averaging 90-100 ms. To accelerate the loop times, I removed the code that waited for sensor readiness. Consequently, the revised PID loop now operates at 8-9 ms per cycle, with the majority of this time, approximately 7 ms, being attributed to reading from the Time-of-Flight (TOF) sensor.
PID Implementation
The way I implement PID for setting oriantation is shown as follows.
After testing, I set Kp = 8 , Ki = 1, Kd = 0.05
Result
In the test, the car's response was as follows, and I have plotted the data accordingly. I believe the car performed well.
Wind-up protection for integrator (5000+ level)
Windup presents a problem in the integral part of PID control, resulting in overshoot by permitting the integral term to build up beyond the saturation limits of the control signal. To mitigate this issue, I set limits on the integral term's accumulation.
Lab Seven
The objective of Lab 7 is to implement a Kalman Filter, which can help to execute PID control faster.
Lab Two Information
Estimate drag and momentum
In the lecture,professor provides an in-depth discussion on the Kalman filter. To tailor a Kalman filter for my robot, I must construct a state space model of the system. This involves calculating the drag and momentum terms for the A and B matrices, as outlined below.
where d is drag term and m is momentum term.
To determine the necessary parameters for the A and B matrices in your state space model, such as the drag and momentum coefficients, you'll utilize a step response procedure. This method involves directing the robot toward a wall while meticulously documenting the data, which includes PWM signals, distance sensor outputs, and time stamps throughout the operation. Your goal is to allow the robot to reach and maintain a steady state for a certain duration, achieving a top speed of 100. To conclude the experiment, you'll halt the robot's progress by allowing it to contact the wall.
The distance measurement and velocity is plotted as below, which shows the distance between robot and wall during the whole step response measurement process.
I can get the steady state velocity to be about 2000 mm/s (90% is about 1800 mm/s) and the 90% rise time to be 2 s.
Estimate drag and momentum
Therefore, based on the step response data I obtain, I can compute drag and momentum terms based on the fomula before. And then I can calculate the parameters using function mentioned above.
Kalman Filter Initialization
After obtaining the necessary values and estimating the A and B matrices from the step response experiment, the next step involves discretizing these matrices according to the sampling rate. This discretization process is essential for adapting the continuous system dynamics to a form suitable for digital implementation, enabling the use of the discretized matrices within the Kalman filter framework.
With the parameters established and the A and B matrices discretized, the setup for the Kalman filter is complete. At this stage, the computed parameters can be integrated into the template for the Kalman filter as provided by the professor. This integration involves substituting the specifically computed values into the predefined structure of the Kalman filter template. The resulting code snippet, which represents the final version of the Kalman filter tailored to the robot, is as follows.
Kalman Filter Implementation in python
To evaluate the efficacy of my Kalman filter, I plan to apply it to the PID control data gathered from the robot. Prior to importing this control data into the Kalman filter, it's necessary to establish the initial state parameters of the filter:
The 'x' matrix, which encapsulates the initial conditions of the robot, such as its starting position (derived from the first distance measurement in the PID distance data) and its initial velocity (assumed to be 0, given that the robot starts from a standstill).
The 'sig' matrix, which delineates the initial uncertainty in the robot's position and velocity. For both parameters, this uncertainty is quantified as a standard deviation of 5, reflecting an initial estimate of the variance in both position and velocity.
The plot below demonstrates the result of Kalman filter state estimation on the data I obtained from robot. It shows a good fit with almost no error.
I also perform experiments with varying levels of measurement noise variance while maintaining a constant process noise variance, as the latter is solely determined by the sampling rate. An example of this approach involves configuring the Kalman filter model to account for a high measurement noise variance. This adjustment signifies that I am attributing less significance to the distance data acquired from the Time-of-Flight (ToF) sensor, due to its high measurement error.
Kalman Filter Implementation in python
Similar to last task, the parameters for Kalman filter need to be defined first, while as I have confirmed that previous parameters are good enough, I am using completely same parameters on Artemis side. Below are the code snippets for parameters initilization and Kalman filter definition.
Video below shows the working Kalman filter integrated with PID control on the robot.
Lab Eight
Lab two is about make the robot move fast, flip then come back.
Lab Eight Information
Lab Tasks
Although this lab builds upon the position control outcomes from Lab 6, PID control is omitted to allow the robot to accelerate at maximum speed towards the wall, and then reverse at full throttle to achieve a flip. Without the fine-tuning afforded by PID, I had to halt the robot at a greater distance than 500 mm from the wall to accommodate for drift.
After testing a lot, I halt the robot at 1200mm, so it can flip around 500mm away from the wall instead crushing. (1200 is the final value a used in the successful demo.)
Within the Artemis command handler, the robot is programmed to execute a flip once it reaches the designated distance, by immediately reversing at maximum speed.
Test Result 1 (not perfect)
In Result 1, the robot is not stable enough when fliping, so I make the robot do a quick stop after fliping to perfectly solve the problem.(In result 2 and result 3)
Test Result 2 and 3
Lab Nine
This lab generates a map of obstacles and walls in a static room by spinning the robot on axis while taking TOF sensor reading.
Lab Nine Information
Lab Tasks
Angular Speed Control
To ensure accurate distance measurements, the robot is programmed to turn at a low speed. A PID (Proportional-Integral-Derivative) control mechanism, specifically using only the proportional component, has proven effective in maintaining a consistent turning speed. This control mechanism is integrated with readings from an IMU (Inertial Measurement Unit) and a TOF (Time of Flight) sensor during the turning process.
Below is a successful control with kp = 0.5, targetAngle = 360 degree, basicPWM = 116, and targetAngularSpeed = 25 degree/sec, where all the parameters are tuned by experiments.
The video demonstrates that the robot experiences minor drift during its turning motion, leading to an error of approximately 50 mm. Additionally, there's an error of about 30 mm because the TOF (Time of Flight) sensor is not positioned exactly at the pivot point of the turn. To adjust for this, the measured distances can be corrected by subtracting this error value.
Read out Distances
The polar plot of point (5.3) (5,-3) (-3,-2) (0,0) (0,3) are shown as follows.
Merge and plot
After gathering distance measurements from various points in the laboratory, the subsequent step involves integrating these data into a map. I transformed the robot's readings from the initial frame of reference, which is centered on the TOF sensor's position, to the room's inertial reference frame. Considering that the TOF sensor is mounted 50 mm in front of the robot, I accounted for this offset when translating the polar distance measurements into Cartesian coordinates. Additionally, I converted the TOF sensor's measurements from millimeters to feet for consistency.
Line-Based Map
A line-based map is estimated from the scatter plot to later be used in the simulator:
Lab Ten
This lab implements a grid localization using Bayes Filter in a simulated environment.
Lab Ten Information
Lab Tasks
Implementation
Compute Control
The compute_control function calculates values for u, which include delta_rot1, delta_trans, and delta_rot2, using both the current and previous positions of the robot. The function utilizes a series of equations to perform these calculations.
Odometry Motion Model
The odom_motion_model function estimates the probability that the robot moved from a previous position prev_pose to a current position cur_pose given a specific control input u. First, the compute_control function is used to determine the necessary control action to transition from prev_pose to cur_pose. Subsequently, noise is incorporated using a Gaussian distribution to estimate the likelihood that the robot successfully executed the transition with the calculated control action. The probability is calculated as the product of the Gaussian functions applied to all three control parameters.
Prediction Step
The prediction_step function updates the probability bel_bar by calculating the actual control input u from the current and previous positions of the robot, labeled cur_odom and prev_odom. If the belief value for any previous data point falls below 0.0001, it is deemed negligible and is thus ignored. Conversely, if the belief value exceeds this threshold, the function continues to process all current data points, updating bel_bar accordingly.
Sensor Model
The sensor_model function processes a pre-defined array of actual observations for a specific grid pose. It calculates the likelihood of each sensor reading by comparing it to the robot's sensor data recorded after moving, using a Gaussian function. In this function, the true observation serves as the mean, and sensor_sigma acts as the standard deviation, representing the noise level of the sensor model. The function returns an array of calculated probabilities for all 18 measurements, essentially assessing the probability that the sensor readings align with the true observations for that particular grid pose.
Update Step
In the update step, the potential current poses of the robot are examined using three nested "for" loops. An intermediate belief value (loc.bel_bar, which was updated during the prediction step) is computed for each potential pose. This value is then multiplied by the probability that the current sensor readings accurately reflect that specific pose. The belief value for each pose is updated accordingly. Similarly to the prediction step, normalization is carried out to ensure that the sum of all belief values remains consistent.
Demonstration
Here is a simulation of the Bayes filter for a robot following a preplanned trajectory. The odometry readings (sensor values) are shown in red, the ground truth of the robot's position is depicted in green, and the belief is represented in blue. Additionally, the values of loc.bel_bar are plotted at each point
.Lab Eleven
In this lab, the localization with Bayes filter simulated in Lab 10 is performed on the actual robot.
Lab Eleven Information
Lab Tasks
The notebook lab11_sim.ipynb is initially executed to assess the localization functions in a simulated environment. The resulting plot displays odometry, ground truth, and belief data.
The plot indicates that the belief (represented in blue) closely matches the ground truth (shown in green), suggesting that the localization functions are performing effectively.
Implementation on real robot
The perform_observation_loop function issues a BLE command to the robot, instructing it to rotate a specified number of degrees and gather 18 uniformly spaced (angularly) TOF readings. These angular data and TOF readings are utilized as sensor bearings and ranges, respectively, in the Localization module.
To allow time for the robot to collect this data, I implemented a 40-second delay using asyncio. Initially, I attempted to define an async function and use the await keyword, but encountered errors likely due to missing asyncio components in the environment. Therefore, I resorted to directly invoking the asyncio sleep coroutine.
On the Artemis side, I adapted the code from Lab 9 to execute the OBSERVATION command with a few modifications: I removed the PID control and programmed the robot to halt at every 20-degree interval to collect data.
Results & Discussion
I positioned the robot at four predefined poses in the lab and executed the update step of the Bayes filter at each location. To showcase the outcomes, I've included a polar plot of the collected data, logs from the update step that detail the belief with associated probabilities, and a map plot displaying the belief (in blue) and the ground truth (in green) at each pose. All measurements in the logs and plots are expressed in micrometers.
Pose (-3 ft, -2 ft, 0 deg)
Pose (0 ft, 3 ft, 0 deg)
Pose (5 ft, -3 ft, 0 deg)
Pose (5 ft, 3 ft, 0 deg)
Conclusion
The results indicate that the localization for position was generally effective, with the belief and ground truth points perfectly aligning at three out of four points. The deviation at the last point was minor, with an error of just one grid. Additionally, there was an orientation belief error of approximately ±10 degrees. Notably, the probabilities associated with all beliefs were extremely close to 1.0.
A potential reason for the less accurate localization at the position (5 ft, 3 ft, 0 deg) could be the symmetrical nature of the surroundings at that location, as suggested by the polar plot. This symmetry might have led to confusion in estimation due to similar Time-of-Flight (TOF) readings from different locations. The orientation error could likely be attributed to drift in the robot's turning and noise during TOF data collection.
Overall, the lab results can be deemed successful, as the estimations were largely accurate and closely matched the actual poses of the robot, accompanied by high confidence in the beliefs.