CU SAIL Navigation Team Spring 2017 Semester Report

Transcription

1 CU SAIL Navigation Team Spring 2017 Semester Report Professor Andy Ruina s Biorobotics and Locomotion Lab Cornell University Alicia Coto, Alex Pomerenk, Caleb Koch, Orla MacLean, Alec Dean, Arjan Singh May 21, 2017 Abstract CU Sail s navigation system allows our boat, Sail-Vane II, to autonomously sail from one global position (waypoint) to another. A combination of an electrical array of sensors and a short term navigation algorithm determine appropriate sailing maneuvers for our sailboat to reach its desired destination. The sensor array comprises of a Global Positioning System sensor, an Inertial Measurement Unit and a magnetic Rotary Sensor, which allow the navigation algorithm to access wind direction, current position and boat orientation. The sailing maneuvers thus determined by the navigation algorithm are executed by a control system consisting of servo motors, that rotate the sail and tail to appropriate angles. Feedback is provided by a short-distance radio communication system as well as visible LED lights. A microcontroller allows for communication between the sensor array, the navigation algorithm, the control system and the feedback system. In addition, the microcontroller also handles scheduling tasks. The navigation system is powered by a rechargeable 6V, 330 amp hr, NiMH battery pack. Power from the battery is directly supplied to the control system and the microcontroller. The microcontroller in turn, supplies power to the sensor array and the feedback system. The Navigation algorithm operates by first, determining the waypoint to sail to and then finding the optimal heading to sail in, in order to reach that waypoint. Then, by taking into account data given by the sensor array, the algorithm determines appropriate sail and tail-vane angles that would execute those maneuvers. 1

2 Contents 1 Introduction 3 2 Microcontroller and Sensors Microcontroller Magnetic Rotary Sensor Inertial Measurement Unit GPS Sensor Servo Motors Communication System 7 4 Remote Control 7 5 Power System 7 6 Circuit Design and Organization 8 7 Navigation Algorithm Basic Structure Buoy Rounding Station Keeping Error Analysis Distance Calculation Course Error Graphical User Interface Pixy CMUcam Setup Obstacle avoidance Searching Future Work for Pixy CMUcam Appendix Component List IMU Calibration Setting up the XBEE Radio Receiver/Transmitter Soldering A PCB Pin din Connector Male connector Female connector

3 1 Introduction Sail-Vane must be able to sail to a designated set of coordinates by taking into account its present latitude and longitude, perceived wind direction, the boat heading, and the boat direction. The boat direction refers to the direction the nose of the boat points to, with respect to true North, while the boat heading refers to the direction the boat is moving, also with respect to true North. A sailboat generally does not move in the direction that its nose points, which makes this distinction between heading and direction important. We approach the task in three parts; 1) Acquisition of data relevant for navigation; 2) Using the acquired data to determine appropriate sailing maneuvers; and 3) Executing the sailing maneuvers. The first part of the process is achieved by a set of sensors; A magnetic rotary sensor allows the detection of wind direction, a GPS sensor determines our boats coordinates and boat heading, and an Inertial Measurement Unit provides boat direction. The second part is done via an algorithm which uses sensor data to determine the next direction of the boat, and the relevant sailing maneuver to sail in that direction. The final part is accomplished by a set of servo motors. A microcontroller acts as a bridge between the three parts and allows the flow of data from the sensors, to the algorithm and finally to the motors. Sail-Vane also has supplementary sub-systems that allow for communication and data-logging; a set of on-board LED lights and a pair of radio transceivers. CU Sail also started developing other sub-systems to allow for an expanded feature set in future iterations of Sail-Vane. Such systems include a vision system that will be used for obstacle detection and solar power for the vessel, which will allow the boat to be on the water for large instances of time. 2 Microcontroller and Sensors 2.1 Microcontroller The microcontroller handles scheduling tasks, determining how often the sensors must activate and collect data, a new heading must be calculated, a new maneuver needs to be executed to stay on course and communication must be made. We are currently using the Arduino Due as the sailboat s micro-controller. The processor core of the Due is made by Atmel (Atmel SAM3X8E), and runs at 84 MHz. This microcontroller has enough ports and pins to accommodate our current sensor array, and even additions we might make in the future. The high clock speed of the processor also allows us to stay clear of potential bottlenecks in terms of the frequency at which we might want to run the navigation algorithm. The processor communicates with the sensor array through various communication protocols that vary between devices. The following protocols are used by our navigation system: 1. Serial Communication. Communication between two devices is considered serial when data is transferred one bit at a time. This serial communication can be aided by a clock signal (a constant sequence of alternating 1s and 0s) generated by the processor. In such a case, data will only be transferred when the clock signal changes from 0 to 1 (rising edge) or when the clock signal changes from 1 to 0 (falling edge). This serial protocol is considered synchronous. In the case where the clock is not used, the serial protocol is considered asynchronous. A commonly used asynchronous serial communication protocol (which the Arduino Due also uses), is known simply as Serial Communication. This communication protocol uses two channels for communication between devices: one to receive data (RX) and one to transfer data (TX). The connection between two devices is made as shown in Figure 1. The RX port of one device connects to the TX port of the other device. The Arduino Due has four serial ports labeled Serial 0, Serial 1, Serial 2 and Serial 3. By default, the Arduino uses Serial 0 to communicate with the users personal computer (PC). Serial 0 is used to print statements on the screen and also to upload program(s) to the microcontroller. 2. Serial Peripheral Interface or SPI, is an example of Synchronous Serial Communication. Bits (1 or 0) are transferred one at a time through designated Transfer and Receive channels. Because this communication is synchronous, the transfer of bits is synchronized with the clock signal produced by the Arduino. This requires one more channel of communication between the devices- The clock signal. Furthermore, in SPI protocol, it is possible for multiple devices 3

