SEA_IMC_controller

        近期基于串联弹性驱动器的机械臂项目结项，为了抑制其残余振动基于内模控制原理设计了内模控制器，在此开源代码。由于电机，传感器等知识产权多在生产公司，虽然自己写了底层硬件代码，但不公开这部分代码。

        控制器代码及参数如下：

# 定义参数
T = 0.13  # 电机时间常数
M = 0.02    # 连杆转动惯量
C = 0.05    # 连杆阻力系数
G = 1.70    # 连杆重力矩
K = 3.21    # 弹簧等效刚度
q = 0       # 连杆角度
Ta = 0.020  # 采样时间
lamda = 0.20  # 滤波器参数
L = 0.020  # 传感器延迟参数
sleep_time = 0.033

# 计算e的系数
def enc(kq):
    if kq>1:
        kq = 1
    elif kq<-1:
        kq = -1
    return 8*T*M + 4*Ta*(T*C+M)+2*math.pow(Ta, 2)*(T*(K+kq*G)+C)+math.pow(Ta, 3)*(K+kq*G)


def en_1c(kq):
    if kq > 1:
        kq = 1
    elif kq < -1:
        kq = -1
    return (-24)*T*M - 4*Ta*(T*C+M)+2*math.pow(Ta, 2)*(T*(K+kq*G)+C)+math.pow(Ta, 3)*(K+kq*G)


def en_2c(kq):
    if kq > 1:
        kq = 1
    elif kq < -1:
        kq = -1
    return 24*T*M - 4*Ta*(T*C+M)-2*math.pow(Ta, 2)*(T*(K+kq*G)+C)+math.pow(Ta, 3)*(K+kq*G)


def en_3c(kq):
    if kq > 1:
        kq = 1
    elif kq < -1:
        kq = -1
    return (-8)*T*M + 4*Ta*(T*C+M)-2*math.pow(Ta, 2)*(T*(K+kq*G)+C)+math.pow(Ta, 3)*(K+kq*G)


# 定义系数 u
Un_c = 8*math.pow(lamda, 3)*K+12*K*math.pow(lamda, 2)*Ta+2*K*(3*lamda+L)*math.pow(Ta, 2)
Un_1_c = (-24)*math.pow(lamda, 3)*K-12*K*math.pow(lamda, 2)*Ta+2*K*(3*lamda+L)*math.pow(Ta, 2)
Un_2_c = 24*math.pow(lamda, 3)*K-12*K*math.pow(lamda, 2)*Ta-2*K*(3*lamda+L)*math.pow(Ta, 2)
Un_3_c = (-8)*math.pow(lamda, 3)*K+12*K*math.pow(lamda, 2)*Ta-2*K*(3*lamda+L)*math.pow(Ta, 2)


def get_un(en, en1, en2, en3, un1, un2, un3, kq):
    un_0 = (enc(kq)*en + en_1c(kq)*en1 + en_2c(kq)*en2 + en_3c(kq)*en3 - Un_1_c*un1
            - Un_2_c*un2 - Un_3_c*un3)/Un_c
    return un_0

        具体控制流程如下（第一版代码，后期代码的流程相同，不过会更加精细）：

# 电机初始化
my_motor = motor_class.Motor("COM13")
my_motor.move_absolute(0)


# 传感器初始化
sensor = deviceModel.DeviceModel(
        "My_JY901",
        WitProtocolResolver(),
        JY901SDataProcessor(),
        "51_0"
    )
sensor.serialConfig.portName = "COM10"          #设置串口   Set serial port
sensor.serialConfig.baud = 115200                   #设置波特率  Set baud rate
sensor.openDevice()                                 #打开串口   Open serial port
readConfig(sensor)                                  #读取配置信息 Read configuration information
sensor.dataProcessor.onVarChanged.append(onUpdate)  #数据更新事件 Data update event


def reference_input(t):
    """
    设计参考输入为斜坡再稳定的一条曲线
    :param t: 时间变量
    :return: 参考输入
    """
    if t < 0.5:
        return t * 2.266 * 0.795813  # 参考输入
    else:
        return 1


# 电机保证为启动状态，此后只需要更改速度即可使电机追踪某一速度运行
my_motor.set_running_speed(0)
my_motor.start()


# 设置差分方程初始值
en1_initial = 0
en2_initial = 0
en3_initial = 0

un1_initial = 0
un2_initial = 0
un3_initial = 0

# 设置差分方程变量
en = 0
en1 = 0
en2 = 0
en3 = 0

