ការ Control រ៉ូបូតតាមរយះ វិធីសាស្រ្ត NMPC ដោយប្រើ GMRES

class HJBEquation:

    def __init__(self, q1, q2, q3, r1, r2, u_max, u_min):

        self.q1 = q1
        self.q2 = q2
        self.q3 = q3

        self.r1 = r1
        self.r2 = r2

        self.u1_min = u_min[0]
        self.u2_min = u_min[0]

        self.u1_max = u_max[0]
        self.u2_max = u_max[1]

    def dhdx(self, x, y, yaw, x_r, y_r, yaw_r, u1, lam1, lam2):

        dhdx1 = (x-x_r)*self.q1
        dhdx2 = (y-y_r)*self.q2
        dhdx3 = (yaw-yaw_r)*self.q3-u1*lam1*np.sin(yaw)+u1*lam2*np.cos(yaw)

        return dhdx1, dhdx2, dhdx3

    def dhdu(self, yaw, u1, u2, u1_r, u2_r, lam1, lam2, lam3, mu1, mu2):

        dhdu1 = self.r1*(u1-u1_r)+lam1*np.cos(yaw)+lam2*np.sin(yaw)+2*mu1*u1*(u1+(self.u1_max+self.u1_min)/2)
        dhdu2 = self.r2*(u2-u2_r)+lam3+2*mu2*(u2+(self.u2_max+self.u2_min)/2)

        return dhdu1, dhdu2

    def dphidx(self, x, y, yaw, x_r, y_r, yaw_r):
        dphidx1 = self.q1*(x-x_r)
        dphidx2 = self.q2*(y-y_r)
        dphidx3 = self.q3*(yaw-yaw_r)

        return dphidx1, dphidx2, dphidx3

class NMPCCGMRES:

    def __init__(self):

        ## ប៉ារ៉ាម៉ែត្រ NMPC
        self.x_dim = 3
        self.u_dim = 2
        self.c_dim = 2
        self.pred_horizons = 10

        ### ប៉ារ៉ាម៉ែត្រ Continuation
        self.ht = 1e-5
        self.zeta = 1/self.ht
        self.alpha = 0.5
        self.tf = 1.0

        ### ម៉ាទ្រីស

        self.q1 = 1
        self.q2 = 6
        self.q3 = 1
        self.r1 = 1
        self.r2 = 0.1

        ### កំហិត
        self.u_min = [ 0,  0]
        self.u_max = [ 2,  1.57]
        self.u_dummy = [0.01, 0.01]

        ### ចាប់ផ្តើមម៉ាទ្រីសដំណោះស្រាយ
        self.dU = np.zeros((self.u_dim, self.pred_horizons))

        ### ចំណុចយោង
        self.X_r = [3, 2, 0.0]
        self.U_r = [0, 0]

X = np.zeros((x_dim, pred_horizons+1))      ## ស្ថានភាពសម្រាប់ N-ព្យាករណ៍+1
U = np.zeros((u_dim, pred_horizons))        ## ត្រួតពិនិត្យធាតុចូលសម្រាប់ N-ព្យាករណ៍
Lambda = np.zeros((x_dim, pred_horizons+1)) ## វ៉ិចទ័រកូស្ថានភាពសម្រាប់ N-ព្យាករណ៍+1
dt = 0.01                                   ## ពេលវេលាគំរូ

def calc_state(self, X, U, dt):

        for i in range(self.pred_horizons):
            x_dot, y_dot, yaw_dot = self.robot_model.forward_kinematic(X[2, i], U[0, i], U[1, i])
            
            X[0, i+1] = X[0, i] + x_dot * dt
            X[1, i+1] = X[1, i] + y_dot * dt
            X[2, i+1] = X[2, i] + yaw_dot * dt

        return X