4 Figure 1: Connection of devices through the Serial communication channel (source: sparkfun.com) to use the same clock, transfer, and receive channels. The restriction, however, is that in the case of our processor, only one device can use the channels at any given moment. This gives rise to a fourth channel, called the chip select or slave select. The device with an active chip select signal from the Arduino will be allowed to use the SPI protocol of the microcontroller. Once that chip select signal becomes inactive, the device can no longer use the SPI protocol. The transfer and receive channels are referred to as MOSI (Master Out Slave In) and MISO (Master In Slave Out). In our case, the Master is the Arduino Due, and the Slave is the sensor communicating with the Arduino Due. The clock channel is the CLK or SCK channel, and the chip select channel is abbreviated to CSn or SS. 3. Inter-Integrated Circuit or I 2 C, is another form of Synchronous Serial Communication. I 2 C requires two channels for communication between devices: The SDA (or the data line) and the SCL (or the clock line). Unlike SPI and Asynchronous Serial Communication, I 2 C handles sending and receiving data on one channel. Since a pair of I 2 C lines can be used to drive multiple devices, each device has a pre-defined address which is either 7-bits or 10-bits long. These addresses can be found in the data-sheet of the device. The function of this address can be thought of as being similar to that of the chip select signal used in SPI communication (See item above). In our case, the Arduino would serve as the "master" and communicating sensors would serve as "slaves". This means that the Arduino would control the clock rate and also initiate data transfers over the SDA line, allowing it (and inevitably, us) to control the flow of data across the channel. 4. Pulse Width Modulation or PWM, is a timed digital signal. A digital signal is one that is either low (0) or high (1). A PWM signal is one where the duration for which a digital signal is high (or low) is measured to produce a duty cycle. The percentage of time a signal is high in a period of time compared to the period is called the signals duty cycle. For example, if the signal is high for half the period of time, the duty cycle is 50% (see Figure 2). At a high enough frequency ( 100 Hz), changing the duty cycle of a signal can result in analog values (values across a range; not just high or low ) obtained from the digital signal. PWM is used to control the position of the Servo motors 2.5. Figure 2: Different duty cycle examples (source: sparkfun.com) 2.2 Magnetic Rotary Sensor A rotary sensor is a device that allows the measurement of the angular position of a shaft. Our magnetic sensor does this task by measuring the orientation of the magnetic field of a permanent magnet which is mounted underneath the shaft, whose angular position is to be determined. Figure 3 below demonstrates the position of the sensor relative to the magnet. We need this sensor to obtain the wind direction with respect to the boat direction. A magnet is attached to the bottom of the shaft of a wind vane, below which sits the rotary sensor. As the wind vane rotates due to the wind, the magnet underneath moves. The associated change in magnetic field is detected 4

5 by the rotary sensor, which outputs an angular position. The Magnetic Rotary Sensor uses the SPI protocol for communication, which requires a 4-channel connection to the microcontroller in addition to power, as described in section 2.1. Problems encountered: 1. Housing design Although a very rugged, low cost and accurate sensor, mechanical integration with the wind-vane has been a source of problems. Designing a housing for the sensor that places a magnet at a favorable distance from the chip while ensuring a freely rotating shaft and a water-proof environment has been a challenge. 2. SPI Signal Disturbances This semester the wind vane and rotary sensor was moved to the top of the sail to facilitate better wind direction detection, to remove noise induced from other parts of the boat and the water. The connection between the microcontroller and the chip now had to travel a much longer distance and as a result the SPI protocol (see Section 2.1) suffered. Over long distances, SPI signals, especially and high frequencies tend to get attenuated as they travel through the length of the wire. This affected the reliability of the communication between he microcontroller and Rotary Sensor, often resulting in a stalled system. To remedy this, we continued to add extend the length of the connection between the microcontroller and Rotary Sensor until the output readings became inaccurate, or the system stalled. Once a maximum length was reached, we also added an 84 Ohm resistor on the MISO wire at the end near the rotary sensor. Whenever dealing with SPI signals, check that the wire length is not causing disturbances in the signal. When adding a resistor to extend the maximum usable length, be sure to test for the right value of resistance that matches the impedance of the connecting wire. Figure 3: Structure of a magnetic rotary sensor unit after installation, displaying the sensor(bottom), the shaft and a cylindrical magnet. (source: machinedesign.com) 2.3 Inertial Measurement Unit The inertial measurement unit, or IMU, allows measurement of the orientation of the boat, in terms of the roll, pitch and yaw see figure 4. It does this using a combination of 3 sensors: a gyroscope, a magnetometer and an accelerometer. A gyroscope measures the rate of change of the three angles. A magnetometer measures the direction of the earths magnetic field relative to the boat. This allows for two things: determining the boat heading with respect to North, and determining wind direction with respect to North. In the previous section about the Magnetic Rotary Sensor, I mentioned that the sensor determines wind direction with respect to the boat direction. Knowing the boat direction with respect to North allows us to obtain wind direction with respect to true North. Lastly, the accelerometer measures the acceleration due to the forces on it. In the case of our boat, this means detecting the direction of Earths gravitational force relative to the boat. In other words, it detects which way is down, with respect to the boat, measuring the tip of the boat. With a combination of these three outputs from the 3 sensors, and an on-board processor, an IMU determines the roll, pitch and yaw i.e. the orientation, of the boat. The IMU also uses 5

