Tremendous industrial and academic progress and investments have been made in au-tonomous driving, but still many aspects are unknown and require further investigation,development and testing. A key part of an autonomous driving system is an efficient plan-ning algorithm with potential to reduce accidents, or even unpleasant and stressful drivingexperience. A higher degree of automated planning also makes it possible to have a betterenergy management strategy with improved performance through analysis of surroundingenvironment of autonomous vehicles and taking action in a timely manner. This thesis deals with planning of autonomous vehicles in different urban scenarios, road,and vehicle conditions. The main concerns in designing the planning algorithms, are realtime capability, safety and comfort. The planning algorithms developed in this thesis aretested in simulation traffic situations with multiple moving vehicles as obstacles. The re-search conducted in this thesis falls mainly into two parts, the first part investigates decou-pled trajectory planning algorithms with a focus on speed planning, and the second sectionexplores different coupled planning algorithms in spatiotemporal environments where pathand speed are calculated simultaneously. Additionally, a behavioral analysis is carried outto evaluate different tactical maneuvers the autonomous vehicle can have considering theinitial states of the ego and surrounding vehicles. Particularly relevant for heavy duty vehicles, the issues addressed in designing a safe speedplanner in the first part are road conditions such as banking, friction, road curvature andvehicle characteristics. The vehicle constraints on acceleration, jerk, steering, steer ratelimitations and other safety limitations such as rollover are further considerations in speedplanning algorithms. For real time purposes, a minimum working roll model is identified us-ing roll angle and lateral acceleration data collected in a heavy duty truck. In the decoupledplanners, collision avoiding is treated using a search and optimization based planner. In an autonomous vehicle, the structure of the road network is known to the vehicle throughmapping applications. Therefore, this key property can be used in planning algorithms toincrease efficiency. The second part of the thesis, is focused on handling moving obstaclesin a spatiotemporal environment and collision-free planning in complex urban structures.Spatiotemporal planning holds the benefits of exhaustive search and has advantages com-pared to decoupled planning, but the search space in spatiotemporal planning is complex.Support vector machine is used to simplify the search problem to make it more efficient.A SVM classifies the surrounding obstacles into two categories and efficiently calculate anobstacle free region for the ego vehicle. The formulation achieved by solving SVM, con-tains information about the initial point, destination, stationary and moving obstacles.These features, combined with smoothness property of the Gaussian kernel used in SVMformulation is proven to be able to solve complex planning missions in a safe way. Here, three algorithms are developed by taking advantages of SVM formulation, a greedysearch algorithm, an A* lattice based planner and a geometrical based planner. One general property used in all three algorithms is reduced search space through using SVM. In A*lattice based planner, significant improvement in calculation time, is achieved by using theinformation from SVM formulation to calculate a heuristic for planning. Using this heuristic,the planning algorithm treats a simple driving scenario and a complex urban structureequal, as the structure of the road network is included in SVM solution. Inspired byobserving significant improvements in calculation time using SVM heuristic and combiningthe collision information from SVM surfaces and smoothness property, a geometrical planneris proposed that leads to further improvements in calculation time. Realistic driving scenarios such as roundabouts, intersections and takeover maneuvers areused, to test the performance of the proposed algorithms in simulation. Different roadconditions with large banking, low friction and high curvature, and vehicles prone to safetyissues, specially rollover, are evaluated to calculate the speed profile limits. The trajectoriesachieved by the proposed algorithms are compared to profiles calculated by optimal controlsolutions.