[发明专利]一种通过插入运动段实现机器人平滑运动的控制方法

权利要求书

1.一种通过插入运动段实现机器人平滑运动的控制方法，其特征在于，包括以下步骤：

步骤1：判断机器人实际位置是否偏离理论轨迹，如果是，则根据实际位置及当前规划的命令位置插入运动段；否则，按照原轨迹正常规划；

步骤2：按照插入运动段后的轨迹进行规划；

所述判断机器人实际位置是否偏离理论轨迹包括以下过程：

步骤11：采集各方向实际位置，并计算实际位置与命令位置之间的实际轨迹方向矢量；

步骤12：根据实际轨迹方向矢量与理论轨迹方向矢量，计算两者之间夹角；

步骤13：如果实际轨迹方向矢量与理论轨迹方向矢量的夹角小于系统参数配置所确定的允许角度，则正常进行轨迹规划；否则计算实际位置与命令位置之间的实际轨迹方向矢量的模的大小，并继续判断实际位置与命令位置之间的实际轨迹方向矢量的模的大小是否大于系统参数配置所确定的允许长度；

步骤14：如果实际位置与命令位置之间的实际轨迹方向矢量的模的大小大于系统参数配置所确定的允许长度，则根据实际位置及当前规划的命令位置插入运动段；否则，按照原轨迹正常规划；

所述实际位置与命令位置之间的实际轨迹方向矢量为：

其中，设命令位置坐标为P(x,y)，实际位置坐标为P'(x',y')；

所述实际轨迹方向矢量与理论轨迹方向矢量的夹角包括以下计算过程：

设运动段的起点坐标为O(x0,y0)，则理论轨迹方向矢量为：

矢量的单位化：

矢量的夹角θ为：

θ＝acos(d)

其中，插入运动段前理论轨迹的命令位置P(x,y)，插入运动段前理论轨迹的实际位置P'(x',y')，为矢量的模，为矢量的模，d为矢量单位化矢量与的数量积，θ为实际轨迹方向矢量与理论轨迹方向矢量间的夹角，acos(d)为计算d的反余弦函数；

所述根据实际位置及当前规划的命令位置插入运动段包括以下过程：修改插入运动段前的理论轨迹信息，然后初始化所插入的运动段信息，形成插入运动段后的理论轨迹；

所述修改插入运动段前的理论轨迹信息，然后初始化所插入的运动段信息，形成插入运动段后的理论轨迹；

插入运动段前的理论轨迹的修改包括：将插入运动段前的理论轨迹的起点修改为当前的规划的命令位置；将规划速度修改为零；将规划长度修改为从起点到目标点的距离；将已规划长度修改为零；

所述初始化所插入的运动段信息包括：

设置插入运动段的起点位置为当前反馈位置；设置目标位置为插入运动段前的目标位置；设置当前规划速度、加速度为插入运动段前的规划速度、加速度；设置规划长度为从起点到目标点的距离；设置已规划长度为零。