6 the SPI protocol. It can thus, share the MOSI, MISO and CLK (see section 2.1) with the Rotary Sensor, but requires a different chip select channel for successful communication with the Arduino Due. Figure 4: The roll, pitch and yaw angles (source: boatsafe.com) Problems encountered: The principle purpose of the IMU on Sail-Vane is the detection of boat direction. The boat direction is given by the Yaw angle measured by the IMU. This measurement has been subject to a substantial amount of inaccuracy during lake and land testing. This has been attributed to the following factors: 1. Presence of magnetic interference: This was a great factor when testing the device indoors. Depending on where testing was done, the yaw angle obtained had an error of anywhere between 0 to 20 degrees. When testing inside the boat, the problem persisted due to the IMU being in close proximity to the GPS Sensor (See section 2.4) and the Battery. To avoid such inaccuracies in the Yaw angle reading, the IMU must be isolated from all other electronics on the boat. 2. Calibration: The IMU s accelerometer and magnetometer can be calibrated using the YEI 3-Space Sensor Software Suite provided by Yost Labs for the Sensor. See 10.2 for instructions. 2.4 GPS Sensor A global positioning system sensor, or GPS Sensor, is a sensor that, through connection with GPS satellites provides latitude, longitude, time, heading and speed. The GPS uses Serial communication to communicate with the microcontroller. The RX port of the GPS connects to the TX port of the Serial channel on the microcontroller, and the TX port of the GPS connects to the RX port of the same Serial channel on the microcontroller (see section 2.1). Problems encountered: The GPS has thus far functioned very well in all testing environments. The only fact to keep in mind is that the GPS does not function indoors and takes 2 minutes to start providing position data once outside. 2.5 Servo Motors A servo motor or a servo, is a device that is a combination of a motor and an angular position sensor (the magnetic rotary sensor is an example of such a sensor). The motor allows a shaft to rotate as required, while the position sensor keeps track of the position of the shaft. Such sensing allows us to move the shaft to designated angles. Two servos are used on the boat - one for the sail and the other for the tail. Each of the servos have signal channels to PWM pins 2.1 on the microcontroller. The three sensors above (magnetic rotary sensor, IMU, GPS) allow the microcontroller to obtain the necessary data required to successfully implement our navigation algorithm, which then assigns the sail and tail to be set to the required angles. The servo motors then use that output from the algorithm to set the sail and tail accordingly. 6

7 3 Communication System Our sailboat achieves short distance communication through small radio modules manufactured by Digi International. These Xbee-Pro S3B modules allow for wireless communication between the microcontroller and a personal computer (PC). An Xbee module can transmit, in a Serial manner (see section 2.1), data to a linked Xbee module. One of these modules is connected to a Serial channel of the Arduino Due (Serial1), while the other is connected via USB (which also uses a form of serial protocol), to a PC. The Xbee modules thus channel data from the Arduino Due to the user s computer. It is important to keep in mind that in real world scenarios, the range is about 100m when maintaining a line-of-sight. This range reduces greatly when there are obstructions between the receiver and transmitter. 4 Remote Control The remote control (RC) system was developed as a fail safe to bring the boat back to shore in case the navigation algorithm makes the boat malfunction. The circuit for the RC involves the use of a multiplexer. A multiplexer is a device that has multiple inputs and one output. The device selects one of the inputs to pass through and become the output depending on the state of a digital select signal. In our case, the multiplexer has two inputs; one input is the remote control, while the other is the microcontroller. The digital select signal is controlled by a switch on the transmitter. The overview of the system can be seen in figure 5. On receiving power, the multiplexer defaults to outputting the signal from the remote control, which means that unless the RC functions, the boat will not run. This acts as an effective safeguard that ensures the boat is never completely out of our control. Figure 5: Schematic of current RC system of the sailboat 5 Power System Below is a table of the maximum power consumed by each individual system on the boat. This information was collected from the datasheets of each device. The different components of the navigation system have different optimal input voltage requirements. The Arduino Due requires an input voltage in the range of 6-16V. This input voltage is then regulated down to the operating voltage of the processor, 3.3V, by the microcontroller circuit board. The three sensors were chosen such that they do not operate at a voltage higher than the operating voltage of the microcontroller. The IMU, GPS Sensor and the Rotary Sensor require 3.3V for their operation. This 3.3V can be provided directly by the microcontroller through one of its power-output pins. The sail servo motor requires 4.8-6V, while the tail-vane servo motor requires 6-7 V. With these requirements in mind, the following system has been put in place: Two external battery sources of 6V power the microcontroller, sensor array and servo motors. The 7

8 Device Voltage Supplied (V) Current (A) Maximum Power Consumed (W) IMU Tail Servo (HS-5646WP) Sail Servo (HS-785HB) GPS Rotary Sensor Arduino Due Table 1: Approximate Power Consumption of each device used on CU Sail s boat. microcontroller is supplied power via a 6V NiMH battery, which in turn supplies the required 3.3V to each of the sensors. A separate pack of 4 AA cells, totaling 6V, powers the two servo motors and the multiplexer that selects the signal for the motors. This semester we also tried a battery of a higher energy capacity, since the boat must sail an endurance challenge in competition. The endurance challenge requires the boat to sail for 8 consecutive hours. The new nimh battery supplies 10.8 Volts with an energy capacity of 58 Watt-hours. With this battery, our boat should run for approximately 13.8 hours. Due to issues regarding the weight of our boat, we had to switch back to our original, lighter battery set up. In the future, simply using this battery to power every device on the boat would be preferable. Problems encountered: We have thus far been unsuccessful in powering the every single component of the electronic system using a single 6V NiMH source. The two components that prevent such a system from working are the tail servo and the multiplexer. We suspect that even though the voltage requirements of the two components are met by a 6V source, the current the sources we have so far tried can provide has not been sufficient. However, more testing is required to say so conclusively. 6 Circuit Design and Organization The sensor array (IMU, Rotary Sensor, and GPS Sensor), the microcontroller, the autonomous control signals to the servo motors, and the power to the microcontroller are arranged on a custommade printed circuit board. A printed circuit board, or PCB, electrically connects electronic components using copper etchings that are essentially printed wires on a non-conducting plate or board. It acts as the hub of the electronic system, to which the microcontroller and sensors attach. Signals setting the motor positions for autonomous navigation also originate from the board. Components such as resistors and capacitors are soldered on the board itself. This makes the setup modular and easy to set up. Once attached, these components are electrically connected through the previously mentioned conductive tracks made by the copper etchings on the board, removing the need for loose wires. The PCB was designed on a computer-aided design (CAD) software called EagleCAD. After the board is printed, the different pins and parts are manually soldered onto the board. Figure 6 shows the layout of the PCB. The microcontroller attaches right to the middle of the board. The various sensors and servos attach to the top of the board as indicated by the labels. The bottom of the board consists of 3 LEDs and headers for an Xbee Unit (See section 3). 7 Navigation Algorithm In these sections, we outline the navigation algorithm background, the goals of the current algorithm, the theory the current algorithm is based on, and the future work that we aim to accomplish. 8

