Esempio n. 1
0
        }//end combined Check angle

        //This function searched for a right turn angle which the robot can avoid an obstacle to the right.
        // The function starts out by looking at the target point in, and finds a turn to the right of that angle, not to the right of straight forward.
        public static  double combinedRightWheelScan(LidarPoint target)
        {
            LocationPoint source = new LocationPoint();// source is the point at the front right corner of the right wheel
            double target_angle = adjust_angle(Math.Atan2(target.X, target.Y), 2.0*Math.PI);
            // this is the desired angle to the target point, //this function limits the angle between (-pi & pi) or (-180 & 180)

            double sample_phi; // temporary variable which holds various angles to look at
            int i = 0;// set up iterator to initailize at 0

            source.X = combinedHalfRobotWidth;// assigning source point to the front right corner of the right wheel
            source.Y = 0; //assigning the source point to a value of Y=0.
            
            do// find the closest turn angle on the right of the target turn angle which will avoid obstacles
            {
                LocationPoint sample_point = new LocationPoint(); // create a new point which might be the point we can go towards
                sample_point.X = combinedBufferLength * Math.Sin(target_angle - SAMPLING_ANGLE * i);//assign the sample point X Value
                sample_point.Y = combinedBufferLength * Math.Cos(target_angle - SAMPLING_ANGLE * i);//assign sample point Y Value
                sample_phi = adjust_angle(Math.Atan2(sample_point.X, sample_point.Y), 2.0 * Math.PI);//this function limits the angle between (-pi & pi) or (-180 & 180)

                //check if there is anything in the way of this turn angle. Combined Vector Scan will return
                //true when it is possible for the robot to take the input turn angle, and false if there is something in the way.
                if (combinedVectorScan(source, sample_point))
                {
                    //the turn angle of sample_phi is safe for this wheel
                    if (combinedCheckAngle(sample_phi)) //double check this turn angle is safe for both wheels
                    {
                        if (i == 0)//if this is the first itration (IE the entered desired turn angle is safe), then return 0.
                        {
                            return 0;
                        }

                        return sample_phi;//otherwise return the turn angle needed to avoid the obstacle
                    }//end Combined Check ANgle
                }//end COmbined Vector Scan
                i++;
            } while (i < ANGLE_SAMPLES);
            return DOOM;//if it is impossible to avoid an obstacle then return doom which is a large turn angle....
        }//end Combined Right Wheel Scan
Esempio n. 2
0
        ////}

        /// <summary>
        /// inital landmark scan correction
        /// </summary>
        /// <param name="CLM">landmarks found in lidar scan</param>
        /// <param name="KLM">landmarks found in text file</param>
        public static void CorrectLandmarks(List <LocationPoint> CLM, ref List <LidarPoint> KLM)
        {
            //loop through each landmark found in the list of known landmarks which were read in from a file
            foreach (var knownLandmark in KLM)
            {
                double minMatchDist = 15;// the inital (purposefully large) distance between a KLM and a CLM. This is updated
                //to the distance between the CLM and the KLM each time the distance is smaller than any previous distance.

                double        maxSeperationDist = .5;   // the maximum distance between the KLM and CLM for a the CLM to be asscocaited with the KLM
                LocationPoint closestLandmark   = null; // create local temporary landmark

                //look through landmarks that were found in the LiDAR scan in attempt to associate one to the list of known landmarks
                foreach (var currentLandmark in CLM)
                {
                    //calculate the distance between the current known landmark to the current found landmark
                    double dist_KLM__to_CLM = Math.Sqrt(Math.Pow(knownLandmark.X - currentLandmark.X, 2) + Math.Pow(knownLandmark.Y - currentLandmark.Y, 2));

                    //if the distance between the CLM and the KLM is smaller than the smallest distance found between the current KLM and any other CLMs
                    if (dist_KLM__to_CLM < minMatchDist)
                    {
                        //if the distance between the CLM and the KLM is below the seperation threshold
                        if (dist_KLM__to_CLM < maxSeperationDist)
                        {
                            minMatchDist    = dist_KLM__to_CLM; // update the matched distance to the distance from this CLM and this KLM
                            closestLandmark = currentLandmark;  // update the closest landmark to this CLM
                        }
                    }
                }
                if (closestLandmark != null)                      //if a CLM was found close enough to a KLM and is associated to it.
                {
                    knownLandmark.X = closestLandmark.correctedX; //update the KLM X value
                    knownLandmark.Y = closestLandmark.correctedY; //Update the KLM Y value
                }
                else                                              //dont update, keep landmark the same
                {
                }
            } //end CLM loop
        }     //emd KLM loop
Esempio n. 3
0
        //this function checks a turn angle (provided by wheel scans) to see if there is anything in the way of the desired turn angle.
        //It returns true if the robot can turn that direction without hitting an obstacle, and returns false if there is something in
        //the way of the entered turn angle.
        static bool combinedVectorScan(LocationPoint source, LocationPoint destination)
        {
            double target_angle = adjust_angle(Math.Atan2(destination.X - source.X, destination.Y - source.Y), 2.0*Math.PI); //limit angle between (-pi & pi) or (-180 & 180)
            double target_dist = destination.DistanceBetween(source); //find distance between corner of wheel to desired target. this equals Buffer length
            double dist;
            int i = 0, j = 0;//set up two iterators
            double tempDist = 0.0;

            LocationPoint sample_point= new LocationPoint();// create temporary point which is updated each iteration
               
            do// find out if there is anything in the way of the 
            {
                sample_point.X = (i * SAMPLING_DISTANCE) * Math.Sin(target_angle);//create temporary XY point along the desired target angle at varying distances
                sample_point.Y = (i * SAMPLING_DISTANCE) * Math.Cos(target_angle);//create temporary XY point along the desired target angle at varying distances

                foreach (var cpoint in combinedBufPoints)//look through each point point in the buffer LiDAR points to see if any are by the temporary point
                {
                    if (cpoint.Y < 0) //if it's behind the robot, skip it
                    {
                        continue;
                    }
                    dist = sample_point.DistanceBetween(cpoint); //figure out distance between the temporary point and this point in the buffer LiDAR points
                    if (dist < combinedBufferWidth) // if the dcurrent LiDAR point is close to the temporary point
                    {
                        badCount++;//increment the number of LiDAR points in the way of this target angle
                        if (badCount > BADCOUNTTHRESH) //if there's too many lidar points in the way then this is not a valid turn angle
                        {
                            return false;//return false to indicate that this turn angle is not good.
                        }//end BAD COUNT THRESH
                    }//END dist<combinedBufferWidth
                }//end For each LiDAR point
                i++;
            } while (SAMPLING_DISTANCE * i < target_dist); // while the sampling distance along the target angle is less than the target distance

            return true;//return true when it is possible for the robot to take this turn angle without hitting obstacles
        }//end CombinedVectorScan
