机械臂力控----积分自适应导纳控制

原理讲解
源代码

该控制算法为我在传统导纳和自适应导纳的基础上改进的算法，其优点是解决了传统导纳稳态误差问题，克服了自适应导纳离散非线性的题，同时该算法通过参数的选取实现不变代码的前提下，实现导纳控制，自适应导纳控制。

原理讲解

源代码

#!/usr/bin/env python
# -*-coding:utf-8-*-
#本文档用于阻抗基本函数
#程序员：陈永*
#版权：哈尔滨工业大学
#日期：初稿：2019.11.21
import numpy as np
from math import pi
import math
import numpy.linalg as nla
import sys
import timeimport matplotlib.pyplot as plt#自定义文件
import BaseFunction as bf
import Kinematics as kin
import RobotParameter as rp
import DataProcessing as dp
import Filter
#解决中文显示问题
plt.rcParams['font.sans-serif'] = ['SimHei'] # 指定默认字体
plt.rcParams['axes.unicode_minus'] = False # 解决保存图像是负号'-'显示为方块的问题#================创建一个适用于积分自适应的类=================#
#基于迭代求解自适应导纳方程
class IIMPController_iter(object):#**定义属性**##阻抗参数M = np.zeros(6)B = np.zeros(6)K = np.zeros(6)I = np.zeros(6)#构造函数def __init__(self):# 位置、力误差self.Ex = np.zeros(6)   #Ex=Xr - Xd=>Xr = Xd + Exself.Ef = np.zeros(6)   #Ef=Fs - Fdself.Ex_d = np.zeros(6)self.Ef_i = np.zeros(6)# 周期self.T = 0.01#第一次标签self.first_flag = True#**定义方法**##获取控制周期def get_period(self, T):self.T = np.copy(T)#获取阻抗参数def get_imp_parameter(self,Md,Bd,Kd,Ki):self.M = np.copy(Md)self.B = np.copy(Bd)self.K = np.copy(Kd)self.I = np.copy(Ki)def get_robot_parameter(self, DH_0, q_max, q_min,):#DH参数self.DH0 = np.copy(DH_0)#关节极限self.qq_max = np.copy(q_max)self.qq_min = np.copy(q_min)#求取关节个数self.n = len(self.qq_max)#创建运动学类self.kin = kin.GeneralKinematic(DH_0, q_min, q_max)def get_current_joint(self, qq):self.qq_state = np.copy(qq)def get_current_force(self, F_t):#更新力误差self.Ef = F_t - self.ff_dprint "力误差：EF", np.round(self.Ef,2)def get_expect_joint(self, qd):Xd = self.kin.fkine_euler(qd)self.xx_d = np.copy(Xd)def get_expect_pos(self, Xd):self.xx_d = np.copy(Xd)def get_expect_force(self, Fd):self.ff_d =np.copy(Fd)def int_adp_iter_solve(self, m, b, k, ki, T, ef, ef_i, ex, ex_d):# 求当前时刻积分项efk_i = ef_i + T * ef# 计算当前加速度ex_dd = (ef + ki * efk_i - b * ex_d - k * ex) / mprint "ex_dd:", ex_dd# 求当前时刻速度exk_d = ex_d + ex_dd * Tprint "exk_d:", exk_dexk = ex + exk_d * T# print "积分项：",efk_ireturn [exk, exk_d, efk_i]def compute_imp_joint(self):#计算末端位置偏差for i in range(6):if(self.M[i] > math.pow(10, -6)):[self.Ex[i], self.Ex_d[i], self.Ef_i[i]] = self.int_adp_iter_solve(self.M[i], self.B[i], self.K[i], self.I[i],self.T, self.Ef[i], self.Ef_i[i],self.Ex[i], self.Ex_d[i])#计算参考位置print "误差修正项：", np.round(self.Ex, 3)Te = self.kin.fkine(self.qq_state)Re = Te[0:3, 0:3]self.Ex[0:3] = np.dot(Re, self.Ex[0:3])self.Ex[3:6] = np.dot(Re, self.Ex[3:6])beta = 1Xr = self.xx_d + beta*self.ExTr = np.eye(4)Rr = bf.euler_zyx2rot(Xr[3:6])Tr[0:3, 0:3] = RrTr[0:3, 3] = Xr[0:3]qr = self.kin.iterate_ikine_limit(self.qq_state, Tr)return qr#基于迭代求解自适应导纳方程:TCP坐标系中求解
class IIMPController_iter_TCP(object):# **定义属性**## 阻抗参数M = np.zeros(6)B = np.zeros(6)K = np.zeros(6)I = np.zeros(6)# 构造函数def __init__(self):# 位置、力误差self.Ex = np.zeros(6)  # Ex=Xr - Xd=>Xr = Xd + Exself.Ef = np.zeros(6)  # Ef=Fs - Fdself.Ex_d = np.zeros(6)self.Ef_i = np.zeros(6)# 周期self.T = 0.01# 第一次标签self.first_flag = True# **定义方法**## 获取控制周期def get_period(self, T):self.T = np.copy(T)# 获取阻抗参数def get_imp_parameter(self, Md, Bd, Kd, Ki):self.M = np.copy(Md)self.B = np.copy(Bd)self.K = np.copy(Kd)self.I = np.copy(Ki)def get_robot_parameter(self, DH_0, q_max, q_min):# DH参数self.DH0 = np.copy(DH_0)# 关节极限self.qq_max = np.copy(q_max)self.qq_min = np.copy(q_min)# 求取关节个数self.n = len(self.qq_max)# 创建运动学类self.kin = kin.GeneralKinematic(DH_0, self.qq_min, self.qq_max)def get_current_joint(self, qq):self.qq_state = np.copy(qq)def get_current_force(self, F_t):# 转换到基坐标系self.tcp_f = np.copy(F_t)# 更新力误差self.Ef = self.tcp_f - self.ff_d#print "力误差：EF", np.round(self.Ef, 2)def get_expect_joint(self, qd):Xd = self.kin.fkine_euler(qd)self.xx_d = np.copy(Xd)def get_expect_pos(self, Xd):self.xx_d = np.copy(Xd)def get_expect_force(self, Fd):self.ff_d = np.copy(Fd)def force_end_to_base(self, F):base_F = np.zeros(6)Te = self.kin.fkine(self.qq_state)Re = Te[0:3, 0:3]base_F[0:3] = np.dot(Re, F[0:3])base_F[3:6] = np.dot(Re, F[3:6])return base_Fdef int_adp_iter_solve(self, m, b, k, ki, T, ef, ef_i, ex, ex_d):# 求当前时刻积分项efk_i = ef_i + T * ef# 计算当前加速度ex_dd = (ef + ki * efk_i - b * ex_d - k * ex) / mprint "ex_dd:", ex_dd# 求当前时刻速度exk_d = ex_d + ex_dd * Tprint "exk_d:", exk_dexk = ex + exk_d * T# - ex_dd * T * T / 2.0# print "积分项：",efk_ireturn [exk, exk_d, efk_i]def compute_imp_joint(self):# 计算末端位置偏差for i in range(6):if (self.M[i] > math.pow(10, -6)):[self.Ex[i], self.Ex_d[i], self.Ef_i[i]] = self.int_adp_iter_solve(self.M[i], self.B[i], self.K[i], self.I[i],self.T, self.Ef[i], self.Ef_i[i],self.Ex[i], self.Ex_d[i])print "误差修正项：", np.round(self.Ex, 3)#转换修正项,将TCP修正量转化到基座标系Ex = np.zeros(6)Te = self.kin.fkine(self.qq_state)Ex[0:3] = np.dot(Te[0:3, 0:3], self.Ex[0:3])Ex[3:6] = np.dot(Te[0:3, 0:3], self.Ex[3:6])#计算参考位置beta = 1Xr = np.copy(self.xx_d + beta * Ex)Tr = np.eye(4)Rr = bf.euler_zyx2rot(Xr[3:6])Tr[0:3, 0:3] = RrTr[0:3, 3] = Xr[0:3]# [qr, succeed_label] = kin.arm_angle_ikine_limit(Tr, self.qq_state, self.DH0, self.qq_min, self.qq_max)qr = self.kin.iterate_ikine_limit(self.qq_state, Tr)return qr# 基于迭代求解自适应导纳方程:加入速度反馈
class IIMPController_iter_vel(object):# **定义属性**## 阻抗参数M = np.zeros(6)B = np.zeros(6)K = np.zeros(6)I = np.zeros(6)# 构造函数def __init__(self):# 位置、力误差self.Ex = np.zeros(6)  # Ex=Xr - Xd=>Xr = Xd + Exself.Ef = np.zeros(6)  # Ef=Fs - Fdself.Ex_d = np.zeros(6)self.Ef_i = np.zeros(6)# 周期self.T = 0.01# 第一次标签self.first_flag = True# 建立高增益观测器,观测速度self.hgo = dp.HighGainObserver()# **定义方法**## 获取控制周期def get_period(self, T):self.T = np.copy(T)# 获取阻抗参数def get_imp_parameter(self, Md, Bd, Kd, Ki):self.M = np.copy(Md)self.B = np.copy(Bd)self.K = np.copy(Kd)self.I = np.copy(Ki)def get_robot_parameter(self, DH_0, q_max, q_min, ):# DH参数self.DH0 = np.copy(DH_0)# 关节极限self.qq_max = np.copy(q_max)self.qq_min = np.copy(q_min)# 求取关节个数self.n = len(self.qq_max)# 创建运动学类self.kin = kin.GeneralKinematic(DH_0)def get_current_joint(self, qq):self.qq_state = np.copy(qq)X = self.kin.fkine_euler(qq)if(self.first_flag):self.Xx_list = np.zeros([3, 6])self.Xx_list[0, :] = Xself.Xx_list[1, :] = Xself.Xx_list[2, :] = Xself.Xv_list = np.zeros([2, 6])else:self.Xx_list[0, :] = self.Xx_list[1, :]self.Xx_list[1, :] = self.Xx_list[2, :]self.Xx_list[2, :] = XXv = np.zeros(6)for i in range(6):Xv[i] = self.hgo.observer(self.Xx_list[:, i], self.Xv_list[:, i])self.Xv_list[0, :] = self.Xv_list[1, :]self.Xv_list[1, :] = Xvdef get_current_force(self, F_t):# 转换到基坐标系self.base_f = self.force_end_to_base(F_t)# 更新力误差self.Ef = self.base_f - self.ff_dprint "力误差：EF", np.round(self.Ef, 2)def get_expect_joint(self, qd):Xd = self.kin.fkine_euler(qd)self.xx_d = np.copy(Xd)def get_expect_pos(self, Xd):self.xx_d = np.copy(Xd)def get_expect_force(self, Fd):self.ff_d = self.force_end_to_base(Fd)def force_end_to_base(self, F):base_F = np.zeros(6)Te = self.kin.fkine(self.qq_state)Re = Te[0:3, 0:3]base_F[0:3] = np.dot(Re, F[0:3])base_F[3:6] = np.dot(Re, F[3:6])return base_Fdef int_adp_iter_solve(self, m, b, k, ki, T, ef, ef_i, ex, ex_d):# 求当前时刻积分项efk_i = ef_i + T * ef# 计算当前加速度ex_dd = (ef + ki * efk_i - b * ex_d - k * ex) / m# print "ex_dd:", ex_dd# 求当前时刻速度exk_d = ex_d + ex_dd * T# print "exk_d:", exk_dexk = ex + exk_d * T - ex_dd * T * T / 2.0# print "积分项：",efk_ireturn [exk, exk_d, efk_i]def compute_imp_joint(self):# 计算末端位置偏差for i in range(6):if (self.M[i] > math.pow(10, -6)):[self.Ex[i], self.Ex_d[i], self.Ef_i[i]] = self.int_adp_iter_solve(self.M[i], self.B[i], self.K[i], self.I[i],self.T, self.Ef[i], self.Ef_i[i],self.Ex[i], -self.Xv_list[-1, i])# 计算参考位置print "误差修正项：", np.round(self.Ex, 3)beta = 0.5Xr = np.copy(self.xx_d + beta * self.Ex)Tr = np.eye(4)Rr = bf.euler_zyx2rot(Xr[3:6])Tr[0:3, 0:3] = RrTr[0:3, 3] = Xr[0:3]# [qr, succeed_label] = kin.arm_angle_ikine_limit(Tr, self.qq_state, self.DH0, self.qq_min, self.qq_max)qr = self.kin.iterate_ikine(self.qq_state, Tr)return qr#基于差分方程求解自适应导纳方程:调试成功,理论上效果更佳
class IIMPController_diff(object):#**定义属性**##阻抗参数M = np.zeros(6)B = np.zeros(6)K = np.zeros(6)I = np.zeros(6)def __init__(self):#位置、力误差self.E_f_list = np.zeros([4, 6])self.E_x_list = np.zeros([3, 6])self.qq_state = np.zeros(7)#周期self.T = 0.01self.init_flag = Trueself.expect_joint_flag = False#**定义方法**##获取控制周期def get_period(self, T):self.T = np.copy(T)#获取阻抗参数def get_imp_parameter(self, Md, Bd, Kd, Ki):self.M = np.copy(Md)self.B = np.copy(Bd)self.K = np.copy(Kd)self.I = np.copy(Ki)self.diff_paramter(Md, Bd, Kd, Ki)def get_robot_parameter(self, DH_0, q_max, q_min):#DH参数self.DH0 = np.copy(DH_0)#关节极限self.qq_max = np.copy(q_max)self.qq_min = np.copy(q_min)#求取关节个数self.n = len(self.qq_max)#创建运动学类self.kin = kin.GeneralKinematic(DH_0, q_min, q_max)def get_current_joint(self, qq):self.qq_state = np.copy(qq)def get_current_force(self, F_t):Ef = F_t - self.ff_dprint "期望力误差Ef:", np.around(Ef, 3)self.E_f_list[3, :] = self.E_f_list[2, :]self.E_f_list[2, :] = self.E_f_list[1, :]self.E_f_list[1, :] = self.E_f_list[0, :]self.E_f_list[0, :] = Efdef get_expect_joint(self, qd):#print "qd:", np.around(qd*180.0/np.pi, 3)self.expect_joint_flag = TrueTd = self.kin.fkine(qd)self.Td = np.copy(Td)def get_expect_pos(self, Xd):self.expect_joint_flag = Falseself.xx_d = np.copy(Xd)def get_expect_force(self, Fd):self.ff_d = np.copy(Fd)#计算参数差分方程参数def diff_paramter(self, M, B, K, Ki):#计算参数矩阵D=[M, B*T, K*T**2, Ki*T]D = np.ones([6, 5])D[:, 0] = MD[:, 1] = B*self.TD[:, 2] = K*self.T*self.TD[:, 3] = Ki*self.T#z函数分子：C=[B0, B1, B2, B3]C_c = np.array([[1, 2],[3, 2],[3, -2],[1, -2.0]]).Tself.C = np.dot(D[:, 3:5], C_c)#print "C：", self.C#z函数分母:A = [A0, A1, A2, A3]A_c = np.array([[4, 2, 1],[-12, -2, 1],[12, -2, -1],[-4, 2, -1.0]]).Tself.A = np.dot(D[:, 0:3], A_c)#print "A:", self.A#基于双曲变换求取积分自适应导纳方程,单自由度方程def int_adp_imc_diff_eq(self, Ef, Ex):''':param ef: 4个时刻的力偏差:param ex: 3个时刻的位置误差:return:'''E = np.zeros(6)for i in range(6):if (abs(self.A[i, 0]) < math.pow(10,-6)):E[i] = 0.0else:E[i] = (0.5*self.T*self.T*np.dot(self.C[i, :], Ef[:, i])\- np.dot(self.A[i, 1:], Ex[:, i]))/self.A[i, 0]return Edef compute_imp_joint(self):# print "位置误差：", np.round(self.E_x_list, 3)# print "力误差：", np.round(self.E_f_list, 3)#计算末端位置偏差E = self.int_adp_imc_diff_eq(self.E_f_list, self.E_x_list)self.E_x_list[2, :] = self.E_x_list[1, :]self.E_x_list[1, :] = self.E_x_list[0, :]self.E_x_list[0, :] = E#计算参考位置:转换到基座标#print "误差修正项：", np.round(E, 6)Te = self.kin.fkine(self.qq_state)Re = Te[0:3, 0:3]base_E = np.zeros(6)base_E[0:3] = np.dot(Re, E[0:3])base_E[3:6] = np.dot(Re, E[3:6])print "基坐标系修正量：", np.around(base_E, 6)Tr = np.eye(4)if(self.expect_joint_flag):Tr[0:3, 0:3] = self.Td[0:3, 0:3]Tr[0:3, 3] = self.Td[0:3, 3] + base_E[0:3]else:Xr = self.xx_d + base_ETr[0:3, 0:3] = bf.euler_zyx2rot(Xr[3:6])Tr[0:3, 3] = Xr[0:3]#print "Tr:\n", np.around(Tr, 3)qr = self.kin.iterate_ikine_limit(self.qq_state, Tr)return qr# 基于差分方程求解自适应导纳方程:加入加入设计好的低通滤波器
class IIMPController_diff_filter(object):def __init__(self, wc=0.001, N=30):#位置、力误差self.E_f_list = np.zeros([4, 6])self.E_x_list = np.zeros([3, 6])self.qq_state = np.zeros(7)#周期self.T = 0.01self.init_flag = Trueself.wc = wcself.N = Nself.FIR_filt = Filter.FIRFilter(wc, N)#**定义方法**##获取控制周期def get_period(self, T):self.T = np.copy(T)#获取阻抗参数def get_imp_parameter(self, Md, Bd, Kd, Ki):self.M = np.copy(Md)self.B = np.copy(Bd)self.K = np.copy(Kd)self.I = np.copy(Ki)self.diff_paramter(Md, Bd, Kd, Ki)def get_robot_parameter(self, DH_0, q_max, q_min):#DH参数self.DH0 = np.copy(DH_0)#关节极限self.qq_max = np.copy(q_max)self.qq_min = np.copy(q_min)#求取关节个数self.n = len(self.qq_max)#创建运动学类self.kin = kin.GeneralKinematic(DH_0, q_min, q_max)def get_current_joint(self, qq):self.qq_state = np.copy(qq)def get_current_force(self, F_t):if (self.init_flag):self.FIR_filt.set_input_init(F_t)self.FIR_filt.set_hanning_filter()self.init_flag = False# 加入滤波F_t = self.FIR_filt.hanning_filter(F_t)Ef = F_t - self.ff_dself.E_f_list[3, :] = self.E_f_list[2, :]self.E_f_list[2, :] = self.E_f_list[1, :]self.E_f_list[1, :] = self.E_f_list[0, :]self.E_f_list[0, :] = Efdef get_expect_joint(self, qd):Xd = self.kin.fkine_euler(qd)self.xx_d = np.copy(Xd)def get_expect_pos(self, Xd):self.xx_d = np.copy(Xd)def get_expect_force(self, Fd):self.ff_d = np.copy(Fd)#计算参数差分方程参数def diff_paramter(self, M, B, K, Ki):#计算参数矩阵D=[M, B*T, K*T**2, Ki*T]D = np.ones([6, 5])D[:, 0] = MD[:, 1] = B*self.TD[:, 2] = K*self.T*self.TD[:, 3] = Ki*self.T#z函数分子：C=[B0, B1, B2, B3]C_c = np.array([[1, 2],[3, 2],[3, -2],[1, -2.0]]).Tself.C = np.dot(D[:, 3:5], C_c)#print "C：", self.C#z函数分母:A = [A0, A1, A2, A3]A_c = np.array([[4, 2, 1],[-12, -2, 1],[12, -2, -1],[-4, 2, -1.0]]).Tself.A = np.dot(D[:, 0:3], A_c)#print "A:", self.A#基于双曲变换求取积分自适应导纳方程,单自由度方程def int_adp_imc_diff_eq(self, Ef, Ex):''':param ef: 4个时刻的力偏差:param ex: 3个时刻的位置误差:return:'''E = np.zeros(6)for i in range(6):if (abs(self.A[i, 0]) < math.pow(10,-6)):E[i] = 0.0else:E[i] = (0.5*self.T*self.T*np.dot(self.C[i, :], Ef[:, i])\- np.dot(self.A[i, 1:], Ex[:, i]))/self.A[i, 0]return Edef compute_imp_joint(self):# print "位置误差：", np.round(self.E_x_list, 3)# print "力误差：", np.round(self.E_f_list, 3)#计算末端位置偏差E = self.int_adp_imc_diff_eq(self.E_f_list, self.E_x_list)self.E_x_list[2, :] = self.E_x_list[1, :]self.E_x_list[1, :] = self.E_x_list[0, :]self.E_x_list[0, :] = E#计算参考位置:转换到基座标print "误差修正项：", np.round(E, 6)Te = self.kin.fkine(self.qq_state)Re = Te[0:3, 0:3]base_E = np.zeros(6)base_E[0:3] = np.dot(Re, E[0:3])base_E[3:6] = np.dot(Re, E[3:6])beta = 1Xr = self.xx_d - beta*Eqr = self.kin.iterate_ikine_limit_xyz(self.qq_state, Xr)return qr#=============测试方程=============#
def test():# 创建阻抗#imp = IIMPController_iter()imp = IIMPController_diff()#创建滤波器wc = 0.001N = 30my_filter = Filter.FIRFilter(wc=wc, N=N)my_filter.set_input_init(np.zeros(6))my_filter.set_hanning_filter()# 输入阻抗参数M = np.array([0, 0, 1, 0, 0, 0])B = np.array([0, 0, 200, 0, 0, 0])K = np.array([0, 0, 0, 0, 0, 0])I = np.array([0, 0, 5, 0, 0, 0])# 控制周期T = 0.01imp.get_period(T)# 输入DH参数DH = rp.DHfa_armcimp.get_robot_parameter(DH, rp.q_max_armc, rp.q_min_armc)num_imp = 2001num_init = 1000num = num_init + num_impk = 10# 环境刚度参数ke = 27000'''多次实验表明，需要解决两个问题(1)从初始位置到接触的瞬间冲击问题：超调量问题(2)接触后,环境曲面变化的适应问题：快速响应性'''# 建立期望轨迹ld = 0.05Xd = np.array([0.42, 0, 0.000, 0, np.pi, 0])t = np.linspace(0, T * (num - 1), num)tt = np.linspace(0, T * (num_imp - 1), num_imp)Xd_array = np.zeros([num, 6])for i in range(num):if (i < num_init):Xd_array[i, :] = Xdelse:Xd_array[i, :] = XdXd_array[i, 0] = Xd[0] + ld * np.sin(0.5 * np.pi / k * tt[i - num_init])# 建立实际环境le = 0.01led = - 0Xe = np.copy(Xd)Xe_array = np.zeros([num, 6])for i in range(num):if (i < num_init):Xe_array[i, :] = Xeelse:Xe_array[i, :] = XeXe_array[i, 0] = Xe[0] + ld * np.sin(0.5 * np.pi / k * tt[i - num_init])Xe_array[i, 2] = Xe[2] - le * np.sin(2 * np.pi / k * tt[i - num_init]) - led# 建立期望力lf = 10Fd = np.array([0, 0, -10.0, 0, 0.0, 0])Fd_array = np.zeros([num, 6])t_init = (num_init-1)*T[Fd_array[0:num_init, 2], _, _] = bf.interp5rdPoly1(0, 0, 0, Fd[2], 0, 0, t_init, T)Fd_array[num_init:, :] = np.dot(np.ones([num_imp, 6]), np.diag(Fd))dpi = 1000#绘制期望力plt.figure(1)plt.plot(t, Fd_array[:, 2], label='Fz', color='b')plt.title("Fd")plt.xlabel("t/s")plt.ylabel("Fz/N")plt.legend()plt.figure(2)plt.plot(t, 1000*Xe_array[:, 2], label='Xz', color='b')plt.title("Xe")plt.xlabel("t/s")plt.ylabel("Xz/mm")plt.legend()#plt.show()# 建立运动学kin1 = kin.GeneralKinematic(DH)# 获取初始状态# 初始位置q_guess = np.array([0.0, 30, 0, 90, 0, 60, 0])Te = np.eye(4)Te[0:3, 0:3] = bf.euler_zyx2rot(Xd_array[0, 3:6])Te[0:3, 3] = Xd_array[0, 0:3]qq_state = kin1.iterate_ikine(q_guess, Te)print "qq_state:", np.around(qq_state, 3)# 初始力Fs = np.zeros(6)Ff = np.zeros(6)Fs_array = np.zeros([num, 6])Ff_array = np.zeros([num, 6])# 机械臂末端位置Xs_array = np.zeros([num, 6])# 输入阻抗参数imp.get_imp_parameter(M, B, K, I)# 合成阻抗轨迹for i in range(num):# 读取期望位姿和关节角imp.get_expect_pos(Xd_array[i, :])imp.get_current_joint(qq_state)# 读取当前关节角和力imp.get_expect_force(Fd_array[i, :])imp.get_current_force(Fs)# 计算修正关节角qr = imp.compute_imp_joint()# 建设机器人底层能跟踪qq_state = np.copy(qr)Xs = kin1.fkine_euler(qq_state)Xs_array[i, :] = Xs# 反馈力if (Xe_array[i, 2] > Xs[2]):Fs = -ke * (Xe_array[i, :] - Xs)# + np.random.randn(6)Ff = my_filter.hanning_filter(Fs)else:Fs = np.zeros(6)Ff = my_filter.hanning_filter(Fs)Fs_array[i, :] = np.copy(Fs)Ff_array[i, :] = np.copy(Ff)# 绘制力跟踪图plt.figure(3)plt.plot(t, Fs_array[:, 2], label='Fz', color='r')plt.title("Fs = Fs_d+random(1)")plt.xlabel("t/s")plt.ylabel("F/N")plt.legend()plt.figure(4)plt.plot(t, Ff_array[:, 2], label='Fz', color='r')plt.title("Ff")plt.xlabel("t/s")plt.ylabel("F/N")plt.legend()plt.figure(5)plt.plot(t, Fd_array[:, 2] - Fs_array[:, 2], label='Fz_err', color='b',linestyle=':', marker='o', markerfacecolor='r', markersize=2)plt.title("Fz_error")plt.xlabel("t/s")plt.ylabel("F/N")plt.legend()plt.figure(6)plt.plot(t, Fd_array[:, 2] - Ff_array[:, 2], label='Fz_err', color='b',linestyle=':', marker='o', markerfacecolor='r', markersize=2)plt.title("Fz_error")plt.xlabel("t/s")plt.ylabel("F/N")plt.legend()plt.figure(7)plt.plot(t, Xe_array[:, 2], label='Xz', color='b',linestyle=':', marker='o', markerfacecolor='r', markersize=2)plt.title("Xz")plt.xlabel("t/s")plt.ylabel("x/m")plt.legend()plt.show()#=============测试方程=============#
def test_armt():# 创建阻抗imp = IIMPController_iter_vel()#imp = IIMPController_diff()#创建滤波器wc = 0.001N = 15my_filter = Filter.FIRFilter(wc=wc, N=N)my_filter.set_input_init(np.zeros(6))my_filter.set_hanning_filter()# 输入阻抗参数M = np.array([0, 0, 0.4, 0, 0, 0])B = np.array([0, 0, 80, 0, 0, 0])K = np.array([0, 0, 0, 0, 0, 0])I = np.array([0, 0, 0.8, 0, 0, 0])# 控制周期T = 0.01imp.get_period(T)# 输入DH参数DH = rp.DHf_armtq_max= rp.q_max_armtq_min = rp.q_min_armtimp.get_robot_parameter(DH, q_max, q_min)num_imp = 2001num_init = 1000num = num_init + num_impk = 10# 环境刚度参数ke = 27000'''多次实验表明，需要解决两个问题(1)从初始位置到接触的瞬间冲击问题：超调量问题(2)接触后,环境曲面变化的适应问题：快速响应性'''# 建立期望轨迹ld = 0.05Xd = np.array([0.42, 0, 0.000, 0, np.pi, 0])t = np.linspace(0, T * (num - 1), num)tt = np.linspace(0, T * (num_imp - 1), num_imp)Xd_array = np.zeros([num, 6])for i in range(num):if (i < num_init):Xd_array[i, :] = Xdelse:Xd_array[i, :] = XdXd_array[i, 0] = Xd[0] + ld * np.sin(0.5 * np.pi / k * tt[i - num_init])# 建立实际环境le = 0.01led = - 0Xe = np.copy(Xd)Xe_array = np.zeros([num, 6])for i in range(num):if (i < num_init):Xe_array[i, :] = Xeelse:Xe_array[i, :] = XeXe_array[i, 0] = Xe[0] + ld * np.sin(0.5 * np.pi / k * tt[i - num_init])Xe_array[i, 2] = Xe[2] - le * np.sin(2 * np.pi / k * tt[i - num_init]) - led# 建立期望力lf = 10Fd = np.array([0, 0, -10.0, 0, 0.0, 0])Fd_array = np.zeros([num, 6])t_init = (num_init-1)*T[Fd_array[0:num_init, 2], _, _] = bf.interp5rdPoly1(0, 0, 0, Fd[2], 0, 0, t_init, T)Fd_array[num_init:, :] = np.dot(np.ones([num_imp, 6]), np.diag(Fd))#建立期望力曲线dpi = 500plt.figure(1)plt.plot(t, Fd_array[:, 2], linewidth='2', label='Fz', color='b')plt.title(u"z方向期望力")plt.xlabel("t(s)")plt.ylabel("Fz(N)")plt.legend()plt.rcParams['savefig.dpi'] = dpi  # 图片像素#期望位置plt.figure(2)plt.plot(t, 1000*Xe_array[:, 2], linewidth='2', label='Xz', color='b')plt.title(u"环境z方向位置")plt.xlabel("t(s)")plt.ylabel("Xz(mm)")plt.ylim(-15, 15)plt.legend()plt.rcParams['savefig.dpi'] = dpi  # 图片像素# 建立运动学kin1 = kin.GeneralKinematic(DH, q_min, q_max)# 获取初始状态# 初始位置q_guess = np.array([0.0, 30, 0, 90, 0, 60, 0])Te = np.eye(4)Te[0:3, 0:3] = bf.euler_zyx2rot(Xd_array[0, 3:6])Te[0:3, 3] = Xd_array[0, 0:3]qq_state = kin1.iterate_ikine(q_guess, Te)print "qq_state:", np.around(qq_state, 3)# 初始力Fs = np.zeros(6)Ff = np.zeros(6)Fs_array = np.zeros([num, 6])Ff_array = np.zeros([num, 6])# 机械臂末端位置Xs_array = np.zeros([num, 6])# 输入阻抗参数imp.get_imp_parameter(M, B, K, I)# 合成阻抗轨迹for i in range(num):# 读取期望位姿和关节角imp.get_expect_pos(Xd_array[i, :])imp.get_current_joint(qq_state)# 读取当前关节角和力imp.get_expect_force(Fd_array[i, :])imp.get_current_force(Fs)# 计算修正关节角qr = imp.compute_imp_joint()# 建设机器人底层能跟踪qq_state = np.copy(qr)Xs = kin1.fkine_euler(qq_state)Xs_array[i, :] = Xs# 反馈力if (Xe_array[i, 2] > Xs[2]):Fs = -ke * (Xe_array[i, :] - Xs) + 0.1*np.random.randn(6)Ff = my_filter.hanning_filter(Fs)else:Fs = np.zeros(6)Ff = my_filter.hanning_filter(Fs)Fs_array[i, :] = np.copy(Fs)Ff_array[i, :] = np.copy(Ff)# 绘制力跟踪图和滤波力plt.figure(3)plt.plot(t, Fs_array[:, 2], label='Fz', color='r')plt.plot(t, Ff_array[:, 2], label='Fz_filter', color='black')plt.title(u"积分自适应导纳控制仿真力")plt.xlabel("t(s)")plt.ylabel("Fz(N)")plt.legend(bbox_to_anchor=(1, 1))plt.rcParams['savefig.dpi'] = dpi  # 图片像素#绘制力误差plt.figure(4)plt.plot(t, Fd_array[:, 2] - Ff_array[:, 2], label='Fz_err', color='b',linestyle=':', marker='o', markerfacecolor='r', markersize=2)plt.title(u"积分自适应导纳控制仿真力误差")plt.xlabel("t(s)")plt.ylabel("Fz(N)")plt.legend(bbox_to_anchor=(1, 1))plt.show()def main():#创建阻抗#imp = IIMPController_iter()imp = IIMPController_diff()#创建阻抗参数调节器imp_param = IMPParamater()#输入阻抗参数M = np.zeros(6)M[2] = 1dM = -0B = np.zeros(6)B[2] = 600dB = -200K = np.zeros(6)K[2] = 0dK = 0I = np.zeros(6)I[2] = 5dI = 25a = 1000b = 1000imp_param.get_init_imp_paramter(M[2], B[2], K[2], I[2])imp_param.get_diff_imp_paramter(dM, dB, dK, dI)imp_param.get_function_paramter(a, b)#控制周期T = 0.01imp.get_period(T)#输入DH参数imp.get_robot_parameter(rp.DHfa_armc, rp.q_max_armc, rp.q_min_armc)num_imp = 2001num_init = 1000num = num_init + num_impk = 5# 环境刚度参数ke = 27000'''多次实验表明，需要解决两个问题(1)从初始位置到接触的瞬间冲击问题：超调量问题(2)接触后,环境曲面变化的适应问题：快速响应性'''#建立期望轨迹ld = 0Xd = np.array([0.40, 0, 0, 0, np.pi, 0])t = np.linspace(0, T*(num - 1), num)tt = np.linspace(0, T*(num_imp - 1), num_imp)Xd_array = np.zeros([num, 6])for i in range(num):if(i<num_init):Xd_array[i, :] = XdXd_array[i, 0] = Xd[0] + ld * np.sin(0.5*np.pi / k * tt[0])else:Xd_array[i, :] = XdXd_array[i, 0] = Xd[0] + ld * np.sin(0.5*np.pi / k * tt[i-num_init])#建立实际环境le = 0.000led = - 0Xe = np.copy(Xd)Xe_array = np.zeros([num, 6])for i in range(num):if(i<num_init):Xe_array[i, :] = XeXe_array[i, 0] = Xe[0] + ld * np.sin(0.5*np.pi / k * tt[0])Xe_array[i, 2] = Xe[2] - le * np.cos(0.1*np.pi / k * tt[0]) - ledelse:Xe_array[i, :] = XeXe_array[i, 0] = Xe[0] + ld * np.sin(0.5*np.pi/k * tt[i-num_init])Xe_array[i, 2] = Xe[2] - le * np.cos(2*np.pi/k * tt[i-num_init]) - led#建立期望力lf = -10Fd = np.array([0, 0, -15.0, 0, 0.0, 0])Fd_array = np.zeros([num, 6])for i in range(num):if(i<num_init):Fd_array[i, :] = FdFd_array[i, 2] = Fd[2] + lf * np.sin(2 * np.pi/k * tt[0])else:Fd_array[i, :] = FdFd_array[i, 2] = Fd[2] + lf * np.sin(2*np.pi/k * tt[i-num_init])#建立运动学kin1 = kin.GeneralKinematic(rp.DHfa_armc)#获取初始状态#初始位置q_guess = np.array([0.0, 30, 0, 90, 0, 60, 0])Te = np.eye(4)Te[0:3, 0:3] = bf.euler_zyx2rot(Xd_array[0, 3:6])Te[0:3, 3] = Xd_array[0, 0:3]qq_state = kin1.iterate_ikine(q_guess, Te)print "qq_state:", np.around(qq_state, 3)#初始力Fs = np.zeros(6)Fs_array = np.zeros([num, 6])#机械臂末端位置Xs_array = np.zeros([num, 6])#合成阻抗轨迹for i in range(num):#变阻抗参数#[M[2], B[2], K[2], I[2]] = imp_param.out_expect_imp_paramter(i)#[M[2], B[2], K[2], I[2]] = imp_param.out_sfun_paramter(i)#输入阻抗参数imp.get_imp_parameter(M, B, K, I)# 读取期望位姿和关节角imp.get_expect_pos(Xd_array[i, :])imp.get_current_joint(qq_state)# 读取当前关节角和力imp.get_expect_force(Fd_array[i, :])imp.get_current_force(Fs)# 计算修正关节角qr = imp.compute_imp_joint()#建设机器人底层能跟踪qq_state = np.copy(qr)Xs = kin1.fkine_euler(qq_state)Xs_array[i, :] = Xs#print "Xs_z:", Xs[2]#反馈力if (Xe_array[i, 2] >  Xs[2] ):Fs = -ke*(Xe_array[i, :] - Xs)else:Fs = np.zeros(6)#print "Fs_z:", Fs[2]Fs_array[i, :] = np.copy(Fs)#绘制力跟踪图plt.figure(1)plt.plot(t, Fs_array[:, 2], label='Fz', color='r')plt.title("Fz")plt.xlabel("t/s")plt.ylabel("F/N")plt.legend()plt.figure(2)plt.plot(t, Fd_array[:, 2] - Fs_array[:, 2], label='Fz_err', color='b',linestyle=':', marker='o', markerfacecolor='r', markersize=2)plt.title("Fz_error")plt.xlabel("t/s")plt.ylabel("F/N")plt.legend()plt.figure(3)plt.plot(t, Xe_array[:, 2], label='Xs', color='b',linestyle=':', marker='o', markerfacecolor='r', markersize=2)plt.title("Xs")plt.xlabel("t/s")plt.ylabel("x/m")plt.legend()plt.show()if __name__ == "__main__":#main()test_armt()print "finish!"

