一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法

文档序号:10673830阅读:639来源:国知局
一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法
【专利摘要】本发明属于机械臂控制领域,涉及一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法。本发明包括:在通用工业机械臂的腕部安装六维力/力矩传感器;根据传感器与机械臂末端工具参数,建立实际接触力的数学模型;根据力传感器的测量值,使用卡尔曼滤波器计算实际接触力;根据机械臂六个关节的位置信息,使用正运动学计算机械臂末端的当前位置等。本发明能在线实时估计环境刚度,并有效抑制力传感器中的干扰,增加了柔顺控制的稳定性,可使通用的工业机械臂具有柔顺能力,完成如轮廓跟踪、研磨、去毛刺及装配等多种复杂任务。
【专利说明】
一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法
技术领域
[0001] 本发明属于机械臂控制领域,涉及一种基于卡尔曼滤波器的工业机械臂力/位置 混合控制方法。
【背景技术】
[0002] 在工业机械臂的许多应用中,如轮廓跟踪、研磨、去毛刺和装配任务,机械臂末端 需要与刚性环境接触。为了成功执行这些任务,必须精确控制机械臂的位置与接触力,使机 械臂在接触时具有一定水平的柔顺能力。更具体的说,在需要高性能的任务中,对机械臂与 环境之间产生的接触力进行最优化对于保证稳定性并完成期望操作任务而言,是至关重要 的。
[0003] 最近的研究表明,机械臂的柔顺控制已经成为机器人发展中的中心问题。高性能 的力控制结构是直接在机械臂的各个轴上作用电机力矩,但通用的工业机械臂是通过运动 控制板卡进行控制的,其只提供位置控制模式。而且,工业机械臂通常是进行离线轨迹规 划,一旦机械臂开始执行轨迹后,便不能更改轨迹。所以,通用工业机械臂的灵活性差,很难 完成柔顺控制这样的复杂任务。
[0004] 所以,对通用的商业工业机械臂进行简单快捷地改造,在机械臂的腕部安装力/力 矩传感器,设计力/位置混合控制算法,使其具备柔顺控制的能力,完成更多复杂的任务。但 是,在物理交互控制中,一个主要的挑战是环境刚度未知或环境中有不可预测的干扰。而 且,在工业环境中,力传感器直接采集的信号中通常包含热噪声、环境噪声等其他噪声,导 致输出信号偏离真实值,甚至会影响力控制性能。本发明提供了一种基于卡尔曼滤波器的 工业机械臂力/位置混合控制方法,可使通用工业机械臂具有同时控制机械臂末端力与位 置的能力,而且本发明可以有效抑制力传感器的噪声干扰,并能在线实时估计环境刚度,增 加柔顺控制的稳定性。

【发明内容】