Esempio n. 4
0
        }//end main

        private static void ScanThread(object sender = null)
        {
            //READ IN NEW LIDAR DATA
            int?[] lidarData = lidarController.ScanData();
            if (EnableGUICalls)
            {
                NewLidarData.Invoke(lidarData, EventArgs.Empty);
            }                                                                       //print LiDAR data to GUI

            //create list of lidar points which actually returned a value.(This list of points will be smaller than 1081, because all points which did not return are not added to this list
            var lidarDataNonNull = lidarData.Select(x => x.HasValue ? x.Value : int.MaxValue).ToList();

            ////OutputToFile(lidarDataNonNull);//output LiDAR data to file if you want to replay it later

            //LOCALIZATION
            var currentLandmarks = Localization.ScanLandmarks(lidarData.ToLidarPoints(19000), knownLandmarks, YetiLocation);                  //find the landmarks

            YetiLocation = Localization.DetermineRobotLocation(currentLandmarks, YetiLocation, 0.001, last_lSpeed, last_rSpeed, (float)0.01); //find where robot is based of landmarks

            //FIND OBSTACLEs
            var myObstacles     = Obstacle.FindObstacles(lidarDataNonNull, YetiLocation, lidarData.ToLidar1081Points(19000), currentLandmarks); //returns a list of obstacles
            var movingObstacles = (from p in myObstacles where p.moving == true select true).ToArray().FirstOrDefault();                        //checks if any of the found obstacles were moving.

            if (EnableGUICalls)
            {
                NewObstacleData.Invoke(myObstacles, EventArgs.Empty);
            }                                                                            //print the obstacles to the GUI

            //SLIP Dection
            //check that the Queue which saves yeti location is full. Only Dequeue old location if it is full
            if (YetiHistory.Count > YetiHistorySize)
            {
                YetiHistory.Dequeue();                                                                    // if it is full then remove the oldest location
            }
            YetiHistory.Enqueue(new LocationPoint(YetiLocation.X, YetiLocation.Y, YetiLocation.Heading)); //add the newest location

            //check that the Queue which saves yeti location is full. Stall detection should only trigger if the queue is full!
            if (YetiHistory.Count >= YetiHistorySize)
            {
                //Find Maximum and Minimums of X, Y and Heading
                var minX       = YetiHistory.Min(item => item.X);
                var maxX       = YetiHistory.Max(item => item.X);
                var minY       = YetiHistory.Min(item => item.Y);
                var maxY       = YetiHistory.Max(item => item.Y);
                var minHeading = YetiHistory.Min(item => item.Heading);
                var maxHeading = YetiHistory.Max(item => item.Heading);

                // if no XY movement, no rotation, and beyond first waypoint.Beyond first way point is key!!!!
                //Otherwise yeti will back up at the beginning if you wait to long!
                if ((Math.Pow(maxX - minX, 2) + Math.Pow(maxY - minY, 2) < NoMovementTolerance) &&
                    maxHeading - minHeading < NoRotationTolerance && TargetLocation.location.id > 1 && !movingObstacles)
                {
                    lastStuckTime = DateTime.Now; //save the time which slipping was detected
                    YetiHistory.Clear();          // clear the QUeue so that stall detection cannot occur again until the queue is full
                    if (EnableGUICalls)
                    {
                        StallDetected.Invoke(null, EventArgs.Empty);
                    }                                                                   //print the Stall info to the GUI
                }
            }

            //Correct the list of known landmarks from the text file to reflect where the landmarks are actually seen in the field
            if (keepCorrecting)                                                      //if this is the first LiDAR scan
            {
                Localization.CorrectLandmarks(currentLandmarks, ref knownLandmarks); //inital landmark snapshot is corrected
                keepCorrecting = false;                                              // flag to signal that the landmarks have been corrected, and don't need to be corrected again.
            }
            if (EnableGUICalls)
            {
                NewLandmarkData.Invoke(currentLandmarks, EventArgs.Empty);
            }                                                                                 //print the Landmark locations to the GUI

            //OBSTACLE AVOIDANCE
            Buffer.combinedUpdatePoints(lidarData.ToLidarPoints(19000));
            //Creates List of LiDAR points which have a positive Y value, and are within the Buffer distance threshold

            //look at angle needed to go to target waypoint, if there is an obstacle in the way, then find what turn angle is needed to avoid it to the right.
            double Right = Buffer.combinedRightWheelScan(TargetLocation.location.ToLidarPoint(YetiLocation));
            //look at angle needed to go to target waypoint, if there is an obstacle in the way, then find what turn angle is needed to avoid it to the left.
            double Left = Buffer.combinedLeftWheelScan(TargetLocation.location.ToLidarPoint(YetiLocation));

            //WAYPOINT NAVIGATION
            double turn = Control.cvar.turn;       //create local variable, and initialized it to the last turn angle
            int    dir  = (int)TargetLocation.dir; //read in the direction(forward or backward) which was read in from the navigation waypoint text file

            //Navigate!
            if (Control.areWeThereYetAndTurnPID(YetiLocation, TargetLocation)) //has Yeti arrived at the target waypoint?
            {                                                                  //true when target is reached.
                //reached target, change target to next waypoint from waypoint navigation list
                Control.initGuide();                                           //reset PD controller errors to 0
                PrevTargetLocation = TargetLocation;                           //update previous target the the just reached target

                //assign new target from the next waypoint in the list of waypoints
                var newTargetLocation = TargetLocationList.SingleOrDefault(x => x.location.id == PrevTargetLocation.location.id + 1);
                if (newTargetLocation != null)          //if it is null, we've hit the last target so don't update the target
                {
                    TargetLocation = newTargetLocation; //otherwise go towards new target
                }
            }


            //DECIDING WHERE TO GO/TURN based off of found information
            //create local variables to assemble wheel speeds.
            double speed;
            float  lSpeed;
            float  rSpeed;

            if (movingObstacles)//if we see a movin obstacle, then stop no matter what
            {
                speed  = 0;
                turn   = 0;
                lSpeed = 0;
                rSpeed = 0;
            }
            else if (Right == 0 && Left == 0)// if there no obstacles in our way, then we are good to go and can turn to wherever waypoint navigation wants to go!
            {
                //no obsticals to avoid
                turn = Control.cvar.turn;
                //speed slower as turn steeper
                speed  = 1 / (1 + 1 * Math.Abs(turn)) * (double)dir;
                speed  = (double)dir * Math.Min(Math.Abs(speed), 1.0);
                lSpeed = (float)((speed + turnBoost * turn) * maxSpeed * Control.cvar.speed);//controlvarspeed is read in from text file, and limits speed by a percentage
                rSpeed = (float)((speed - turnBoost * turn) * maxSpeed * Control.cvar.speed);
            }
            else if (Right == Buffer.DOOM && Left == Buffer.DOOM) //There is no way to avoid anything to the left or the right, so back up.
            {
                lSpeed = reverseSpeed * (float)maxSpeed * (float).25;
                rSpeed = reverseSpeed * (float)maxSpeed * (float).25;
                Console.WriteLine("I reached DOOM!");
            }
            else // there is obstacles to avoid, so the avoidance turn angle which deviates least from the waypoint is selected
            {
                if (Math.Abs(Right - turn) <= Math.Abs(Left - turn))
                {
                    //move right of obstacle
                    turn = Right;
                }
                else if (Math.Abs(Right - turn) > Math.Abs(Left - turn))
                {
                    //move Left of obstacle
                    turn = Left;
                }
                //speed slower as turn steeper
                speed  = 1 / (1 + 1 * Math.Abs(turn)) * (double)dir;
                speed  = (double)dir * Math.Min(Math.Abs(speed), 1.0);
                lSpeed = (float)((speed + turnBoost * .5 * turn) * maxSpeed * Control.cvar.speed);
                rSpeed = (float)((speed - turnBoost * .5 * turn) * maxSpeed * Control.cvar.speed);
            }

            //Send Wheel Speeds and Control Yeti
            //If we should still be backing up because of slip detection, then continue backing up. And double check we actually want to be moving.
            if (lastStuckTime.Add(reverseDuration).CompareTo(DateTime.Now) > 0 && Control.cvar.speed > 0)
            {
                motorController.SetMotorValues(reverseSpeed * (float)maxSpeed, reverseSpeed * (float)maxSpeed);//go backwards
            }
            else//otherwise send wheel speeds to yeti
            {
                motorController.SetMotorValues(lSpeed, rSpeed);//use normal values
            }
            //save previous wheel speeds.
            last_lSpeed = lSpeed;
            last_rSpeed = rSpeed;

            //print motor speed information to GUI
            if (EnableGUICalls)
            {
                double[] dataToSend = { YetiLocation.X, YetiLocation.Y, /*debug1*/ YetiLocation.Heading, Left, Right, turn, lSpeed, rSpeed, currentLandmarks.Count, Control.cvar.pErr };
                NewLocationPoint.Invoke(dataToSend, EventArgs.Empty);
            }
        } //end Scan thread
