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

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

SMC_SetForecast

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

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

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

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

范围	名称	数据类型	说明
输入/输出	Axis	AXIS_REF_SM3	指定要操作的轴
输入	Execute	BOOL	上升沿：设置预测持续时间
	ForecastDuration	LREAL	轨迹预测的持续时间（以秒为单位）
输出	Done	BOOL	TRUE:预测已成功设置
	Error	BOOL	TRUE: 在执行过程中发生了错误
	ErrorID	SMC_ERROR	错误ID

SMC_GetForecast

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

范围	名称	数据类型	说明
输入/输出	Axis	AXIS_REF_SM3	指定要操作的轴
输入	Enable	BOOL	TRUE: 预测是持续读取的
输出	Valid	BOOL	TRUE: 预测已成功读取
	Busy	BOOL	TRUE: 正在执行中
	Error	BOOL	TRUE: 正在运行中
	ErrorID	SMC_ERROR	执行过程中出现了错误
	ForecastDuration	LREAL	预测持续时间以秒为单位

SMC_GetTravelTime

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

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

范围	名称	数据类型	说明
输入/输出	Axis	AXIS_REF_SM3	指定要操作的轴
输入	Enable	BOOL	使能功能块
	Position	LREAL	指定轴的位置
	Direction	SMC_CAMTAPPETTYPE	功能块对轴的移动方向是有要求的，它只关注轴按照预设方向到达位置的情况
输出	Valid	BOOL	输出的持续时间是否有效
	Duration	LREAL	以秒为单位的从当前控制周期开始到达特定位置的预计时间，具有亚周期精度。 预计时间为0秒表示轴已在上一个周期到达位置，而等于任务间隔的预计时间则意味着轴将在当前周期内到达位置。
	Error	BOOL	是否发生了错误
	ErrorId	SMC_ERROR	错误ID

SMC_ReadSetValues

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

范围	名称	数据类型	说明
输入/输出	Axis	AXIS_REF_SM3	指定要操作的轴
输入	Enable	BOOL	使能功能块
	TimeOffset	LREAL	时间偏移量，必须位于0和使用SMC_SetForecast设置的预测时长（ForecastDuration）之间。 如果时间偏移量超出此范围，将报告错误SMC_INVALID_PARAMETER。
输出	Valid	BOOL	TRUE: 输出有效标志
	Busy	BOOL	TRUE: 正在执行中
	Error	BOOL	TRUE: 发生错误
	ErrorID	SMC_ERROR	错误ＩＤ
	Position	LREAL	位置信息
	Velocity	LREAL	速度信息
	Acceleration	LREAL	加速度信息
	Jerk	LREAL	加加速度信息

程序示例

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);

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