0

I am trying to achieve autonomous GPS navigation on my RC car. I am using a very high quality GNSS/INS sensor (Advanced Navigation Spatial), from which I am getting lat, long and heading values. The sensor is connected to a Raspberry Pi from which its data is parsed using their special protocol and then passed to Arduino Mega via serial using ROS.

I have a route made out of picked waypoints from Google Maps. I am using Haversine distance and bearing formulas, from there I determine which direction to turn the servo and when to move to the next waypoint.

The vehicle follows the route, but sways around a lot. There are also problematic waypoints at which the vehicle just goes crazy and looks like it's getting +-5 meters error. Funnily enough I observed the output of the sensor through their software whilst driving the vehicle, and the path it followed looked smooth without any anomalies, with an error margin of +-0.25m.

At this point I am not what the problem is. Is it the sensor, my code or RC car itself (maybe something to do with steering)?

Here's the minimal version of my Arduino Sketch:

#include <ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <Servo.h>

#define STEERING_ADDITION 12

#define NEUTRAL 94
#define LEFT (NEUTRAL+STEERING_ADDITION)
#define RIGHT (NEUTRAL-STEERING_ADDITION)

//#define BEARING_THRESHOLD 3.5
#define BEARING_THRESHOLD 2.5
#define DISTANCE_THRESHOLD 0.5

#define NUMBER_OF_WAYPOINTS 23

Servo myservo;

ros::NodeHandle nh;

float gps_current[3];
float gps_target[NUMBER_OF_WAYPOINTS][2] = {
    {xxx, xxx},
    {xxx, xxx},
    ...
};

void messageCb(const std_msgs::Float32MultiArray& msg) {
    gps_current[0] = msg.data[0]; //Lat
    gps_current[1] = msg.data[1]; //Long
    gps_current[2] = msg.data[2]; //Heading
}

int waypoint = 0;

float required_bearing = 0;
float rel_bearing = 0;

float dist = 0;

bool navigation = true;

ros::Subscriber<std_msgs::Float32MultiArray> sub("gps", &messageCb );

void setup() {
    pinMode(12, OUTPUT);
    digitalWrite(12, LOW);
    nh.initNode();
    nh.subscribe(sub);
    myservo.attach(9);
}

void loop() {

    required_bearing = bearing(gps_current[0], gps_current[1], 
 gps_target[waypoint][0], gps_target[waypoint][1]);
    rel_bearing = required_bearing - gps_current[2]

    dist = distance(gps_current[0], gps_current[1], gps_target[waypoint][0], 
    gps_target[waypoint][1]);

    if(dist < DISTANCE_THRESHOLD) {
        waypoint++;
        digitalWrite(12, HIGH);
        delay(100);
    }
    else {
        digitalWrite(12, LOW);
    }

    if (navigation) {

        if(rel_bearing < -BEARING_THRESHOLD) {
            myservo.write(RIGHT);
        }
        else if(rel_bearing > BEARING_THRESHOLD) {
            myservo.write(LEFT);
        }
        else {
            myservo.write(NEUTRAL);
        }
    }

    nh.spinOnce();
}


float bearing(float lat1,float lon1,float lat2,float lon2) {

    float teta1 = radians(lat1);
    float teta2 = radians(lat2);
    float delta1 = radians(lat2-lat1);
    float delta2 = radians(lon2-lon1);

    //==================Heading Formula Calculation================//

    float y = sin(delta2) * cos(teta2);
    float x = cos(teta1)*sin(teta2) - sin(teta1)*cos(teta2)*cos(delta2);
    float brng = atan2(y,x);
    brng = degrees(brng);// radians to degrees
    brng = ( ((int)brng + 360) % 360 ); 

    return brng;
}

float distance(float flat1, float flon1, float flat2, float flon2) {

    float dist_calc=0;
    float dist_calc2=0;
    float diflat=0;
    float diflon=0;

    //I've to spplit all the calculation in several steps. If i try to do it in a single line the arduino will explode.
    diflat=radians(flat2-flat1);
    flat1=radians(flat1);
    flat2=radians(flat2);
    diflon=radians((flon2)-(flon1));

    dist_calc = (sin(diflat/2.0)*sin(diflat/2.0));
    dist_calc2= cos(flat1);
    dist_calc2*=cos(flat2);
    dist_calc2*=sin(diflon/2.0);
    dist_calc2*=sin(diflon/2.0);
    dist_calc +=dist_calc2;

    dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));

    dist_calc*=6371000.0; //Converting to meters

    return dist_calc;
}

I have double checked the values I am getting on Arduino and they are identical to the ones that are being sent from the sensor, so no data is being corrupted.

Any suggestions are greatly appreciated!

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.