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

文摘科技 2024-11-20 07:30 山东

在运动控制中，轨迹执行时间的预测对于多轴的协同、路径预处理等方面有着重要的作用。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 OFSTATE_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_IFEND_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);

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

获取更多可视化程序案例：点赞+在看，回复20241120获取

工控大侠

关注我，持续分享工控技术干货、行业动态！笔者20余年的自动化从业经验相信可以帮到您，有问题可留言。

最新文章