# Motion Planning for Mobile Robot with Modified BIT+
## 3 MPC Method
### Implementation of MPC
The obstacle constraints cannot be expressed directly in analytic form, and it may be not convenient to add the obstacle constrains in the optimization. So we designed the repulsive function to penalize the linear velocity and angle velocity that make the robot move towards the obstacle. Actually, we only consider the nearest obstacles on the left and right sides in front of the robot. \(g(d)\) is a function of the obstacle distance \(d\), which is positively correlated with \(d\). \(d_{1}\) and \(d_{2}\) represent the distances to the nearest obstacle on the left and right respectively. \(\theta_{1}\) and \(\theta_{2}\) denote the angle between the current velocity direction of the robot and the direction of the nearest obstacle on the left and right sides respectively. In our implementation, we choose the form of \(g(d)\) as follows:  
\[g(d)= \begin{cases}p*d+q&d\leq\text{threshold}\\ \infty&d>\text{threshold}\end{cases}  \tag{12}\]  
\(p\) and \(q\) are both positive constants. \(q\) is set so that \(g(d)\) is always greater than 0 and it is set to a small value. The obstacles beyond the threshold can be ignored. In order to avoid  
Figure 6: The definition of tracking error and yaw error. collision with obstacles, the robot need to choose appropriate angular velocity to prevent it from being too close to the obstacles. For example, in Figure 7, suppose the robot is more affected by the repulsive function of the obstacle on the left side, the angular velocity that makes the robot approach the left obstacle will be more penalized by the repulsive function. So the angular velocity is set to negative, making the robot turn right to avoid getting close to the obstacle. The last two terms of Equation (10) is to make the robot maintain a desired stable speed during the movement and slow down when approaching obstacles. The desired speed \(v_{d}\) can be set to a value smaller than the maximum speed of the robot according to different requirements. In addition, the robot needs to slow down according to the distance from the nearest obstacle. In summary, the parameters \(a_{1}\), \(a_{2}\), \(A_{3}\), \(a_{4}\), \(a_{5}\) and \(a_{6}\) in objective function determine different optimization objectives. For instance, when \(a_{1}\) and \(a_{2}\) are increased, the robot tends to follow the reference path. When \(a_{4}\) is increased, the robot is more likely to respond to the nearby obstacles, resulting in the actual path deviates from the reference path. Equation (11) are the constrains of the optimization problem. \(v_{max}\) and \(\omega_{max}\) are the maximum linear velocity and maximum angular velocity of the mobile robot respectively. \(a_{max}\) and \(\alpha_{max}\) are the maximum linear and angular acceleration respectively. These constraints are determined by the robot hardware and cannot be violated.With objective function (10) and constraints Equation (11), the optimal control inputs can be calculated by solving the nonlinear programming problem. In our work, we design the controller based on kinematics in ROS, and then send the linear velocity and angular velocity to the bottom controller of the robot in the simulation platform. The bottom controller based on the dynamics of the robot has been designed so we do not care about it here.