Robust_stability.py - master - Chronos/Control_lib

Robust_stability.py
292 строки11 KB
Chronos
Final version (left only Kharitonov theorem)
06 май 2025, 13:14
06 май 2025, 13:1487d8d1a
import numpy as np
import matplotlib.pyplot as plt
from control import ss, frequency_response
from scipy import optimize
from numpy import linalg
import Operator_Norms


def robust_stability_square(ncffs, bcffs):
    # задаем номинальные значения коэффициентов многочлена
    # (порядок от старшего коэффициента к младшему)
    # ncffs = np.array([1, 1, 3, 4, 1, 3])
    #
    # # задаем границы изменения коэффициентов
    # bcffs = np.array([1, 2, 2, 1, 2, 1])

    # находим вспомогательные полиномы
    polydim = len(ncffs)
    u0cffs  = [x if i % 2 == 0 else -x for i, x in enumerate(ncffs[::-2])][::-1]
    uacffs = bcffs[::-2][::-1]

    v0cffs = [x if i % 2 == 0 else -x for i, x in enumerate(ncffs[-2::-2])][::-1]
    vacffs = bcffs[-2::-2][::-1]

    # вычисляем годограф Цыпкина-Поляка
    count = 5001
    omega_ticks = np.linspace(0, 100, count)
    xy_ticks = np.zeros((2, count))
    for k in range(count):
        omega = omega_ticks[k]
        xy_ticks[0, k] = np.polyval(u0cffs[::-1], omega) / np.polyval(uacffs[::-1], omega)
        xy_ticks[1, k] = np.polyval(v0cffs[::-1], omega) / np.polyval(vacffs[::-1], omega)

    # вычисляем координаты вершин квадратов, лежащих на годографе
    wp = np.polymul(u0cffs[::-1], vacffs[::-1]) - np.polymul(v0cffs[::-1], uacffs[::-1])
    wm = np.polymul(u0cffs[::-1], vacffs[::-1]) + np.polymul(v0cffs[::-1], uacffs[::-1])
    rts_wp = np.roots(wp)
    rts_wm = np.roots(wm)
    rts = np.concatenate((rts_wp, rts_wm))
    rts = rts[(rts > 0) & (np.imag(rts) == 0)]
    deltas = []
    for root in rts:
        deltas.append(np.polyval(u0cffs[::-1], root) / np.polyval(uacffs[::-1], root))
    strs = [f"{abs(delta)}" for delta in deltas]

    my_col = plt.cm.jet(np.linspace(0, 1, 5))[::-1]

    # строим годограф Цыпкина-Поляка
    plt.figure()
    plt.plot(xy_ticks[0, :], xy_ticks[1, :], 'b', linewidth=2.0)

    for k in range(len(deltas)):
        # задаем координату вершины квадрата
        delta = deltas[k]
        vtx_pnts = np.array([
            [delta, -delta, -delta, delta, delta],
            [delta, delta, -delta, -delta, delta]
        ])
        plt.plot(vtx_pnts[0, :], vtx_pnts[1, :], color=my_col[k], linewidth=2.0)

    plt.grid(True)
    plt.axis('equal')
    plt.legend(strs)
    plt.show()



def robust_stability_disk(ncffs, bcffs):
    m = len(ncffs) - 1  # Degree of the polynomial

    # Form U0, Ua, V0, Va polynomials
    U0_coeffs = [x if i % 2 == 0 else -x for i, x in enumerate(ncffs[::-2])][::-1]
    Ua_coeffs = bcffs[::-2][::-1]

    V0_coeffs = [x if i % 2 == 0 else -x for i, x in enumerate(ncffs[-2::-2])][::-1]
    Va_coeffs = bcffs[-2::-2][::-1]

    # Compute derivatives of U0, Ua, V0, Va
    U0_prime = np.polyder(U0_coeffs)
    Ua_prime = np.polyder(Ua_coeffs)
    V0_prime = np.polyder(V0_coeffs)
    Va_prime = np.polyder(Va_coeffs)

    # Calculate A = U0'*Ua - U0*Ua' and B = V0'*Va - V0*Va'
    A = np.polysub(np.polymul(U0_prime, Ua_coeffs), np.polymul(U0_coeffs, Ua_prime))
    B = np.polysub(np.polymul(V0_prime, Va_coeffs), np.polymul(V0_coeffs, Va_prime))

    # Compute Va^3 and Ua^3
    Va_cubed = np.polymul(np.polymul(Va_coeffs, Va_coeffs), Va_coeffs)
    Ua_cubed = np.polymul(np.polymul(Ua_coeffs, Ua_coeffs), Ua_coeffs)

    # Form the terms of the equation polynomial
    Term1 = np.polymul(np.polymul(U0_coeffs, A), Va_cubed)
    Term2 = np.polymul(np.polymul(V0_coeffs, B), Ua_cubed)

    # Combine terms to form the equation polynomial: Term1 + Term2 = 0
    equation_poly = np.polyadd(Term1, Term2)

    # Find roots and filter real positive ones
    roots = np.roots(equation_poly)
    real_roots = [rt.real for rt in roots if np.isreal(rt) and rt.real > 0]
    real_roots = np.unique(np.round(real_roots, decimals=6))  # Remove duplicates

    # Calculate radii from the roots
    radii = []
    for root in real_roots:
        x_val = np.polyval(U0_coeffs, root) / np.polyval(Ua_coeffs, root)
        y_val = np.polyval(V0_coeffs, root) / np.polyval(Va_coeffs, root)
        radii.append(np.sqrt(x_val**2 + y_val**2))
    radii = sorted(radii)

    # Generate the Tsypkin-Polyak locus
    omega = np.linspace(0, 100, 10001)
    x = np.polyval(U0_coeffs, omega) / np.polyval(Ua_coeffs, omega)
    y = np.polyval(V0_coeffs, omega) / np.polyval(Va_coeffs, omega)

    # Plotting
    plt.figure()
    plt.plot(x, y, 'b')
    phi = np.linspace(0, 2 * np.pi, 100)
    for r in radii:
        plt.plot(r * np.cos(phi), r * np.sin(phi), '--', label=f'R={r:.2f}')
    plt.axis('equal')
    plt.grid(True)
    plt.legend()
    plt.show()

    return min(radii) if radii else np.inf


