[发明专利]一种铸锭过程动态除渣的机器人轨迹规划方法

权利要求书

1.一种铸锭过程动态除渣的机器人轨迹规划方法，包括铸模流水线和设置在铸模流水线一侧的机器人，所述的铸模流水线用于传输多个铸模朝一个方向运动，所述机器人包括六自由度机器人本体和设置于机器人末端作为末端执行器的扒渣铲，铸模流水线正上方设置有相机，其特征在于，所述方法包括下列步骤：

S1：利用相机得到铸模图像信息，以铸模靠近机器人一侧边框的中点A在图像中的图像像素坐标为(u_A,v_A)，计算点A在以铸模流水线为参考物设定的基础坐标系下的位置(⁰x_A,⁰y_A,⁰z_A)：

S2：利用时间间隔为Δt的两帧图像中点A位置计算点A的实时速度：

其中Δ⁰x_A,Δ⁰y_A,Δ⁰z_A表示点A在时间间隔为Δt的两帧图像中移动的距离；

S3：以轨迹规划算法的计算时间为Δt_cal、工业机器人的初始关节角度为q_st＝[q_st1 q_st2q_st3 q_st4 q_st5 q_st6]^T和初始关节速度为其中q_st1、q_st2、q_st3、q_st4、q_st5和q_st6分别是六自由度机器人本体六个关节的初始关节角度；

S4：确定搜索区间，选取铸模流水线上离机器人最近的点a，计算机器人以关节最大速度从初始位置运动到该处的时间t_a，再过a点向铸模流水线另一侧作垂线，以垂线与铸模流水线中线的交点为点b，计算机器人以三分之一关节最大速度从初始位置运动到该处的时间t_b，由此构成搜索区间[t_a,t_b]；

S5：以机器人运行机械臂末端从初始位置运动到点A的轨迹的执行时间为t_exe＝(t_a+t_b)/2；

S6：经过时间间隔Δt₁＝Δt_cal+Δt_exe后，机器人跟踪上了铸模目标点A，即扒渣铲到达目标点A的位置并且与铸模保持相对静止，计算此时点A的位置坐标(⁰x′_A,⁰y′_A,⁰z′_A)：

S7：运用机器人逆运动学求解扒渣铲在位置坐标(⁰x′_A,⁰y′_A,⁰z′_A)与水平面成向下的30°夹角时的关节角度q_f＝f^-1(y_A)，其中y_A表示机器人位置坐标的矩阵，f^-1表示求解函数，并根据能量最优法则选取最优解q_fmin；

S8：根据上述已知条件进行五次多项式插值法来完成关节空间轨迹规划，得到轨迹q(t)；

S9：对q(t)求导以及二次求导得到和其中和分别表示关节角速度以及关节角加速度，判断其是否满足速度约束和加速度约束，即：

如果满足则t_b＝t_exe，否则t_a＝t_exe，其中和是所使用的机器人能够实现的最小与最大角速度及角加速度；

S10：重复步骤S5至S9，直至搜索区间[t_a,t_b]足够小，即t_b-t_a≤Δε，其中Δε是预设的允许误差，则求得满足机器人关节空间物理约束的最小时间mint_exe，并输出对应的轨迹q(t),t∈(0,Δt₁)；

S11：已知浮渣的分布情况和厚度信息，即已知铸模两侧边框距离浮渣的水平宽度分别是y_l和y_r，熔融液态金属离铸模顶部的距离为h_f，氧化浮渣陷入熔融液态金属的最大深度为h，铸模长边的长度也已知，表示为L_m，由此设计任务空间扒渣轨迹为多段匀速直线运动，其端点分别为点B、C、D、E，其中点B为浮渣相应坐标分别是：

[⁰x_B⁰y_B⁰z_B]^T＝[⁰x_A⁰y_A+y_l⁰z_A-h_f]^T

[⁰x_C⁰y_C⁰z_C]^T＝[⁰x_A⁰y_A+y_l⁰z_A-h_f-h]^T

[⁰x_D⁰y_D⁰z_D]^T＝[⁰x_A⁰y_A+L_m-y_r⁰z_A-h_f-h]^T

[⁰x_E⁰y_E⁰z_E]^T＝＝[⁰x_A⁰y_A+L_m-y_r⁰z_A-h_f]^T；

其中T为矩阵转置符号；

S12：设扒渣铲深入铸模到达氧化渣底部这一阶段总时间为Δt₂＝t₂-Δt₁，t₂代表该阶段结束时刻，各个部分的运动时间分别为Δt₂₁、Δt₂₂、Δt₂₃和Δt₂₄，由于铸模随流水线运动，则实际轨迹点B’、C’、D’、E’坐标为：

[⁰x′_B⁰y′_B⁰z′_B]^T＝[⁰x′_A⁰y′_A+y_l⁰z′_A-h_f]^T+⁰v_A·Δt₂₁

[⁰x′_C⁰y′_C⁰z′_C]^T＝[⁰x′_A⁰y′_A+y_l⁰z′_A-h_f-h]^T+⁰v_A·(Δt₂₁+Δt₂₂)

[⁰x′_D⁰y′_D⁰z′_D]^T＝[⁰x′_A⁰y′_A+L_m-y_r⁰z′_A-h_f-h]^T+⁰v_A·(Δt₂₁+Δt₂₂+Δt₂₃)

[⁰x′_E⁰y′_E⁰z′_E]^T＝＝[⁰x′_A 0y′_A+L_m-y_r⁰z′_A-h_f]^T+⁰v_A·(Δt₂₁+Δt₂₂+Δt₂₃+Δt₂₄)；

S13：使用三次非均匀B样条曲线插值对机器人进行关节空间的运动轨迹规划得到q(t),t∈(Δt₁,t₂)。