localization - Robot Navigation


Localization

As discussed,Localization is the process of position it self with respect to the surrounding, surrounding can be define as the global or the local . Consider a person who is in a dark room. if he does not have sens of where he is with respect to the features of the room , next movement could not grantee that he will not hit a wall. So it is important to localize itself if it is moving with respected to it surrounding. If the system has map of it's surrounding , once he has the current and goal positions, actions are straight forward. If there is no map,like in our case, it need to build a accurate enough map while exploring. And also movement should not be too costly to the goal. So to resolve this problem karlman filter integrated particle filter is used to probabilistically map the position and the surrounding.

Lidar Scan Data

since 2D Lidar is used , Algorithm is only compatible with signal ,one set of distance measurement around fix z axis coordinate with resolution of 1degree.

Encoder Data

Here encoder Data will be two set of counts related to left and right wheel ,generated optical encoders connected to left and right wheels

Algorithm

localization is determining the position with respected to it surrounding , surrounding may be the nearest set of object which it can detect or the globally it position respect to the earth. Determining the it position is important since it determine how much it can move round with out bumping to passing object and also for get to its goal position with minimum number of maneuver .

we cannot totally rely on the measurement from integrated sensors since they obviously have errors. So compunction for the errors is necessary.

So To the compunction for the error and build out accurate set of prediction,we need,really good mathematical model of the system .

Robot Dynamics

As discuss before system uses left and right speed variations for manure, as bellow we can define right and left motor travel as r and l for distance travel for unit time ,then we can calculate the steering angle alpha.

 and Its steering radius R as following,
  


















despite 4 motors, for this design ,motors works left motors as one and right motors as one , for the simplification of the dynamics. Robot uses speed of the left and right motor to rotate, That means if left motors rotate faster than the right motors ,Robot  will rotate to right side.

  To make a dynamic equation, robot state can be define as the x and y coordinate related to the reference point and also the robot heading angle to theta . So it can be move that the new position P'  from the current position  P.

equation to the next position can be derive using last position P as following,     

                        

 It can be seen that there is  two possible cases, l = r and l != r, And also that  model is nonlinear one.
                        
So, to apply karlman filter approach for the system, equations  should converted to a linearize one, like following                       
It can be derive that the At and Bt vectors as,
                                

So we have to calculate the two G's partial derivatives with respect to the state and control vectors

first we can define function G as following
then, G/ state can be calculate as,
                         




and G/control can be define as,

                                





          

















Finally we have to consider the observation of the system, since observation are distances and the angles to the landmark with respected to the robot position

 So,
                         
 

r and l can be define derive as following   when coordinate of the landmark is (Xm ,Ym) and coordinate of the sensor is (Xe ,Ye) 

                           

 So this is also non-linear and need to convert to linear model.
                             
                             





















It can be seen that landmarks does not integrated to the system model .since it is part of the system state,it is essential to integrated.


so as bellow calculation proven that initial model that we build does not affect much with land mark but state vector and other transition vector getting bigger every time sensor get new landmark to the system . This should be expected since It tend to build the local map.


 With this new state element system going to extend like this,

                       

then, equation will be,
 so that will affect to the calculation as




and








Now we can apply this calculation to the kalmarn filter

Comments

Post a Comment

Popular posts from this blog

Measuring 3D Distances between points using Kinect

Image
Now time to move on to some simple application from the kinect depth image. As I mentioned , Kinect keep track of depth values of all the pixels related to pixel position.  That points are corresponding to the real world instances of points. So every point on this depth image  can be considered as three dimensional vector as (X, Y, V) as below. X Value, Y Value, Z Value,  from the Cartesian coordinate system. We can demonstrate the point as 3 Dimensional vector  shown below, So if we denote two instant of point as Vector, We can easily calculate distances between them.   C = V1 – U1 Then we can get magnitude of the C, that’s mean the distance between Point1 and Point2. Before  Explaining the How I code , I will show the Code and Result,   Result has pretty good accuracy as I expected. To implement a vector, we can use PVector Data type form the processing with tree argument...
Read more

Implementation - Robot Navigation

Image
 Block Diagram Initialization of Pyro object  robot = Pyro4.Proxy("PYRONAME:example.robot") # use name server object lookup uri shortcut con =connect() #############..............................  Define particles  defining number of particles,initiating particles array and assigning initial position # Generate initial particles. Each particle is (x, y, theta).  number_of_particles = 25 start_state = np.array([500.0, 0.0, 45.0 / 180.0 * pi])  initial_particles = [copy.copy(Particle(start_state)) for _ in xrange(number_of_particles)] Initiating SLAM object fs = FastSLAM(initial_particles, robot_width, scanner_displacement, control_motion_factor, control_turn_factor, measurement_distance_stddev, measurement_angle_stddev, minimum_correspondence_likelihood)   Loop check whether incoming data is valid try :      while True:         ...
Read more

Full stack development of Robot navigation

Image
Hi everyone, Well, Developing a system that can behave intelligently in environment that previously unknown is challenging? because decision, it has to make is totally, well i can say mostly,? unpredictable before hand . Saying totally unpredictable is wrong which leads to unresolvable problem,So? that means every environment should have some way of predictability in order to get confident judgment about next move . As in controlled environment like assembly line next movement is measured and predictable , if we consider a car in the road , judgment of the next set of action can be predicted using the road signs , other vehicle movement , position and destination of the car and etc.. with tolerable perturbations handling . But if you consider the rover in the mars , only what system has is estimated model how environment could be , build using previous observation , that mean system has to predicted the action mostly according to the what it sens. So that sot of Automated...
Read more