问题描述
我正在研究以下 QP 后退地平线路径规划问题:
问题说明
在每个时间步长,一个点(模拟机器人)计算其相对于其他点的 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。
有人可以帮助我开发更好的代码版本吗? 提前致谢
解决方法
暂无找到可以解决该程序问题的有效方法,小编努力寻找整理中!
如果你已经找到好的解决方法,欢迎将解决方案带上本链接一起发送给小编。
小编邮箱:dio#foxmail.com (将#修改为@)