问题说明

在每个时间步长，一个点（模拟机器人）计算其相对于其他点的 Voronoi Cell (VC)，在 VC 内规划一条路径，然后沿着该路径执行第一步。在整个算法执行过程中，机器人位置、VC 和计划路径一起演变。假设规划范围是 T 步，对于机器人 i 在每个时间实例，我们将规划轨迹的位置表示为 p_{i,1},p_{ i,2},...,p_{i,T},对于 {1,2,n} 中的所有 i。该算法要求每个机器人在每个时间步求解一个 QP，例如，对于机器人 i，路径和输入（或速度）通过求解以下 QP Receding Horizon Path Planning Problem 给出。

在问题1中，成本函数J_{i}是中间状态成本和终端成本的总和，p_{i,f}是目标位置，{p}{i,0}到{ p}{i,T} 和 u_{i,0} 到 u_{i,T-1} 分别是要规划的路径和输入。正定或半定矩阵 Q、R 和 Q_{f} 是平衡三个成本的权重因子。这个标准 QP 问题的决策变量是 {p}{i,1} 到 {p}{i,T}。约束 (i) 确保路径对于机器人动力学是可行的。约束 (ii) 将计划路径限制在机器人 i 的相应 BVC 中。约束 (ii) 可以明确地写成一组 linear inequality constrain

其中 epsilon_{j} 是一个 2 x 1 的向量，对应于 VC V_{i} 的一条边。约束 (iii) 确保路径从机器人的当前位置开始，并且输入 u_{i,t} 的下限和上限以组件方式写入约束 (iv) 和 (v)。

代码尝试

我有以下代码：

# Import packages.

import cvxpy as cp
import numpy as np

def optimizer(p_i,p_f,vertices):

    np.random.seed(1)

    # Dimensions
    T = 1 # Steps
    m = 2 # u dimension
    n = 2 # x dimension

    # Mechanics
    alpha = 0.2
    A = np.eye(n) + alpha*np.random.randn(n,n)
    B = np.random.randn(n,m)

    # Parameters
    R = cp.Parameter((m,m),PSD = True)
    Q = cp.Parameter((n,n),PSD = True)
    Qf = cp.Parameter((n,PSD = True)
    R.value = np.identity(m)
    Q.value = np.identity(n)
    Qf.value = np.identity(n)

    # Variables
    p = cp.Variable((n,T+1))
    u = cp.Variable((m,T))
    u_max_x = 1
    u_max_y = 1

    # Cost and Constraints
    cost = 0
    constr = []
    constr += [p[:,0] == p_i]
    for t in range(T):
        cost += cp.quad_form(p[:,t]-p_f,Q) + cp.quad_form(u[:,t],R)
        constr += [p[:,t+1] == A@p[:,t] + B@u[:,cp.abs(u[0,t]) <= u_max_x,cp.abs(u[1,t]) <= u_max_y]
                                                                
        for j in range(len(vertices)-1):
            constr += [((vertices[j%3][0] - p[0,t]) * (vertices[(j+1)%3][1] - p[1,t])) <= ((vertices[j%3][1] - p[1,t]) * (vertices[(j+1)%3][0] - p[0,t]))]
       
    # Last case
    cost += cp.quad_form(p[:,T]-p_f,Qf)
    for j in range(len(vertices)):
        constr += [((vertices[j%3][0] - p[0,T]) * (vertices[(j+1)%3][1] - p[1,T])) <= ((vertices[j%3][1] - p[1,T]) * (vertices[(j+1)%3][0] - p[0,T]))]

    # Objective
    prob = cp.Problem(cp.Minimize(cost),constr)

    # Solve with no disciplined Geometric Programming
    prob.solve(gp=False)

    # Check status
    print(prob.status)

    #print(cost.is_dcp(dpp=True))

    # Optimal solution
    optimal_value = prob.value

    # Return
    return optimal_value

# Testing to move a point c within a triangle to the target g
v1 = [-2,1]
v2 = [2,2]
v3 = [1,-3]
c = np.array([1,1])
g = np.array([3,-2])
inst_bvc = np.array([v1,v2,v3])

solution = optimizer(c,g,inst_bvc)
print(solution)

运行代码，我得到了 DCPError：问题不遵循 DCP 规则。约束不是 DCP。

有人可以帮助我开发更好的代码版本吗？提前致谢