First off, I would highly recommend checking ROS (Robot Operating System) out, the ROS community includes researchers and enthusiasts as well as professionals from all around the world and thus most of what you need to solve the problem is already available there. You would have to just implement what others have done, on your system.
I, rather me and my team, have done the exact same thing as you want and the problem becomes much easier if you are using ROS.
There are pre-built SLAM packages available open source which you can use and for making maps and localizing your robot. Once you have the robot's position in a map that also contains all of the obstacles, you just have to do the path planning so that you do not hit any of the obstacles and reach your goal as fast as possible. Then break your path into neared goal points which are simpler to reach so that just by getting to these points your robot can get to the final goal.
As the problem you have put forth is a rather large problem, I feel it is hard to explain it completely here, but I will try to do as much as I can (When I get time I will add more links to help).
Let us break this problem into several parts,
(Don't worry about some occasional ROS jargon, you'll catch them during the tutorials).
- If you are unaware of ROS, I suggest taking a look at it and here is a place where you can learn it hands-on.
- Next you will have to get your sensors working with ROS, this should be easy as for most of them you will have
packages already made and open source.
- Once all your sensors are working with ROS, you will have
topics which contain have current sensor data, now you can do anything with them, i would use filters on the data and fuse to find your robots estimated pose. (You should also look into Kalman and Extended Kalman Filters at this point.)
- Now is the time when you can use some SLAM (Simultaneous Localization and Mapping) algorithms to have a map with obstacles and your robot at all times.
Do the Motion Planning, break the path into smaller pieces and feed them to your robot in the format it wants (You will need some knowledge about Control Systems at this point).
From my experience you can use these packages : gmapping, robot-poe-ekf, moveit. (Google these by adding ros)
I hope this helps, and all the best man.