Esempio n. 5
0
        static int LM_POINTS_THRESH = 3;                                          // this is the minimum number of LiDAR points needed for a landmark to be considered a landmark



        /// <summary>
        /// //The brains of Yeti!
        //Localizatino revolves around this function entirely, which is by far the most complicated thing about yeti,
        // There are really three necassary things for this to work:CLM, Robot Location, Tolerance
        /// </summary>
        /// <param name="CLM">list of landmarks which the robot currently sees</param>
        /// <param name="robotLocation">the previously calculated position of the robot</param>
        /// <param name="tolerance">the tolerance of the derivative of change in sum of error squared to be considered a local minimum</param>
        /// <param name="Lspeed">not needed, but is used to determine if the robot is trying to move</param>
        /// <param name="Rspeed">not needed, but is used to determine if the robot is trying to move</param>
        /// <param name="minSpeed">not needed, but is used to determine if the robot is trying to move</param>
        /// <returns></returns>
        public static LocationPoint DetermineRobotLocation(List <LocationPoint> CLM, LocationPoint robotLocation, double tolerance, float Lspeed, float Rspeed, float minSpeed)
        {
            int JMAX = 15;                      //Maximum number of attempts to converge on a local minimum of the robots location

            double mu = 0.1;                    //initial learning rate(used in Gradient Descent)

            bool updateh = false;               //flag for it the hessian matrix should be updated

            double[,] H     = new double[3, 3]; // Hessian of 2nd Derivatives
            double[,] H_lm  = new double[3, 3];
            double[,] H_inv = new double[3, 3]; // Matrix Inverse of H_lm
            double x_err = 0;                   // Gradient E [0]
            double y_err = 0;                   // Gradient E [1]
            double t_err = 0;                   // Gradient E [2]
            double ex;                          // Landmark error in X
            double ey;                          // Landmark error in Y
            double dx;                          // Robot Delta X
            double dy;                          // Robot Delta Y
            double dt;                          // Robot Delta Theta
            double det;                         // Determinant of H_lm

            //initially use the last robots location as the intial guess of where the robot is.
            var thisRobotLocation = new LocationPoint(robotLocation.X, robotLocation.Y, robotLocation.Heading);

            double errsqrd     = 0.0;     // Sum of Errors Squared
            double derr        = 0.0;     // Sum of Errors (not squared)
            double lasterrsqrd = 99999.0; //inital error is set to infinity.
            double lambda      = 10.0;    // Levenburg Marquardt step Value

            /*INitialize Hessian Matrix To Zero*/
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    H[i, j] = H_lm[i, j] = H_inv[i, j] = 0;
                }
            }

            /*Attempt to Converge on robot location*/
            for (int j = 0; j < JMAX; j++)
            {
                errsqrd = 0;               // reset the error squared for the new position which is to be calculated
                derr    = 0;               // reset the errors derivative.

                if (j == 0)                // on first attempt to converge
                {
                    updateh     = true;    // it is necassary to update the hessian matrix
                    lasterrsqrd = 99999.0; //initial error squared is set to inifinity so that first attempt at convergence is accepted
                }

                if (updateh) //update the hessian matrix(IE Calculate new position entirely)
                {
                    //reset the hessian matrix to zeros
                    for (int i = 0; i < 3; i++)
                    {
                        for (int k = 0; k < 3; k++)
                        {
                            H[i, k] = 0.0;
                        }
                    }

                    //place learning rate on diagonal of the hessian
                    H[0, 0] = H[1, 1] = H[2, 2] = mu * CLM.Count;

                    //initialize error to non-zero (further away robot positions should be punished a little more heavily than previous
                    x_err = mu * CLM.Count * (previousRobotLocation.X - thisRobotLocation.X);
                    y_err = mu * CLM.Count * (previousRobotLocation.Y - thisRobotLocation.Y);
                    t_err = mu * CLM.Count * (previousRobotLocation.Heading - thisRobotLocation.Heading);

                    foreach (var currentLandmark in CLM)//loop through each landmark yeti can see
                    {
                        //figure out the cost in x and cost in y
                        ex = currentLandmark.X - thisRobotLocation.X -
                             currentLandmark.Distance * Math.Sin(thisRobotLocation.Heading + currentLandmark.Heading);   // this is delta X for landmark i (equation 2 in ION GNSS Paper)
                        ey = currentLandmark.Y - thisRobotLocation.Y
                             - currentLandmark.Distance * Math.Cos(thisRobotLocation.Heading + currentLandmark.Heading); //this is delta y for landmark i(equation 3 in ION GNSS Paper)

                        //saving corrected landmark locations for correct landmarks
                        currentLandmark.correctedX = currentLandmark.X - ex; // save the correct X coordinate of the landmark for the correct landmarks function
                        currentLandmark.correctedY = currentLandmark.Y - ey; // save the correct Y coordinate of the landmark for the correct landmarks function.
                        //Note that this is for this robot location, which may be rejected.

                        derr    += Math.Abs(ex) + Math.Abs(ey);       // add the errors in both coordinates
                        errsqrd += Math.Pow(ex, 2) + Math.Pow(ey, 2); // add this error to the sum of error squared. (equation 1 in ION GNSS Paper)

                        x_err += ex;                                  //increment the total error in the X coordinate for this robot location.
                        y_err += ey;                                  //increment the total erro rin the Y coordinate for this robot location
                        //increment the total error in of theta
                        t_err += ex * (currentLandmark.Distance * Math.Cos(thisRobotLocation.Heading + currentLandmark.Heading)) -
                                 ey * (currentLandmark.Distance * Math.Sin(thisRobotLocation.Heading + currentLandmark.Heading));

                        //Updating the Hessian Matrix
                        H[0, 0] += 1;                                                                                           // (equation 9 in ION GNSS Paper)
                        H[0, 1] += 0;                                                                                           // (equation 10 in ION GNSS Paper)
                        H[0, 2] += (currentLandmark.Distance * Math.Cos(thisRobotLocation.Heading + currentLandmark.Heading));  // (equation 11 in ION GNSS Paper)
                        H[1, 0] += 0;                                                                                           // (equation 12 in ION GNSS Paper)
                        H[1, 1] += 1;                                                                                           // (equation 13 in ION GNSS Paper)
                        H[1, 2] += -(currentLandmark.Distance * Math.Sin(thisRobotLocation.Heading + currentLandmark.Heading)); // (equation 14 in ION GNSS Paper)
                        H[2, 0] += (currentLandmark.Distance * Math.Cos(thisRobotLocation.Heading + currentLandmark.Heading));  // (equation 15 in ION GNSS Paper)
                        H[2, 1] += -(currentLandmark.Distance * Math.Sin(thisRobotLocation.Heading + currentLandmark.Heading)); // (equation 16 in ION GNSS Paper)
                        H[2, 2] += Math.Pow(currentLandmark.Distance, 2);                                                       // Formula 17
                    }//end for each CLM
                }//end update h

                // calculating the Matrix H * lamba(Diagonal Matrix). (necassary for equation 19 in ION GNSS Paper)
                for (int i = 0; i < 3; i++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        H_lm[i, k] = H[i, k];

                        if (i == k)
                        {
                            H_lm[i, k] += lambda * 1.0;
                        }
                    }
                }

                //finding the determinant , which is used to calculate the inverse of the Matrix H * lamba(Diagonal Matrix)
                det = H_lm[0, 0] * (H_lm[2, 2] * H_lm[1, 1] - H_lm[2, 1] * H_lm[1, 2]) -
                      H_lm[1, 0] * (H_lm[2, 2] * H_lm[0, 1] - H_lm[2, 1] * H_lm[0, 2]) +
                      H_lm[2, 0] * (H_lm[1, 2] * H_lm[0, 1] - H_lm[1, 1] * H_lm[0, 2]);


                // Find inverse of Matrix H * lamba(Diagonal Matrix). (Part of equation 19 in ION GNSS Paper)
                H_inv[0, 0] = (H_lm[2, 2] * H_lm[1, 1] - H_lm[2, 1] * H_lm[1, 2]) / det;
                H_inv[0, 1] = -(H_lm[2, 2] * H_lm[0, 1] - H_lm[2, 1] * H_lm[0, 2]) / det;
                H_inv[0, 2] = (H_lm[1, 2] * H_lm[0, 1] - H_lm[1, 1] * H_lm[0, 2]) / det;
                H_inv[1, 0] = -(H_lm[2, 2] * H_lm[1, 0] - H_lm[2, 0] * H_lm[1, 2]) / det;
                H_inv[1, 1] = (H_lm[2, 2] * H_lm[0, 0] - H_lm[2, 0] * H_lm[0, 2]) / det;
                H_inv[1, 2] = -(H_lm[1, 2] * H_lm[0, 0] - H_lm[1, 0] * H_lm[0, 2]) / det;
                H_inv[2, 0] = (H_lm[2, 1] * H_lm[1, 0] - H_lm[2, 0] * H_lm[1, 1]) / det;
                H_inv[2, 1] = -(H_lm[2, 1] * H_lm[0, 0] - H_lm[2, 0] * H_lm[0, 1]) / det;
                H_inv[2, 2] = (H_lm[1, 1] * H_lm[0, 0] - H_lm[1, 0] * H_lm[0, 1]) / det;


                /* Update Here*/
                /* Hessian Inverse times Gradiant! (equation 19 in ION GNSS Paper)*/
                // this is the change in the robot location from the intial guess used.
                dx = (H_inv[0, 0] * x_err) + (H_inv[0, 1] * y_err) + (H_inv[0, 2] * t_err);  // (equation 19 in ION GNSS Paper)
                dy = (H_inv[1, 0] * x_err) + (H_inv[1, 1] * y_err) + (H_inv[1, 2] * t_err);  // (equation 19 in ION GNSS Paper)
                dt = (H_inv[2, 0] * x_err) + (H_inv[2, 1] * y_err) + (H_inv[2, 2] * t_err);  // (equation 19 in ION GNSS Paper)

                // if the error(not squared) is less than tolerance, meaning the position guess is almost exactly correct, so there is no need to converge further.
                if (Math.Abs(derr) < tolerance)
                {
                    j = JMAX;  // break;
                }

                //compared this robot location guess sum of error squared to the last
                if (errsqrd < lasterrsqrd)
                {
                    //if this position has better error than the last
                    updateh                    = true;    // if it does, then recalculate the hessian matrix for a new position.
                    lambda                    /= 10;      // divide lambda by 10, and slow the learning rate to converge on a local minimum slower.
                    lasterrsqrd                = errsqrd; // save the error squared so it can be compared to next iteration.
                    thisRobotLocation.X       += dx;      // (equation 19 in ION GNSS Paper)
                    thisRobotLocation.Y       += dy;      // (equation 19 in ION GNSS Paper)
                    thisRobotLocation.Heading += dt;      // (equation 19 in ION GNSS Paper)
                }
                else// this position has more error than the last
                {
                    updateh = false; //do not calculate new hessian matrix
                    lambda *= 10;    //just increase lambda
                }
            }//end Convergence attempts. (J=MAX or Derivative of Error is 0).

            previousRobotLocation.X       = thisRobotLocation.X;
            previousRobotLocation.Y       = thisRobotLocation.Y;
            previousRobotLocation.Heading = thisRobotLocation.Heading;

            return(thisRobotLocation);
        }//end determine robot location
