一种基于力矩控制的工业机器人避障轨迹规划方法与流程
本发明涉及一种工业机器人轨迹规划方法，尤其是一种工业机器人避障轨迹规划方法。
背景技术：
近十几年工业机器人的制造和控制技术发展迅速，已经广泛地应用于汽车制造业并能达到很高的控制精度。随着我国劳动力成本增加，在“机器换人”和产业升级的要求下各行业的自动化生产水平亟需提高。应用工业机器人能够避免重复繁琐的人工劳动、实现高稳定、高精度生产，从而使工作效率得到很大提升。对单个机器人的路径、轨迹规划大多能通过人工示教完成，所以已经有越来越多的工厂使用可编程的工业机器人代替生产线上的人工。但是对于一类需要多机器人互相协调配合才能完成的任务，如何让机器人能够避免可能出现的干涉和碰撞同时能快速准确地完成任务，这便对机器人关节运动控制以及避障路径规划提出了更高要求。
技术实现要素：
为了克服已有工业机器人轨迹规划方法的控制精度较低、无法有效避障、安全性较低的不足，本发明提供了一种控制精度较高、有效实现避障、安全性较高的基于力矩控制的工业机器人避障轨迹规划方法。
本发明解决其技术问题所采用的技术方案是：
一种基于力矩控制的工业机器人避障轨迹规划方法，首先，通过辨识各关节运动学以及惯性参数建立机器人动力学模型；然后，通过逆运动学求解得到运动到终点姿态时各关节角的位移；最后，以关节角位移作为运动路程，采用一种正弦加加速度规划方法进行各关节的速度加速度规划，将速度和加速度代入动力学模型即可求解运动路径中的控制力矩；
对于运行过程中可能发的碰撞，采用一种检测关节投影干涉情况然后判断关键点距离的方法进行碰撞预测：将机器人关节向速度向量的法平面投影，若关节投影发生干涉则对关节轴线上各关键点最短距离进行判断，若小于运行时的安全距离则认为存在碰撞的风险；预测到碰撞可能发生，然后对危险关节施加一种阻抗加速度降低关节运动速度从而实现避障。
进一步，所述规划方法包括以下步骤：
步骤1：工业机器人的动力学参数辨识
采用对机器人整体进行试验的整体辨识法对机器人的动力学参数进行辨识。为了使机器人能实现任意轨迹规划，采用周期性轨迹，每个关节的激励轨迹是正弦和余弦函数的代数和，即有限的傅立叶级数函数，则机器人每个关节的关节位置q规划如下：
其中q表示关节转角，ai、bi表示轨迹的常系数，ωf为轨迹的基频，iωf表示第i阶的频率，t＝kts表示采样时刻，k表示采样个数，ts表示采样周期；
以有限的傅立叶级数函数为机器人待辨识关节的辨识激励轨迹，机器人沿预设轨迹运动过程中，通过测电流法采集关节的驱动力矩；通过安装在电机上的编码器采集关节转角，对转角微分得到动态参数中的角速度
从而求得精确地动力学模型：
步骤2：工业机器人运动学逆解
采用解析解法求解机器人从初始位置到末端位置各关节运动角位移，对于逆运动学求解出现多解和奇异解的问题，除了通过设定各个关节的旋转范围限制来去除无效解和避免奇异解的方法以外，还根据运动能量最小原则选择消耗能量最小的逆解值作为关节的执行量，计算方法如下：计算与前一个关节差的平方和，选择计算结果最小的一组关节位置作为下一个关节执行的位置，逆解选择的表达式如下：
步骤3：正弦加加速度关节轨迹规划
采用正弦加加速度规划方法能够对各关节轨迹进行高速、平滑的规划，完成从起点到终点的姿态规划。
为了能够实现运动速度以及加速度控制的能力，对于机器人末端操作器的速度，加速度的边界进行限制，有如下三点：
1)运动角位移：d；
2)运动学上边界：最大角速度ωmax，最大角加速度αmax，最大角加加速度jmax。
3)运动学下边界：
将加加速度设定为正弦函数曲线：
在运动轨迹的限制条件下，运动距离具有两个临界点，其分别为dmin和dmin2，在维持加加速度不变的情况下，最大速度和最大加速度都能够达到，运动位移有一个临界最小值，称为dmin：
其中dmin为初速为零时加速到ωmax时的运动距离daccinc表示加速到最大速度时的运动距离；daccdec表示加ω速度减小到最小值时运动的距离；如果最大加速度未能够达到但是恰好达到了最大速度，运动位移有另一个临界值最小值，被称为dmin2：
其中dt′为重新计算后的加速运动时间，然后根据运动边界值的不同计算出不同阶段的运动时间，通过对正弦曲线的加加速度积分得到加速度、速度以及位移的运动轨迹；
步骤4：碰撞预测
对于两个机器人同时作业，按照关节几何形状，采用半球端面圆柱体包络两个关节从而得到标准化后的关节边界，两球面端点连线为关节轴线，长度为li，i＝1,2；圆柱体半径为ri，在各轴线按等间距取
根据dhit设定碰撞预测的极限距离dlim＝k1*dhit以及安全距离dsafe＝k2*dhit，k1,k2为安全系数；
根据关节角位移以及瞬时角速度可由正运动学求解得各关键点的笛卡尔坐标
则说明两个关节在速度向量法平面的投影发生干涉，则进一步判断各关节关键点之间最短距离，若有：
d＝min{||p11-p21||，||p11-p22||，||p11-p23||...||p11-p24||}＜dsafe
则判定为有可能碰撞的危险状态，否则认为是安全运行；
步骤5：阻抗控制避障规划
如果步骤4中的关键点之间最短距离d<dsafe，则在后续的运行过程中可能会发生碰撞，采用一种通过判断d与dsafe、dlim之间大小关系确定阻抗加速度αv的方法使各关节减速从而避免障碍：
直到d>dsafe则更新角速度为当前运动角速度ωi以及路径距离di，然后继续按步骤3的正弦加加速度方法进行速度和加速度规划。
再进一步，所述步骤3中，如果任务要求的执行距离d大于最大加速度和最大速度同时能够达到的最小距离dmin，d≥dmin2，那么将根据如下公式：
taccinc表示加速到最大速度时的运动时间；taccdec表示加ω速度减小到最小值时运动的时间；tacc表示在加速度增加到最大值恒定阶段时加速过程中运动的时间，tret表示在负加速度增加到最大值恒定阶段时减速过程中运动的时间，tvel表示恒速运动的时间。
所述步骤3中，如果任务要求的运动距离d小于满足最大加速度的最小距离dmin，但是大于能够达到最大速度距离，d≥dmin2；这时运动达到的最大加速度以及加速运动时间需要重新计算得：
taccinc＝taccdec＝dt′,tretinc＝tretdec＝dt,tacc＝0,
所述步骤3中，如果任务给定的运动距离d不足以使得速度和加速度都达到给定的速度和加速度，d≤dmin2，那么采取先匀速后减速的运动策略：
当d>αmaxdt，
当d<αmaxdt时减速运动时间和最大加速度需要根据距离重新计算：
利用正弦加加速度轨迹规划根据当前速度以及路径距离，可以对后续运动的加速度以及速度进行平滑地规划从而实现运行过程中的平稳力矩控制。
本发明的技术构思为：本发明根据精确的参数辨识对工业机器人进行动力学和运动学建模，以此为基础实现对机器人各关节基于模型的力矩控制：通过逆运动学求解各关节运动角度后，采用正弦加加速度进行各关节加速度规划并通过机器人动力学模型求解控制力矩。采用一种建立虚拟阻抗的方法平滑地避免两个机器人运动时可能出现的碰撞：通过判断关节在运动方向向量法平面上投影的干涉情况以及关节上所选点的距离完成碰撞预测，通过建立关节之间最短距离阻与抗加速度的函数关系进行危险运动区域的速度调节实现各关节的安全运行。
本发明的有益效果主要表现在：能够根据关节运动路程以及当前角速度完成其后续运动的速度、加速度规划；提出了一种检测关节投影干涉情况然后判断关键点距离的方法进行碰撞预测，相较于单纯的最短距离检测能够更有效地完成机器人关节运动过程的碰撞预测；提出了一种根据关节轴线关键点最短距离建立阻抗加速度函数的方法进行避障路径规划，有效实现避障、安全性较高。
附图说明
图1是动力学参数辨识过程图。
图2是正弦加加速度轨迹规划流程图。
图3为碰撞预测流程图。
图4为轨迹规划完整流程图。
具体实施方式
下面结合附图对本发明作进一步描述。
参照图1～图4，一种基于力矩控制的工业机器人避障轨迹规划方法，首先，通过辨识各关节运动学以及惯性参数建立精确的机器人动力学模型；然后，通过逆运动学求解得到运动到终点姿态时各关节角的位移；最后，以关节角位移作为运动路程，采用一种正弦加加速度规划方法进行各关节的速度加速度规划。将速度和加速度代入动力学模型即可求解运动路径中的控制力矩。对于运行过程中可能发的碰撞，本发明采用一种检测关节投影干涉情况然后判断关键点距离的方法进行碰撞预测：将机器人关节向速度向量的法平面投影，若关节投影发生干涉则对关节轴线上各关键点最短距离进行判断，若小于运行时的安全距离则认为存在碰撞的风险。预测到碰撞可能发生，然后对危险关节施加一种阻抗加速度降低关节运动速度从而实现避障。所述规划方法包括以下步骤：
步骤1：工业机器人的动力学参数辨识
本发明采用对机器人整体进行试验的整体辨识法对机器人的动力学参数进行辨识。为了使机器人能实现任意轨迹规划本方法中采用周期性轨迹，每个关节的激励轨迹是正弦和余弦函数的代数和，即有限的傅立叶级数函数，则机器人每个关节的关节位置q规划如下：
其中q表示关节转角，ai、bi表示轨迹的常系数，ωf为轨迹的基频，iωf表示第i阶的频率，t＝kts表示采样时刻，k表示采样个数，ts表示采样周期。以有限的傅立叶级数函数为机器人待辨识关节的辨识激励轨迹，机器人沿预设轨迹运动过程中，通过测电流法采集关节的驱动力矩；通过安装在电机上的编码器采集关节转角，对转角微分可以得到动态参数中的角速度
从而求得精确地动力学模型：
步骤2：工业机器人运动学逆解
采用解析解法求解机器人从初始位置到末端位置各关节运动角位移。对于逆运动学求解出现多解和奇异解的问题，除了通过设定各个关节的旋转范围限制来去除无效解和避免奇异解的方法以外，还根据运动能量最小原则选择消耗能量最小的逆解值作为关节的执行量，具体计算方法如下：计算与前一个关节差的平方和，选择计算结果最小的一组关节位置作为下一个关节执行的位置。逆解选择的表达式如下：
步骤3：正弦加加速度关节轨迹规划
采用正弦加加速度规划方法能够对各关节轨迹进行高速、平滑的规划，高效准确地完成从起点到终点的姿态规划。
为了能够实现运动速度以及加速度控制的能力，对于机器人末端操作器的速度，加速度的边界进行限制，具体有如下三点：
1)运动角位移：d
2)运动学上边界：最大角速度ωmax，最大角加速度αmax，最大角加加速度jmax。
3)运动学下边界：
这里将加加速度设定为正弦函数曲线：
在运动轨迹的限制条件下，运动距离具有两个临界点，其分别为dmin和dmin2。在维持加加速度不变的情况下，最大速度和最大加速度都能够达到，运动位移有一个临界最小值，称为dmin：
其中dmin为初速为零时加速到ωmax时的运动距离daccinc表示加速到最大速度时的运动距离；daccdec表示加ω速度减小到最小值时运动的距离；如果最大加速度未能够达到但是恰好达到了最大速度，运动位移有另一个临界值最小值，被称为dmin2：
其中dt′为重新计算后的加速运动时间。然后根据运动边界值的不同计算出不同阶段的运动时间，通过对正弦曲线的加加速度积分得到加速度、速度以及位移的运动轨迹。
对轨迹规划的各个步骤以及计算表达式介绍如下：
1：在此步骤中，根据工作任务要求的运动速度和加速度以及运动的位移被初始化。在对距离的大小进行判断后进行各阶段时间的计算。
2：如果任务要求的执行距离d大于最大加速度和最大速度同时能够达到的最小距离dmin(d≥dmin2)，那么将根据如下公式：
taccinc表示加速到最大速度时的运动时间；taccdec表示加ω速度减小到最小值时运动的时间；tacc表示在加速度增加到最大值恒定阶段时加速过程中运动的时间。tret表示在负加速度增加到最大值恒定阶段时减速过程中运动的时间。tvel表示恒速运动的时间。对各个阶段运动的时间进行计算，这是由于速度和加速度都能够充分加速到最大值，所以其中有加速度恒定时间和速度恒定匀速时间需要根据实际距离计算其大小。
3：如果任务要求的运动距离d小于满足最大加速度的最小距离dmin，但是大于能够达到最大速度距离(d≥dmin2)这时运动达到的最大加速度以及加速运动时间需要重新计算得：
taccinc＝taccdec＝dt′,tretinc＝tretdec＝dt,tacc＝0,
4：如果任务给定的运动距离d不足以使得速度和加速度都达到给定的速度和加速度(d≤dmin2)那么在这个步骤中采取先匀速后减速的运动策略:当d>αmaxdt:
当d<αmaxdt时减速运动时间和最大加速度需要根据距离重新计算：
利用正弦加加速度轨迹规划根据当前速度以及路径距离，可以对后续运动的加速度以及速度进行平滑地规划从而实现运行过程中的平稳力矩控制。
步骤4：碰撞预测
对于两个机器人同时作业，其各关节为复杂的圆周运动，在这样的运动状态下单纯的根据关节间距离并不能准确地预判碰撞的发生。本发明采用一种先检测关节投影干涉情况然后判断关键点之间最短距离的方法完成完成运动过程中的碰撞预测。以两个机器人末端关节为例进行说明：
按照关节几何形状，采用半球端面圆柱体包络两个关节从而得到标准化后的关节边界，两球面端点连线为关节轴线，长度为li(i＝1,2)；圆柱体半径为ri。在各轴线按等间距取
根据dhit设定碰撞预测的极限距离dlim＝k1*dhit以及安全距离dsafe＝k2*dhit，k1,k2为安全系数可根据实际选取大于一的常数。
根据关节角位移以及瞬时角速度可由正运动学求解得各关键点的笛卡尔坐标
则说明两个关节在速度向量法平面的投影发生干涉，则进一步判断各关节关键点之间最短距离，若有：
d＝min{||p11-p21||，||p11-p22||，||p11-p23||...||p11-p24||}＜dsafe
则判定为有可能碰撞的危险状态否则认为是安全运行。
步骤5：阻抗控制避障规划
在上一步中如果关键点之间最短距离d<dsafe，则在后续的运行过程中可能会发生碰撞。本发明采用一种通过判断d与dsafe、dlim之间大小关系确定阻抗加速度αv的方法使各关节减速从而避免障碍：
直到d>dsafe则更新角速度为当前运动角速度ωi以及路径距离di，然后继续按步骤3的正弦加加速度方法进行速度和加速度规划。
- 还没有人留言评论。精彩留言会获得点赞！