9 Figure 6: PCB layout 7.1 Basic Structure The goal of the navigation algorithm is to sail an autonomous boat between a series of waypoints (our target latitude longitude coordinates). In order to accomplish this, the boat must be able to go upwind, downwind, recognize when it has hit a waypoint, and turn. In the past, we attempted to use a finite state machine to solve this problem. This approach had a series of states (such as "tack left", "head upwind to the right", "head directly to the waypoint"... ). For each state, there were triggers to transition to different states (for example, when the state is tack left, if the boat is facing directly towards the next waypoint, "stop turning" and transition to the state "head directly to the waypoint"). While this approach worked in a two-dimensional simulator, when the boat was placed in the water, there were too many variables such as constantly changing boat orientation and wind direction, that were unaccounted for in the transition model of the finite state machine. To remedy this issue, we changed our approach from a finite state machine to a solely position based algorithm. In this approach, we aimed to have the only determinants of position be the boats position, the wind direction, and the location of the next waypoint. This eliminated a lot of the variables that were introduced by a state machine, and so would be able to get the best sail and tail angle at any position, regardless of past behavior. Overall, our algorithm is: 1. Check if the boat has hit a waypoint 2. If so, head to the next waypoint 3. If not, find the intended angle the boat should be sailing at 4. Set the sail and tail of the boat according to the intended angle We aimed to take advantage of the directional stability of the boat in order to sail according to these steps, diagrammed in Figure 7. To accomplish step four in the overall algorithm, we created 9

10 Figure 7: The algorithm steps taken. functions that set intended angle of the boat, named for their intended angle with respect to (WRT) the wind: upright, upleft, righttarget, lefttarget, downleft, and downright. Then with this intended angle, we set the sail and tail of the wind according to the offset of the intended angle to the boat orientation. In order to call these functions, we determine which sector of the navigation diagram (pictured in figure 8) the boat is in. Depending on what the angle to the waypoint is, we select a sector. For example, if the waypoint is directly upwind, the angle to the waypoint would be 0 WRT the wind. If the boat is facing right, we would look in the first circle below. Because the angle to the waypoint is 0, which falls in sector 0, we would call the upright function, and would set the intended angle to be upright of the wind. These different sectors select an intended angle, and a sail angle of attack (AoA), according to Table 2: Sector Intended Angle WRT North Sail AoA WRT North Upwind Right WindDir + Optpolartop Positive Upwind Left WindDir - Optpolartop Negative Downwind Right WindDir Optpolarbot Positive Downwind Left WindDir Optpolartop Negative Left to Waypoint AngletoWaypoint Negative Right to Waypoint AngletoWaypoint Positive Table 2: Where WindDir is the angle of the wind WRT North, Optpolartop is the optimal upwind angle, Optpolarbot is the optimal downwind angle, AngletoWaypoint is the angle to the waypoint WRT North All of our functions that set the sail and tail rely on the same basic structure. At any time, the boat has an intended angle that it is trying to course correct to sail at. The goal is to set the sail and to tail at the angle that they would be at if the boat was heading in the direction of the intended angle. To accomplish this, first, it calculates the difference (Offset) between the current boat direction, and the intended angle that the boat should be sailing at. This offset can be positive or negative, depending on whether the intended angle is clockwise or counterclockwise from the current boat orientation. The offset is then added to the wind direction to get the tail angle, then 10

11 Figure 8: The directions that we sail depending on where the boat is in relation to the wind and the waypoint. the sail AoA determined from the function called (positive or negative) is added to the tail angle. This produces a sail and tail angle that match the intended angle. In the example below, given the wind direction and angle to the waypoint, we aim to find the intended angle and set the sail and tail accordingly. In the example on the left, the boat orientation is to the right of the wind, and the angle to the waypoint is upwind. Because of this, we aim to sail upwind to the right, so we call the function upright, and get an intended angle of WindDir+OptPolarTop. Figure 9: An example of the offset correction to sail upwind. 11

12 In the example on the right, we similarly see the boat orientation is to the right of the wind, and the angle to the waypoint is upwind. Because of this, we aim to sail upwind to the right, so we call the function upright, and get an intended angle of WindDir+OptPolarTop. In both examples, the intended angle is the same. To get the sail and tail angles, we then find the difference between the boat orientation and the intended angle, and then add this difference to the wind to get the tail angle. Then, because we called the upright function, the sail AoA is positive, so we add the angle of attack to the tail angle to get the sail angle. In this way, the boat is able to correct its course, depending on which angle it should be sailing at. When implemented, this allows the boat to sail upwind, downwind, to the target, and turn. In the same example above, with the waypoint upwind, the boat would select sector 0 first. As it sails upwind and to the right, the angle to the waypoint will increase(shown below). Figure 10: A trace of the boat position when sailing upwind. As the boat sails upwind to the right, the angle to the waypoint increases. At the last position, where it is 45 degrees, when going into the sector diagram, it will now fall in sector 1, turn. Now, the boat will call the lefttarget function, which will adjust the boats position to sail to the waypoint to the left. In this way, the boat will sail upwind, turn left, and hit a waypoint. This algorithm achieves our basic goals of sailing upwind and downwind, and hitting waypoints. The full code is referenced here on our Github. However, the tasks that still have yet to be implemented that are necessary for competition are: Buoy rounding, maximum tacking width, and task-specific algorithms. To accomplish buoy rounding, we could manually set waypoints around a buoy to simulate rounding. However, this is not fundamentally correct, and could lead to issues if the boat starts rounding a waypoint but fails. In order to achieve this, we will need to modify our approach to waypoint hitting, to instead constantly aim to round a buoy. Another task that we aim to complete next semester is a function of maximum tacking width. Currently, the algorithm aims to sail to a waypoint in one turn only. While this is usually the most optimal route, as turns slow the overall traversal down, it leads to issues when distances become large. For example, when trying to sail up Cayuga lake, one end cannot be reached from the other in one maneuver. Instead, the code should execute a series of tacks in accordance with the below example. In Figure 11, the boat is trying to sail upwind to the waypoint. Instead of trying to achieve this 12

