[发明专利]一种机器人关节空间传送带跟随运动的轨迹规划方法

权利要求书

1.一种机器人关节空间传送带跟随运动的轨迹规划方法，其步骤如下：

步骤一，根据视觉模块输出的视觉坐标系下目标物体的当前位置(x_vision,y_vision,z_vision)，通过坐标系转换获得目标物体在直角坐标系下的位置(x_base,y_base,z_base)：

(x_base,y_base,z_base,1)^T＝^baseT_vision·(x_vision,y_vision,z_vision,1)^T

式中，^baseT_vision为视觉坐标系相对于直角坐标系的坐标转换矩阵；

通过编码器实时反馈的脉冲数据，采用差分的方法获得目标物体在直角坐标系下的线速度v_base；

步骤二，预测目标物体t时刻所在的位置(x_t,y_t,z_t)：

(xt,yt,zt)=(x0,y0,z0)+vconvery·t+12·aconvery·t2]]>

其中：t＝f(Δs,ΔV)，(x₀,y₀,z₀)目标物体t时刻所在的位置；v_convery为传送带的运动速度，a_convery为传送带的运动加速度，Δs是机器人与目标物体当前的距离，ΔV是机器人与目标物体速度矢量差；

步骤三，根据目标物体在机器人直角坐标系下的位置(x_base,y_base,z_base)和速度矢量v_base，将目标物体位置逆解至关节空间：位置逆解通过运动学逆解模型，得到各个关节的目标位置q；采用雅克比矩阵的方法，得到各个关节目标速度矢量

q·=J-1(q)·V]]>

式中，J^-1(q)为雅克比矩阵的逆矩阵，V为笛卡尔速度；

步骤四，根据关节初始的位置q₀和速度矢量以及各个关节的目标位置q和目标速度矢量在关节空间内进行关节轨迹规划，步骤如下：

①各关节以梯形加减速进行轨迹规划：根据关节位移q-q₀和初速度矢量末速度矢量规划出速度曲线，以最大加速度进行加减速，得出各关节规划的最短时间T_imin；

②判定各关节该速度规划下是否存在死区，若存在，给出死区的左右边界T_left、T_right；

③以max{T_imin,i＝1…n}为基准时间，其中n为关节数,若该时间在某一关节的死区内，则以该关节死区的右边界T_right为轨迹规划的基准时间，依次判断各关节，最终得到跨越了各关节死区的最优的轨迹规划时间T_min；

④以最优的轨迹规划时间T_min为轨迹规划时间，拓展其他关节的轨迹规划时间，重新对各个关节轨迹进行规划；

步骤五，根据步骤四所作的关节轨迹规划，实时输出各关节的目标位置，再通过脉冲输出模块，转换为各个关节对应的电机脉冲值，驱动机器人运动至目标位置，实现实时动态地跟踪目标物体。