[0005] 本发明的目的在于提供一种克服通用工业机械臂灵活性差、抗干扰能力差的缺 陷,通过对其简单改造,使工业机械臂具有稳定的柔顺控制能力的基于卡尔曼滤波器的工 业机械臂力/位置混合控制方法。
[0006] 本发明的目的是这样实现的:
[0007] -种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法,包括如下步骤:
[0008] (1)在通用工业机械臂的腕部安装六维力/力矩传感器;
[0009] (2)根据传感器与机械臂末端工具参数,建立实际接触力的数学模型;
[0010] (3)根据力传感器的测量值,使用卡尔曼滤波器计算实际接触力;
[0011] (4)根据机械臂六个关节的位置信息,使用正运动学计算机械臂末端的当前位置;
[0012] (5)根据真实的接触力信息与机械臂末端的当前位置,使用递推最小二乘法实时 在线计算接触环境的刚度;
[0013] (6)并行计算位置控制器与力控制器,得到机械臂末端最终的期望位置,使用六自 由度机械臂逆运动学计算机械臂各轴的期望位置;
[0014] (7)使用工业机械臂运动控制卡的位置跟踪模式,实现通用工业机械臂的力/位置 混合控制。
[0015] 所述的实际接触力的数学模型具体为:
[0016] (2.1)建立机械臂末端执行器及力传感器的坐标系;
[0017] (2 · 2)设任务过程中,忽略加速度引起的惯性力与力矩的影响,补偿重力与重力矩 的影响,得到实际的环境接触力与力矩的数学模型:
[0018]
[0019] 其中,if为环境接触力与力矩,校,.Mf为六维力传感器测量的力与力矩,8*1是 相对世界坐标系的重力加速度,m为末端执行器的质量;#是坐标系Σ」相对于坐标系Σ ,的 旋转矩阵,其中,设E为末端执行器与环境的接触点,坐标系为ΣΕ,力/力矩传感器的质心为 S,传感器坐标系位Σ s,末端执行器的质心为C,坐标系Σ c,世界坐标系定义为Σ w; S(b) = Sb 为向量b的斜对称算子,&为末端执行器质心C到力传感器质心S的向量在末端执行器坐标 系中的表示,rg为末端执行器质心C到环境接触点E的向量在末端执行器坐标系中的表示。
[0020] 所述的计算实际接触力的卡尔曼滤波器具体为:
[0021 ] (3.1)在系统中加入力的变化速度足的影响,取扩展状态向量λ- e R24和测量向量 >,日股丨2分别为:
[0034]其中,下角标k,k_l表示离散时间步,wk,Vk分别表示离散系统噪声与离散量测噪 声,并假设为高斯白噪声,其协方差矩阵Qk=Q(tk);
[0022]
[0023]
[0024]
[0025]
[0026]
[0027]
[0028]
[0029]
[0030]
[0031]
[0032]
[0033]
[0035] (3.5)根据卡尔曼滤波算法,基于k-1时刻的结果计算k时刻的预测状态;和预 测协方差,得时间更新方程:
[0036]
[0037]
[0038] (3.6)计算卡尔曼增益Kk并更新协方差Pk:
[0039]
[0040]
[0041] (3.7)根据当前k时刻实际的测量值yk更新估计状态美:
[0042]
[0043] (3.8)根据新估计的状态毛计算系统输出yk;
[0044]
[0045] 所述的实时在线计算接触环境刚度的递推最小二乘法具体为:
[0046] (5.1)根据力传感器测量的力信息,使用卡尔曼滤波器计算得到实际接触力与力 矩Fe;
[0047] (5.2)把接触环境建模为刚度为Ke的弹簧,并且环境刚度Ke远小于力传感器的刚 度;把末端执行器与环境的接触力为Fe = O时的接触点E的位置设为接触参考位置XEQ,则接 触位置差 Δ Xe为 Δ Xe = Xe_Xeo = Fe/Ke;
[0048] (5.3)设机械臂末端执行器沿接触面的法线方向与接触面接触,通过各关节角与 机械臂的正运动学方程计算得到末端沿法线方向的位移X,当机械臂末端与环境接触后,根 据测得的一组值(xt,F E,t)可求得4的初值:
[0049]
[0050] (5.4)使用递推最小二乘法(RLS)迭代计算环境刚度龙/;尽.,-兌,.,X, !P1 = I; < I =0;Ct为遗忘因子,根据误差与实时调整Ct的值;ai,a^a 3可取经验值;取力传感器的多 个测量值作为接触力Fe的测量值,当I FE-Fd I彡0.5N时,认为已稳定,停止RLS计算;同理, 可计算其他方向上的乂;
[0051]
[0052]
[0053]所述的并行计算位置控制器与力控制器实现通用工业机械臂的力/位置混合控制 具体为:
[0054] (6.1)利用对角矩阵S将整个任务空间分为两个子空间,接触面的法线方向为力控 制子空间,接触面切线方向为位置控制子空间;对角阵S定义了位置的控制方向,当其主对 角线元素为1时,进行轨迹位置控制;同理,对角阵S'定义了力控制的方向;
[0055] (6.2)位置控制器与力控制器的输出最终要转换为机械臂关节空间的位置控制 量;位置控制器根据期望位置Xd计算得期望轨迹对应的笛卡尔空间的末端位置Pd;使用PI控 制器作为力控制器,根据期望力Fd、实际接触力Fe与当前的环境刚度计算期望力对应的 笛卡尔空间的末端位置Pf;位置控制器与力控制器并行计算得到机械臂末端在笛卡尔空间 的总期望位置P:
[0056]
[0057] (6.3)根据六自由度机械臂的运动学逆解,把机械臂末端的总期望位置转换为机 械臂各关节的期望位置将作业空间任意方向的力和位置,分配到各个关节,机械臂以独 立的形式同时控制力和位置;
[0058] (6.4)使用工业机械臂运动控制卡的位置跟踪模式,其特点是中间任意时刻都可 随时改变期望位置,然后以期望速度到达新的期望位置,以此实现通用工业机械臂的力/位 置混合控制。
[0059]本发明的有益效果在于:
[0060]本发明能在线实时估计环境刚度,并有效抑制力传感器中的干扰,增加了柔顺控 制的稳定性,可使通用的工业机械臂具有柔顺能力,完成如轮廓跟踪、研磨、去毛刺及装配 等多种复杂任务。
【附图说明】
[0061 ]图1是机械臂末端工具与环境的接触示意图;
[0062]图2是基于卡尔曼滤波的力/位置混合控制结构图。
【具体实施方式】
[0063] 下面结合附图对本发明做进一步描述。
[0064] 本发明提供的是一类基于卡尔曼滤波器的工业机械臂力/位置混合控制方法,属 于机械臂控制领域。通用的工业机械臂灵活性差,很难完成需要同时控制力与位置的复杂 任务,而且传感器中有大量噪声,环境刚度未知,严重影响机械臂的控制性能。针对这一现 状,本发明提出了一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法。该方法首先 根据真实接触力的数学模型,利用卡尔曼滤波器计算机械臂末端的真实接触力;然后根据 真实的接触力信息与机械臂末端的当前位置,使用递推最小二乘法实时在线计算接触环境 的刚度。最后,并行计算位置控制器与力控制器,得到机械臂末端最终的期望位置,通过逆 运动学转换为机械臂各轴的期望位置,使用工业机械臂运动控制卡的位置跟踪模式,实现 通用工业机械臂的力/位置混合控制。本发明的有益效果是,能在线实时估计环境刚度,并 有效抑制力传感器中的干扰,增加了柔顺控制的稳定性,可使通用的工业机械臂具有柔顺 能力,完成如轮廓跟踪、研磨、去毛刺及装配等多种复杂任务。
[0065]本发明采用的技术方案是:
[0066]基于卡尔曼滤波的工业机械臂力/位置混合控制算法,其中控制方法如下:
[0067] (1)在通用工业机械臂的腕部安装六维力/力矩传感器;
[0068] (2)根据传感器与机械臂末端工具参数,建立实际接触力的数学模型;
[0069] (3)根据力传感器的测量值,使用卡尔曼滤波器计算实际接触力;
[0070] (4)根据机械臂六个关节的位置信息,使用正运动学计算机械臂末端的当前位置;
[0071] (5)根据真实的接触力信息与机械臂末端的当前位置,使用递推最小二乘法实时 在线计算接触环境的刚度;
[0072] (6)并行计算位置控制器与力控制器,得到机械臂末端最终的期望位置,使用六自 由度机械臂逆运动学计算机械臂各轴的期望位置;
[0073] (7)使用工业机械臂运动控制卡的位置跟踪模式,实现通用工业机械臂的力/位置 混合控制。
[0074]作为一种优化的技术方案,控制方法具体如下:
[0075]首先,在通用工业机械臂的腕部安装六维力/力矩传感器,根据真实接触力的数学 模型,利用卡尔曼滤波器计算机械臂末端的真实接触力;然后根据真实的接触力信息与机 械臂末端的当前位置,使用递推最小二乘法实时在线计算接触环境的刚度。最后,并行计算 位置控制器与力控制器,得到机械臂末端最终的期望位置,通过逆运动学转换为机械臂各 轴的期望位置,使用工业机械臂运动控制卡的位置跟踪模式,实现通用工业机械臂的力/位 置混合控制。
[0076]作为一种优化的技术方案,实际接触力的数学模型具体为:
[0077] (1)建立机械臂末端执行器及力传感器的坐标系;
[0078] (2)设任务过程中,机械臂平稳运行且速度较低,加速度引起的惯性力与力矩的影 响可以忽略,只需补偿重力与重力矩的影响,得到实际的环境接触力与力矩的数学模型:
[0079]
[0080]其中,F/,Aif为环境接触力与力矩为六维力传感器测量的力与力矩,gw是 相对世界坐标系的重力加速度,m为末端执行器的质量;丨是坐标系Σ」相对于坐标系Σ ,的 旋转矩阵,其中,设E为末端执行器与环境的接触点,坐标系为ΣΕ,力/力矩传感器的质心为 S,传感器坐标系位Σ s,末端执行器的质心为C,坐标系Σ c,世界坐标系定义为Σ w; S(b) = Sb 为向量b的斜对称算子,r兰为末端执行器质心C到力传感器质心S的向量在末端执行器坐标 系中的表示,为末端执行器质心C到环境接触点E的向量在末端执行器坐标系中的表示。
[0081] 作为一种优化的技术方案,计算实际接触力的卡尔曼滤波器具体为:
[0082] (1)在系统中加入力的变化速度尤的影响,取扩展状态向量e R24和测量向量 _>,eR12分别为:
[0083]
[0084]
[0085] (2)连续线性系统模型与测量模型为:
[0086]
[0087]
[0088]
[0089]
[0090]
[0093]
[0091 ] (4)线性系统方程与量测方程的随机差分方程:[0092]
[0094]
[0095] 其中,下角标k,k_l表示离散时间步,wk,Vk分别表示离散系统噪声与离散量测噪 声,并假设为高斯白噪声,其协方差矩阵Qk=Q(tk)。
[0096] (5)根据卡尔曼滤波算法,基于k-1时刻的结果计算k时刻的预测状态毛〗^和预测 协方差,_得时间更新方程:
[0097]
[0098]
[0099] (6)计算卡尔曼增益Kk并更新协方差Pk:
[0100]
[0101]
[0102] (7)根据当前k时刻实际的测量值yk更新估计状态毛:
[0103]
[0104] (8)根据新估计的状态4计算系统输出yk。
[0105]
[0106] 作为一种优化的技术方案,实时在线计算接触环境刚度的递推最小二乘法具体 为:
[0107] (1)根据力传感器测量的力信息,使用卡尔曼滤波器计算得到实际接触力与力矩 Fe 〇
[0108] (2)把接触环境建模为刚度为Ke的弹簧,并且环境刚度Ke远小于力传感器的刚度。 把末端执行器与环境的接触力为Fe = O时的接触点E的位置设为接触参考位置XEQ,则接触位 置差 Δ Xe为 Δ Xe = Xe_Xeo = Fe/Ke〇
[0109] (3)设机械臂末端执行器沿接触面的法线方向与接触面接触,通过各关节角与机 械臂的正运动学方程计算得到末端沿法线方向的位移X,当机械臂末端与环境接触后,根据 测得的一组值(xt,Fe, t)可求得的初值:
[0110]
[0111] (4)使用递推最小二乘法(RLS)迭代计算环境刚度= ^丨=〇;ct为遗忘因子,根据误差色实时调整Ct的值。&1,助及 &3可取经验值。取力传感器的多 个测量值作为接触力Fe的测量值,当I FE-Fd I彡0.5N时,认为已稳定,停止RLS计算。同理, 可计算其他方向上的犮f
[0112]
[0113]
[0114] 作为一种优化的技术方案,并行计算位置控制器与力控制器实现通用工业机械臂 的力/位置混合控制具体为:
[0115] (1)利用对角矩阵S将整个任务空间分为两个子空间,接触面的法线方向为力控制 子空间,接触面切线方向为位置控制子空间。对角阵S定义了位置的控制方向,当其主对角 线元素为1时,进行轨迹位置控制。同理,对角阵S'定义了力控制的方向。
[0116] (2)位置控制器与力控制器的输出最终要转换为机械臂关节空间的位置控制量。 位置控制器根据期望位置Xd计算得期望轨迹对应的笛卡尔空间的末端位置Pd;使用PI控制 器作为力控制器,根据期望力Fd、实际接触力Fe与当前的环境刚度计算期望力对应的笛 卡尔空间的末端位置Pf。位置控制器与力控制器并行计算得到机械臂末端在笛卡尔空间的 总期望位置P:
[0117]
[0118] (3)根据六自由度机械臂的运动学逆解,把机械臂末端的总期望位置转换为机械 臂各关节的期望位置匕。将作业空间任意方向的力和位置,分配到各个关节,机械臂以独立 的形式同时控制力和位置。
[0119] (4)使用工业机械臂运动控制卡的位置跟踪模式,其特点是中间任意时刻都可随 时改变期望位置,然后以期望速度到达新的期望位置,以此实现通用工业机械臂的力/位置 混合控制。
[0120] (1)实际接触力的数学模型
[0121] 在通用工业机械臂的腕部安装力/力矩传感器,传感器测量的力Fs与力矩Ms包括: Fe ,Me为环境接触力与力矩,Fi ,Mi为惯性力与力矩,Fg ,Mg为重力与重力矩。
[0122]
(1)
[0123] 如图1所示,设C为末端执行器的质心,坐标系Xc的原点位于C,同样,设E为末端执 行器与环境的接触点,坐标系为Σ Ε。力/力矩传感器的质心为S,传感器坐标系位Σ5,世界坐 标系定义为Σ w。相对于末端执行器坐标系Σ e的牛顿-欧拉方程为:
[0124]
[0125] (3:)
[0126] 其中,末端执行器的质量为m,ge为重力加速度向量,af为工具的平移加速度,为 相对于C的转动惯量,ω Vae分别为末端执行器的角速度与角加速度。S(b) = Sb为向量b的斜 对称算子。
[0127] 设机械臂在任务过程中平稳运行且速度较低,加速度的影响可以忽略。所以,可以 忽略末端执行器的惯性力与力矩,只需补偿重力与重力矩的影响即可。所以,环境接触力与 力矩可写为:
[0128]
(4)
[0129]
(5)
[0130] 需要注意的是,式(5)中的量大部分都是在自身的坐标系内测量的,设矩阵g是坐 标系Σ」相对于X1的旋转矩阵。所以实际应用时需要对公式进行变换,写成矩阵形式为:
[0131]
[0132] (2)计算实际接触力的卡尔曼滤波器
[0133] 设所有的变量都是相对于坐标系表示的,为了方便表示,省略了上角标C。因为 力是实时变化的,所以需要考虑力的变化速度尤对系统的影响,取状态向量xeiP和测量 向量分别为:
[0134]
[0135]
[0136]
[0137]
[0138]
[0139]
[0140]
[0141] 其中,wx, vy为高斯白噪声,其协方差分别为Q,R。
[0142] 对连续线性系统进行离散化,得离散状态变量& eK24与离散量测向量乃eK12:
[0143]
[0144]
[0145]
[0146]
[0147]
[0148]
[0149] 其中,下角标k,k_l表示离散时间步,wk,Vk分别表示离散系统噪声与离散量测噪 声,其协方差矩阵Qk=Q(tk)。
[0150] 根据卡尔曼滤波算法,基于k-1时刻的结果计算k时刻的预测状态^和预测协方差 Pk,得时间更新方程:
[0151]
ii( I
[0152] 然后,基于k时刻实际的测量值使用测量更新方程修正这些预测值,其中,第一个 方程计算卡尔曼增益Kk,第二个方程根据当前的测量值y k更新估计状态xk,第三个方程更新 协方差Pk:
[0153]
(Π )
[0154] 最后,根据新估计的状态计算系统输出yk。
[0155] (3)实时在线计算接触环境刚度的递推最小二乘法
[0156] 假设机械臂末端执行器接触的环境为刚性的,则可把接触环境建模为刚度为Ke的 弹簧。其中环境刚度Ke远小于力传感器的刚度,即由接触力导致的传感器形变与导致的环 境形变相比,可以忽略不计。设末端执行器与环境的接触力为Fe = O时的接触点E(图1)的位 置为接触参考位置Xeq。接触示意图如图1所示,则相对于Xeq的接触位置差A Xe定义为:
[0157]
[0158] 设机械臂末端执行器沿接触面的法线方向与接触面接触,末端沿法线方向的位移 X可通过测量的各关节角与机器人的运动学方程计算得到;力传感器测量的沿接触面法线 方向的实际接触力Fe由卡尔曼滤波器得到。当机械臂末端与环境接触后,根据测得的一组 值(xt,Fe , t)可求得^的初值:
[0159]
(19)
[0160] 然后由的迭代计算式:其中,
~遗忘因 子。根据误差4实时调整Ct的值,当Ct较小时,A收敛快但对干扰比较敏感,反之,A收敛慢 但有较强的抗干扰能力。B1,&2及&3可取经验值。取Fe的多个测量值的均值作为实际接触力, 当|FE-Fd|彡0.5N时,认为已稳定,停止RLS计算。同理,可计算其他方向上的。
[0161] f 1,1
[0162]
[0163]
[0164] (23)
[0165] (5)力/位置混合控制算法
[0166] 如图2所示,工业机械臂的位置控制器与力控制器并行计算运行,Xd和Fd分别为期 望位置向量与期望力向量;
[0167] 利用对角矩阵S将整个任务空间分为两个子空间,接触面的法线方向为力控制子 空间,接触面切线方向为位置控制子空间。对角阵S定义了位置的控制方向,当主对角线元 素为1时,进行轨迹位置控制。同理,对角阵S'定义了力控制的方向。例如,在z轴方向控制接 触力,在xy平面上控制轨迹位置,其选择矩阵为:
[0168] S = diag(l,l,0,l,l,l),S'=diag(0,0,l,0,0,0)· (24)
[0169] 位置控制器与力控制器的输出最终要转换为机械臂关节空间的位置控制量。位置 控制器根据期望位置Xd与机械臂末端实际位置X计算得期望轨迹对应的笛卡尔空间的末端 位置Pd;力控制器,如采用PI控制器,根据期望力Fd、实际接触力Fe与当前的环境刚度^计 算期望力对应的笛卡尔空间的末端位置Pf,得机械臂末端在笛卡尔空间的总期望位置P。
[0170]
[0171] (33)
[0172] 通过六自由度机械臂的运动学逆解,把总期望位置转换为机械臂各关节的期望位 置匕。将作业空间任意方向的力和位置,分配到各个关节上,机器人以独立的形式同时控制 力和位置。
[0173] (6)通用工业机械臂的力/位置混合控制的实现
[0174]由于使用的工业机械臂只提供位置控制模式,力控制可以理解为对由力引起的位 移的控制,所以最终都是以关节位置的形式实现力/位置混合控制。
[0175]以实验工业机械臂为例,运动控制卡的位置跟踪模式为PA模式,指定期望位置后, 各关节即以期望速度到达期望位置,中间任意时刻都可随时改变期望位置,然后运动控制 卡重新规划关节运动轨迹,以期望速度到达新的期望位置。这是实现实时调整当前位置与 力的基础。
【主权项】
1. 一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法,其特征在于,包括如下 步骤: (1) 在通用工业机械臂的腕部安装六维力/力矩传感器; (2) 根据传感器与机械臂末端工具参数,建立实际接触力的数学模型; (3) 根据力传感器的测量值,使用卡尔曼滤波器计算实际接触力; (4) 根据机械臂六个关节的位置信息,使用正运动学计算机械臂末端的当前位置; (5) 根据真实的接触力信息与机械臂末端的当前位置,使用递推最小二乘法实时在线 计算接触环境的刚度; (6) 并行计算位置控制器与力控制器,得到机械臂末端最终的期望位置,使用六自由度 机械臂逆运动学计算机械臂各轴的期望位置; (7) 使用工业机械臂运动控制卡的位置跟踪模式,实现通用工业机械臂的力/位置混合 控制。2. 根据权利要求1中所述的一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方 法,其特征在于: 所述的实际接触力的数学模型具体为: (2.1) 建立机械臂末端执行器及力传感器的坐标系; (2.2) 设任务过程中,忽略加速度引起的惯性力与力矩的影响,补偿重力与重力矩的影 响,得到实际的环境接触力与力矩的数学模型:其中,为环境接触力与力矩,为六维力传感器测量的力与力矩,gw是相对 世界坐标系的重力加速度,m为末端执行器的质量;<是坐标系5^相对于坐标系5^的旋转 矩阵,其中,设E为末端执行器与环境的接触点,坐标系为2 E,力/力矩传感器的质心为S,传 感器坐标系位S s,末端执行器的质心为C,坐标系2 c,世界坐标系定义为5: w; S(b) = Sb为向 量b的斜对称算子,感为末端执行器质心C到力传感器质心S的向量在末端执行器坐标系中 的表示,为末端执行器质心C到环境接触点E的向量在末端执行器坐标系中的表示。3. 根据权利要求1中所述的一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方 法,其特征在于: 所述的计算实际接触力的卡尔曼滤波器具体为: (3 . 1)在系统中加入力的变化速度尤的影响,取扩展状态向量x e 和测量向量 jeK12分别为:(3.2) 连续线性系统模型与测量模型为: ⑴=4,(〇 + u',v y(t) =Hox(t)+Dog+vy (3.3) 取离散状态变量巧€1?24与离散量测向量%6?12为:(3.4) 线性系统方程与量测方程的随机差分方程: Xk - Axk-1+Wk-1,yk - Hxk+Dg+Vk其中,下角标k,k-l表示离散时间步,Wk,vk分别表示离散系统噪声与离散量测噪声,并 假设为高斯白噪声,其协方差矩阵Qk=Q(tk); (3.5) 根据卡尔曼滤波算法,基于k-1时刻的结果计算k时刻的预测状态元M和预测协方 差巧m,得时间更新方程:(3.6) 计算卡尔曼增益Kk并更新协方差Pk: Kk =Pklk_lHT(HPklk_lH + RT1 R=(I-KkH)F,, (3.7) 根据当前k时刻实际的测量值yk更新估计状态4 : +Kk(yk -H-w-i) (3.8) 根据新估计的状态%计算系统输出yk; yk=Hxk+Dge4.根据权利要求1中所述的一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方 法,其特征在于: 所述的实时在线计算接触环境刚度的递推最小二乘法具体为: (5.1) 根据力传感器测量的力信息,使用卡尔曼滤波器计算得到实际接触力与力矩Fe; (5.2) 把接触环境建模为刚度为Ke的弹簧,并且环境刚度Ke远小于力传感器的刚度;把 末端执行器与环境的接触力为Fe = 0时的接触点E的位置设为接触参考位置XEQ,则接触位置 差 A XE为 A XE = XE-XE0 = Fe/Ke ; (5.3) 设机械臂末端执行器沿接触面的法线方向与接触面接触,通过各关节角与机械 臂的正运动学方程计算得到末端沿法线方向的位移X,当机械臂末端与环境接触后,根据测 得的一组值(xt,F E,t)可求得(的初值:(5.4) 使用递推最小二乘法(乩5)迭代计算环境刚度&4=/^-丈',心=1; ^ 〇 ;ct为遗忘因子,根据误差3,实时调整Ct的值;m,&2及 &3可取经验值;取力传感器的多 个测量值作为接触力Fe的测量值,当| FE-Fd |彡0.5N时,认为已稳定,停止RLS计算;同理, 可计算其他方向上的犮£ ;5.根据权利要求1中所述的一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方 法,其特征在于: 所述的并行计算位置控制器与力控制器实现通用工业机械臂的力/位置混合控制具体 为: (6.1) 利用对角矩阵S将整个任务空间分为两个子空间,接触面的法线方向为力控制子 空间,接触面切线方向为位置控制子空间;对角阵S定义了位置的控制方向,当其主对角线 元素为1时,进行轨迹位置控制;同理,对角阵S'定义了力控制的方向; (6.2) 位置控制器与力控制器的输出最终要转换为机械臂关节空间的位置控制量;位 置控制器根据期望位置Xd计算得期望轨迹对应的笛卡尔空间的末端位置Pd;使用PI控制器 作为力控制器,根据期望力Fd、实际接触力Fe与当前的环境刚度(计算期望力对应的笛卡 尔空间的末端位置Pf;位置控制器与力控制器并行计算得到机械臂末端在笛卡尔空间的总 期望位置P:(6.3) 根据六自由度机械臂的运动学逆解,把机械臂末端的总期望位置转换为机械臂 各关节的期望位置匕;将作业空间任意方向的力和位置,分配到各个关节,机械臂以独立的 形式同时控制力和位置; (6.4) 使用工业机械臂运动控制卡的位置跟踪模式,其特点是中间任意时刻都可随时 改变期望位置,然后以期望速度到达新的期望位置,以此实现通用工业机械臂的力/位置混 合控制。
【文档编号】B25J9/16GK106041926SQ201610416348
【公开日】2016年10月26日
【申请日】2016年6月12日
【发明人】夏桂华, 李超, 朱齐丹, 谢心如, 蔡成涛, 吕晓龙, 张智, 郑越, 班瑞阳, 刘志林
【申请人】哈尔滨工程大学
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1