Esempio n. 6
0
        }//end determine robot location

        //Scan landmarks is used to find all of the surrounding landmark locations. To do this the LiDAR data is needed, the list of known landmark locations
        //from a text file are needed, and the robots current location (to convert each point in the LiDAR data to an XY coordinate in the field).
        public static List <LocationPoint> ScanLandmarks(List <LidarPoint> LMSdata, List <LidarPoint> KLM, LocationPoint robotLocation)
        {
            var currentLandmarks = new List <LocationPoint>(); //anytime a landmark is found in a scan the landmark is saved to this arrray

            foreach (var knownLandmark in KLM)                 //loop through each known landmark in the list of imported landmarks
            {
                //smallestSeperation is the maximum distance a point can be from where a landmark should be to be considered part of the landmark
                //double smallestSeparation = 0.55;  ////landmark_tolerance;
                double smallestSeparation = 0.45;  ////landmark_tolerance;
                //double smallestSeparation = 1;  ////landmark_tolerance;
                //double smallestSeparation = 1.75;

                LocationPoint matchedLandmark = null;// create a point to match the landmark too if it is found

                //The following variables must be reset to zero at the beginning of each landmark search.
                double Xsum = 0, ysum = 0, headingsum = 0; //everytime a point is found to be part of a landmark the X, Y heading
                // are summed together, then divded by number of points found to find the X, Y and heading averages for the points associated with the landmark
                int found_points     = 0;                  //save the number of points that become associated with  the landmark
                int twiceDist_points = 0;                  //used for debuging

                //loop through every point in the LiDAR scan
                foreach (var currentPoint in LMSdata)
                {
                    if (currentPoint == null)                                                //check if the LiDAR point did not return because there was nothing for the laser to bounce off of
                    {
                        continue;                                                            //forget about this point
                    }
                    var landmarkLocationPoint = currentPoint.ToLocationPoint(robotLocation); // convert the current point we are looking at
                    //into a point in the XY field based off of where the point is in the scan and where the robot is in the field.

                    // figure out the difference between where the landmark should be and where the landmark is in the X coordinate
                    double xSeparation = knownLandmark.X - landmarkLocationPoint.X;
                    // figure out the difference between where the landmark should be and where the landmark is in the Y coordinate
                    double ySeparation = knownLandmark.Y - landmarkLocationPoint.Y;

                    // figure out the distance between the LiDAR point we are looking at compared to where
                    double currentSeparation = Math.Sqrt(Math.Pow(xSeparation, 2) + Math.Pow(ySeparation, 2));
                    //the landmark should be

                    // if the seperation is beneath the set tolerance. IE the point is close enough to the landmarks expected position to be considered part
                    if (currentSeparation < smallestSeparation)
                    //of the landmark
                    {
                        Xsum       += currentPoint.X;       //add the point to the Xsum
                        ysum       += currentPoint.Y;       //add the point to the Ysum
                        headingsum += currentPoint.Heading; //add the point to the heading sum
                        found_points++;                     //increment the number of points found
                    }
                }//end loop through LiDAR points


                //now check if the points which were associated with the landmark were enough to be considered a landmark
                // if the number of points is low (less than thresh), then this likely isnt a landmark and it should not be assumed that this is a landmark
                if (found_points > LM_POINTS_THRESH)
                {
                    //This is considered a landmark!
                    double XAverage        = Xsum / (double)found_points;                          //find average of all X values of the points associated with the landmark
                    double YAverage        = ysum / (double)found_points;                          //find average of all Y values of the points associated with the landmark
                    double headingAverage  = headingsum / (double)found_points;                    //find average of all heading values of the points associated with the landmark
                    double distanceAverage = Math.Sqrt(XAverage * XAverage + YAverage * YAverage); //find average distance from the robot to the landmark point
                    if (distanceAverage < 15)                                                      ////MAX_LANDMARKS_DISTANCE)// check if the landmark is less than 15M away
                    {
                        matchedLandmark   = new LocationPoint();                                   // we have a new landmark!
                        matchedLandmark.X = knownLandmark.X;                                       //save the landmarks X value
                        matchedLandmark.Y = knownLandmark.Y;                                       //save the landmarks Y value

                        //the next part is saving the distance between the robot and the landmark.
                        //40% is the distance average, and 60% is the distance from the point & the robot
                        //this is done because neither of the distance vaues are exactly correct due to the fact that
                        //the landmark locations are not exactly at the center of the landmark(as they should be)
                        //so it is assumed that the distance from the selected point is slightly more correct
                        //than the distance average(which is the sum of the distancese from all of the found points)
                        //40% of the distance from the landmark is the distance average
                        matchedLandmark.Distance = 0.4 * distanceAverage;
                        //60% of the distance is the distance from the found landmarks position from the robot
                        matchedLandmark.Distance += 0.6 * robotLocation.DistanceBetween(matchedLandmark);
                        matchedLandmark.Heading   = headingAverage; // heading average is saved
                        currentLandmarks.Add(matchedLandmark);      //yay we have a new landmark so add it to our list of landmarks!
                        continue;
                    }
                }
            }//end for each landmark loops
            return(currentLandmarks);//yay here are all of our landmarks!
        }
