参考上一篇博客，这里给出一个具体案例讲解。
Q&A
通过一个二维的例子讲解
我通过一个简单的二维平面运动规划问题来说明本文方法是如何将有约束问题转化为无约束优化的。
考虑一个质点在平面上从起点
p
0
=
(
0
,
0
)
\mathbf{p}_0=(0,0)
p0=(0,0)运动到终点
p
1
=
(
1
,
1
)
\mathbf{p}_1=(1,1)
p1=(1,1),中间要经过
q
1
=
(
0.5
,
0.2
)
\mathbf{q}_1=(0.5,0.2)
q1=(0.5,0.2)和
q
2
=
(
0.7
,
0.8
)
\mathbf{q}_2=(0.7,0.8)
q2=(0.7,0.8)两点,且要避开一个圆形障碍物
O
=
{
(
x
,
y
)
∣
(
x
−
0.6
)
2
+
(
y
−
0.4
)
2
≤
0.
1
2
}
\mathcal{O}=\{(x,y)|(x-0.6)^2+(y-0.4)^2\leq 0.1^2\}
O={(x,y)∣(x−0.6)2+(y−0.4)2≤0.12}。我们希望规划一条加速度最小的轨迹,即求解如下问题:
min
x
(
t
)
,
y
(
t
)
∫
0
1
(
x
¨
2
+
y
¨
2
)
d
t
s.t.
(
x
(
0
)
,
y
(
0
)
)
=
(
0
,
0
)
(
x
(
1
)
,
y
(
1
)
)
=
(
1
,
1
)
(
x
(
0.5
)
,
y
(
0.5
)
)
=
(
0.5
,
0.2
)
(
x
(
0.7
)
,
y
(
0.7
)
)
=
(
0.7
,
0.8
)
(
x
(
t
)
−
0.6
)
2
+
(
y
(
t
)
−
0.4
)
2
≥
0.
1
2
,
∀
t
∈
[
0
,
1
]
\begin{aligned} \min_{x(t),y(t)} \quad & \int_{0}^{1}(\ddot{x}^2+\ddot{y}^2)dt \\ \textrm{s.t.}\quad & \begin{aligned} (x(0),y(0))&=(0, 0)\\ (x(1),y(1))&=(1,1)\\ (x(0.5),y(0.5))&=(0.5,0.2)\\ (x(0.7),y(0.7))&=(0.7,0.8)\\ (x(t)-0.6)^2+&(y(t)-0.4)^2\geq 0.1^2,\forall t\in[0,1] \end{aligned} \end{aligned}
x(t),y(t)mins.t.∫01(x¨2+y¨2)dt(x(0),y(0))(x(1),y(1))(x(0.5),y(0.5))(x(0.7),y(0.7))(x(t)−0.6)2+=(0,0)=(1,1)=(0.5,0.2)=(0.7,0.8)(y(t)−0.4)2≥0.12,∀t∈[0,1]
根据第3节,无约束最优解
(
x
∗
(
t
)
,
y
∗
(
t
)
)
(x^*(t),y^*(t))
(x∗(t),y∗(t))可用3次多项式表示,系数可由边界条件和中间点条件唯一确定。设:
x
(
t
)
=
c
1
,
0
+
c
1
,
1
t
+
c
1
,
2
t
2
+
c
1
,
3
t
3
y
(
t
)
=
c
2
,
0
+
c
2
,
1
t
+
c
2
,
2
t
2
+
c
2
,
3
t
3
\begin{aligned} x(t)&=c_{1,0}+c_{1,1}t+c_{1,2}t^2+c_{1,3}t^3\\ y(t)&=c_{2,0}+c_{2,1}t+c_{2,2}t^2+c_{2,3}t^3 \end{aligned}
x(t)y(t)=c1,0+c1,1t+c1,2t2+c1,3t3=c2,0+c2,1t+c2,2t2+c2,3t3
代入边界条件和中间点条件得:
(
1
0
0
0
0
0
0
0
1
1
1
1
0
0
0
0
1
1
2
1
4
1
8
0
0
0
0
1
7
10
49
100
343
1000
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
1
1
1
1
0
0
0
0
1
1
2
1
4
1
8
0
0
0
0
1
7
10
49
100
343
1000
)
(
c
1
,
0
c
1
,
1
c
1
,
2
c
1
,
3
c
2
,
0
c
2
,
1
c
2
,
2
c
2
,
3
)
=
(
0
1
0.5
0.7
0
1
0.2
0.8
)
\begin{pmatrix} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 1 & 1 & 1 & 1 & 0 & 0 & 0 & 0\\ 1 & \frac{1}{2} & \frac{1}{4} & \frac{1}{8} & 0 & 0 & 0 & 0\\ 1 & \frac{7}{10} & \frac{49}{100} & \frac{343}{1000} & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 1 & 1 & 1\\ 0 & 0 & 0 & 0 & 1 & \frac{1}{2} & \frac{1}{4} & \frac{1}{8}\\ 0 & 0 & 0 & 0 & 1 & \frac{7}{10} & \frac{49}{100} & \frac{343}{1000}\\ \end{pmatrix} \begin{pmatrix} c_{1,0}\\c_{1,1}\\c_{1,2}\\c_{1,3}\\c_{2,0}\\c_{2,1}\\c_{2,2}\\c_{2,3} \end{pmatrix}= \begin{pmatrix} 0\\1\\0.5\\0.7\\0\\1\\0.2\\0.8 \end{pmatrix}
1111000001211070000014110049000001811000343000000001111000001211070000014110049000001811000343
c1,0c1,1c1,2c1,3c2,0c2,1c2,2c2,3
=
010.50.7010.20.8
线性方程组的系数矩阵和右端项分别对应第3节的矩阵 M \mathbf{M} M和向量 b b b。求解该方程组即可得到最优轨迹。这里我们用Python实现:
import numpy as np
import matplotlib.pyplot as plt
# 定义矩阵M和向量b
M = np.array([[1, 0, 0, 0, 0, 0, 0, 0],
[1, 1, 1, 1, 0, 0, 0, 0],
[1, 0.5, 0.25, 0.125, 0, 0, 0, 0],
[1, 0.7, 0.49, 0.343, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 1, 1, 1],
[0, 0, 0, 0, 1, 0.5, 0.25, 0.125],
[0, 0, 0, 0, 1, 0.7, 0.49, 0.343]])
b = np.array([0, 1, 0.5, 0.7, 0, 1, 0.2, 0.8])
# 求解线性方程组
coeff = np.linalg.solve(M, b)
# 生成轨迹
t = np.linspace(0, 1, 100)
x = coeff[0] + coeff[1]*t + coeff[2]*t**2 + coeff[3]*t**3
y = coeff[4] + coeff[5]*t + coeff[6]*t**2 + coeff[7]*t**3
# 绘制轨迹
plt.figure()
circle = plt.Circle((0.6, 0.4), 0.1, color='r', alpha=0.3)
plt.gcf().gca().add_artist(circle)
plt.plot(x, y, 'b', linewidth=2)
plt.plot([0, 0.5, 0.7, 1], [0, 0.2, 0.8, 1], 'bo')
plt.title('Unconstrained optimal trajectory')
plt.xlabel('x')
plt.ylabel('y')
plt.axis('equal')
plt.show()
结果如图所示,可见轨迹穿过了障碍物,是不可行的。
为处理避障约束,根据第4.2节,我们可将障碍物外的可行域用球形区域
F
=
{
(
x
,
y
)
∣
(
x
−
0.6
)
2
+
(
y
−
0.4
)
2
≥
0.
1
2
}
\mathcal{F}=\{(x,y)|(x-0.6)^2+(y-0.4)^2\geq 0.1^2\}
F={(x,y)∣(x−0.6)2+(y−0.4)2≥0.12}近似,再用反球极投影和正交投影将其映射为无约束的空间:
F
≈
P
B
=
{
(
x
,
y
)
∣
(
x
~
−
0.6
)
2
+
(
y
~
−
0.4
)
2
≥
0.
1
2
}
(
x
~
,
y
~
)
=
2
(
x
,
y
)
(
x
−
0.6
)
2
+
(
y
−
0.4
)
2
+
0.
1
2
+
(
0.6
,
0.4
)
\begin{aligned} \mathcal{F}&\approx\mathcal{P}_B=\{(x,y)|(\tilde{x}-0.6)^2+(\tilde{y}-0.4)^2\geq 0.1^2\}\\ (\tilde{x},\tilde{y})&=\frac{2(x,y)}{(x-0.6)^2+(y-0.4)^2+0.1^2}+(0.6,0.4) \end{aligned}
F(x~,y~)≈PB={(x,y)∣(x~−0.6)2+(y~−0.4)2≥0.12}=(x−0.6)2+(y−0.4)2+0.122(x,y)+(0.6,0.4)
于是原问题可改写为:
min
x
~
(
t
)
,
y
~
(
t
)
∫
0
1
(
x
~
¨
2
+
y
~
¨
2
)
d
t
s.t.
(
x
~
(
0
)
,
y
~
(
0
)
)
=
(
0
,
0
)
(
x
~
(
1
)
,
y
~
(
1
)
)
=
(
1
,
1
)
(
x
~
(
0.5
)
,
y
~
(
0.5
)
)
=
(
0.5
,
0.2
)
(
x
~
(
0.7
)
,
y
~
(
0.7
)
)
=
(
0.7
,
0.8
)
\begin{aligned} \min_{\tilde{x}(t),\tilde{y}(t)} \quad & \int_{0}^{1}(\ddot{\tilde{x}}^2+\ddot{\tilde{y}}^2)dt \\ \textrm{s.t.}\quad & \begin{aligned} (\tilde{x}(0),\tilde{y}(0))&=(0, 0)\\ (\tilde{x}(1),\tilde{y}(1))&=(1,1)\\ (\tilde{x}(0.5),\tilde{y}(0.5))&=(0.5,0.2)\\ (\tilde{x}(0.7),\tilde{y}(0.7))&=(0.7,0.8) \end{aligned} \end{aligned}
x~(t),y~(t)mins.t.∫01(x~¨2+y~¨2)dt(x~(0),y~(0))(x~(1),y~(1))(x~(0.5),y~(0.5))(x~(0.7),y~(0.7))=(0,0)=(1,1)=(0.5,0.2)=(0.7,0.8)
现在问题变成无约束的了。我们仍用3次多项式 ( x ~ ( t ) , y ~ ( t ) ) (\tilde{x}(t),\tilde{y}(t)) (x~(t),y~(t))表示轨迹,系数矩阵和向量不变。得到系数后,再用映射 f B − 1 f_B^{-1} fB−1将 ( x ~ , y ~ ) (\tilde{x},\tilde{y}) (x~,y~)变换回 ( x , y ) (x,y) (x,y),就得到了满足避障约束的最优轨迹。具体实现如下:
import numpy as np
import matplotlib.pyplot as plt
def mapB(x, y):
xt = 2*x / ((x-0.6)**2 + (y-0.4)**2 + 0.1**2) + 0.6
yt = 2*y / ((x-0.6)**2 + (y-0.4)**2 + 0.1**2) + 0.4
return xt, yt
# 定义矩阵M和向量b
M = np.array([[1, 0, 0, 0, 0, 0, 0, 0],
[1, 1, 1, 1, 0, 0, 0, 0],
[1, 0.5, 0.25, 0.125, 0, 0, 0, 0],
[1, 0.7, 0.49, 0.343, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 1, 1, 1],
[0, 0, 0, 0, 1, 0.5, 0.25, 0.125],
[0, 0, 0, 0, 1, 0.7, 0.49, 0.343]])
b = np.array([0, 1, 0.5, 0.7, 0, 1, 0.2, 0.8])
# 求解线性方程组得到变换后的轨迹系数
coeff = np.linalg.solve(M, b)
# 生成变换后的轨迹
t = np.linspace(0, 1, 100)
xt = coeff[0] + coeff[1]*t + coeff[2]*t**2 + coeff[3]*t**3
yt = coeff[4] + coeff[5]*t + coeff[6]*t**2 + coeff[7]*t**3
# 将轨迹变换回原空间
x, y = mapB(xt, yt)
# 绘制轨迹
plt.figure()
circle = plt.Circle((0.6, 0.4), 0.1, color='r', alpha=0.3)
plt.gcf().gca().add_artist(circle)
plt.plot(x, y, 'b', linewidth=2)
plt.plot([0, 0.5, 0.7, 1], [0, 0.2, 0.8, 1], 'bo')
plt.title('Constrained optimal trajectory')
plt.xlabel('x')
plt.ylabel('y')
plt.axis('equal')
plt.show()
可见轨迹绕开了障碍物,是可行解,而且在变换后的空间中仍是最优的。这就是本文方法的核心思想:利用可微映射将约束消除,在无约束空间求解,再映射回原空间得到满足约束的最优解。即使实际问题的约束更复杂,原理也是类似的。
标签：轨迹,机器人,0.5,0.4,0.7,0.6,plt,无约束,tilde From： https://blog.csdn.net/jiayoushijie/article/details/139203403