运动控制中如何实现路径轨迹预测

在运动控制中，轨迹执行时间的预测对于多轴的协同、路径预处理等方面有着重要的作用。CODESYS系列控制器提供了预测轨迹执行时间功能块。要实现完整的预测功能，需要使用到以下的功能块。

SMC_SetForecast

这个功能块专门用于为指定轴设定预测，其预测时长被限定在最多512个任务周期。通过这种预测设置，用户能够预先获取轴在未来特定时间点的状态信息，包括：

持续时间预测：利用SMC_GetTravelTime功能，可以计算出轴到达某一特定位置所需的时间。

动态信息获取：通过SMC_ReadSetValues，能够在特定时间点获取轴的动态信息。

预测时长查询：使用SMC_GetForecast可以读取当前的预测时长。

SMC_GetForecast

该功能块返回给定轴的预测时间，是由SMC_SetForecast 功能块设置的。

SMC_GetTravelTime

此功能块能够预测轴到达指定位置所需的时间。

要获取有效的时间预测，需要先通过SMC_SetForecast设置预测持续时间，确保轴在预测时间内能到达指定位置。如果轴没有移动，即使它正好在指定位置，Valid也会显示为FALSE。

SMC_ReadSetValues

该功能块用于读取轴的设定位置、速度、加速度和加加速度。允许设置一个正的时间偏移量（TimeOffset），以读取未来的设定值。当时间偏移量（TimeOffset）= 0时，将读取当前周期开始时的设定值。

程序示例

CASE state OF
STATE_POWER_DRIVE:    //轴使能
  mcp.Enable := TRUE;
  mcp.bDriveStart := TRUE;
  mcp.bRegulatorOn := TRUE;


  IF mcp.Status THEN
    state := STATE_SET_FORECAST;
  ELSIF mcp.Error THEN
    error := mcp.ErrorID;
    state := state + STATE_ERROR;  
  END_IF


STATE_SET_FORECAST:    //设置轨迹预测时长
  setForecast.Execute := TRUE;
  setForecast.ForecastDuration := 0.3;  //0.3S


  getForecast.Enable := TRUE;


  IF setForecast.Done THEN
    state := STATE_MOVE_TO_POSITION;
  ELSIF setForecast.Error THEN
    error := setForecast.ErrorID;
    state := state + STATE_ERROR;  
  END_IF


STATE_MOVE_TO_POSITION:    //MC_MoveAbsolute
  ma.Execute := TRUE;
  ma.Position := 10;
  ma.Velocity := 20;
  ma.Acceleration := 200;
  ma.Deceleration := 200;
  ma.Jerk := 1000;


  timeToPosition.Enable := TRUE;
  timeToPosition.Position := 8;  //预测到达8时，所用的时间


  IF ma.Error THEN
    error := ma.ErrorID;
    state := state + STATE_ERROR;  
  END_IF
END_CASE


setForecast(Axis:= Drive);
getForecast(Axis:= Drive);


mcp(Axis:= Drive);
ma(Axis:= Drive);


timeToPosition(Axis:= Drive);
readSetValues(
  Axis:= Drive, 
  Enable:= timeToPosition.Valid, 
TimeOffset:=timeToPosition.Duration);

可通过采集到的波形图观察对于轨迹预测的效果。