Esempio n. 7
0
        /// the function previously known as guide, returns true when the robot has reached it's waypoint.
        public static bool areWeThereYetAndTurnPID(LocationPoint robot, Target currentTarget)
        {
            double heading = robot.Heading;      // save the robots heading in the field locally
            int    dir = (int)currentTarget.dir; //obtain if the robot should go forward or backward (based on read in text file)
            double dx, dy;                       //Delta X & Delta Y. Difference in X & Y coordinates the robot is from the target
            //double s, c;//Sin and cosine of the robots heading
            double dt;                           //Delta time. Holds the difference in time from the last time this function was called to this time.
            double desiredAngle;                 // the desired heading. The heading which would cause the robot to directly face the target
            bool   reachedTarget;                //flag to detect if the robot has made it to its waypoint

            if (dir < 0)                         //if direction from text file says to go backward, turn heading around
            {
                heading = heading - Math.PI * Math.Sign(heading);
            }

            //FIND DISTANCE IN X & Y COORDINATES TO TARGET
            dx = currentTarget.location.X - robot.X; //find difference in X coordinate from robot locaiton to target location
            dy = currentTarget.location.Y - robot.Y; //find difference in X coordinate from robot locaiton to target location

            //FIND DISTANCE AND ANGLE TO DESTINATION
            cvar.targdist = Math.Sqrt(dx * dx + dy * dy);//current distance from the robot to the target
            // desired angle is the desired Heading the robot should have at this instance if it were to be facing the target.
            desiredAngle = adjust_angle(Math.Atan2(dx, dy), 2.0 * Math.PI);
            //DEBUG(heading); DEBUG(desiredAngle);


            //USED FOR WAYPOINT NAVIGATION
            //cvar.right = dx * c - dy * s;
            //cvar.front = dy * c + dx * s;
            //c = Math.Cos(heading);//find Cosine term of the robots heading
            //s = Math.Sin(heading);//find sine term of the robots heading

            //RETRIEVE HOW FAST THE ROBOT SHOULD BE MOVING TOWARDS THIS WAYPOINT
            cvar.speed = currentTarget.speed;//retrive the speed for this waypoint from navigation text file

            //TIMING UPDATES FOR PID
            thisTime = System.DateTime.Now.Ticks / (TICKS_PER_SECOND); //find and save the current time
            dt       = thisTime - lastTime;                            //save the difference from the last time to the current time.

            /*Current Target Heading PID Calculations*/
            cvar.lastpErr = cvar.pErr;                                                       //save the last proportional error
            cvar.pErr     = adjust_angle(heading - desiredAngle, 2.0 * Math.PI);             //calculate the current propotional error betweem our current heading and the target heading
            cvar.iErr     = cvar.iErr + cvar.pErr * dt;                                      // increase the cumulated error.
            cvar.iErr     = Math.Sign(cvar.iErr) * Math.Min(Math.Abs(cvar.iErr), maxIntErr); //limit the maxmium integral error

            if (dt != 0)                                                                     //if the time has changed since the last iteration of guide. (cannot divide by 0).
            {
                cvar.dErr = (cvar.pErr - cvar.lastpErr) / dt;                                // calculate the derrivative error
            }
            if (Math.Cos(cvar.pErr) > 0.5)                                                   // if the robot is not facing more than +-60 degrees away from the target
            {
                //cvar.kP = 3 / Math.Max(cvar.targdist, 3); //  * 0.7112
                ////cvar.kP = 1;
                cvar.kP   = 0.5;
                cvar.turn = -(cvar.kP * Math.Sin(cvar.pErr) * 2 + cvar.kI * cvar.iErr + cvar.kD * cvar.dErr);  // calulate how much the robot should turn at this instant.Nattu
                ////cvar.turn = -(cvar.kP * cvar.pErr + cvar.kI * cvar.iErr + cvar.kD * cvar.dErr);
            }
            else//if the robot is facing more than 60 degrees away from the target
            {
                cvar.turn = -0.5 * Math.Sign(cvar.pErr); //if you need to turnin place, then ignore PID temporarily
            }
            lastTime = thisTime;//update the times

            //inLastTarget = (lastDist < leaveTargetThresh);
            //approachingTarget = (cvar.targdist < approachingThresh);
            reachedTarget = (cvar.targdist < destinationThresh); //if the robot is very close to the target, then it has hit the target.

            return(reachedTarget);                               //return true if the robot is close to the target.
        }