机械臂力控----积分自适应导纳控制相关推荐

彻底搞懂阻抗控制、导纳控制、力位混合控制
彻底搞懂阻抗控制.导纳控制.力位混合控制本人在学习的机械臂力控时,始终觉得这三个概念太过抽象,不能很好理解,读了很多博客和文献,仍然感觉没有参透,今天在读李正义博士的毕业论文<机器人与环境间力 ...
反积分饱和程序_用抗积分饱和PID控制传递函数为G(s)的被控对象
题目:用抗积分饱和PID控制传递函数为G(s)的被控对象 G(s)=523500/(s^3+87.35s^2+10470s) 二.抗积分饱和原理积分饱和现象是在系统存在一个方向的偏差,PID控制器的 ...
干货 | 阻抗与导纳控制：一种使机器人刚中带柔的控制方法
" 本期技术干货,我们邀请到了小米机器人实验室工程师任赜宇,和大家分享在机器人力控方法中最为经典的一类控制方法,即阻抗与导纳控制. " 一.前言在传统机器人尤其是工业机械臂的应用 ...
神经网络自适应PID控制及其应用
神经网络自适应PID控制及其应用总结来自重庆大学宋永瑞教授2022暑期校园行学术会议 1. 研究背景目前人工智能的发展为很多领域里的研究提供了可延展性,提供了新的研究问题的思路,无人系统和人工 ...
VSC/SMC(十五)——基于模糊逼近的积分滑模控制
目录前言 1. 一阶系统积分滑模 1.1 一阶系统 1.2 控制器设计 1.2.1 选取积分滑模面 1.2.2 选取指数趋近律 1.2.3 Lypunov闭环系统稳定性证明 1.3 仿真分析 1.4 ...
matlab自适应逆控制,基于matlab仿真模块的自适应有源噪声逆控制研究
ATE 农业技术与装备 X2 c(q-1) p(q-1) u2 C( x) p(q-1) LMS M(q-1) p(q-1) w( q-1) w( q-1) ! ! e2 d2 + - - + p(q ...
java组件自适应窗口大小_java swing 窗口和控件自适应大小
本文记录java开发CS结构时怎么自适应屏幕大小以及控件跟随frame大小变化大小.位置和字体大小需要注意: 1.代码必须放置在其构造方法中.如:我的frame1是我frame.java的名,则代码 ...
控制div的大小自适应_干货 | 浅谈模糊自适应PID控制
认真沉稳进取认真努力的人都会关注这个公众号 PID控制(比例-积分-微分控制)是一种我们比较常见的控制算法,由比例单元P,积分单元I和微分单元D组成,控制基础为比例控制,积分控制,微分控制. ...
【机器人基础】阻抗/导纳控制深度解析
之前写过一篇阻抗控制的文章,[机器人基础]机器人阻抗控制概念写的比较浅也不是很专业,最近上了桂凯博士的动力学课程之后有了更深入的认识,认真整理总结了一下,这次详细地分析一下阻抗/导纳控制. 1.柔顺控 ...

机械臂力控----积分自适应导纳控制

机械臂力控----积分自适应导纳控制

原理讲解

源代码

机械臂力控----积分自适应导纳控制相关推荐

最新文章

热门文章