def calc_costate(self, X, U, Lambda, dt):

        x_N, y_N, yaw_N = self.hjb_equation.dphidx(X[0, -1], X[1, -1], X[2, -1],
                                                   self.X_r[0], self.X_r[1], self.X_r[2])
        Lambda[0, -1] = x_N
        Lambda[1, -1] = y_N
        Lambda[2, -1] = yaw_N

        for i in reversed(range(1, self.pred_horizons)):

            dhdx1, dhdx2, dhdx3 = self.hjb_equation.dhdx(
                X[0, i], X[1, i], X[2, i],
                self.X_r[0], self.X_r[1], self.X_r[2],
                U[0, i], Lambda[0, i+1], Lambda[1, i+1]
            )

            Lambda[0, i] = Lambda[0, i+1] + dhdx1 * dt
            Lambda[1, i] = Lambda[1, i+1] + dhdx2 * dt
            Lambda[2, i] = Lambda[2, i+1] + dhdx3 * dt
    

        return Lambda

def calc_f(self, X, U, Lambda, Mu, dt):

        X_new = self.calc_state(X, U, dt)

        Lambda_new = self.calc_costate(X_new, U, Lambda, dt)

        F = np.zeros((self.u_dim, self.pred_horizons))

        for i in range(self.pred_horizons):
            dhdu1, dhdu2 = self.hjb_equation.dhdu(X_new[2, i], U[0, i], U[1, i],
                                                  self.U_r[0], self.U_r[1],
                                                  Lambda_new[0, i+1], Lambda_new[1, i+1], Lambda_new[2, i+1],
                                                  Mu[0, i], Mu[1, i])
            F[0, i] = dhdu1
            F[1, i] = dhdu2
            
        F = F.T.ravel().reshape(-1, 1)

        return F

# ផ្តេកជំហានពេលវេលា
dt = self.tf * (1-np.exp(-self.alpha*time)) / self.pred_horizons
# F(x,u,t+h)
F = self.calc_f(X, U, Lambda, Mu, dt)
# F(x+hdx,u,t+h)
x_dot, y_dot, yaw_dot = self.robot_model.forward_kinematic(X[2, 0], U[0, 0], U[1, 0])
X[:, 0] = X[:, 0] + np.array([x_dot, y_dot, yaw_dot]) * self.ht
Fxt = self.calc_f(X, U, Lambda, Mu, dt)
# F(x+hdx,u+hdx,t+h)
Fuxt = self.calc_f(X, U + self.dU*self.ht, Lambda, Mu , dt)

# កំណត់សមីការខាងស្តាំ-ឆ្វេង
left = (Fuxt - Fxt) / self.ht
right = -self.zeta * F - (Fxt - F)/self.ht

# កំណត់សមីការខាងស្តាំ-ឆ្វេង
left = (Fuxt - Fxt) / self.ht
right = -self.zeta * F - (Fxt - F)/self.ht

# កំណត់ការធ្វើម្តងមួយៗ
m = (self.u_dim ) * self.pred_horizons
# កំណត់ r0
r0 = right - left
# print(r0)

# GMRES
Vm = np.zeros((m, m+1))
Vm[:, 0:1] = r0 / np.linalg.norm(r0)
# print("Vm", Vm[:, 0])
Hm = np.zeros((m+1, m))
# print(Vm[:, 0])

for i in range(m):
    Fuxt = self.calc_f(
        X, U + Vm[:, i:i+1].reshape(self.u_dim, self.pred_horizons, order='F') * self.ht,
        Lambda, Mu,
        dt
    )

    Av = (Fuxt - Fxt) / self.ht

    for k in range(i+1):
        Hm[k, i] = np.dot(Av.T, Vm[:, k:k+1])

    temp_vec = np.zeros((m, 1))

    for k in range(i+1):
        temp_vec = temp_vec + np.dot(Hm[k, i], Vm[:, k:k+1])

    v_hat = Av - temp_vec

    Hm[i+1, i] = np.linalg.norm(v_hat)

    Vm[:,i+1:i+2] = v_hat/Hm[i+1, i]

e = np.zeros((m+1, 1))
e[0, 0] = 1.0
gm = np.linalg.norm(r0) * e
# print(gm)

UTMat, gm = self.ToUTMat(Hm, gm, m)

min_y = np.zeros((m, 1))

for i in range(m):
    min_y[i][0] = (gm[i, 0] - np.dot(UTMat[i:i+1 ,:] ,min_y))/UTMat[i, i]

dU_new = self.dU + np.dot(Vm[:, 0:m], min_y).reshape(self.u_dim, self.pred_horizons, order='F')

self.dU = dU_new

U = U + self.dU * self.ht