Esempio n. 8
0
        public static List <Obstacle> FindObstacles(List <int> data, LocationPoint robot,
                                                    List <Hardware.Lidar.LidarPoint> LMSdata, List <LocationPoint> landmarks) //data is a list of lidar points
        {
            ClearObstacles();                                                                                                 // clear obstacles found
            ClearState();                                                                                                     // reset the state machine for finding obstacles

            int  i = 0;
            bool flag_already_linking = false;//once two points are linked this flag is set to true. And is set to false after the end of an obstacle is found.

            // loop through each point of non-empty lidar data
            //loop through each point which is in front of yeti, not to the side. IE only points with a positive Y value
            for (int j = 360; j < data.Count - 361; j++)
            {
                var this_point         = LMSdata[j];                               //create an XY point for this distance from the LiDAR Data.
                var flag_points_linked = false;                                    //set to 1 when points are part of the same obstacle, and set to 0 at each increase of j

                if (this_point.Distance < maxRadius)                               //if the LiDAR point is closer than the max Radius (typically 15M as this is max size of the field.)
                {
                    for (i = 1; i < forgive_count + 1; i++)                        // look at the next forgive_count number of points in the lidar scan to see if they are close to this point
                    {
                        var next_point = LMSdata[j + i];                           // create and XY point from the next LiDAR point

                        double this_dist = this_point.DistanceBetween(next_point); // find a distance between the current lidar point (i) and the next lidar point(i+j)
                        if (this_point.DistanceBetween(next_point) < NONSEPARATION_THRESH * i * MM2M)
                        // if the distance between the points is less than Nonseperation thresh * the index away from the jth lidar point(converted to meters)
                        {
                            //it has been determined that point j and point i+j are part of the same obstacle, so we need to link them togethjer
                            LinkPoint(j, this_point.Distance, this_point.DistanceBetween(next_point)); //link the points together!
                            flag_points_linked = true;                                                 // we set this flag to true to indicate that points have been linked so we can assemble a
                            if (!flag_already_linking)                                                 //if this Lidar point j is the first Lidar point in an obstacle
                            {
                                obj_start_ind        = j;                                              //save the starting index of the obstacles
                                flag_already_linking = true;                                           //set the flag so that it is known that the lidar points are currently being linked.
                            }
                            break;                                                                     // we have found a point at most forgive_count away from lidar point j, so we can stop set j = to j+i now, and keep looking at future points
                        }
                    }//end for
                }
                if (flag_points_linked == false)                   // if lidar point j+i+forgive_count through j+1 are not close enough to lidar point j to be considered the same obstalce
                {
                    if (flag_already_linking)                      //the linked points from previous iterations were linking so we need to close the obstacle.
                    {
                        obj_end_ind = j;                           // store  the last lidar point index of the obstacle
                        AddObstacle(j, robot, LMSdata, landmarks); // add the obstacle to the obstacle list!
                    }
                    flag_already_linking = false;                  //flag that the obstacle has ended
                    ClearState();                                  //reset the state of variables used in "link_points"
                }
                else// if lidar point j is clsoe enough to lidar point j+i, then we continue to link the points
                {
                    j = j + i - 1;                                     //update what j should be (used if i > 0), subtract 1 because j will be incremented at start of forloop
                    if (j > data.Count - 361)                          // if the lidar point j is behind the robot (IE the point has a negative Y value)
                    {
                        if (flag_already_linking)                      // if the points were being linked
                        {
                            obj_end_ind = j;                           // store  the last lidar point index of the obstacle
                            AddObstacle(j, robot, LMSdata, landmarks); // add the obstacle to the obstacle list!
                            ClearState();                              // clear the state variables used in link_points
                        }
                    }
                }
            }
            //printObstacles(obstacles);
            return(obstacles);//return the list of obstacles.
        }//end find obstacles