un = 0
un1 = 0
un2 = 0
un3 = 0
# 开始记录数据    Start recording data
startRecord()
# 记录轨迹与时间
un_list = []
qn_list = []
reference_list = []
time_list = []
start_time = time.time()
stop = 0
flag = True
for i in range(1000):
    # 处理第一拍
    q = ((90 - onUpdate(sensor))/180)*math.pi
    print(str(i)+":"+str(q))
    if i == 0:
        # 将角度添加到角度列表里
        qn_list.append(q)
        # 计算当前时间以及相应的参考输入并存储
        current_time = time.time()
        delta_time = start_time - current_time
        r = reference_input((i+1)*Ta)
        reference_list.append(r)
        # 在第一拍的时候初始条件为0
        en = r - q
        en1 = en1_initial
        en2 = en2_initial
        en3 = en3_initial
        un1 = un1_initial
        un2 = un2_initial
        un3 = un3_initial
        # 防止divided by zero
        q = abs(q)+0.00000001
        kq = math.sin(q)/q
        # 得到un，从而通过位置差与时间差得到速度并运行
        un = get_un(en, en1, en2, en3, un1, un2, un3, kq)
        un_list.append(un)
        vn = (un - un1) / sleep_time  # 单位为弧度秒
        vn = vn/(2*math.pi)*my_motor.reduction_ratio   # 单位为转秒,需要乘以电机减速比
        if abs(vn) > 30:
            pass
        if abs(vn) < 0.05:  # 电机最小速度限制
            my_motor.set_running_speed(0)
        elif vn > 0:
            my_motor.set_running_speed(vn)
            my_motor.forward()
            # time.sleep(sleep_time)
        elif vn < 0:
            my_motor.set_running_speed(abs(vn))
            my_motor.reverse()
            # time.sleep(sleep_time)
    # 处理第二拍
    elif i == 1:
        # 将角度添加到角度列表里
        qn_list.append(q)
        # 计算当前时间以及相应的参考输入并存储
        current_time = time.time()
        delta_time = start_time - current_time
        r = reference_input((i+1)*Ta)
        reference_list.append(r)
        # 在第一拍的时候初始条件为0
        en = r - q
        en1 = reference_list[i-1]-qn_list[i-1]
        en2 = en1_initial
        en3 = en2_initial
        un1 = un_list[i-1]
        un2 = un1_initial
        un3 = un2_initial
        # 防止divided by zero
        q = abs(q) + 0.00000001
        kq = math.sin(q) / q
        # 得到un，从而通过位置差与时间差得到速度并运行
        un = get_un(en, en1, en2, en3, un1, un2, un3, kq)
        un_list.append(un)
        vn = (un - un1) / sleep_time  # 单位为弧度秒
        vn = vn/(2*math.pi)*my_motor.reduction_ratio   # 单位为转秒,需要乘以电机减速比
        if abs(vn) > 30:
            pass
        if abs(vn) < 0.05:
            my_motor.set_running_speed(0)
        elif vn > 0:
            my_motor.set_running_speed(vn)
            my_motor.forward()
            # time.sleep(sleep_time)
        elif vn < 0:
            my_motor.set_running_speed(abs(vn))
            my_motor.reverse()
            # time.sleep(sleep_time)
    elif i == 2:
        # 将角度添加到角度列表里
        qn_list.append(q)
        # 计算当前时间以及相应的参考输入并存储
        current_time = time.time()
        delta_time = start_time - current_time
        r = reference_input((i+1)*Ta)
        reference_list.append(r)
        # 在第一拍的时候初始条件为0
        en = r - q
        en1 = reference_list[i - 1] - qn_list[i - 1]
        en2 = reference_list[i-2] - qn_list[i-2]
        en3 = en1_initial
        un1 = un_list[i - 1]
        un2 = un_list[i-2]
        un3 = un2_initial
        # 防止divided by zero
        q = abs(q) + 0.00000001
        kq = math.sin(q) / q
        # 得到un，从而通过位置差与时间差得到速度并运行
        un = get_un(en, en1, en2, en3, un1, un2, un3, kq)
        un_list.append(un)
        vn = (un - un1) / sleep_time  # 单位为弧度秒
        vn = vn/(2*math.pi)*my_motor.reduction_ratio   # 单位为转秒,需要乘以电机减速比
        if abs(vn) > 30:
            pass
        if abs(vn) < 0.05:
            my_motor.set_running_speed(0)
        elif vn > 0:
            my_motor.set_running_speed(vn)
            my_motor.forward()
            # time.sleep(sleep_time)
        elif vn < 0:
            my_motor.set_running_speed(abs(vn))
            my_motor.reverse()
            # time.sleep(sleep_time)
    elif flag:
        # 将角度添加到角度列表里
        qn_list.append(q)
        # 计算当前时间以及相应的参考输入并存储
        current_time = time.time()
        delta_time = start_time - current_time
        r = reference_input((i+1)*Ta)
        reference_list.append(r)
        # 在第一拍的时候初始条件为0
        en = r - q
        en1 = reference_list[i - 1] - qn_list[i - 1]
        en2 = reference_list[i - 2] - qn_list[i - 2]
        en3 = reference_list[i - 3] - qn_list[i - 3]
        un1 = un_list[i - 1]
        un2 = un_list[i - 2]
        un3 = un_list[i - 3]
        # 防止divided by zero
        q = abs(q) + 0.00000001
        kq = math.sin(q) / q
        # 得到un，从而通过位置差与时间差得到速度并运行
        un = get_un(en, en1, en2, en3, un1, un2, un3, kq)
        un_list.append(un)
        vn = (un - un1) / sleep_time  # 单位为弧度秒
        vn = vn/(2*math.pi)*my_motor.reduction_ratio   # 单位为转秒,需要乘以电机减速比
        if abs(vn) > 30:
            pass
        if abs(vn) < 0.05:
            my_motor.set_running_speed(0)
        elif vn > 0:
            my_motor.set_running_speed(vn)
            my_motor.forward()
        elif vn < 0:
            my_motor.set_running_speed(abs(vn))
            my_motor.reverse()
end_time = time.time()
print(end_time-start_time)


time.sleep(1)
onUpdate(sensor)
my_motor.set_running_speed(5)
my_motor.move_absolute(0)
sensor.closeDevice()
endRecord()                                         #结束记录数据 End record data