CN112828894B - 一种冗余度机械臂的位置与力混合控制方法 - Google Patents
一种冗余度机械臂的位置与力混合控制方法 Download PDFInfo
- Publication number
- CN112828894B CN112828894B CN202110116435.2A CN202110116435A CN112828894B CN 112828894 B CN112828894 B CN 112828894B CN 202110116435 A CN202110116435 A CN 202110116435A CN 112828894 B CN112828894 B CN 112828894B
- Authority
- CN
- China
- Prior art keywords
- redundant manipulator
- end effector
- redundant
- manipulator
- force
- 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.)
- Active
Links
Images
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
-
- 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/1628—Programme controls characterised by the control loop
- B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Abstract
本发明提供了一种冗余度机械臂的位置与力混合控制方法,属于机械臂控制技术领域,包括如下步骤:S1:建立冗余度机械臂的运动学方程;S2:获取冗余度机械臂末端执行器的方向保持运动学公式;S3:建立二次型优化方案;S4:转化为一个标准的二次规划;S5:利用二次规划求解器进行求解;S6:对冗余度机械臂进行位置与力的混合控制。本发明能够精确地完成冗余度机械臂在位置与力上的混合控制,同时也可以实现冗余度机械臂的物理约束躲避和末端执行器方向保持功能,使得机械臂在执行过程更加安全、高效。
Description
技术领域
本发明涉及一种冗余度机械臂的位置与力混合控制方法,属于机械臂控制技术领域,尤其适用于冗余度机械臂的位置与力混合控制。
背景技术
随着科技的进步,机器人已广泛应用于生产自动化中。冗余度机械臂在从事装配、医疗技术、抛光和擦洗等与环境有接触的作业时,需要对机械臂同时进行位置和力的控制,来达到最佳的作业效果。就这一点而言,机械臂的位置和力混合控制可以增强机械臂在弱结构环境下的鲁棒性和灵活性,从而增强机械臂的可操作能力。位置和力混合控制要求将任务空间划分为两个正交互补的子空间-力控制子空间和位置控制子空间,在力控制子空间中利用力控制策略进行力控制,在位置控制子空间中利用位置控制策略进行位置控制。如何更好地解耦力控制子空间和位置控制子空间,达到更好的控制效果,是冗余度机械臂的控制方面亟需解决的技术问题。
然而,现有的冗余度机械臂位置与力控制方法全都是基于机器动力学,需要考虑多个动力学参数,如对象的质量和惯性,无法高效地实现位置跟踪任务。基于运动学角度的控制方法能够实现目标函数和多个附加约束条件,如物理约束和轨迹任务等,提高了控制方法的安全性和有效性,但目前对于该领域的研究还处于空白。
发明内容
有鉴于此,为弥补现有技术的空白,本发明提出了一种冗余度机械臂的位置与力混合控制方法,可以精确地在接触面法线方向上实现力控制,在接触面上完成轨迹追踪任务,并且能够同时考虑机械臂末端执行器的方向保持和各电机的物理约束,提高了控制方法的安全性和有效性。
为达到上述目的,本发明提供如下技术方案:
一种冗余度机械臂的位置与力混合控制方法,包括如下步骤:
S1:根据冗余度机械臂末端执行器与环境接触时的力误差与位置误差,建立冗余度机械臂的运动学方程;
S2:根据末端执行器坐标系下的环境接触面法向量,得到冗余度机械臂末端执行器的方向保持运动学关系;
S3:根据步骤S1的运动学方程和步骤S2的方向保持运动学关系,对冗余度机械臂的运动规划进行速度层逆运动学解析,建立二次型优化方案;
S4:将步骤S3的二次型优化方案转化为一个线性二次规划;
S5:将步骤S4的线性二次规划用二次规划求解器进行求解;
S6:根据步骤S5中得到的求解结果对冗余度机械臂进行位置与力的混合控制。
进一步,所述的步骤S1中所述的冗余度机械臂末端执行器与环境接触时的力误差与位置误差是在基坐标系下进行分析的,具体误差描述为:
(1)冗余度机械臂末端执行器与环境接触时所产生的力误差:e1=αHΔHT(r-rd)-fd;其中:为冗余度机械臂末端执行器的刚性系数,可以通过实验测试得出;为冗余度机械臂的旋转矩阵;上标T为矩阵的转置操作;对角阵Δ=diag(0,0,1);为冗余度机械臂末端执行器的位置;为设定的轨迹追踪任务;为末端执行器与环境接触时所产生的期望力;
更进一步,e1,e2,H,r,rd,fd中的元素都含有时间为变量的多项式,如无特殊说明,本发明中的求导都是对时间进行的求导。
进一步,所述的步骤S1中所述的建立冗余度机械臂的运动学方程为通过借助神经动力学方法和冗余度机械臂关节角度的非线性方程F(θ)=r将冗余度机械臂末端执行器与环境接触时的力误差与位置误差进行显式微分方程形式的描述,具体描述为:其中,e=[e1;e2],为e关于时间的导数,为冗余度机械臂的关节角度,为冗余度机械臂的关节角速度,m为机械臂自由度,为期望的末端执行器速度,为冗余度机械臂的雅可比矩阵,
更进一步,θ中的元素也含有时间为变量的多项式,β可以通过实验测试数据经过深度学习等方法训练求出。
进一步,步骤S2所述的末端执行器坐标系下的环境接触面法向量可以表示为其中,为基座标系下的环境接触面的法向量;考虑到末端执行器姿态需要保持在环境接触面法向量方向上,末端执行器的方向保持运动学关系的公式表示为其中,为向量关于θ的雅可比矩阵。
进一步,所述步骤S3中的二次型优化方案可以表示为:设计的最小化性能指标为冗余度机械臂的关节角速度向量的二次函数,受约束于冗余度机械臂的运动学方程、末端执行器的方向保持运动学关系、关节物理约束;即最小化性能指标为约束条件为 和其中,Λ为非零系数矩阵,且和由待优化的目标所决定,ω为冗余度机械臂的关节角速度可行域。
进一步,步骤S4所述的二次型优化方案转化为一个线性二次规划,具体为引入新向量x替换原来的导数变量将二次型优化方案改写为:最小化性能指标为xTΛx/2+qTx,约束条件为Ax=sr,Kx=0,ω-≤x≤ω+,其中,A=NJ,ω+、ω-分别表示冗余度机械臂关节角速度可行域的上、下限。
进一步,步骤S5中,将线性二次规划通过二次规划求解器进行求解,从而得到冗余度机械臂位置与力混合控制的最优解。需要说明的是,线性二次规划的求解器有很多,商业优化器包括Gurobi,Cplex,Xpress和Mosek;免费优化器包括SCIP,CBC,GLPK等,以及Matlab和其他教学当中常用的优化器;具体的选择需要结合具体的机械臂的应用场景来确定。同时,具体的求解算法,会要求对本发明方法的线性规划方程进行适当的调整,以便能够适应。
进一步,步骤S6具体为将求解器求解的二次规划结果通过控制关系转化为对应的机械臂电机驱动所需要的控制信号,从而驱动冗余度机械臂实现位置与力的混合控制。
本发明的有益效果在于:本发明提供了一种冗余度机械臂的位置与力混合控制方法,从运动学的角度,建立速度层的冗余度机械臂位置与力混合控制的线性二次规划方案,能够实现冗余度机械臂的物理约束躲避和末端执行器的方向保持功能,更加安全和可靠。
附图说明
为了使本发明的目的、技术方案,本发明提供如下附图进行说明:
图1为一种冗余度机械臂的位置与力混合控制方法流程图;
图2为本发明实施例的运动过程图,其中,子图1、2、3、4、5、6分别为冗余度机械臂不同时刻的运动状态图;
图3为本发明实施例的关节速度图,横坐标为时间(单位:秒),纵坐标为角位移(单位:弧度/秒);
图4为本发明实施例的末端执行器在x和y方向上的位置误差图,横坐标为时间(单位:秒),纵坐标为误差大小(单位:米);
图5为本发明实施例的运动过程中的末端执行器受力变化图,横坐标为时间(单位:秒),纵坐标为力的大小(单位:牛顿);
图6为本发明实施例的末端执行器方向变化图,横坐标为时间(单位:秒),纵坐标为环境接触面法向量。
具体实施方式
为使本发明的目的和技术方案更加清晰明白,下面结合附图及实施例对本发明进行详细的描述。
实施例:假设一个的冗余度机械臂(七自由度的KUKA LWR4+机械臂)的控制场景,,设定环境接触面为xoy平面,接触面的单位法向量即为n=[0,0,1]T,已知:冗余度机械臂末端执行器的刚性系数α=1000,机械臂末端执行器与环境接触时所产生的期望力fd=[0,0,1](单位:牛顿),冗余度机械臂的关节速度的物理约束为ω+=-ω-=[0.5,0.5,0.5,0.5,0.5,0.5,0.5]T(单位:弧度/秒),各关节初始角度为θ(0)=[0,-0.2,0,2.5,0,-0.5,0]T(单位:弧度),现设定的10秒的时间内,执行任务为四叶草轨迹追踪任务。针对这一场景,本发明提出“一种冗余度机械臂的位置与力混合控制方法”,结合图1,包含以下步骤:
步骤S1:根据冗余度机械臂末端执行器与环境接触时的力误差与位置误差,建立冗余度机械臂的运动学方程。
进一步,所述的冗余度机械臂末端执行器与环境接触时的力误差与位置误差是在基坐标系下进行分析的,具体误差描述为:
(1)冗余度机械臂末端执行器与环境接触时所产生的力误差:e1=αHΔHT(r-rd)-fd;其中:为冗余度机械臂的旋转矩阵,由机械臂型号确定;上标T为矩阵的转置操作;对角阵Δ=diag(0,0,1);为冗余度机械臂末端执行器的位置;为设定的轨迹追踪任务;
所述的建立冗余度机械臂的运动学方程为通过借助神经动力学方法和冗余度机械臂关节角度的非线性方程F(θ)=r将冗余度机械臂末端执行器与环境接触时的力误差与位置误差进行显式微分方程形式的描述,具体描述为:其中,e=[e1;e2],为e关于时间的导数,为冗余度机械臂的关节角度,为冗余度机械臂的关节角速度,m为机械臂自由度,为期望的末端执行器速度,为冗余度机械臂的雅可比矩阵,非线性方程F(θ)=r由机械臂型号确定,可以通过理论分析或者大量实验获取。
步骤S2:根据末端执行器坐标系下的环境接触面法向量,得到冗余度机械臂末端执行器的方向保持运动学关系。
所述的末端执行器坐标系下的环境接触面法向量可以表示为其中,为基座标系下的环境接触面的法向量;考虑到末端执行器姿态需要保持在环境接触面法向量方向上,末端执行器的方向保持运动学关系的公式表示为其中,为向量关于θ的雅可比矩阵。
步骤S3:根据步骤S1的运动学方程和步骤S2的方向保持运动学关系,对冗余度机械臂的运动规划进行速度层逆运动学解析,建立二次型优化方案。
设计的最小化性能指标为冗余度机械臂的关节角速度向量的二次函数,受约束于冗余度机械臂的运动学方程、末端执行器的方向保持运动学关系、关节物理约束;即最小化性能指标为约束条件为 和其中,Λ为非零系数矩阵,且和由待优化的目标所决定,ω为冗余度机械臂的关节角速度可行域。
本实施例中,Λ设定为单位矩阵,q设定为零向量。
步骤S4:将步骤S3的二次型优化方案转化为一个线性二次规划。
具体为引入新向量x替换原来的导数变量将二次型优化方案改写为:最小化性能指标为xTΛx/2+qTx,约束条件为Ax=sr,Kx=0,ω-≤x≤ω+,其中,A=NJ,ω+、ω-分别表示冗余度机械臂关节角速度可行域的上、下限。
步骤S5:将步骤S4的线性二次规划用二次规划求解器进行求解。
本实施例使用Karush-Kuhn-Tucker条件和对偶空间法,将线性二次规划问题等价为分段线性投影方程组求解问题。
具体的分段线性投影方程组为:
步骤S6:将求解器求解的二次规划结果通过控制关系转化为对应的机械臂电机驱动所需要的控制信号,从而驱动冗余度机械臂实现位置与力的混合控制。
本实施例利用MATLAB和CoppeliaSim软件进行仿真实验,以验证本发明方法的正确性和优越性。具体的实验结果如图2、图3、图4、图5、图6所示。
进一步,从图2可以看出冗余度机器人顺利地完成了所给定的四叶草轨迹追踪任务。
从图3可以看出在整个任务执行过程中关节速度起始于0弧度/秒,停止于0弧度/秒,并且一直保持在物理约束内部,体现了本发明能够实现约束控制。
从图4可以看出冗余度机械臂在环境接触面上的轨迹追踪误差处于10-4米的数量级,体现了本发明的准确性。
从图5可以看出冗余度机械臂末端执行器在接触面法线方向上的力在1秒内从0牛顿变为了1牛顿,且在剩余的任务时间内一直保持1牛顿,体现了本发明在力的控制方面的高效性。
从图6可以看出冗余度机器人末端执行器沿着接触面法线方向上的方向参数保持不变,体现了末端执行器的方向保持功能的有效性。
最后说明的是,以上优选实施例仅用以说明本发明的技术方案而非限制,尽管通过上述优选实施例已经对本发明进行了详细的描述,但本领域技术人员应当理解,可以在形式上和细节上对其做出各种各样的改变,而不偏离本发明权利要求书所限定的范围。
Claims (1)
1.一种冗余度机械臂的位置与力混合控制方法,其特征在于,该方法包含以下步骤:
S1:根据冗余度机械臂末端执行器与环境接触时的力误差与位置误差,建立冗余度机械臂的运动学方程;
S2:根据末端执行器坐标系下的环境接触面法向量,得到冗余度机械臂末端执行器的方向保持运动学关系;
S3:根据步骤S1的运动学方程和步骤S2的方向保持运动学关系,对冗余度机械臂的运动规划进行速度层逆运动学解析,建立二次型优化方案;
S4:将步骤S3的二次型优化方案转化为一个线性二次规划;
S5:将步骤S4的线性二次规划用二次规划求解器进行求解;
S6:根据步骤S5中得到的求解结果对冗余度机械臂进行位置与力的混合控制;
所述的步骤S1中所述的冗余度机械臂末端执行器与环境接触时的力误差与位置误差是在基坐标系下进行分析的,具体误差描述为:
(1)冗余度机械臂末端执行器与环境接触时所产生的力误差:e1=αHΔHT(r-rd)-fd;其中:为冗余度机械臂末端执行器的刚性系数;为冗余度机械臂的旋转矩阵;上标T为矩阵的转置操作;对角阵Δ=diag(0,0,1);为冗余度机械臂末端执行器的位置;为设定的轨迹追踪任务;为末端执行器与环境接触时所产生的期望力;
所述的步骤S1中所述的建立冗余度机械臂的运动学方程为通过借助神经动力学方法和冗余度机械臂关节角度的非线性方程F(θ)=r将冗余度机械臂末端执行器与环境接触时的力误差与位置误差进行显式微分方程形式的描述,具体描述为:其中,e=[e1;e2],为e关于时间的导数,为冗余度机械臂的关节角度,为冗余度机械臂的关节角速度,m为机械臂自由度,为期望的末端执行器速度, 为冗余度机械臂的雅可比矩阵,
步骤S2所述的末端执行器坐标系下的环境接触面法向量可以表示为其中,为基座标系下的环境接触面的法向量;考虑到末端执行器姿态需要保持在环境接触面法向量方向上,末端执行器的方向保持运动学关系的公式表示为其中,为向量关于θ的雅可比矩阵;
所述步骤S3中的二次型优化方案可以表示为:设计的最小化性能指标为冗余度机械臂的关节角速度向量的二次函数,受约束于冗余度机械臂的运动学方程、末端执行器的方向保持运动学关系、关节物理约束;即最小化性能指标为约束条件为和其中,Λ为非零系数矩阵,且和由待优化的目标所决定,ω为冗余度机械臂的关节角速度可行域;
步骤S4所述的二次型优化方案转化为一个线性二次规划,具体为引入新向量x替换原来的导数变量将二次型优化方案改写为:最小化性能指标为xTΛx/2+qTx,约束条件为Ax=sr,Kx=0,ω-≤x≤ω+,其中,A=NJ,ω+、ω-分别表示冗余度机械臂关节角速度可行域的上、下限;
步骤S6具体为将求解器求解的二次规划结果通过控制关系转化为对应的机械臂电机驱动所需要的控制信号,从而驱动冗余度机械臂实现位置与力的混合控制。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110116435.2A CN112828894B (zh) | 2021-01-21 | 2021-01-21 | 一种冗余度机械臂的位置与力混合控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110116435.2A CN112828894B (zh) | 2021-01-21 | 2021-01-21 | 一种冗余度机械臂的位置与力混合控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112828894A CN112828894A (zh) | 2021-05-25 |
CN112828894B true CN112828894B (zh) | 2022-09-02 |
Family
ID=75932172
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110116435.2A Active CN112828894B (zh) | 2021-01-21 | 2021-01-21 | 一种冗余度机械臂的位置与力混合控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112828894B (zh) |
Family Cites Families (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7313463B2 (en) * | 2005-03-31 | 2007-12-25 | Massachusetts Institute Of Technology | Biomimetic motion and balance controllers for use in prosthetics, orthotics and robotics |
DE102013010290A1 (de) * | 2013-06-19 | 2014-12-24 | Kuka Laboratories Gmbh | Überwachen eines kinematisch redundanten Roboters |
CN103543750B (zh) * | 2013-07-26 | 2016-03-02 | 西北工业大学 | 基于二次型规划的在轨服务航天器推力分配优化方法 |
CN108908331B (zh) * | 2018-07-13 | 2022-04-22 | 哈尔滨工业大学(深圳) | 超冗余柔性机器人的避障方法及系统、计算机存储介质 |
CN109623827B (zh) * | 2019-01-21 | 2023-04-07 | 兰州大学 | 一种高性能冗余度机械臂重复运动规划方法与装置 |
CN109514564B (zh) * | 2019-01-22 | 2021-11-30 | 江西理工大学 | 一种复合二次型多关节机械臂最优控制方法 |
EP3753684B1 (en) * | 2019-06-21 | 2022-08-10 | Robert Bosch GmbH | Method and system for robot manipulation planning |
CN112182498B (zh) * | 2020-10-10 | 2023-04-18 | 深圳市万佳安物联科技股份有限公司 | 一种基于网络表示学习的老人看护装置与方法 |
-
2021
- 2021-01-21 CN CN202110116435.2A patent/CN112828894B/zh active Active
Non-Patent Citations (1)
Title |
---|
现代优化理论与应用;邓琪;《现代优化理论与应用》;中国科学:数学;20200629;第900-968页 * |
Also Published As
Publication number | Publication date |
---|---|
CN112828894A (zh) | 2021-05-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Kaspar et al. | Sim2real transfer for reinforcement learning without dynamics randomization | |
Liu et al. | Cooperation control of multiple manipulators with passive joints | |
CN106041926A (zh) | 一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法 | |
CN110103225B (zh) | 一种数据驱动的机械臂重复运动控制方法与装置 | |
KR20110041950A (ko) | 여유자유도 제어를 이용한 로봇의 교시 및 재현 방법 | |
CN110394801B (zh) | 一种机器人的关节控制系统 | |
CN109623827B (zh) | 一种高性能冗余度机械臂重复运动规划方法与装置 | |
CN109129487B (zh) | 在周期噪声下基于泰勒型离散周期节律神经网络的冗余度机械臂重复运动规划方法 | |
CN106547989A (zh) | 具有关节柔性/臂杆柔性机械臂的位置内环阻抗控制算法 | |
CN111515928B (zh) | 机械臂运动控制系统 | |
CN108098777A (zh) | 一种冗余度机械臂力矩层重复运动控制方法 | |
CN110561441B (zh) | 一种冗余度机械臂位姿控制的单94lvi迭代算法 | |
Nemec et al. | Learning by demonstration and adaptation of finishing operations using virtual mechanism approach | |
Yuan et al. | Analytical and numerical methods for the stiffness modeling of cable-driven serpentine manipulators | |
CN112828894B (zh) | 一种冗余度机械臂的位置与力混合控制方法 | |
CN110434854B (zh) | 一种基于数据驱动的冗余度机械臂视觉伺服控制方法与装置 | |
Arimoto et al. | Iterative learning of specified motions in task-space for redundant multi-joint hand-arm robots | |
Malhan et al. | Integrating impedance control and learning based search scheme for robotic assemblies under uncertainty | |
CN112605992A (zh) | 一种双臂机器人的循环运动控制方法及装置 | |
CN113043286A (zh) | 一种多自由度机械臂实时避障路径规划系统及方法 | |
JP2024512827A (ja) | 適応コンプライアンスに基づいたロボット組立のためのシステムおよび方法 | |
CN109648567B (zh) | 一种具有容噪特性的冗余度机械臂规划方法 | |
Caccavale et al. | Achieving a cooperative behavior in a dual‐arm robot system via a modular control structure | |
Yazdani et al. | An experimental study on the failure tolerant control of a redundant planar serial manipulator via pseudo-inverse approach | |
Li et al. | Teleoperation of upper-body humanoid robot platform with hybrid motion mapping strategy |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |