迭代学习笔记

一、迭代学习定义和分类

1、直观理解

迭代学习一般应用于重复性的场景。比如控制一个单自由度的小车以特定的速度曲线移动到指定位置，整个时间是10s，控制频率是0.01，那么整个控制序列就会有1000个点。这1000个点在10s内依次发出，完成依次控制，目的就是让小车的速度或者位置跟踪上特定的曲线。我们有一个优势在于跟踪曲线不变，那么我们可以不断进行尝试，只要获得一组良好的控制序列，那么就可以完成控制。

2、更新率

如果我们可以设置一个更新率，根据每一个点面临的误差对控制量进行修改，那么就可以在若干次更新后获得完美的控制序列。
$$新控制量=旧控制量+更新率$$

根据更新率与什么参数有关，可以分为开环、闭环、开闭环。如果跟上一次的误差有关就是开环，如果跟此刻的误差有关就是闭环，如果都有关就是开闭环。根据更新率的结构还可以分为D型、PD型等。

二、实现例子

网上关于迭代学习的代码实现比较少，很多还是用simulink搭的，不方便复现。这里给一个D型闭环迭代学习例子，被控对象是一个钟摆小车，全部代码用python实现，直接运行即可。

1、动力学模型

# 动力学模型函数
def dynamics(state, u):
    x, theta, dx, dtheta = state
    dd_x = (u + m2*l*dtheta*dtheta + m2*g*np.sin(theta)*np.cos(theta)) / (m1 + m2*np.sin(theta)*np.sin(theta))
    dd_theta = (-dd_x*np.cos(theta) + dx*np.sin(theta)*dtheta - np.sin(theta)*(dx*dtheta + g)) / l
    return np.array([dx, dtheta, dd_x, dtheta])

2、四阶龙格库塔积分器和控制器更新率

# 四阶龙格库塔法函数
def RK4(t_start, t_end, fun, Npoints, init_state, u_list, pos_des, spd_des):
    t = np.linspace(t_start, t_end, Npoints)
    dt = t[1] - t[0]
    state = np.zeros((Npoints, len(init_state)))
    state[0, :] = init_state
    e = np.zeros((Npoints, 2))
        
    for i in range(Npoints-1):
        u_list[i] = u_list[i] + 1 * (spd_des[i] - state[i, 2])  # 闭环D型更新率
        u = u_list[i]

        e[i, 0] = pos_des[i] - state[i, 0]
        e[i, 1] = spd_des[i] - state[i, 2]

        k1 = fun(state[i, :], u)
        k2 = fun(state[i, :] + 0.5 * dt * k1, u)
        k3 = fun(state[i, :] + 0.5 * dt * k2, u)
        k4 = fun(state[i, :] + dt * k3, u)
        state[i + 1, :] = state[i, :] + (1/6) * dt * (k1 + 2 * k2 + 2 * k3 + k4)

    return state, u_list, e

3、进行迭代

for i in range(10):
    # 运行RK4
    state, u_list, e_buff = RK4(t_start, t_end, dynamics, Npoints, [0, 0, 0, 0], u_list, pos_des, spd_des)

4、完整代码和效果展示

import numpy as np
import matplotlib.pyplot as plt
import pandas as pd

# 动力学模型函数
def dynamics(state, u):
    x, theta, dx, dtheta = state
    dd_x = (u + m2*l*dtheta*dtheta + m2*g*np.sin(theta)*np.cos(theta)) / (m1 + m2*np.sin(theta)*np.sin(theta))
    dd_theta = (-dd_x*np.cos(theta) + dx*np.sin(theta)*dtheta - np.sin(theta)*(dx*dtheta + g)) / l
    return np.array([dx, dtheta, dd_x, dtheta])

# 四阶龙格库塔法函数
def RK4(t_start, t_end, fun, Npoints, init_state, u_list, pos_des, spd_des):
    t = np.linspace(t_start, t_end, Npoints)
    dt = t[1] - t[0]
    state = np.zeros((Npoints, len(init_state)))
    state[0, :] = init_state
    e = np.zeros((Npoints, 2))
        
    for i in range(Npoints-1):
        u_list[i] = u_list[i] + 1 * (spd_des[i] - state[i, 2])  # 闭环D型更新率
        u = u_list[i]

        e[i, 0] = pos_des[i] - state[i, 0]
        e[i, 1] = spd_des[i] - state[i, 2]

        k1 = fun(state[i, :], u)
        k2 = fun(state[i, :] + 0.5 * dt * k1, u)
        k3 = fun(state[i, :] + 0.5 * dt * k2, u)
        k4 = fun(state[i, :] + dt * k3, u)
        state[i + 1, :] = state[i, :] + (1/6) * dt * (k1 + 2 * k2 + 2 * k3 + k4)

    return state, u_list, e

# 参数设置
m1 = 5
m2 = 1
l = 1
g = 9.8

t_start = 0
t_end = 10
Npoints = 10000
t = np.linspace(t_start, t_end, Npoints)

# 期望位置和速度
pos_des = np.sin(t) + t
spd_des = np.cos(t) + 1

pos_des = t * t + 2 * t
spd_des = 2 * t + 2

# 初始状态和控制输入
u_list = [0] * 10000

# 创建图形对象
plt.figure(figsize=(12, 12))

for i in range(20):
    # 运行RK4
    state, u_list, e_buff = RK4(t_start, t_end, dynamics, Npoints, [0, 0, 0, 0], u_list, pos_des, spd_des)
    
    # 清除当前图像
    plt.clf()
    
    # 绘制实际位置和期望位置的对比图
    plt.subplot(3, 1, 1)
    plt.plot(t, state[:, 0], label='Actual x (position)')
    plt.plot(t, pos_des, label='Desired x (position)', linestyle='--')
    plt.xlabel('Time (s)')
    plt.ylabel('Position')
    plt.legend()
    plt.title(f'Iteration {i+1}')
    
    # 绘制实际速度和期望速度的对比图
    plt.subplot(3, 1, 2)
    plt.plot(t, state[:, 2], label='Actual dx (velocity)')
    plt.plot(t, spd_des, label='Desired dx (velocity)', linestyle='--')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity')
    plt.legend()
    
    # 绘制控制输入（力）曲线
    plt.subplot(3, 1, 3)
    plt.plot(t, u_list, label='Control input (force)')
    plt.xlabel('Time (s)')
    plt.ylabel('Force')
    plt.legend()
    
    # 显示图像
    plt.pause(0.01)

# 最后显示图像
plt.show()

# 将误差和力存储到CSV文件
data = np.hstack((e_buff, np.array(u_list).reshape(-1, 1)))
df = pd.DataFrame(data, columns=['Position Error', 'Velocity Error', 'Control Input'])
df.to_csv('error_and_force_data.csv', index=False)