I have a field filled with obstacles, I know where they are located, and I know the robot's position. Using a path-finding algorithm, I calculate a path for the robot to follow.
Now my problem is, I am guiding the robot from grid to grid but this creates a not-so-smooth motion. I start at A, turn the nose to point B, move straight until I reach point B, rinse and repeat until the final point is reached.
So my question is: What kind of techniques are used for navigating in such an environment so that I get a smooth motion?
The robot has two wheels and two motors. I change the direction of the motor by turning the motors in reverse.
EDIT: I can vary the speed of the motors basically the robot is an arduino plus ardumoto, I can supply values between 0-255 to the motors on either direction.
You need feedback linearization for a differentially driven robot. This document explains it in Section 2.2. I've included relevant portions below:
The simulated robot required for the
project is a differential drive robot
with a bounded velocity. Since
the differential drive robots are
nonholonomic, the students are encouraged to use feedback linearization to
convert the kinematic control output
from their algorithms to control the
differential drive robots. The
transformation follows:
where v, ω, x, y are the linear,
angular, and kinematic velocities. L
is an offset length proportional to the
wheel base dimension of the robot.
One control algorithm I've had pretty good results with is pure pursuit. Basically, the robot attempts to move to a point along the path a fixed distance ahead of the robot. So as the robot moves along the path, the look ahead point also advances. The algorithm compensates for non-holonomic constraints by modeling possible paths as arcs.
Larger look ahead distances will create smoother movement. However, larger look ahead distances will cause the robot to cut corners, which may collide with obstacles. You can fix this problem by implementing ideas from a reactive control algorithm called Vector Field Histogram (VFH). VFH basically pushes the robot away from close walls. While this normally uses a range finding sensor of some sort, you can extrapolate the relative locations of the obstacles since you know the robot pose and the obstacle locations.
My initial thoughts on this(I'm at work so can't spend too much time):
It depends how tight you want or need your corners to be (which would depend on how much distance your path finder gives you from the obstacles)
Given the width of the robot you can calculate the turning radius given the speeds for each wheel. Assuming you want to go as fast as possible and that skidding isn't an issue, you will always keep the outside wheel at 255 and reduce the inside wheel down to the speed that gives you the required turning radius.
Given the angle for any particular turn on your path and the turning radius that you will use, you can work out the distance from that node where you will slow down the inside wheel.
An optimization approach is a very general way to handle this.
Use your calculated path as input to a generic non-linear optimization algorithm (your choice!) with a cost function made up of closeness of the answer trajectory to the input trajectory as well as adherence to non-holonomic constraints, and any other constraints you want to enforce (e.g. staying away from the obstacles). The optimization algorithm can also be initialised with a trajectory constructed from the original trajectory.
Marc Toussaint's robotics course notes are a good source for this type of approach. See in particular lecture 7:
http://userpage.fu-berlin.de/mtoussai/teaching/10-robotics/
Related
An issue arises with contact modeling in robotics simulation package drake.
I try to position-control an iiwa manipulator to affect a body, attached to a screw joint.
What I expect is the nut progressing downwards. What I see is the end-effector sliding around the nut, unable to induce a nuts rotation along the bolt.
note: This is a continued investigation into this question.
The simplifying experiment is to use ExternallyAppliedSpatialForce, instead of the manipulator.
I apply torque around the bolts's Z axis in the centre of bolts axis. The amount of torque is 1 Nm. And, this produces the desired effect:
Then, I stop artificial force application, and try manipulation again, combined with the contact visualization by ContactResultsToMeshcat:
I also print the contact force magnitudes. Forces are in the range of 50-200 N.
But the nut does not move.
My question is, where to take the investigation next, when the 1 Nm torque is able to turn a screw joint, while the 200N force cannot?
So far, after watching 10 repeats of the visualization, I notice that only one finger of end-effector is in contact at any given moment. So, I think, I must prepare less sloppy trajectories for the fingers of end effector [and for the whole arm, for that matter].
It could be the inverse dynamics controller used in ManipulationStation, which is different from (and softer than) iiwa’s real controller, that’s making the robot’s joints too soft and unable to turn the nut.
Specifically, the inverse dynamics controller scales the joint stiffness matrix by the mass matrix of the robot, whereas iiwa's real controller does not do that.
The problem most likely was in having actuation port of bolt-and-nut model not connected to any input port and hence not simulated properly (more details here).
After fix the simulation looks that way:
I have a kuka iiwa (7 joint robot arm). Attached to it is a circular aluminum platter with a steel ball on it. The goal of the project (for giggles/ challenge) was to use the internal torque sensors of the robot in order to balance the ball in the middle of the platter. Due to the fact that I was unable/ not allowed to use FRI (fast robot interface) whereby I can control the robot from C at about 3ms feedback loop, I can only update the robot position at about 4Hz... My quick and dirty solution consisted of the following:
Measure the torque on the final two axes of the arm and apply a mapping to generate the ball's position (filtration and hysteresis were well implemented to improve the data quality). If the ball velocity was sufficiently stable, generate a motion that would cancel out that velocity (with an impulse "go to angle and return to neutral position" motion). Overlaid on that was also a small proportional gain which would tend the ball towards the center of the platter.
My question: What is the professional/ correct solution to this situation (where your controller can only hit the system with impulses rather than continuous feedback)?
Here is a picture of the setup:
A slightly dampened negative feedback loop.
Where bal.posX dictates strength y.rot.arm ?
https://en.wikipedia.org/wiki/Control_system
I once coded something alike it, my colleague who's into PLC's called it that.
I did fuzzy logic optimization then..
I have four cameras which are at fixed position. So I can measure the distance (even rotation) among them using a ruler (physically). Camera one and two gives me a point cloud and camera three and four gives me another point cloud. I need to merge these point clouds.
As far I have understood that ICP and other such algorithms do a rigid transformation of one point cloud to match the other. My question is how can I use the extra knowledge (distance between the cameras in centimeter) to do this transformation.
I am quite new to such work so, please correct me if I misunderstood something. And thanks in advance.
First, what kind of accuracy are you looking for, and over what volume of space? Achieving 0.1 mm registration accuracy over a 0.5 m tabletop scene is a completely different problem (in terms of mechanical design and constraints) than a few mm over a floor tens of meters wide.
Generally speaking, with a well reconstructed and unambiguous object shape, ICP will always give you a better solution than manual measurements.
If the cameras are static, then what you have is really a calibration problem, and you need to calibrate your 4-camera rig only at setup and when its configuration changes for whatever reason.
I suggest using a calibration object of precisely known size and geometry, e.g. a machined polyhedron. You can generate and ICP-register point clouds for it, then fit the merged cloud to the known geometry, thus obtaining position and orientation of every individual point cloud with respect to the fixed object. From these you can work out the poses of the cameras w.r.t. each other.
How can I implement the A* algorithm on a gridless 2D plane with no nodes or cells? I need the object to maneuver around a relatively high number of static and moving obstacles in the way of the goal.
My current implementation is to create eight points around the object and treat them as the centers of imaginary adjacent squares that might be a potential position for the object. Then I calculate the heuristic function for each and select the best. The distances between the starting point and the movement point, and between the movement point and the goal I calculate the normal way with the Pythagorean theorem. The problem is that this way the object often ignores all obstacle and even more often gets stuck moving back and forth between two positions.
I realize how silly mu question might seem, but any help is appreciated.
Create an imaginary grid at whatever resolution is suitable for your problem: As coarse grained as possible for good performance but fine-grained enough to find (desirable) gaps between obstacles. Your grid might relate to a quadtree with your obstacle objects as well.
Execute A* over the grid. The grid may even be pre-populated with useful information like proximity to static obstacles. Once you have a path along the grid squares, post-process that path into a sequence of waypoints wherever there's an inflection in the path. Then travel along the lines between the waypoints.
By the way, you do not need the actual distance (c.f. your mention of Pythagorean theorem): A* works fine with an estimate of the distance. Manhattan distance is a popular choice: |dx| + |dy|. If your grid game allows diagonal movement (or the grid is "fake"), simply max(|dx|, |dy|) is probably sufficient.
Uh. The first thing that come to my mind is, that at each point you need to calculate the gradient or vector to find out the direction to go in the next step. Then you move by a small epsilon and redo.
This basically creates a grid for you, you could vary the cell size by choosing a small epsilon. By doing this instead of using a fixed grid you should be able to calculate even with small degrees in each step -- smaller then 45° from your 8-point example.
Theoretically you might be able to solve the formulas symbolically (eps against 0), which could lead to on optimal solution... just a thought.
How are the obstacles represented? Are they polygons? You can then use the polygon vertices as nodes. If the obstacles are not represented as polygons, you could generate some sort of convex hull around them, and use its vertices for navigation. EDIT: I just realized, you mentioned that you have to navigate around a relatively high number of obstacles. Using the obstacle vertices might be infeasible with to many obstacles.
I do not know about moving obstacles, I believe A* doesn't find an optimal path with moving obstacles.
You mention that your object moves back and fourth - A* should not do this. A* visits each movement point only once. This could be an artifact of generating movement points on the fly, or from the moving obstacles.
I remember encountering this problem in college, but we didn't use an A* search. I can't remember the exact details of the math but I can give you the basic idea. Maybe someone else can be more detailed.
We're going to create a potential field out of your playing area that an object can follow.
Take your playing field and tilt or warp it so that the start point is at the highest point, and the goal is at the lowest point.
Poke a potential well down into the goal, to reinforce that it's a destination.
For every obstacle, create a potential hill. For non-point obstacles, which yours are, the potential field can increase asymptotically at the edges of the obstacle.
Now imagine your object as a marble. If you placed it at the starting point, it should roll down the playing field, around obstacles, and fall into the goal.
The hard part, the math I don't remember, is the equations that represent each of these bumps and wells. If you figure that out, add them together to get your final field, then do some vector calculus to find the gradient (just like towi said) and that's the direction you want to go at any step. Hopefully this method is fast enough that you can recalculate it at every step, since your obstacles move.
Sounds like you're implementing The Wumpus game based on Norvig and Russel's discussion of A* in Artifical Intelligence: A Modern Approach, or something very similar.
If so, you'll probably need to incorporate obstacle detection as part of your heuristic function (hence you'll need to have sensors that alert your agent to the signs of obstacles, as seen here).
To solve the back and forth issue, you may need to store the traveled path so you can tell if you've already been to a location and have the heurisitic function examine the past N number of moves (say 4) and use that as a tie-breaker (i.e. if I can go north and east from here, and my last 4 moves have been east, west, east, west, go north this time)
I'm writing a simulation in which a creature object should be able to move towards some other arbitrary object in the environment, sliding around obstacles rather than doing any intelligent pathfinding. I'm not trying to have it plan a path -- just to move in one general direction, and bounce around obstacles.
It's a 2D environment (overhead view), and every object has a bounding rectangle for collision detection. There is no grid, and I am not looking for A* solution.
I haven't been able to find any tutorials on this kind of "dumb" collision-based pathfinding, so I might not be describing this using the most common terms.
Any recommendations on how to implement this (or links to tutorials)?
Expanding on what Guillaume said about obstacle avoidance, a technique that would work well for you is anti-gravity movement. You treat local obstacles as point sources of antigravity, the destination as gravity, and your computer controlled character will slip (like soap!) around the obstacles to get to the destination.
you can combine two steering algorithm :
seek : you apply a steering force in the direction which is the difference between the current velocity and the desired velocity towards the target
Obstacle Avoidance : you anticipates the vehicle's future using a box whose length is a constant time multiplied by the current velocity of the vehicle. Any obstacle that intersects this box is a potential collision threat. The nearest such threat is chosen for avoidance. To avoid an obstacle, a lateral steering force is applied opposite to the obstacle's center. In addition, a braking (deceleration) force is applied. These forces vary with urgency (the distance from the tip of the box to the point of potential collision). Steering varies linearly, braking varies quadratically.
You can find more on the website "Steering Behaviors For Autonomous Characters"
regards
Guillaume
PS : this assume you're using a point/velocity/acceleration method for the object's movement.
Maybe you could use Pledge's algorithm
Whenever your creature, travelling in vector direction v, collides with a wall whose direction is represented by vector w, the direction that you need to "slide" is given by the vector that is the projection of v onto w. This can be found using
v . w
--------- w
|w|*|w|
where . is the vector dot product and |w| is the magnitude of vector w ( = sqrt(w . w)). If w is a unit vector, this becomes simply
(v . w) w
Using the resulting vector as your creature's speed will mean your creature travels quickly when it just "grazes" the wall, and slowly when it hits the wall nearly dead-on. (This is how most first-person shooter games manage collisions for the human player.)
If instead you want your creature to always travel at full speed, you only need the sign of v . w -- you will always be travelling either in the direction the wall faces (w) or the opposite direction (-w).
The issue that you will have is when your creature hits the wall dead-on. In that case your projected vector will be (0, 0), and you need some other technique to decide which way (w or -w) to go. The usual approach here is A*, although this may be unnecessary if your environment possesses enough structure.
I posted a pathfinding algorithm in C# a while back
Here's the link
You can try and use it as a starting point, ie, you could modify the function that checks the next cell to see if it's valid to check for the obstacles, and you could feed it small intervals instead of the starting and end points, kinda like multiple mini-pahfinding routes.
(The text is in spanish, but you can download the application from the link at the top)