Mobile robot navigation with obstacle avoidance based on the nonlinear least squares optimization method using the cost function and the sub-goal switching

We define the mobile robot navigation problem as an optimization problem to minimize the cost function with the pose error between the goal position and the position of a mobile robot. Using Gauss-Newton method for optimization, the optimal speeds of the left and right wheels can be found as the solution of the optimization problem. Especially, the rotational speed of wheels of a mobile robot can be directly related to the overall speed of a mobile using the Jacobian derived from the kinematic model. When the robot detects the obstacle using sensors, the sub-goal switching method is adopted for the efficient obstacle avoidance during the navigation. The performance was evaluated using the simulation and the simulation results show the validity of the proposed method.