How Does the Backend MINCO Trajectory Ensure Nonholonomic Constraints After Kino A*?

[zhanghuzhenyu]

After generating a series of discrete waypoints using Kino A* in the frontend, how is it ensured that the trajectory optimized by the backend (using MINCO) does not violate the nonholonomic constraints of mobile platform?

Additionally, I noticed that the planning_horizon_ parameter is set to 1000 in kino_astar.cpp. Is the intention behind this setting to force the robot to always move forward?

[CK1201]

After generating a series of discrete waypoints using Kino A* in the frontend, how is it ensured that the trajectory optimized by the backend (using MINCO) does not violate the nonholonomic constraints of mobile platform?

Ensuring Nonholonomic Constraints with MINCO

The backend trajectory optimization using MINCO ensures that the nonholonomic constraints of the differential drive mobile platform are satisfied by deriving the platform's orientation directly from the velocity components of the generalized joint configuration.

The nonholonomic constraint for a differential drive robot is given by

$$\dot{q}_x\sin(\psi) - \dot{q}_y\cos(\psi) = 0.$$

In the backend, the trajectory is planned in the generalized joint configuration space $q=[q_x,q_y,q_m]$, . From the nonholonomic constraint, the orientation $\psi$ can be expressed as a function of the first-order derivatives of the generalized joint configuration:

$$\psi=atan2(\eta \dot{q}_y, \eta  \dot{q}_x).$$

By defining the robot's orientation $\psi$ in this way, the trajectory generated by MINCO inherently satisfies the nonholonomic constraint because the orientation is always consistent with the velocity direction of the platform. The optimizer effectively works with a state representation that implicitly embeds this constraint.

Purpose of planning_horizon_

planning_horizon_ is used to truncate the path for optimization, focusing on a segment rather than the entire trajectory to the final goal. This can significantly reduce computation time. However, in our code, re-planning is not utilized. Therefore, we've set planning_horizon_ to a large value (1000) to ensure that the optimizer considers the path all the way to the intended destination. The intention isn't specifically to force the robot to "always move forward" in a general sense, but rather to allow the planner to optimize the complete path to the goal in a single planning instance.