13 Figure 11: A trace of the boat position when sailing upwind, making multiple maneuvers to not exceed the maximum tacking width. in one tack, and possibly hit the shore or an area that we do not want to head, the boat should sail with multiple maneuvers to hit the target. This can be remedied by modifying our current code structure. We will create a line using two GPS coordinates that represents the center of whatever body of water we are trying to sail across. Then, in the algorithm before checking for waypoints, we will check if the perpendicular distance from the center line to the current boat position is greater than a threshold we set. If it is, then the boat must immediately turn, as it is past the maximum tacking width. 7.2 Buoy Rounding In order to round a buoy, we have tried two approaches. The first was to simulate rounding a buoy by placing a sequence of waypoints around the buoy in the path that we intend the boat to take. This is shown below: This approach achieved our goal of rounding a waypoint in numerous quad tests. However, there is a possibility in the above scenario that the boat hits the first waypoint, then sails upwind to the left. Instead of rounding the buoy in the counterclockwise direction, as intended, the boat could never round the buoy. This is shown below: This scenario did not happen to us in quad testing, but is possible that it would happen in the water. To remedy this issue, a more robust buoy rounding code was needed. To illustrate our algorithm, we set an arbitrary sequence of waypoints as shown below as an example: We outline this algorithm in the code below. Let the next buoy needed to be rounded be called B1 and the buoy after B1 be called B2: 1. Let the detection radius be the distance that our boat loses on a turn. Call it DR. 2. Determine the rotation direction needed. Each waypoint must be rounded in a clockwise or counterclockwise direction. Depending on the position of the next waypoint, and the previous waypoint, we can determine if the waypoint must be rounded clockwise or counterclockwise. For example, we know that buoy 1 and buoy 2 in Figure 3 must be rounded counterclockwise. 3. Find the distance from the boat to B1. Call this value Distance. 13

14 Figure 12: Attempting to round buoy (black dot) by placing waypoints in a rounding path (red dots) around the buoy. Wind direction is from north (arrow). Path is 1, 2, 3 Figure 13: Attempting to round buoy (black dot) by placing waypoints in a rounding path (red dots) around the buoy. Actual boat path shown (black curved line). Wind direction is from north (arrow). Path is 1, 2, 3 Figure 14: Sample course. Wind from north (arrow). Path is 1, 2, 3. Intended boat path shown (black curved line) 4. Find the angle that the vector from B1 to B2 makes with North. Let this angle be called 14

15 buoyangle. 5. Find the angle that the vector from the boat to B2 makes with North. Let this angle be called nextbuoyangle. 6. Add buoyangle and nextbuoyangle. Call this angle roundingangle. It is the angle formed by boat, B1, B2. At this point, we are able to create a triangle, boat->b1->b2. We can find the angles and lengths of the sides using trigonometry. 7. Find the perpendicular distance from the line from boat to B2, to the point B1. Call this PD. 8. If roundingangle is more than 180, or PD is less than DR, keep rounding the buoy. 9. If the rounding angle is more than 180, then the boat has not gone far enough around the buoy. If the boat started to turn to try to go to the next waypoint, its path would not round the buoy. Then we must continue to round the buoy 10. If PD is less than DR, then if the boat started to turn, we would potentially lose ground and head under the buoy. Then we must continue to round the buoy. 11. To round a buoy, find the vector from boat to B1. Find the angle formed from boat, B1, and a perpendicular line to boat, B1, of length PD. Call this angle offset. Use the intended angle found in the algorithm from boat to B1. If we need to round clockwise, add offset to the intended angle. If we need to round counterclockwise, subtract offset from intended angle. 12. Else, we have successfully rounded a buoy. Increment our waypoint counter and in doing so, head to the next buoy. In this way, we are able to round any waypoint. Even if our boat gets knocked down by wind or waves, it will always round a buoy. This remedies the issue introduced by setting arbitrary waypoints around a buoy in the first approach. 7.3 Station Keeping Station keeping involves keeping the boat within a certain radius (40 meters) for an extended time period (5 minutes). To implement this, we add a time modulus to our code and update the algorithm to continually sail to the same waypoint at the center of the station keeping radius. To do this we could set several waypoints to be the same, however for space efficiency we chose to have a waypoint array of only two waypoints. By not updating which waypoint in the array to sail to until the time period has ended we keep our boat within a 5 meter radius as it continually circles one point. Our future work on this algorithm will involve analyzing our location at the time we can now leave the radius and selecting a waypoint on the fly which is optimally close to the boat and outside the radius, rather than having an arbitrary and predetermined location to sail to at the timeout. Additionally, we anticipate that by changing our boats angle of attack to be much smaller and sailing upwind we can remain sailing in one direction at a very slow speed which will cause us to remain in the radius for the required time period, without continually sailing around one waypoint. 8 Error Analysis To optimize navigation between waypoints, we introduce a method to calculate how far off the boat is from its current course. The idea is that given two waypoints w 0, w f which are pairs of latitude/longitude values (i.e. w n = (λ n, ϕ n ) where λ is longitude and ϕ is latitude) then we want to calculate how far away the boat is from where it would be on a linear course from w 0 to w f. The situation is depicted graphically in Figure 15. This calculation involves determining the distance between points of latitude and longitude (e.g. the distance between w 0 and w f ). Since we want to be able to calculate this distance in multiple instances, we develop it separately. 15

