CN106041926B - 一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法 - Google Patents
一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法 Download PDFInfo
- Publication number
- CN106041926B CN106041926B CN201610416348.8A CN201610416348A CN106041926B CN 106041926 B CN106041926 B CN 106041926B CN 201610416348 A CN201610416348 A CN 201610416348A CN 106041926 B CN106041926 B CN 106041926B
- Authority
- CN
- China
- Prior art keywords
- force
- mechanical arm
- contact
- environment
- power
- 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
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/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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Control Of Position Or Direction (AREA)
Abstract
本发明属于机械臂控制领域,涉及一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法。本发明包括:在通用工业机械臂的腕部安装六维力/力矩传感器;根据传感器与机械臂末端工具参数,建立实际接触力的数学模型;根据力传感器的测量值,使用卡尔曼滤波器计算实际接触力;根据机械臂六个关节的位置信息,使用正运动学计算机械臂末端的当前位置等。本发明能在线实时估计环境刚度,并有效抑制力传感器中的干扰,增加了柔顺控制的稳定性,可使通用的工业机械臂具有柔顺能力,完成如轮廓跟踪、研磨、去毛刺及装配等多种复杂任务。
Description
技术领域
本发明属于机械臂控制领域,涉及一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法。
背景技术
在工业机械臂的许多应用中,如轮廓跟踪、研磨、去毛刺和装配任务,机械臂末端需要与刚性环境接触。为了成功执行这些任务,必须精确控制机械臂的位置与接触力,使机械臂在接触时具有一定水平的柔顺能力。更具体的说,在需要高性能的任务中,对机械臂与环境之间产生的接触力进行最优化对于保证稳定性并完成期望操作任务而言,是至关重要的。
最近的研究表明,机械臂的柔顺控制已经成为机器人发展中的中心问题。高性能的力控制结构是直接在机械臂的各个轴上作用电机力矩,但通用的工业机械臂是通过运动控制板卡进行控制的,其只提供位置控制模式。而且,工业机械臂通常是进行离线轨迹规划,一旦机械臂开始执行轨迹后,便不能更改轨迹。所以,通用工业机械臂的灵活性差,很难完成柔顺控制这样的复杂任务。
所以,对通用的商业工业机械臂进行简单快捷地改造,在机械臂的腕部安装力/力矩传感器,设计力/位置混合控制算法,使其具备柔顺控制的能力,完成更多复杂的任务。但是,在物理交互控制中,一个主要的挑战是环境刚度未知或环境中有不可预测的干扰。而且,在工业环境中,力传感器直接采集的信号中通常包含热噪声、环境噪声等其他噪声,导致输出信号偏离真实值,甚至会影响力控制性能。本发明提供了一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法,可使通用工业机械臂具有同时控制机械臂末端力与位置的能力,而且本发明可以有效抑制力传感器的噪声干扰,并能在线实时估计环境刚度,增加柔顺控制的稳定性。
发明内容
本发明的目的在于提供一种克服通用工业机械臂灵活性差、抗干扰能力差的缺陷,通过对其简单改造,使工业机械臂具有稳定的柔顺控制能力的基于卡尔曼滤波器的工业机械臂力/位置混合控制方法。
本发明的目的是这样实现的:
一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法,包括如下步骤:
(1)在通用工业机械臂的腕部安装六维力/力矩传感器;
(2)根据传感器与机械臂末端工具参数,建立实际接触力的数学模型;
(3)根据力传感器的测量值,使用卡尔曼滤波器计算实际接触力;
(4)根据机械臂六个关节的位置信息,使用正运动学计算机械臂末端的当前位置;
(5)根据真实的接触力信息与机械臂末端的当前位置,使用递推最小二乘法实时在线计算接触环境的刚度;
(6)并行计算位置控制器与力控制器,得到机械臂末端最终的期望位置,使用六自由度机械臂逆运动学计算机械臂各轴的期望位置;
(7)使用工业机械臂运动控制卡的位置跟踪模式,实现通用工业机械臂的力/位置混合控制。
所述的实际接触力的数学模型具体为:
(2.1)建立机械臂末端执行器及力传感器的坐标系;
(2.2)设任务过程中,忽略加速度引起的惯性力与力矩的影响,补偿重力与重力矩的影响,得到实际的环境接触力与力矩的数学模型:
其中,为环境接触力与力矩,为六维力传感器测量的力与力矩,gW是相对世界坐标系的重力加速度,m为末端执行器的质量;是坐标系Σj相对于坐标系Σi的旋转矩阵,其中,设E为末端执行器与环境的接触点,坐标系为ΣE,力/力矩传感器的质心为S,传感器坐标系为ΣS,末端执行器的质心为C,坐标系ΣC,世界坐标系定义为ΣW;S(b)=Sb为向量b的斜对称算子,为末端执行器质心C到力/力矩传感器的质心S的向量在末端执行器坐标系中的表示,为末端执行器质心C到环境接触点E的向量在末端执行器坐标系中的表示。
所述使用卡尔曼滤波器计算实际接触力具体为:
(3.1)在系统中加入力的变化速度的影响,取扩展状态向量和测量向量分别为:
(3.2)连续线性系统模型与测量模型为:
y(t)=H0x(t)+D0g+vy
(3.3)取离散状态变量与离散量测向量为:
(3.4)线性系统方程与量测方程的随机差分方程:
xk=Axk-1+wk-1,yk=Hxk+Dg+vk
H=[I6×6 C6×6 06×12],
其中,下角标k,k-1表示离散时间步,wk,vk分别表示离散系统噪声与离散量测噪声,并假设为高斯白噪声,其协方差矩阵Qk=Q(tk);
(3.5)根据卡尔曼滤波算法,基于k-1时刻的结果计算k时刻的预测状态和预测协方差得时间更新方程:
(3.6)计算卡尔曼增益Kk并更新协方差Pk:
(3.7)根据当前k时刻实际的测量值yk更新估计状态
(3.8)根据新估计的状态计算系统输出yk;
所述的使用递推最小二乘法实时在线计算接触环境的刚度具体为:
(5.1)根据力传感器测量的力信息,使用卡尔曼滤波器计算得到实际接触力与力矩FE;
(5.2)把接触环境建模为刚度为的弹簧,并且环境刚度远小于力传感器的刚度;把末端执行器与环境的接触力为FE=0时的接触点E的位置设为接触参考位置XE0,则接触位置差ΔXE为
(5.3)设机械臂末端执行器沿接触面的法线方向与接触面接触,通过各关节角与机械臂的正运动学方程计算得到末端沿法线方向的位移x,当机械臂末端与环境接触后,根据测得的一组值(xt,FE,t)可求得的初值:
(5.4)使用递推最小二乘法(RLS)迭代计算环境刚度 P1=I;Ct为遗忘因子,根据误差实时调整Ct的值;a1,a2及a3可取经验值;取力传感器的多个测量值作为接触力FE的测量值,Fd是期望力,当|FE-Fd|≤0.5N时,认为已稳定,停止RLS计算;同理,可计算其他方向上的
Kt+1=Pt+1xt+1,Pt+1=Pt/(Ct+xt+1Ptxt+1),
所述的并行计算位置控制器与力控制器实现通用工业机械臂的力/位置混合控制具体为:
(6.1)利用对角矩阵S将整个任务空间分为两个子空间,接触面的法线方向为力控制子空间,接触面切线方向为位置控制子空间;对角阵S定义了位置的控制方向,当其主对角线元素为1时,进行轨迹位置控制;同理,对角阵S`定义了力控制的方向;
(6.2)位置控制器与力控制器的输出最终要转换为机械臂关节空间的位置控制量;位置控制器根据期望位置xd计算得期望轨迹对应的笛卡尔空间的末端位置Pd;使用PI控制器作为力控制器,根据期望力Fd、实际接触力FE与当前的环境刚度计算期望力对应的笛卡尔空间的末端位置Pf;位置控制器与力控制器并行计算得到机械臂末端在笛卡尔空间的总期望位置P:
(6.3)根据六自由度机械臂的运动学逆解,把机械臂末端的总期望位置转换为机械臂各关节的期望位置Pj;将作业空间任意方向的力和位置,分配到各个关节,机械臂以独立的形式同时控制力和位置;
(6.4)使用工业机械臂运动控制卡的位置跟踪模式,其特点是中间任意时刻都可随时改变期望位置,然后以期望速度到达新的期望位置,以此实现通用工业机械臂的力/位置混合控制。
本发明的有益效果在于:
本发明能在线实时估计环境刚度,并有效抑制力传感器中的干扰,增加了柔顺控制的稳定性,可使通用的工业机械臂具有柔顺能力,完成如轮廓跟踪、研磨、去毛刺及装配等多种复杂任务。
附图说明
图1是机械臂末端工具与环境的接触示意图;
图2是基于卡尔曼滤波的力/位置混合控制结构图。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明提供的是一类基于卡尔曼滤波器的工业机械臂力/位置混合控制方法,属于机械臂控制领域。通用的工业机械臂灵活性差,很难完成需要同时控制力与位置的复杂任务,而且传感器中有大量噪声,环境刚度未知,严重影响机械臂的控制性能。针对这一现状,本发明提出了一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法。该方法首先根据真实接触力的数学模型,利用卡尔曼滤波器计算机械臂末端的真实接触力;然后根据真实的接触力信息与机械臂末端的当前位置,使用递推最小二乘法实时在线计算接触环境的刚度。最后,并行计算位置控制器与力控制器,得到机械臂末端最终的期望位置,通过逆运动学转换为机械臂各轴的期望位置,使用工业机械臂运动控制卡的位置跟踪模式,实现通用工业机械臂的力/位置混合控制。本发明的有益效果是,能在线实时估计环境刚度,并有效抑制力传感器中的干扰,增加了柔顺控制的稳定性,可使通用的工业机械臂具有柔顺能力,完成如轮廓跟踪、研磨、去毛刺及装配等多种复杂任务。
本发明采用的技术方案是:
基于卡尔曼滤波的工业机械臂力/位置混合控制算法,其中控制方法如下:
(1)在通用工业机械臂的腕部安装六维力/力矩传感器;
(2)根据传感器与机械臂末端工具参数,建立实际接触力的数学模型;
(3)根据力传感器的测量值,使用卡尔曼滤波器计算实际接触力;
(4)根据机械臂六个关节的位置信息,使用正运动学计算机械臂末端的当前位置;
(5)根据真实的接触力信息与机械臂末端的当前位置,使用递推最小二乘法实时在线计算接触环境的刚度;
(6)并行计算位置控制器与力控制器,得到机械臂末端最终的期望位置,使用六自由度机械臂逆运动学计算机械臂各轴的期望位置;
(7)使用工业机械臂运动控制卡的位置跟踪模式,实现通用工业机械臂的力/位置混合控制。
作为一种优化的技术方案,控制方法具体如下:
首先,在通用工业机械臂的腕部安装六维力/力矩传感器,根据真实接触力的数学模型,利用卡尔曼滤波器计算机械臂末端的真实接触力;然后根据真实的接触力信息与机械臂末端的当前位置,使用递推最小二乘法实时在线计算接触环境的刚度。最后,并行计算位置控制器与力控制器,得到机械臂末端最终的期望位置,通过逆运动学转换为机械臂各轴的期望位置,使用工业机械臂运动控制卡的位置跟踪模式,实现通用工业机械臂的力/位置混合控制。
作为一种优化的技术方案,实际接触力的数学模型具体为:
(1)建立机械臂末端执行器及力传感器的坐标系;
(2)设任务过程中,机械臂平稳运行且速度较低,加速度引起的惯性力与力矩的影响可以忽略,只需补偿重力与重力矩的影响,得到实际的环境接触力与力矩的数学模型:
其中,为环境接触力与力矩,为六维力传感器测量的力与力矩,gW是相对世界坐标系的重力加速度,m为末端执行器的质量;是坐标系Σj相对于坐标系Σi的旋转矩阵,其中,设E为末端执行器与环境的接触点,坐标系为ΣE,力/力矩传感器的质心为S,传感器坐标系为ΣS,末端执行器的质心为C,坐标系ΣC,世界坐标系定义为ΣW;S(b)=Sb为向量b的斜对称算子,为末端执行器质心C到力/力矩传感器的质心S的向量在末端执行器坐标系中的表示,为末端执行器质心C到环境接触点E的向量在末端执行器坐标系中的表示。
作为一种优化的技术方案,计算实际接触力的卡尔曼滤波器具体为:
(1)在系统中加入力的变化速度的影响,取扩展状态向量和测量向量分别为:
(2)连续线性系统模型与测量模型为:
y(t)=H0x(t)+D0g+vy
(3)取离散状态变量与离散量测向量为:
(4)线性系统方程与量测方程的随机差分方程:
xk=Axk-1+wk-1,yk=Hxk+Dg+vk
H=[I6×6 C6×6 06×12],
其中,下角标k,k-1表示离散时间步,wk,vk分别表示离散系统噪声与离散量测噪声,并假设为高斯白噪声,其协方差矩阵Qk=Q(tk)。
(5)根据卡尔曼滤波算法,基于k-1时刻的结果计算k时刻的预测状态和预测协方差得时间更新方程:
(6)计算卡尔曼增益Kk并更新协方差Pk:
(7)根据当前k时刻实际的测量值yk更新估计状态
(8)根据新估计的状态计算系统输出yk。
作为一种优化的技术方案,使用递推最小二乘法实时在线计算接触环境的刚度具体为:
(1)根据力传感器测量的力信息,使用卡尔曼滤波器计算得到实际接触力与力矩FE。
(2)把接触环境建模为刚度为的弹簧,并且环境刚度远小于力传感器的刚度。把末端执行器与环境的接触力为FE=0时的接触点E的位置设为接触参考位置XE0,则接触位置差ΔXE为
(3)设机械臂末端执行器沿接触面的法线方向与接触面接触,通过各关节角与机械臂的正运动学方程计算得到末端沿法线方向的位移x,当机械臂末端与环境接触后,根据测得的一组值(xt,FE,t)可求得的初值:
(4)使用递推最小二乘法(RLS)迭代计算环境刚度 P1=I;Ct为遗忘因子,根据误差实时调整Ct的值。a1,a2及a3可取经验值。取力传感器的多个测量值作为接触力FE的测量值,Fd是期望力,当|FE-Fd|≤0.5N时,认为已稳定,停止RLS计算。同理,可计算其他方向上的
Kt+1=Pt+1xt+1,Pt+1=Pt/(Ct+xt+1Ptxt+1),
作为一种优化的技术方案,并行计算位置控制器与力控制器实现通用工业机械臂的力/位置混合控制具体为:
(1)利用对角矩阵S将整个任务空间分为两个子空间,接触面的法线方向为力控制子空间,接触面切线方向为位置控制子空间。对角阵S定义了位置的控制方向,当其主对角线元素为1时,进行轨迹位置控制。同理,对角阵S`定义了力控制的方向。
(2)位置控制器与力控制器的输出最终要转换为机械臂关节空间的位置控制量。位置控制器根据期望位置xd计算得期望轨迹对应的笛卡尔空间的末端位置Pd;使用PI控制器作为力控制器,根据期望力Fd、实际接触力FE与当前的环境刚度计算期望力对应的笛卡尔空间的末端位置Pf。位置控制器与力控制器并行计算得到机械臂末端在笛卡尔空间的总期望位置P:
(3)根据六自由度机械臂的运动学逆解,把机械臂末端的总期望位置转换为机械臂各关节的期望位置Pj。将作业空间任意方向的力和位置,分配到各个关节,机械臂以独立的形式同时控制力和位置。
(4)使用工业机械臂运动控制卡的位置跟踪模式,其特点是中间任意时刻都可随时改变期望位置,然后以期望速度到达新的期望位置,以此实现通用工业机械臂的力/位置混合控制。
(1)实际接触力的数学模型
在通用工业机械臂的腕部安装力/力矩传感器,传感器测量的力FS与力矩MS包括:FE,ME为环境接触力与力矩,FI,MI为惯性力与力矩,FG,MG为重力与重力矩。
如图1所示,设C为末端执行器的质心,坐标系ΣC的原点位于C,同样,设E为末端执行器与环境的接触点,坐标系为ΣE。力/力矩传感器的质心为S,传感器坐标系位ΣS,世界坐标系定义为ΣW。相对于末端执行器坐标系ΣC的牛顿-欧拉方程为:
其中,末端执行器的质量为m,gC为重力加速度向量,为工具的平移加速度,为相对于C的转动惯量,ωC/αC分别为末端执行器的角速度与角加速度。S(b)=Sb为向量b的斜对称算子。
设机械臂在任务过程中平稳运行且速度较低,加速度的影响可以忽略。所以,可以忽略末端执行器的惯性力与力矩,只需补偿重力与重力矩的影响即可。所以,环境接触力与力矩可写为:
需要注意的是,式(5)中的量大部分都是在自身的坐标系内测量的,设矩阵是坐标系Σj相对于Σi的旋转矩阵。所以实际应用时需要对公式进行变换,写成矩阵形式为:
(2)使用卡尔曼滤波器计算实际接触力
设所有的变量都是相对于坐标系ΣC表示的,为了方便表示,省略了上角标C。因为力是实时变化的,所以需要考虑力的变化速度对系统的影响,取状态向量和测量向量分别为:
连续线性系统模型与测量模型为:
y(t)=H0x(t)+D0g+vy (10)
H0=[I6×6 C6×6 06×12],
其中,wx,vy为高斯白噪声,其协方差分别为Q,R。
对连续线性系统进行离散化,得离散状态变量与离散量测向量
得线性系统方程与量测方程的随机差分方程:
xk=Axk-1+wk-1 (13
yk=Hxk+Dg+vk (14)
H=H0,D=D0 (15)
其中,下角标k,k-1表示离散时间步,wk,vk分别表示离散系统噪声与离散量测噪声,其协方差矩阵Qk=Q(tk)。
根据卡尔曼滤波算法,基于k-1时刻的结果计算k时刻的预测状态xk和预测协方差Pk,得时间更新方程:
然后,基于k时刻实际的测量值使用测量更新方程修正这些预测值,其中,第一个方程计算卡尔曼增益Kk,第二个方程根据当前的测量值yk更新估计状态xk,第三个方程更新协方差Pk:
最后,根据新估计的状态计算系统输出yk。
(3)实时在线计算接触环境刚度的递推最小二乘法
假设机械臂末端执行器接触的环境为刚性的,则可把接触环境建模为刚度为KE的弹簧。其中环境刚度KE远小于力传感器的刚度,即由接触力导致的传感器形变与导致的环境形变相比,可以忽略不计。设末端执行器与环境的接触力为FE=0时的接触点E(图1)的位置为接触参考位置XE0。接触示意图如图1所示,则相对于XE0的接触位置差ΔXE定义为:
ΔXE=XE-XE0=FE/KE (18)
设机械臂末端执行器沿接触面的法线方向与接触面接触,末端沿法线方向的位移x可通过测量的各关节角与机器人的运动学方程计算得到;力传感器测量的沿接触面法线方向的实际接触力FE由卡尔曼滤波器得到。当机械臂末端与环境接触后,根据测得的一组值(xt,FE,t)可求得的初值:
然后由RLS得的迭代计算式:其中,P1=I;Ct为遗忘因子。根据误差实时调整Ct的值,当Ct较小时,收敛快但对干扰比较敏感,反之,收敛慢但有较强的抗干扰能力。a1,a2及a3可取经验值。取FE的多个测量值的均值作为实际接触力,当|FE-Fd|≤0.5N时,认为已稳定,停止RLS计算。同理,可计算其他方向上的
Kt+1=Pt+1xt+1 (21)
Pt+1=Pt/(Ct+xt+1Ptxt+1) (22)
(5)力/位置混合控制算法
如图2所示,工业机械臂的位置控制器与力控制器并行计算运行,xd和Fd分别为期望位置向量与期望力向量;
利用对角矩阵S将整个任务空间分为两个子空间,接触面的法线方向为力控制子空间,接触面切线方向为位置控制子空间。对角阵S定义了位置的控制方向,当主对角线元素为1时,进行轨迹位置控制。同理,对角阵S`定义了力控制的方向。例如,在z轴方向控制接触力,在xy平面上控制轨迹位置,其选择矩阵为:
S=diag(1,1,0,1,1,1),S`=diag(0,0,1,0,0,0). (24)
位置控制器与力控制器的输出最终要转换为机械臂关节空间的位置控制量。位置控制器根据期望位置xd与机械臂末端实际位置x计算得期望轨迹对应的笛卡尔空间的末端位置Pd;力控制器,如采用PI控制器,根据期望力Fd、实际接触力FE与当前的环境刚度计算期望力对应的笛卡尔空间的末端位置Pf,得机械臂末端在笛卡尔空间的总期望位置P。
P=S·Pd+S`·Pf (32)
通过六自由度机械臂的运动学逆解,把总期望位置转换为机械臂各关节的期望位置Pj。将作业空间任意方向的力和位置,分配到各个关节上,机器人以独立的形式同时控制力和位置。
(6)通用工业机械臂的力/位置混合控制的实现
由于使用的工业机械臂只提供位置控制模式,力控制可以理解为对由力引起的位移的控制,所以最终都是以关节位置的形式实现力/位置混合控制。
以实验工业机械臂为例,运动控制卡的位置跟踪模式为PA模式,指定期望位置后,各关节即以期望速度到达期望位置,中间任意时刻都可随时改变期望位置,然后运动控制卡重新规划关节运动轨迹,以期望速度到达新的期望位置。这是实现实时调整当前位置与力的基础。
Claims (5)
1.一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法,其特征在于,包括如下步骤:
(1)在通用工业机械臂的腕部安装六维力/力矩传感器;
(2)根据传感器与机械臂末端工具参数,建立实际接触力的数学模型;
(3)根据力传感器的测量值,使用卡尔曼滤波器计算实际接触力;
(4)根据机械臂六个关节的位置信息,使用正运动学计算机械臂末端的当前位置;
(5)根据真实的接触力信息与机械臂末端的当前位置,使用递推最小二乘法实时在线计算接触环境的刚度;
(6)并行计算位置控制器与力控制器,得到机械臂末端最终的期望位置,使用六自由度机械臂逆运动学计算机械臂各轴的期望位置;
(7)使用工业机械臂运动控制卡的位置跟踪模式,实现通用工业机械臂的力/位置混合控制。
2.根据权利要求1中所述的一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法,其特征在于:
所述的实际接触力的数学模型具体为:
(2.1)建立机械臂末端执行器及力传感器的坐标系;
(2.2)设任务过程中,忽略加速度引起的惯性力与力矩的影响,补偿重力与重力矩的影响,得到实际的环境接触力与力矩的数学模型:
其中,为环境接触力与力矩,为六维力传感器测量的力与力矩,gW是相对世界坐标系的重力加速度,m为末端执行器的质量;是坐标系Σj相对于坐标系Σi的旋转矩阵,其中,设E为末端执行器与环境的接触点,坐标系为ΣE,力/力矩传感器的质心为S,传感器坐标系为ΣS,末端执行器的质心为C,坐标系ΣC,世界坐标系定义为ΣW;S(b)=Sb为向量b的斜对称算子,为末端执行器质心C到力/力矩传感器的质心S的向量在末端执行器坐标系中的表示,为末端执行器质心C到环境接触点E的向量在末端执行器坐标系中的表示。
3.根据权利要求1中所述的一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法,其特征在于:
所述使用卡尔曼滤波器计算实际接触力具体为:
(3.1)在系统中加入力的变化速度的影响,取扩展状态向量和测量向量分别为:
(3.2)连续线性系统模型与测量模型为:
y(t)=H0x(t)+D0g+vy
(3.3)取离散状态变量与离散量测向量为:
(3.4)线性系统方程与量测方程的随机差分方程:
xk=Axk-1+wk-1,yk=Hxk+Dg+vk
其中,下角标k,k-1表示离散时间步,wk,vk分别表示离散系统噪声与离散量测噪声,并假设为高斯白噪声,其协方差矩阵Qk=Q(tk);
(3.5)根据卡尔曼滤波算法,基于k-1时刻的结果计算k时刻的预测状态和预测协方差得时间更新方程:
(3.6)计算卡尔曼增益Kk并更新协方差Pk:
(3.7)根据当前k时刻实际的测量值yk更新估计状态
(3.8)根据新估计的状态计算系统输出yk;
4.根据权利要求1中所述的一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法,其特征在于:
所述的使用递推最小二乘法实时在线计算接触环境的刚度具体为:
(5.1)根据力传感器测量的力信息,使用卡尔曼滤波器计算得到实际接触力与力矩FE;
(5.2)把接触环境建模为刚度为的弹簧,并且环境刚度远小于力传感器的刚度;把末端执行器与环境的接触力为FE=0时的接触点E的位置设为接触参考位置XE0,则接触位置差ΔXE为
(5.3)设机械臂末端执行器沿接触面的法线方向与接触面接触,通过各关节角与机械臂的正运动学方程计算得到末端沿法线方向的位移x,当机械臂末端与环境接触后,根据测得的一组值(xt,FE,t)可求得的初值:
(5.4)使用递推最小二乘法(RLS)迭代计算环境刚度 Ct为遗忘因子,根据误差实时调整Ct的值;a1,a2及a3可取经验值;取力传感器的多个测量值作为接触力FE的测量值,Fd是期望力,当|FE-Fd|≤0.5N时,认为已稳定,停止RLS计算;同理,可计算其他方向上的
5.根据权利要求1中所述的一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法,其特征在于:
所述的并行计算位置控制器与力控制器实现通用工业机械臂的力/位置混合控制具体为:
(6.1)利用对角矩阵S将整个任务空间分为两个子空间,接触面的法线方向为力控制子空间,接触面切线方向为位置控制子空间;对角阵S定义了位置的控制方向,当其主对角线元素为1时,进行轨迹位置控制;同理,对角阵S`定义了力控制的方向;
(6.2)位置控制器与力控制器的输出最终要转换为机械臂关节空间的位置控制量;位置控制器根据期望位置xd计算得期望轨迹对应的笛卡尔空间的末端位置Pd;使用PI控制器作为力控制器,根据期望力Fd、实际接触力FE与当前的环境刚度计算期望力对应的笛卡尔空间的末端位置Pf;位置控制器与力控制器并行计算得到机械臂末端在笛卡尔空间的总期望位置P:
(6.3)根据六自由度机械臂的运动学逆解,把机械臂末端的总期望位置转换为机械臂各关节的期望位置Pj;将作业空间任意方向的力和位置,分配到各个关节,机械臂以独立的形式同时控制力和位置;
(6.4)使用工业机械臂运动控制卡的位置跟踪模式,其特点是中间任意时刻都可随时改变期望位置,然后以期望速度到达新的期望位置,以此实现通用工业机械臂的力/位置混合控制。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610416348.8A CN106041926B (zh) | 2016-06-12 | 2016-06-12 | 一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610416348.8A CN106041926B (zh) | 2016-06-12 | 2016-06-12 | 一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106041926A CN106041926A (zh) | 2016-10-26 |
CN106041926B true CN106041926B (zh) | 2018-10-19 |
Family
ID=57171057
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610416348.8A Active CN106041926B (zh) | 2016-06-12 | 2016-06-12 | 一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106041926B (zh) |
Families Citing this family (27)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108115669B (zh) * | 2016-11-26 | 2021-03-26 | 沈阳新松机器人自动化股份有限公司 | 一种机器人浮动控制方法、装置及系统 |
CN106891339B (zh) * | 2017-02-10 | 2019-08-06 | 广东省智能制造研究所 | 带重力补偿的打磨机器人及其打磨方法 |
CN106945040A (zh) * | 2017-03-24 | 2017-07-14 | 北京理工大学 | 一种用于单自由度柔性末端的机械臂控制算法 |
CN107009368A (zh) * | 2017-03-30 | 2017-08-04 | 东南大学 | 基于力传感器的机器人打磨作业控制系统及其控制方法 |
US10773383B2 (en) * | 2017-05-22 | 2020-09-15 | Fanuc America Corporation | Robot high frequency position streaming |
CN107315342A (zh) * | 2017-07-03 | 2017-11-03 | 河北工业大学 | 一种改进卡尔曼滤波坐标分离机械手控制算法 |
CN107097231A (zh) * | 2017-07-06 | 2017-08-29 | 哈尔滨工业大学深圳研究生院 | 一种基于视觉伺服的同心管机器人精确运动控制方法 |
CN107390525B (zh) * | 2017-07-27 | 2020-07-10 | 清华大学 | 一种应用于混联机构的控制系统参数整定方法 |
CN107351086B (zh) * | 2017-08-22 | 2020-09-22 | 华南理工大学 | 一种scara机器人关节力矩的卡尔曼估计方法 |
CN107962480B (zh) * | 2017-11-28 | 2019-10-15 | 华中科技大学 | 一种叶片机器人砂带磨削加工力控制方法 |
CN108555903B (zh) * | 2017-12-30 | 2021-12-10 | 芜湖哈特机器人产业技术研究院有限公司 | 一种清洗液压机械臂的力控制方法 |
CN108433814B (zh) * | 2018-03-16 | 2019-12-24 | 微创(上海)医疗机器人有限公司 | 手术机器人系统及其手术器械 |
CN109129482B (zh) * | 2018-08-29 | 2021-05-25 | 武汉理工大学 | 一种动态补偿机器人直线导轨运动误差的方法 |
DE102019108390B3 (de) * | 2019-04-01 | 2020-08-06 | Franka Emika Gmbh | Vorgeben von sicheren Geschwindigkeiten für einen Robotermanipulator |
CN111098309A (zh) * | 2020-01-06 | 2020-05-05 | 江南大学 | 一种机械臂混杂控制方法、装置及系统 |
CN111230879B (zh) * | 2020-02-20 | 2023-07-11 | 佛山科学技术学院 | 一种基于力传感器的机器人末端接触力补偿方法及系统 |
CN111283687B (zh) * | 2020-03-17 | 2021-04-30 | 北京理工大学 | 一种机器人关节位置控制系统及其动态力矩的反馈补偿方法 |
CN111571577B (zh) * | 2020-04-03 | 2021-07-16 | 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) | 绳驱机器人控制方法及系统 |
CN111730599B (zh) * | 2020-07-08 | 2021-09-07 | 深圳市优必选科技股份有限公司 | 阻抗控制方法、装置、阻抗控制器和机器人 |
CN112223788B (zh) * | 2020-09-08 | 2022-02-01 | 武汉大学 | 纤维自动铺放系统以及其铺放轨迹的规划方法 |
CN112372630B (zh) * | 2020-09-24 | 2022-02-22 | 哈尔滨工业大学(深圳) | 一种多机械臂协同打磨力柔顺控制方法和系统 |
CN112549030B (zh) * | 2020-12-08 | 2022-04-22 | 北京航空航天大学 | 一种基于抗干扰滤波的空间机械臂柔性关节位姿估计方法 |
CN113110051B (zh) * | 2021-04-14 | 2022-03-04 | 南开大学 | 考虑误差约束的打磨机器人力/位混合控制方法及系统 |
CN114441807B (zh) * | 2021-07-22 | 2023-07-07 | 荣耀终端有限公司 | 一种接线方法及系统 |
CN114310974B (zh) * | 2021-12-08 | 2023-08-25 | 清华大学 | 一种基于六维力信号的机器人遥操作方法和装置 |
CN114789432B (zh) * | 2022-03-31 | 2023-08-29 | 西安交通大学 | 一种面向建筑板材安装的双臂机器人力位混合控制方法 |
CN116985145B (zh) * | 2023-09-26 | 2023-12-29 | 西北工业大学太仓长三角研究院 | 基于力位混合控制的冗余偏置机械臂末端柔顺控制方法 |
Family Cites Families (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6141863A (en) * | 1996-10-24 | 2000-11-07 | Fanuc Ltd. | Force-controlled robot system with visual sensor for performing fitting operation |
JP2008296310A (ja) * | 2007-05-30 | 2008-12-11 | Fanuc Ltd | 加工ロボットの制御装置 |
US8573070B2 (en) * | 2011-02-22 | 2013-11-05 | The Boeing Company | Force and normality sensing for end effector clamp |
FR2978844B1 (fr) * | 2011-08-04 | 2014-03-21 | Aldebaran Robotics | Robot a articulations de rigidite variable et methode de calcul de ladite rigidite optimisee |
JP2014176940A (ja) * | 2013-03-15 | 2014-09-25 | Yaskawa Electric Corp | ロボットシステム、ロボット制御方法及び被加工物の製造方法 |
CN103433921B (zh) * | 2013-09-11 | 2015-07-08 | 北京邮电大学 | 三自由度平动力反馈手控器 |
DE202013105036U1 (de) * | 2013-11-08 | 2015-02-10 | Daimler Ag | Erfassungseinrichtung |
-
2016
- 2016-06-12 CN CN201610416348.8A patent/CN106041926B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN106041926A (zh) | 2016-10-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106041926B (zh) | 一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法 | |
CN109382828B (zh) | 一种基于示教学习的机器人轴孔装配系统及方法 | |
Yang et al. | Force sensorless admittance control for teleoperation of uncertain robot manipulator using neural networks | |
Li et al. | Reinforcement learning of manipulation and grasping using dynamical movement primitives for a humanoidlike mobile manipulator | |
CN109483556B (zh) | 一种基于示教学习的机器人打磨系统及方法 | |
Zeng et al. | Force/torque sensorless compliant control strategy for assembly tasks using a 6-DOF collaborative robot | |
Cheng et al. | Motion controller design for contour-following tasks based on real-time contour error estimation | |
CN106483964B (zh) | 一种基于接触力观测器的机器人柔顺控制方法 | |
Quigley et al. | Low-cost accelerometers for robotic manipulator perception | |
Karayiannidis et al. | Online contact point estimation for uncalibrated tool use | |
Cui et al. | A Darboux-frame-based formulation of spin-rolling motion of rigid objects with point contact | |
CN111319036A (zh) | 基于自适应算法的移动机械臂位置/力自抗扰控制方法 | |
Fan et al. | Data-driven motion-force control scheme for redundant manipulators: A kinematic perspective | |
Chen et al. | Direct joint space state estimation in robots with multiple elastic joints | |
Li et al. | Model-free impedance control for safe human-robot interaction | |
CN113189950B (zh) | 用于大型弱刚性结构件装配的双机器人协同柔顺装调方法 | |
CN111515928B (zh) | 机械臂运动控制系统 | |
Shen et al. | Leader-follower formation control without leader’s velocity information | |
Ahmad et al. | Shape recovery from robot contour-tracking with force feedback | |
Li et al. | Impedance control for multi-point human-robot interaction | |
Arimoto et al. | Iterative learning of specified motions in task-space for redundant multi-joint hand-arm robots | |
Xia et al. | Hybrid force/position control of industrial robotic manipulator based on Kalman filter | |
CN114671050B (zh) | 基于一体化线性算子和抗饱和技术的航天器跟踪控制方法 | |
CN116141329A (zh) | 一种基于自校正控制的空间闭环双臂机器人运动控制方法 | |
CN114952858A (zh) | 一种基于摩擦补偿控制的工业机器人轨迹跟踪方法和系统 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |