E Motion Control and Planning for Szenergy - szenergy/szenergy-public-resources GitHub Wiki

Speed Planning and Control Longitudinal planning happens in the trajectory_planner node, while some limitations and modifications are done in the motion_control. The control command message contains a limited, gradient limited speed target. The low level controller controls the vehicle speed in closed loop. The planner and the control nodes do the following:

  1. trajectory_planner
  • has two parameters: planner/maximumDrivingSpeed (default: 3 m/s) and planner/maximumApproachSpeed (default 0.75 m/s).
  • the planner takes these two parameters and use them as GLOBAL MAXIMUM VELOCITIES for the following states:
  • planner/maximumDrivingSpeed value for DriveOff and Driving
  • planner/maximumApproachSpeed for Approach
  • 0 m/s for all other states (Stop and Standby).
  • as a final step, the planner makes a limitation (the resulting speed must not be higher than the GLOBAL MAXIMUM VELOCITIES):
  • based on the length of the trajectory in front of the vehicle, the speed is reduced.
  • for this, the motionControl/minimumAcceleration parameter is used (default -0.3 m/s^2). This parameter represents the vehicle capabilities to decelerate. If the remaining length of the trajectory is shorter than the distance needed to stop from the given vehicles speed, considering the minimum acceleration, then the target speed is reduced accordingly.
  1. motion_control
  • the target speed planned by the planner is included in the trajectory_coefficients topic, its 5th element represents the target speed in m/s.
  • the motion controller takes this value and outputs it.
  • as a last step, the planned speed is gradient limited, considering the motionControl/minimumAcceleration and motionControl/maximumAcceleration parameters. This ensures harsh acceleration and braking.