Optimal control is one of the most powerful, important and advantageous topics in control engineering. The two challenges in every optimal control problem are defining the proper cost function and obtaining the best method to minimize it. In this study, innovative optimal control approaches are developed to solve the two problems of consensus and path planning in multi-agent systems (MASs). The consensus problem for general Linear-Time Invariant systems is solved by implementing an inverse optimal control approach which enables us to start by deriving a control law based on the stability and optimality condition and then according to the derived control define the cost function. We will see that this method in which the cost function is not specified a priori as the conventional optimal control design has the benefit that the resulting control law is guaranteed to be both stabilizing and optimal. Three new theorems in related linear algebra are developed to enable us to use the algorithm for all the general LTI systems. The designed optimal control is distributed and only needs local neighbor-to-neighbor information based on the communication topology to make the agents achieve consensus and track a desired trajectory. Path planning problem is solved for a group are Unmanned Aerial Vehicles (UAVs) that are assigned to track the fronts of a fires in a process of wildfire management. We use Partially Observable Markov Decision Process (POMDP) in order to minimize the cost function that is defined according to the tracking error. Here the challenge is designing the algorithm such that (1) the UAVs are able to make decisions autonomously on which fire front to track and (2) they are able to track the fire fronts which evolve over time in random directions. We will see that by defining proper models, the designed algorithms provides real-time calculation of control variables which enables the UAVs to track the fronts and find their way autonomously. Furthermore, by implementing Nominal Belief-state Optimization (NBO) method, the dynamic constraints of the UAVs is considered and challenges such as collision avoidance is addressed completely in the context of POMDP.