Hi everyone,
I’m trying to use Isaac Navigation stack on my real robot. Everything seems working fine, Isaac receives FlatScan messages from the robot’s LiDAR, the odometry values and the local planner is set according to the real robot parameters (it’s a differential drive robot).
The robot localizes and navigates quite well; however, I have an issue with the output speed of the LQR controller. Speed in the forward direction often starts to bounce between its maximum value and 0 even when the robot is simply navigating in a relatively wide area and with a rectilinear trajectory. This causes the robot to move in a non-smooth and ugly way which is not acceptable.
The plot in the figure is the registered output of the LQR controller, you can see the undesired behavior in the time window between 100 and 150 seconds.
Two additional things that worth mentioning about my setup: the robot has two blind spots in the LiDAR readings on its left and on its right of about 90 degrees each. Moreover, the odometry is quite noisy.
Does someone know what causes this issue and a possible solution? I wonder if there are some parameters to change in order to avoid this behavior.
Unfortunately I do not have a magic solution to your problem, but let me try to give you some pointer to look at, and maybe if you provide more information we can further assist you:
My first question would be a clarification, you’re talking about LQR controller, however in our navigation stack, we use the LQR to produce a trajectory, and we then have a controller (PID based controller) that is in charge of following the trajectory.
So which part of the system is producing this output? How are you measuring this output?
If the issue comes from the LQR planner, the first things I would be looking at is what come from the odometry message (especially the speed). If the speed alternate between 0 and a big value, it could explain some of the issue. Make sure you have the flag reuse_lqr_plan on (should be the default value) as it could help.
If the issue comes from the controller, first you should make sure it comes from the controller, you can for example disable the flag use_pid_controller, this is probably not going to yield good result, but it’s to check that now the speed is smoother. You should also display in sight both the desired_speed and commanded_speed.
A possibility is that the PID controller saturate, this could happen if:
the PID is trying to control while the robot is not in autonomous mode (make sure manual_mode_channel is set properly (default is empty I believe), if the robot is always in autonomous mode, then you can ignore this).
The robot can’t physically execute the command, maybe the speed is too fast, or the acceleration is too big. In this case you can try to adjust some LQR gain, increase the control gain for example, or the acceleration’s gains.
I can’t help much more without more information unfortunately, please let me know what happens when you disable the pid controller, and whether the issue come from the LQR or controller.
