CN109955250A - 应用于工业机器人的追踪与实时避障反应规划算法 - Google Patents
应用于工业机器人的追踪与实时避障反应规划算法 Download PDFInfo
- Publication number
- CN109955250A CN109955250A CN201910051019.1A CN201910051019A CN109955250A CN 109955250 A CN109955250 A CN 109955250A CN 201910051019 A CN201910051019 A CN 201910051019A CN 109955250 A CN109955250 A CN 109955250A
- Authority
- CN
- China
- Prior art keywords
- joint
- rigid body
- space
- obstacle
- barrier
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Numerical Control (AREA)
- Feedback Control In General (AREA)
Abstract
本发明提供了一种应用于工业机器人的追踪与实时避障反应规划算法,包括:步骤1,输入待追踪曲线位姿、末端刚体与障碍物间的实时距离;步骤2,通过运动学正解给出从机器人关节空间到工作空间的映射;步骤3,通过视觉传感器检测障碍物并计算与障碍物距离;步骤4,规划路径,工作空间刚体追踪及避障;步骤5,根据关节空间中雅克比伪逆法求关节角;步骤6,利用快速搜索生成树处理角约束问题。步骤7,约束检测,如不满足约束,则重复步骤6;如满足约束,则进入步骤8。步骤8,达到目标,得到关节角并据此计算关节空间控制力矩;达到目标,则结束算法流程;否则进入步骤9;步骤9,更新初始点,重新从步骤3执行。
Description
技术领域
本发明涉及一种工业机器人路径规划与运动控制技术,特别是一种应用于工业机器人的追踪与实时避障应规划算法。
背景技术
在工程上,人们要求机器人能以最快或最小能量消耗的方式去完成特定的任务,因此经常使用优化的方法处理机器人的运动规划问题。但是,机器人运动方程的复杂性会导致相应优化问题的求解非常困难甚至不可解。利用李群和李代数的理论,可以给出复杂机器人运动方程等价的简单描述。更重要的是,可以精确计算力矩关于机器人任一关节变量的一阶和二阶导数。这样在求解最小能量消耗问题时,可以求得对应优化问题目标泛函的梯度和Hessian矩阵的精确表达式,从而避免了用数值近似的手段来处理梯度和Hessian矩阵所带来的昂贵成本、精确性差以及收敛上的不稳定性。另一方面,利用李群给出机器人运动方程的迭代表述公式时(复杂度为O(n)),可以避免使用三角函数及反三角函数所带来的关节角的奇异性问题,并极有效地降低计算复杂度。
考虑到工业机器人的工作环境中的障碍物,在处理机器人的避障问题时,常常采用物体间距离函数来检测障碍。当{遇到非光滑障碍物使}距离函数不可微时,在优化问题中获取梯度有实质性的困难。利用凸分析理论,将障碍约束转变为线性不等式组{作为优化目标的(非线性)约束},可避免上述缺点。在检测障碍碰撞时,使用凸多面体划分,并采用“隐面剔除”策略,使得仅仅需要凸多面体的一小部分面来建立避障约束,从而在障碍物较多时,能极大地提高计算效率。
对于最省时间的最优控制问题,可以利用离散力学和最优控制(DiscreteMechanics andOptimal Control,DMOC)的框架来处理。其中的等式约束由本文提出的Hamel变分积分子得到。
发明内容
本发明的目的在于提供一种应用于工业机器人的追踪与实时避障反应规划算法,包括:
步骤1,输入待追踪曲线位姿、末端刚体与障碍物间的实时距离;
步骤2,通过运动学正解给出从机器人关节空间到工作空间的映射;
步骤3,通过视觉传感器检测障碍物并计算与障碍物距离;
步骤4,规划路径,工作空间刚体追踪及避障;
步骤5,根据关节空间中雅克比伪逆法求关节角;
步骤6,利用快速搜索生成树处理角约束问题。
步骤7,约束检测,如不满足约束,则重复步骤6;如满足约束,则进入步骤8。
步骤8,达到目标,得到关节角(θ1,θ2...,θ6)并据此计算关节空间控制力矩;达到目标,则结束算法流程;否则进入步骤9;
步骤9,更新初始点,重新从步骤3执行。
本发明应用李群理论避免关节角的奇异性问题,并极有效地降低计算复杂度,采用“隐面剔除”策略在障碍物较多时,能极大地提高计算效率。
下面结合说明书附图对本发明作进一步描述。
附图说明
图1为凸多面体示意图。
具体实施方式
本算法目标:实现多自由度工业机器人路径规划的最优控制。
本算法输入信息:机器人的各项参数(包括关节角约束),静态障碍物(凸多面体)信息,机器人的初始状态和终止状态及优化问题的初值,优化目标(最省能量或最省时间)。
本算法输出信息:满足约束和优化目标的各关节上的控制力矩及关节角。
本算法的技术思路:
利用序列二次规划SQP求解n连杆工业机器人的最优路径规划问题,应用机器人动力学的李群理论得到了机器人动力学的几何不变算法及相应的Hamel变分积分子,并采用“隐面剔除”策略实时对障碍约束进行简化。
结合上上述三种理论方法,将机器人避障的最优控制问题转化为一个非线性优化问题(最省能量的最优控制问题),并得到其离散形式(最省能量的离散最优控制框架)。
针对推导得到的最省能量的离散最优控制框架,按以下步骤求解:
a.通过B样条对变量进行参数化;
b.赋初值,运用递推动力学求出控制力矩τ;
c.用控制力矩表示出目标泛函,加上约束,建立优化问题,并运用SQP相关技术对其求解。
本算法主要涉及以下技术方法:
1.最优控制算法
考虑n连杆机器人,向量q表示机器人关节角,向量v为关节角的速度,向量u表示作用在机器人连杆重心的力矩。机器人无避障最优化控制问题的定义:
a.目标泛函
b.约束
机器人运动方程:
q'=v (2)
M(q)v'=G(q,v)+F(q,u) (3)
边界条件:
q(t_0)=q_0,v(t_0)=0,q(t_f)=q_f,v(t_f)=0 (4)
边界约束:
umin≤u≤umax (5)
其中M(q)表示正定质量矩阵,G(q,v)表示广义科里奥利力,F(q,u)表示作用在关节处的力矩和重力。方程(3)是机器人动力学的拉格朗日形式,q0,qf分别表示初始状态和终点状态。
2.递推动力学
本方法是基于李群理论方法构造复杂多体系统的运动方程,该方法关键之处是迭代计算非线性动力学,计算精确的梯度矩阵和数值近似的Hessian矩阵。
给定n杆的串行链,令fk-1,k∈SE(3)表示从第k-1个杆到第k个杆的变换矩阵,表示第k个关节的关节扭转,Mk∈SE(3)表示当qk=0时fk-1,k的值,是第k个杆6维广义速度,Fk是第k-1个杆施加在第k个杆上的6维广义力,τk表示第k个关节所受力矩,Jk表示第k个杆的广义惯性矩阵,形式如下:
其中是第k个杆关于质心的旋转惯量,rk是第k个杆框架原点到其质心的3维向量,mk是第k杆的质量,I是3×3的单位阵,根据这些定义,给出递归方法计算逆动力学:
初始化:
向前迭代,k:=1to n
向后迭代,k:=n to 1
3.避障方法
为了建立避障准则,将机器人近似为一系列凸多面体的并,记为P
其中,np为P中多面体的个数,对于i=1,...,nP,pi是P(i)的面的个数。在(9)中的每个不等式都描述了多面体的一个面。指标"(i)"表示P中第i个多面体。
相似地,障碍近似为凸多面体的集合,记为Q
其中,nQ为Q中多面体的数量,对于j=1,...,nQ,qj表示Q(j)中面的数量。指标"(j)"表示Q中第j个多面体。如图1所示,其中P由5个多面体组成,Q由3个多面体组成。
分别使用凸多面体P和Q表示机器人与障碍物,其不碰撞当且仅当
多面体P(i)和Q(j)的定义说明上述关系与下述线性不等式组不可解等价
根据泛函的Hahn—Banach凸集分离定理得到下述结论:不等式组(12)无解当且仅当存在向量使得
因而机器人P(t)和障碍Q不碰撞,当且仅当对于每一对多面体(P(i),Q(j)),i=1,...,np,j=1,...,nQ,存在向量使得w(i,j)(t)≥0时有
其中S(i)(t)∈SO(3)是对应于关节角的第i个多面体的旋转矩阵。因此,在最优控制中,关于避障的状态约束为对i=1,...,np,j=1,...,nQ有
其中ε>0为给定的距离参数,这些约束称为反碰撞约束。为了方便下面模型的描述,记
因此,结合上述三种理论方法,机器人避障的最优控制问题便可转化为一个非线性优化问题(描述A),并得到其离散形式(描述B)。
描述A:最省能量的最优控制问题
a.目标泛函
b.约束
边界条件:
q(t_0)=q_0,v(t_0)=0,q(t_f)=q_f,v(t_f)=0 (19)
边界约束:
qmin≤q(t)≤qmax (20)
障碍物约束:
Gi,j(q(t))Tw(i,j)(t)=0 (21)
(q(t))Tw(i,j)(t)≤ε,i=1,...,np,j=1,...,nQ (22)
其中,τ(q(t))表示达到当前状态的力矩和重力,qmin和qmax分别表示关节角运动的下限和上限,v(t0)和v(tf)分别表示初始时刻和末端时刻的速度,np和nQ分别表示机器人和障碍凸多面体表示中多面体的个数。
描述B:最省能量的离散最优控制框架
给定时间区间[0,tf],取离散网格节点为tl=lh,l=0,...,tn,其中时间步长为
对于n连杆机器人,为了得到连续光滑的关节路径,使用B样条对关节变量q(t)进行参数化,得到关节轨迹的形式为:
其中Bi(t)为B样条基函数,P=(p1,...,ptn),pi∈Rn为控制节点。
a.递推动力学
初始化:
向前迭代:
向后迭代:
b.目标泛函
c.约束
边界条件:
q(t0,P)=q0,v(t0,P)=0,q(tf,P)=qf,v(tf,P)=0 (31)
边界约束:
qmin≤q(tl,P)≤qmax (32)
障碍物约束:
Gi,j(q(tl,P))Tw(i,j)=0 (33)
gi,j(q(tl,P))Tw(i,j)≤ε (34)
w(i,j)≥0,i=1,...,np,j=1,...,nQ,l=1,...,tn (35)
其中,P=(p_1,...,p_{t_n})。
Claims (6)
1.一种应用于工业机器人的追踪与实时避障反应规划算法,其特征在于,包括:
步骤1,输入待追踪曲线位姿、末端刚体与障碍物间的实时距离;
步骤2,通过运动学正解给出从机器人关节空间到工作空间的映射;
步骤3,通过视觉传感器检测障碍物并计算与障碍物距离;
步骤4,规划路径,工作空间刚体追踪及避障;
步骤5,根据关节空间中雅克比伪逆法求关节角;
步骤6,利用快速搜索生成树处理角约束问题。
步骤7,约束检测,如不满足约束,则重复步骤6;如满足约束,则进入步骤8。
步骤8,达到目标,得到关节角(θ1,θ2...,θ6)并据此计算关节空间控制力矩;达到目标,则结束算法流程;否则进入步骤9;
步骤9,更新初始点,重新从步骤3执行。
2.根据权利要求1所述的方法,其特征在于,步骤4的具体过程为:
步骤401,设刚体位置x=(x,y,z),位姿矩阵R,刚体角速度为ω,质量为m,惯性矩阵为Jx,Jy,Jz,惯性张量J=diag(Jx,Jy,Jz),状态空间Q;
步骤402,假设控制输入u,定义力矩τ,与作用力f,外力矩记为τext,得运动方程:
其中
τext(s)为外力矩,τ(s,u)为施加输入u后的力矩,fext(s)为外作用力,f(s,u)为施加输入u后的作用力,s为自定义空间状态集合,为刚体运动加速度;ωx、ωy、ωz分别为x、y、z轴的角速度分量;
步骤403,记刚体区域A(R,x),区域A1,A2的欧式距离为prox(A1,A2),若刚体不与障碍物O1,..,Ol发生碰撞,则用PQP方法计算prox;
步骤404,假设在参考轨迹xr的路径上突然遇到障碍,确定一个新的可达目标状态xd为移动到目标点的位置,为移动到目标点的速度;若xd=xr(tg),tg∈[0,T],T为参考运动轨迹,使刚体偏离障碍并且朝着xd移动;
步骤405,定义误差项Δx=x-xd,选取控制律为
其中,kx,kv>0,Γ(s)是斜对称矩阵,为刚体当前运动速度;
步骤406,令
其中,给定值kg、vmax>0,刚体到最近障碍物的距离记为d(R,x;Oi)>0,为由刚体质心指向最近障碍的单位向量
步骤407,为了使目标位置状态可达,定义势能函数对其求导并带入运动方程(1)得
根据LaSalle不变性原理,选择使得保证xd渐近可达;
步骤408,为了使目标姿态状态(Rd,ωd)可达,选取对姿态的力矩控制律
其中,skew(A):=(A-AT),算子是帽子映射的逆映射,KR、Kw是正定矩阵,wd为刚体期望体角速度。
3.根据权利要求1所述的方法,其特征在于,步骤5的具体过程为:
步骤501,令关节角的求解多项式为
其中,τ为关节空间控制力矩,f(θ)由关节空间到工作空间的映射,θ为关节角;
步骤502,对微分运动学方程进行式(11)的处理
Δθ=JT(JJT+λ2I)-1Δe (11)
其中,为J的伪逆,J为雅克比矩阵,误差项x*为期望目标,λ是非零的衰减常数。
4.根据权利要求1所述的方法,其特征在于,步骤6的具体步骤为:
设定生成树以概率pg向目标点生长,在均匀采样得到采样点,在已有生成树中选择距离采样点最近的树节点作为父节点,记为treei,父节点treei与采样点之间距离d满足d≤pstep,其中pstep为某一设定的步长;若d>pstep,则从父节点到采样点的方向上选择新的采样点;
采样点部分则在父节点的旋转矩阵邻域均匀采样产生;
采用雅克比伪逆法追踪采样点,若所得关节角满足角度约束及碰撞约束,则将采样点添加到生成树中。否则,重复进行上述步骤,直至到达目标点。
5.根据权利要求4所述的方法,其特征在于,旋转空间采样方法如下:
生成位姿矩阵的一个随机元素
随机选择3个点,u1,u2,u3~[0,1]均匀分布,可以确定一个均匀、随机四元数q=(qr,qi,qj,qk)如下:
从而得到旋转矩阵R∈SO(3):
6.根据权利要求1所述的方法,其特征在于,步骤8中采用式(14)求得关节空间控制力矩
其中,M(θ)为对称正定的惯性矩阵,包括作用于关节的哥氏力和离心力项,包括重力和作用于关节的其它力项。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910051019.1A CN109955250A (zh) | 2019-01-21 | 2019-01-21 | 应用于工业机器人的追踪与实时避障反应规划算法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910051019.1A CN109955250A (zh) | 2019-01-21 | 2019-01-21 | 应用于工业机器人的追踪与实时避障反应规划算法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN109955250A true CN109955250A (zh) | 2019-07-02 |
Family
ID=67023476
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910051019.1A Pending CN109955250A (zh) | 2019-01-21 | 2019-01-21 | 应用于工业机器人的追踪与实时避障反应规划算法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109955250A (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111571586A (zh) * | 2020-05-07 | 2020-08-25 | 江苏原力数字科技股份有限公司 | 一种反向动力学自由度的动态约束方法 |
CN112015183A (zh) * | 2020-09-08 | 2020-12-01 | 安徽工程大学 | 一种能耗约束下在具有凹凸地形中移动机器人避障方法 |
CN113043286A (zh) * | 2020-12-25 | 2021-06-29 | 杭州电子科技大学 | 一种多自由度机械臂实时避障路径规划系统及方法 |
CN113238563A (zh) * | 2021-06-04 | 2021-08-10 | 重庆大学 | 一种高实时性的自动驾驶运动规划方法 |
CN113821108A (zh) * | 2021-11-23 | 2021-12-21 | 齐鲁工业大学 | 基于多模态交互技术的机器人远程控制系统及控制方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20120277907A1 (en) * | 2011-04-28 | 2012-11-01 | Waseda University | Trajectory planning method, trajectory planning system and trajectory planning and control system |
CN106503373A (zh) * | 2016-11-04 | 2017-03-15 | 湘潭大学 | 一种基于b样条曲线的双机器人协调装配的轨迹规划方法 |
CN106990777A (zh) * | 2017-03-10 | 2017-07-28 | 江苏物联网研究发展中心 | 机器人局部路径规划方法 |
WO2017139613A1 (en) * | 2016-02-11 | 2017-08-17 | Massachusetts Institute Of Technology | Motion planning for robotic systems |
CN107234617A (zh) * | 2017-07-10 | 2017-10-10 | 北京邮电大学 | 一种避障任务无关人工势场引导的避障路径规划方法 |
-
2019
- 2019-01-21 CN CN201910051019.1A patent/CN109955250A/zh active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20120277907A1 (en) * | 2011-04-28 | 2012-11-01 | Waseda University | Trajectory planning method, trajectory planning system and trajectory planning and control system |
WO2017139613A1 (en) * | 2016-02-11 | 2017-08-17 | Massachusetts Institute Of Technology | Motion planning for robotic systems |
CN106503373A (zh) * | 2016-11-04 | 2017-03-15 | 湘潭大学 | 一种基于b样条曲线的双机器人协调装配的轨迹规划方法 |
CN106990777A (zh) * | 2017-03-10 | 2017-07-28 | 江苏物联网研究发展中心 | 机器人局部路径规划方法 |
CN107234617A (zh) * | 2017-07-10 | 2017-10-10 | 北京邮电大学 | 一种避障任务无关人工势场引导的避障路径规划方法 |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111571586A (zh) * | 2020-05-07 | 2020-08-25 | 江苏原力数字科技股份有限公司 | 一种反向动力学自由度的动态约束方法 |
CN112015183A (zh) * | 2020-09-08 | 2020-12-01 | 安徽工程大学 | 一种能耗约束下在具有凹凸地形中移动机器人避障方法 |
CN113043286A (zh) * | 2020-12-25 | 2021-06-29 | 杭州电子科技大学 | 一种多自由度机械臂实时避障路径规划系统及方法 |
CN113043286B (zh) * | 2020-12-25 | 2022-05-03 | 杭州电子科技大学 | 一种多自由度机械臂实时避障路径规划系统及方法 |
CN113238563A (zh) * | 2021-06-04 | 2021-08-10 | 重庆大学 | 一种高实时性的自动驾驶运动规划方法 |
CN113821108A (zh) * | 2021-11-23 | 2021-12-21 | 齐鲁工业大学 | 基于多模态交互技术的机器人远程控制系统及控制方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109955250A (zh) | 应用于工业机器人的追踪与实时避障反应规划算法 | |
Bhardwaj et al. | Storm: An integrated framework for fast joint-space model-predictive control for reactive manipulation | |
Gong et al. | Analytical inverse kinematics and self-motion application for 7-DOF redundant manipulator | |
Xu et al. | Motion planning of manipulators for simultaneous obstacle avoidance and target tracking: An RNN approach with guaranteed performance | |
CN109773794A (zh) | 一种基于神经网络的6轴机器人动力学参数辨识方法 | |
Ichnowski et al. | GOMP: Grasp-optimized motion planning for bin picking | |
Qiu et al. | Model predictive control for uncalibrated and constrained image-based visual servoing without joint velocity measurements | |
Xia et al. | Dual fast marching tree algorithm for human-like motion planning of anthropomorphic arms with task constraints | |
Ren et al. | Koopman-operator-based robust data-driven control for wheeled mobile robots | |
CN109986554A (zh) | 一种工业机器人路径规划的几何最优控制方法 | |
Kharidege et al. | Performance study of PID and fuzzy controllers for position control of 6 DOF arm manipulator with various defuzzification strategies | |
Galicki | Control-based solution to inverse kinematics for mobile manipulators using penalty functions | |
Nicola et al. | Optimal task positioning in multi-robot cells, using nested meta-heuristic swarm algorithms | |
Wang et al. | Continuous trajectory point control research of six degree of freedom mechanical intelligent arm position | |
CN114347031B (zh) | 一种考虑时变时滞的移动机械臂避碰控制方法及装置 | |
Ramon et al. | Multi-fingered robotic hand planner for object reconfiguration through a rolling contact evolution model | |
Han et al. | RRT based obstacle avoidance path planning for 6-DOF manipulator | |
Hao et al. | Path planning method of anti-collision for the operation road of port cargo handling robot | |
Xu et al. | Obstacle avoidance for manipulator with arbitrary arm shape using signed distance function | |
Brahmi et al. | Adaptive control of mobile manipulator robot based on virtual decomposition approach | |
Wei et al. | Position/force control of a holonomic-constrained mobile manipulator based on active disturbance rejection control | |
Missura et al. | Efficient kinodynamic trajectory generation for wheeled robots | |
Pradhan et al. | Optimal Control for Structurally Sparse Systems using Graphical Inference | |
Yin et al. | Fractional order nonsingular fast terminal sliding mode control technique for 6-DOF robotic manipulator | |
Xie et al. | Optimal trajectory generation for stewart platform using discrete mechanics and optimal control |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190702 |
|
RJ01 | Rejection of invention patent application after publication |