def hurwitz_matrix_(coefficients):
    n = len(coefficients) - 1  # Степень многочлена
    # Проверка, что многочлен имеет положительный старший коэффициент
    if coefficients[0] <= 0:
        raise ValueError("Старший коэффициент должен быть положительным.")

    # Создание нулевой матрицы размером n x n
    H = np.zeros((n, n))

    # Заполнение матрицы Гурвица

    current_i = 1
    current_j = 0


    num = 0
    while num <= (n//2):
        current_i = 2*num+1
        current_j = num
        counter = 0
        while True:
            try:
                H[current_i, current_j] = coefficients[counter]
            except IndexError:
                pass
            val = counter%4
            if val == 0:
                current_i += -1
                current_j += 0
            if val == 1 or val == 3:
                current_i += 1
                current_j += 1
            if val == 2:
                current_i += -1
                current_j += 0
            counter += 1
            if counter == len(coefficients):
                break
        num += 1

    return H

def check_hurwitz_stability(coefficients):
    """
    Проверяет устойчивость полинома методом Рауса-Гурвица через миноры матрицы Гурвица

    Аргументы:
        coefficients: list - коэффициенты полинома от старшей степени к младшей
                         (например, [a_n, a_{n-1}, ..., a_0] для a_n*s^n + ... + a_0)

    Возвращает:
        (is_stable, hurwitz_matrix, minors) - кортеж из:
            - is_stable: bool - True если полином устойчив
            - hurwitz_matrix: np.array - матрица Гурвица
            - minors: list - значения главных миноров матрицы Гурвица
    """
    n = len(coefficients) - 1  # степень полинома

    # Строим матрицу Гурвица
    hurwitz_matrix = hurwitz_matrix_(coefficients)

    # for i in range(n):
    #     for j in range(n):
    #         k = 2*i + 1 - j
    #         if 0 <= k <= n:
    #             hurwitz_matrix[i, j] = coefficients[k]

    print("Матрица Гурвица:")
    print(hurwitz_matrix)
    print("\n")

    # Вычисляем главные миноры
    minors = []
    for k in range(1, n+1):
        minor = np.linalg.det(hurwitz_matrix[:k, :k])
        minors.append(minor)
        print(f"Минор M{k} (размер {k}x{k}):")
        print(hurwitz_matrix[:k, :k])
        print(f"Значение минора: {minor:.4f}")
        print("---")
        if minor < 0:
            return False

    # Проверяем условие устойчивости (все миноры > 0)

    return True
def check_robust_stability_kharitonov(lower_coeffs, upper_coeffs):
    # Коэффициенты в порядке возрастания степени
    if min(lower_coeffs) <= 0:
        print("Многочлен не является робастно устойчивым,есть отрицательные коэффициенты")
        return False
    l = len(lower_coeffs)
    nn = np.zeros(l)
    for i in range(l):
        if i % 4 <= 1:
            nn[i] = lower_coeffs[i]
        else:
            nn[i] = upper_coeffs[i]
    nv = np.zeros(l)
    for i in range(l):
        if 1 <= i % 4 <= 2:
            nv[i] = upper_coeffs[i]
        else:
            nv[i] = lower_coeffs[i]

    vn = np.zeros(l)
    for i in range(l):
        if 1 <= i % 4 <= 2:
            vn[i] = lower_coeffs[i]
        else:
            vn[i] = upper_coeffs[i]

    vv = np.zeros(l)
    for i in range(l):
        if i % 4 <= 1:
            vv[i] = upper_coeffs[i]
        else:
            vv[i] = lower_coeffs[i]


    p_nn = np.polynomial.Polynomial(nn)
    p_nv = np.polynomial.Polynomial(nv)
    p_vn = np.polynomial.Polynomial(vn)
    p_vv = np.polynomial.Polynomial(vv)

    print("Полиномы Харитонова:")
    print("НН:", p_nn)
    print("НВ:", p_nv)
    print("ВН:", p_vn)
    print("ВВ:", p_vv)

    print("Для каждого проверяем устойчивость методом Раусса-Гурвица:")
    print("НН:\n-----------------------------------")
    is_nn_stable = check_hurwitz_stability(nn[::-1])
    if not is_nn_stable:
        print("Многочлен не является робастно устойчивым")
        return False
    print("\nНВ:\n-----------------------------------")
    is_nv_stable = check_hurwitz_stability(nv[::-1])
    if not is_nv_stable:
        print("Многочлен не является робастно устойчивым")
        return False
    print("\nВН:\n-----------------------------------")
    is_vn_stable = check_hurwitz_stability(vn[::-1])
    if not is_vn_stable:
        print("Многочлен не является робастно устойчивым")
        return False
    print("\nВВ:\n-----------------------------------")
    is_vv_stable = check_hurwitz_stability(vv[::-1])
    if not is_vv_stable:
        print("Многочлен не является робастно устойчивым")
        return False
    else:
        print("Многочлен является робастно устойчивым")
        return True


def robust_stability_radii(A,B,C):
    # Вычисляет комплексный и вещественный радиусы робастной устойчивости системы с неопределённостью
    # A' = A+ B Delta C
    r_c = 1/Operator_Norms.h_inf_norm_lmi_continuous(A,B,C)
    return r_c