Esempio n. 9
0
        static void AddObstacle(int z, LocationPoint robot, List <Hardware.Lidar.LidarPoint> LMSdata, List <LocationPoint> landmarks)
        {
            double index = z - linked_count / 2;                                                                  // save the center index of the obstacle in this variable
            double mag   = sum / linked_count;                                                                    //this finds the average distance from the robot of all the points which make up the obstacle. Saved in meters

            double this_x = robot.X + mag * Math.Sin(((135 - index * 0.25) * (Math.PI / 180.0)) + robot.Heading); //save estimated x value of the center of the obstacle in the field
            double this_y = robot.Y + mag * Math.Cos(((135 - index * 0.25) * (Math.PI / 180.0)) + robot.Heading); //save estimated Y value of the center of the obstacle in the field

            //if obstacle is larger than max radius, or the number of linked points too large, or the number of linked points is too small.
            if (mag > maxRadius || linked_count > highThresh || linked_count < lowThresh)
            {                 //this is not an obstacle.
                ClearState(); //obstacle is not worth acknowledging, clear the state
            }
            else//This is an obscale which is not to small or big, and is less close enough to care
            {
                //figure out where the obstacle is in the XY field
                double obj_x = robot.X + mag * Math.Sin(((135 - index * 0.25) * (Math.PI / 180.0)) + robot.Heading); // save this objects x value
                double obj_y = robot.Y + mag * Math.Cos(((135 - index * 0.25) * (Math.PI / 180.0)) + robot.Heading); // save this objects y value

                bool this_is_a_landmark_flag = false;                                                                //reset the "this is a landmark" flag
                bool outside_of_field        = false;                                                                // reset the "the obstacle is outside of the field" flag

                //Figure out if this is a landmark or not
                for (int k = 0; k < landmarks.Count(); k++)// go through each landmark position from the list of known landmarks
                {
                    if (Math.Sqrt(Math.Pow((obj_x - landmarks[k].X), 2) + Math.Pow((obj_y - landmarks[k].Y), 2)) < is_this_landmark_thresh)
                    {
                        this_is_a_landmark_flag = true;//this is a landmark!
                    }
                }

                //figure out if the object is within the plowing field or not
                if (obj_x > 4.75 || obj_x < -1.750 || obj_y > 11.75 || obj_y < -2.75)//check if obstacle is outside of Triple Ifield
                //if (obj_x > 1.75 || obj_x < -1.750 || obj_y > 11.75 || obj_y < -2.75)//check if obstacle is outside of Single Ifield
                {
                    outside_of_field = true;
                }

                //if the object is in the field, and is not a landmark
                if (!this_is_a_landmark_flag && !outside_of_field)
                {
                    Obstacle newObstacle = new Obstacle();                                                                //create a new obstacle
                    newObstacle.X = robot.X + mag * Math.Sin(((135 - index * 0.25) * (Math.PI / 180.0)) + robot.Heading); // save the obstacles X value
                    newObstacle.Y = robot.Y + mag * Math.Cos(((135 - index * 0.25) * (Math.PI / 180.0)) + robot.Heading); // save the obstacles X value

                    //save start and end indexes
                    newObstacle.obj_start_index = obj_start_ind; // save the obstacles starting index
                    newObstacle.obj_end_index   = obj_end_ind;   //save the obstacles ending index

                    //convert start index into a point
                    newObstacle.start_point = LMSdata[obj_start_ind];
                    newObstacle.end_point   = LMSdata[obj_end_ind];

                    //save characteristics of the obstacle
                    newObstacle.Dist = mag;                                                                         // save the objects distance from the robot
                    newObstacle.rough_size_of_obs = obj_size_sum;                                                   // save the sum of distances of the points (EG would hold the arc length of a landmark
                    newObstacle.line_size_of_obs  = newObstacle.start_point.DistanceBetween(newObstacle.end_point); // This is distance from start point to end point.
                    //if the obstacle is a straight line then the rough size and line size will be the same

                    if (mag < 5)// check if the obstacle is within 5 meters of the robot.
                    //this is done only to react to the moving obstacle if it is within 5meters.
                    {
                        //if (Math.Abs(newObstacle.line_size_of_obs - newObstacle.rough_size_of_obs) < line_size_diff_thresh)//check if the object is flat(NOt used)
                        //{
                        if (Math.Abs(newObstacle.line_size_of_obs - moving_obs_size) < moving_obs_size_thresh) // check if object is the same size as the moving obstacle.
                        {
                            newObstacle.moving = true;                                                         //we have a moving obstacle
                            Console.WriteLine("THIS OBSTACLE IS MOVING!");                                     //print that it has been seen
                        }
                        else
                        {
                            newObstacle.moving = false;
                        }                                   // we know the obstacle isn't moving
                    }
                    else// if the obstacle is more than 5 meters away don't stop for it yet.
                    {
                        newObstacle.moving = false;
                    }
                    numObstacles = numObstacles + 1; //increment number of found obstacles
                    obstacles.Add(newObstacle);      //add the new obstacle to the list of obstacles
                }//end if landmark or or outside field
            }//end check to add obstacle
        }//end add object
Esempio n. 10
0
 public double DistanceBetween(LocationPoint p2)
 {
     return(Math.Sqrt((this.X - p2.X) * (this.X - p2.X) + (this.Y - p2.Y) * (this.Y - p2.Y)));
 }