16 w q w f w a w 0 Figure 15: Setup of problem: w 0 is starting point, w f is the waypoint, w a is where we expect the boat to be, w q is where the boat actually is. We want to calculate dist (w a, w q ) 8.1 Distance Calculation Given two points of latitude and longitude, one can calculate the great-circle distance between them by applying the haversine formula. 1 Alternatively, one can calculate the Euclidean distance by assuming the Earth is locally flat. 2 We study both methods by considering their implementations. Currently, the boat uses the haversine formula. A haversine is by definition expressed as ( hav(x) = sin 2 x ). 2 Using this, we can express the law of haversines in terms of points B, C, D in the figure below. Namely we have hav( > BD) = hav( > BC > CD) + sin( > BC) sin( > CD) hav(c) where C is the angle between > BC and > CD. Figure 16: Sphere with Great Circle Distances 1 The great-circle distance is the shortest distance between points on a sphere, see Figure 16 2 The Euclidian distance is the lengh between points in a plane 16

17 This law is derived from the law of cosines for spheres: cos( > BD) = cos( > BC) cos( > CD) + sin( > BC) sin( > CD) cos(c) using cos( > BC b) = cos( > BC) cos( > CD) + sin( > BC) sin( > CD) and the relation cos(x) = 1 2 hav(x). Hence the haversine formula is the case where we make the point B the north pole and D, C are the points of interest. Thus we have ) where d is distance ( d = 2r arcsin hav(ϕ2 ϕ 1 ) + cos(ϕ 1 ) cos(ϕ 2 ) hav(λ 2 λ 1 ) ( = 2r arcsin r is earth s radius ϕ 1, ϕ 2 are latitude values λ 1, λ 2 are longitude values. sin 2 ( ϕ2 ϕ 1 2 ) ( ) ) + cos(ϕ 1 ) cos(ϕ 2 ) sin 2 λ2 λ 1 2 This is the haversine formula that we implement in our method for the boat. 3 Alternatively, if we assume that we are dealing with a flat surface (rather than a section of a sphere), then we can use right triangle trigonometry to calculate the distance between ϕ 1, λ 1 and ϕ 2, λ 2. Specifically, let θ m = ϕ 1 ϕ 2 and θ o = λ 1 λ 2. Then, the distance between the points is d = r θ 2 m cos 2 ( λ1 + λ 2 2 ) + θ 2 o We compare these two methods at three different distances (approximately): 250 kilometers at the equator, 1 kilometer across Cayuga lake, and kilometers across the engineering quad. H (km) D (km) H Error (%) D Error (%) km (equator) < km (Cayuga lake) km (Quad) In the table, H represents haversine distances and D represents Euclidian distances. Note that for small distances the two methods give virtually the same results. However, in general, as the distances increase, so does the resulting error (but not by much, as evidenced by the error at the equator). Moreover, it is apparent that for the distances in the sailing competition (which are less than 1 kilometer), either method would suffice. 8.2 Course Error Now that we have established the distance calculation between two location points, we can calculate how far off the sailboat is from its expected course. The first step in making this calculation is establishing the expected course between two points. Since our objective is to get the average error, we assume a linear trajectory between the start point and the end waypoint. Furthermore, we assume a constant velocity v. We parametrize the x position of the boat in kilometers by time. To use x as a measure of distance we use the conversion x cos(ϕ). Similarly, we can convert latitude to kilometers via y ϕ where ϕ is the latitude in degrees. 4 Now suppose 3 found at github.com/apomerenk/cusail/blob/master/individual%20functions%20for% 20Arduino%20Code/haversineDistanceCalculator.cpp 4 both conversions averaged from 17

18 the first point is given by w 0 and the waypoint is w f. Assuming that the boat starts at t 0 = 0, the final time t f is given by t f = dist (w 0, w f ). v So, if we are given a time t q and a corresponding location w q = (λ q, ϕ q ) (see Figure 15), we can calculate how far off course the boat is by dist(w q, w a ) where w a = (λ a, ϕ a ) is the predicted location. It is calculated by x q = x 0 + v x t q where v x is the velocity in the x direction and x is calculated by the aforementioned conversion. Once we calculate x q we can calculate y q via y q = y f y 0 x f x 0 (x q x 0 ) + y 0. Notice this assumes a linear course from w 0 to w f. Now that we have w a, we can calculate the error as dist(w a, w q ). 8.3 Graphical User Interface In order to better see how our boat sails, optimize variables, and analyze correctness, we must be able to see the path that the boat took after a test. We created a graphical user interface (GUI) to take the data that our boat sends and plot it. The flow of data is shown below: 1. As the boat is sailing, the Arduino outputs print statements to the serial monitor 2. The XBEEs send the print statements from the Arduino onboard to a reciever connected to a laptop 3. The XBEE receiver sends the print statements to XCTU, an application to read XBEE data 4. Each print statement is logged to a.log file in hex as long as we are recording 5. When we stop recording, the.log file is finished 6. Read the.log file into a python script 7. Convert from hex to ASCII 8. Look for GPS coordinates of waypoints and boat, and wind direction 9. Create vectors for wind direction that correspond to a specific boat location 10. Plot GPS coordinates of waypoints and boat, and wind vectors The end result is shown below for a sample.log file: The frequency of sampling boat location and wind direction can be modified in order to more clearly display the data. However, the main issue in this system is the range of the XBEEs. When we are quad testing, we can walk the boat around and have the laptop close, so we do not lose any data. However, when testing on the water, the boat can reach hundreds of meters away from the laptop. Because of this, a lot of the print statements that are logged come in as garbled data, or dont reach the laptop at all. To remedy this in the short term, we have modified our python script to look for any number that has 6 decimal points of precision. Any number that comes in with that precision, we log as a GPS coordinate. Until we can strengthen the range of the XBEES, or change our testing and logging framework, this is the approach that we plan to use when water testing. 18

19 Figure 17: GUI of a sample boat path. Waypoints in red, boat locations in blue, wind direction in black arrows 9 Pixy CMUcam5 The Pixy CMUcam5 (referred to as Pixy ) is a 4 square inch visual detection system equipped with a dual core processor and is capable of capturing and processing 50 frames per second. It works by detecting color signatures of objects and reporting the location of the blocks containing that objects. The team s need for Pixy is twofold. First, the sailboat needs to detect and avoid objects. Second, it needs to be able to find objects and sail towards them. 5 Both of these functionalities are required by the International Robotic Sailing Competition. Given the object data from Pixy, we can extract the location of the object relative to the frame and react accordingly (depending on whether we want to avoid the object or sail towards it). Figure 18 is a labeled diagram showing the back of the Pixy. 6 Figure 18: Back of Pixy 5 For exact specifications of the object detection and search tasks see sailbot.org/ 6 taken from 19

20 9.1 Setup Pixy is made to communicate easily with an Arduino microcontroller via the Arduino s Serial Peripheral Interface (SPI) bus. The makers of Pixy provide a library, Pixy.h, specifically designed for such communication. With it, one can simply instantiate a Pixy object, then call getblocks() on that object. This method returns the number of objects Pixy has detected. If there are any blocks, one can access their properties (location, signature, dimensions of block) by calling the appropriate methods on the Pixy object.7 Figure 19: Pixy setup The first step in setting up Pixy with the Arduino is to create and upload a program using the aforementioned Pixy library. Then, without connecting Pixy to the computer, one connects Pixy s communication cable to the SPI bus on the Arduino board. The wires of the communication cable should lead away from the Arduino s processing core so that the pins are aligned properly. The setup should resemble the setup in Figure 19. Notice that the Arduino already has power from the computer. To view the output of Pixy, one opens the Serial Monitor in the Arduino IDE (Crtl + shift + M). If the setup is done correctly, and the default test program8 is used on the Arduino, the output should resemble Figure 20.9 Also note that if Pixy is simultaneously connected to the computer it stops communicating with the Arduino. With the current library, Pixy cannot be taught objects programmatically. Instead, one has to either use the native PixyMon software or teach Pixy in real time. To teach Pixy objects in real time one can hold down the white button (see Figure 18) and let the LED cycle through colors. The button should be released once the LED turns red. Then one can position the object to be learned by Pixy in front of the lens. The color of the LED should change to that object s color. Once it does, one can finish by tapping the white button. If it is successful, the LED will flash then turn off. Pixy can be taught multiple objects by repeating this process. If the process fails at some point, one can restart by holding the white button. To get optimal results, it is important to position the object directly in front of the lens and it is also important that the user is not in a dark setting. The PixyMon software can also be used to teach the PixyCam objects. Once the PixyCam is taught an object it can be disconnected from the computer and connected with the Arduino (while 7 The specific methods can be found at at 9 found at 8 found 20

21 still remembering the objects it was taught). Figure 20: Serial Monitor output 9.2 Obstacle avoidance To implement obstacle detection and avoidance we expanded our use of the PIXY Cam. Our boat is best suited to use a camera for obstacle detection and avoidance, rather than a radar, as the roll and pitch of the boat would cause inconsistent data readings. Waves, are problematic for radar detection methods as they may resemble the objects we want to detect. Additionally, waves would cause the plane of the boat to shift several times throughout any competition thus negating the effectiveness of the radar as there is no consistency across time, meaning we cannot efficiently compare current states of detection with past states. The primary benefit of the PIXY Cam is that we can detect objects based on color as well as size. Unfortunately, the camera does not give us depth in the y field assuming an x-y-z axis placed on the boat such that the z axis runs parallel to the sail and the x axis from port to starboard. We have recorded the relative size of a Bema Buoy, which is used in competition, as encoded by the Camera at certain distances and can use this information to extrapolate relative distance of obstacles. Outside of competition, we can use the location of the boat and the recorded size of the obstacle at two different times to extrapolate the y distance from the boat when handling unknown obstacles in the future. Handling obstacle avoidance on unknown obstacles using the camera poses difficulty as the PIXY Cam primarily identifies colors it has been pre-trained on. The next goal for our obstacle avoidance is to expand to unknown obstacles. Obstacle avoidance could be implemented either by manipulating waypoint information, adding, shifting, or changing the waypoints the boat believes it is sailing to, or overwriting the boats current turning code to operate differently when an obstacle is detected. Our algorithm implements a variation of both of these options. We add a Boolean for if an object is detected and if true we modify the boats computed angle to the waypoint to be 90 degrees off from the true angle. This effectively tricks the boat into believing the waypoint is in a different location without modifying the waypoint array directly which reduces runtime as well as the overheard required to reset to a non-obstacle interfered course. The pseudocode for the algorithm is shown in Algorithm 1. The end result of this algorithm is the boat sails in an arc around the detected obstacle. This result is displayed in figure 21, the initial orientation of the boat, and figure 22, the resulting arc course the boat navigates. Note, in these figures WP is the angle to the waypoint, Wi the sensordata angle of the wind, and A the adjusted angle the boat would like to sail toward after detecting an object. 21

22 Algorithm 1 Obstacle avoidance 1: procedure AvoidObstacle 2: if obstacle detected by PIXY Cam then 3: if the obstacle is coming from the right then 4: Change the angle to the waypoint to (angle to waypoint 90 degrees) 5: else if the obstacle is coming from the left then 6: Change the angle to the waypoint to (angle to waypoint + 90 degrees) 7: Modularize the new angle to be within range 8: if the obstacle detected Boolean is still set to true then 9: Set the obstacle detected Boolean to false Figure 21: Obstacle Avoidance First Detection Figure 22: Obstacle Avoidance Corrected Course The distance kept between the boat and the obstacle is dependent on the early detection of the boat rather than on the size of the obstacle e.g. when did the PIXY CAM first notice the object, not how large an object is it. This is effective as our boat turns within a very small radius so late detection will still be efficient, and the size of the object will be accounted for by how long our boat is correcting for the object being in view. The direction of change in the course correction was determined by competition specification as well as practicality outside of competition as the boat should generally sail away from the direction of the obstacle. That said, if the given obstacle is another boat this may result in the boats sailing parallel in a direction which is not advancing our boat towards its next waypoint. Given the current size and speed of our boat it is probable that the other boat will pass us, allowing us to continue on the correct course however this is one known area of weakness in our boats current algorithm and a possible future optimization when working with unknown/pre-trained obstacle avoidance in the future. 22

23 9.3 Searching This part of object detection requires adjusting the direction of the boat so it sails towards a desired object. Our PIXY Cam detects objects reliably up to 40 m away if they are about 1 meter in either height or width. We added a lens with a longer focal length to expand this, however on such a small camera the adjustments only allowed for a marginal increase in range and thus we maintain our boats object detection radius to be 40 meters. In competition we look to find an object within a 100 m radius and thus our algorithm looks to break down this detection radius into 40 m sectors while still being time efficient. To accomplish this we break down the circle into 8 waypoints as shown in 23 to ensure our boat will at least see all possible object locations within the radius. Figure 23: Search Algorithm Waypoint Placement Once an object matching the signature we are looking for is detected we follow a few simple steps to sail toward the object. Mainly, if the object is on the port side, the boat makes a port side turn and vice versa if it is on the starboard side. Moreover, the size of the turn is proportional to the distance from the objects x position to the center of the frame. This algorithm is largely still in the development stages as we have focused primarily on honing in on algorithms we are sure will score perfectly in competition. Thus we have not yet had the opportunity to complete a full iteration of testing on this algorithm and the future work for it may soon involve a new object detection platform outside of the PIXY CAM. 9.4 Future Work for Pixy CMUcam5 In the long term, we would explore alternative devices for obstacle detection since Pixy is limited in both functionality and adaptability. The functionality is limited because Pixy can only differentiate objects based on predefined colors. In the future we would like to detect all objects around the boat not just objects of a certain color. This brings us to another limitation. In the current design, Pixy is fixed at the boat s bow, facing forward. So we are limited to detecting objects directly in front of the boat. This could be problematic in settings where we are trying to make a tack or jib and want to track an object (such as a buoy) around the turn. Moreover, it is difficult to adapt Pixy to any other type of detection since the Arduino library, Pixy.h, provides minimal support for the device s features. One example of the library s limitation is that it does not allow one to programmatically adjust the color of a signature. Instead, this adjustment must be made from the PixyMon software or by manually reprogramming Pixy. Regardless, there are other platforms that solve these issues. In the future, we would investigate such platforms including motion detectors, (ultrasonic) laser range finders, and LiDAR and assess each s potential to replace Pixy. 23

24 10 Appendix 10.1 Component List 1. Microcontroller: Arduino Due 2. IMU: YEI 3 Space Embedded Sensor 3. GPS: Parallax PAM-7Q GPS Module 4. Rotary Sensor: AS5047 by AMS 5. Sail Servo: HS-785HB by Hitec 6. Tail Servo: HS-5646WP by Hitec A, 6V/12V Solar Charger IMU Calibration Before beginning please ensure you have the latest version of the YEI 3-Space Sensor Suite available at this link: https : //old.yostlabs.com/yei 3 space sensor software suite. Please also ensure you are in an environment with not a lot of magnetic interference (ensuring this is not trivial; one way to check for this is to use a different compass and ensure that it points to North. Of course having a rough idea of which was North is, will be helpful). Also note that the following steps are in reference to the Yost Labs 3-Space Embedded Sensor referenced in Ensure the two jumpers on the top left and right are set to position 1 and the power-switch (above the right jumper) is off 2. Connect the sensor to your computer using the USB Mini port and turn the sensor on 3. Start the YEI 3-Space Sensor Suite, select the 3-Space Sensor from the list of connected devices (If the sensor does not show up, click Refresh) and click Connect 4. Once connected, run the Gradient Descent Calibration through the top menu and follow the steps. 5. Once completed, make sure to Save/Commit these settings before disconnecting the sensor from the PC Setting up the XBEE Radio Receiver/Transmitter The XBEE uses a radio signal to send information (including gps, wind readings, current waypoints, etc) from the boat to a laptop. How to use XBEE: 1. XBEE uses the program XCTU. If you do not have the program on your computer, download it here: 2. Plug the XBEE connected to a USB cable into your laptop. 3. After opening the program, click the "Switch to consoles" button in the top right corner. 4. Before adding the Radio module, make sure the other XBEE on the PCB is NOT being powered. 5. Click the "Add Radio Module" button in the top left corner. 24

25 Figure 24: XCTU Home Screen 6. A window will pop up. Make sure the correct port is chosen. The fields below should be as follows: Baud Rate: 9600 Data Bits: 8 Parity: None Stop Bits: 1 Flow Control: None Click finish 7. The XBEE should then connect. A pop-up may inform you to click the restart button on the XBEE before connecting. Typically you have about 10 seconds to press the button, so be ready to do so. 8. To start receiving data, click the open channel button. To close the channel, click the close channel button. 9. To record XBEE data click the record button. Keep in mind if you close the channel while recording, the recording will stop automatically. To stop recording, click the button again Soldering A PCB Setup: Soldering Iron should be set to around 690 F. Gather all the pins and other pieces needed on your board. The solder used in the lab has flux combined with it, so no need to use anything other than the spool of solder. 25