Frenet坐标系与Cartesian坐标系互转（三）：应用示例

已知一个U-turn道路中心线的参数化曲线如下所示：

{x(s)=−1.205e−05s5+0.0004733s4−0.0008037s3−0.09783s2−0.002081s+5y(s)=−5.383e−19s5+0.000294s4−0.009237s3+0.009687s2+0.9875s+0.0007563\left\{\begin{matrix}x(s)=&-1.205e-05 s^5 + 0.0004733 s^4 - 0.0008037 s^3 - 0.09783 s^2 - 0.002081 s + 5 \\ y(s)=&-5.383e-19 s^5 + 0.000294 s^4 - 0.009237 s^3 + 0.009687 s^2 + 0.9875 s + 0.0007563\end{matrix}\right.{x(s)=y(s)=−1.205e−05s5+0.0004733s4−0.0008037s3−0.09783s2−0.002081s+5−5.383e−19s5+0.000294s4−0.009237s3+0.009687s2+0.9875s+0.0007563

例1. 现在我知道车道的宽度为3.03.03.0，利用`frenet_to_cartesian1D`获取车道的边界线：

import numpy as np
from math import *
import matplotlib.pyplot as pltdef frenet_to_cartesian1D(rs, rx, ry, rtheta, s_condition, d_condition):if fabs(rs - s_condition[0])>= 1.0e-6:print("The reference point s and s_condition[0] don't match")cos_theta_r = cos(rtheta)sin_theta_r = sin(rtheta)x = rx - sin_theta_r * d_condition[0]y = ry + cos_theta_r * d_condition[0]    return x, ytheta = np.linspace(0, np.pi, 10)
x = 5.0*np.cos(theta)
y = 5.0*np.sin(theta)
s = theta*5.0fx = np.poly1d(np.polyfit(s,x,5))
dfx = fx.deriv()fy = np.poly1d(np.polyfit(s,y,5))
dfy = fy.deriv()snew = np.linspace(0, 5*np.pi, 1000)newx = fx(snew)
newy = fy(snew)left_bound = []
right_bound = []for i in range(1000):rs = snew[i]rx = fx(rs)ry = fy(rs)drx = dfx(rs)dry = dfy(rs)rtheta = atan2(dry, drx)l_s_condition = np.array([rs])l_d_condition = np.array([1.5])lx, ly = frenet_to_cartesian1D(rs, rx, ry, rtheta, l_s_condition, l_d_condition)left_bound.append(np.array([lx, ly]))r_s_condition = np.array([rs])r_d_condition = np.array([-1.5])rx, ry = frenet_to_cartesian1D(rs, rx, ry, rtheta, r_s_condition, r_d_condition)   right_bound.append(np.array([rx, ry]))
left_bound = np.array(left_bound)
right_bound = np.array(right_bound)plt.plot(newx, newy, 'y')
plt.plot(left_bound[:,0],left_bound[:,1], 'b')
plt.plot(right_bound[:,0],right_bound[:,1], 'b')
plt.show()

车道线如下图蓝色曲线所示：

例2. 已知道路中有一个障碍物的Cartesian坐标为(2.0,3.5)(2.0, 3.5)(2.0,3.5)，求它以道路中心线为base frame的Frenet坐标

障碍物如下图中的黑色圆点所示：

import numpy as np
from math import *
import matplotlib.pyplot as pltdef cartesian_to_frenet1D(rs, rx, ry, rtheta, x, y):s_condition = np.zeros(1)d_condition = np.zeros(1)dx = x - rxdy = y - rycos_theta_r = cos(rtheta)sin_theta_r = sin(rtheta)cross_rd_nd = cos_theta_r * dy - sin_theta_r * dxd_condition[0] = copysign(sqrt(dx * dx + dy * dy), cross_rd_nd)    s_condition[0] = rsreturn s_condition, d_conditiondef find_nearest_rs(fx, fy, x, y):min_dist = 99999.0rs = 0.0for s in snew:dx = x - fx(s)dy = y - fy(s)dist = np.sqrt(dx*dx+dy*dy)if min_dist > dist:min_dist = distrs = sreturn rstheta = np.linspace(0, np.pi, 10)
x = 5.0*np.cos(theta)
y = 5.0*np.sin(theta)
s = theta*5.0fx = np.poly1d(np.polyfit(s,x,5))
dfx = fx.deriv()fy = np.poly1d(np.polyfit(s,y,5))
dfy = fy.deriv()snew = np.linspace(0, 5*np.pi, 1000)newx = fx(snew)
newy = fy(snew)rs = find_nearest_rs(fx, fy, 2.0, 3.5)
rx = fx(rs)
ry = fy(rs)
rtheta = atan2(dfy(rs), dfx(rs))
s_condition, d_condition = cartesian_to_frenet1D(rs, rx, ry, rtheta, 2.0, 3.5)
plt.plot(snew, np.zeros(1000), 'y')
plt.plot(snew, np.zeros(1000)+1.5, 'b')
plt.plot(snew, np.zeros(1000)-1.5, 'b')
plt.scatter(s_condition, d_condition, marker='o', color='k')
plt.axis([0, 16, -8, 8])
plt.show()

障碍物转换到Frenet坐标系，如下图黑色圆点所示（横坐标为s, 纵坐标为d）：

例3. 在Frenet坐标系下生成一簇候选轨迹，将其转换到Cartesian坐标

在Frenet坐标系下生成一簇候选轨迹的程序如下：

import numpy as np
from math import *
import matplotlib.pyplot as plttheta = np.linspace(0, np.pi, 10)
x = 5.0*np.cos(theta)
y = 5.0*np.sin(theta)
s = theta*5.0fx = np.poly1d(np.polyfit(s,x,5))
dfx = fx.deriv()fy = np.poly1d(np.polyfit(s,y,5))
dfy = fy.deriv()snew = np.linspace(0, 5*np.pi, 1000)newx = fx(snew)
newy = fy(snew)plt.plot(np.zeros(1000), snew, 'y')
plt.plot(np.zeros(1000)+1.5, snew, 'b')
plt.plot(np.zeros(1000)-1.5, snew, 'b')def polyfit(coeffs, t):return coeffs[0] + coeffs[1]*t + coeffs[2]*t*t + coeffs[3]*t*t*tdef cubicPolyCurve1d(x0, dx0, x1, dx1, T):coeffs = np.zeros(4)coeffs[0] = x0coeffs[1] = dx0T2 = T*Tcoeffs[2] = (3*x1 - T*dx1 - 3*coeffs[0] - 2*coeffs[1]*T)/T2coeffs[3] = (dx1 - coeffs[1] - 2*coeffs[2]*T)/(3.0*T2)return coeffstargets = []
for i in range(11):x0 = 0dx0 = 0  # np.tan(np.pi/6)x1 = -1.5 + i * 0.3dx1 = 0    targets.append(np.array([x0, dx0, x1, dx1]))t = np.linspace(0, 10, 1000)
for i in range(11):tar = targets[i]coeffs = cubicPolyCurve1d(tar[0], tar[1], tar[2], tar[3], 10.0)d = polyfit(coeffs, t)plt.plot(d, t, 'g')
plt.axis([-8, 8, 0, 16])
plt.show()

绿色曲线簇为生成的Frenet坐标系下的候选轨迹（纵坐标为s，横坐标为d）：

将轨迹转换到cartesian坐标系下显示：

def frenet_to_cartesian1D(rs, rx, ry, rtheta, s_condition, d_condition):if fabs(rs - s_condition[0])>= 1.0e-6:print("The reference point s and s_condition[0] don't match")cos_theta_r = cos(rtheta)sin_theta_r = sin(rtheta)x = rx - sin_theta_r * d_condition[0]y = ry + cos_theta_r * d_condition[0]    return x, yleft_bound = []
right_bound = []for i in range(1000):rs = snew[i]rx = fx(rs)ry = fy(rs)drx = dfx(rs)dry = dfy(rs)rtheta = atan2(dry, drx)l_s_condition = np.array([rs])l_d_condition = np.array([1.5])lx, ly = frenet_to_cartesian1D(rs, rx, ry, rtheta, l_s_condition, l_d_condition)left_bound.append(np.array([lx, ly]))r_s_condition = np.array([rs])r_d_condition = np.array([-1.5])rx, ry = frenet_to_cartesian1D(rs, rx, ry, rtheta, r_s_condition, r_d_condition)   right_bound.append(np.array([rx, ry]))
left_bound = np.array(left_bound)
right_bound = np.array(right_bound)plt.plot(newx, newy, 'y')
plt.plot(left_bound[:,0],left_bound[:,1], 'b')
plt.plot(right_bound[:,0],right_bound[:,1], 'b')for i in range(11):tar = targets[i]coeffs = cubicPolyCurve1d(tar[0], tar[1], tar[2], tar[3], 10.0)d = polyfit(coeffs, t)traj = []for j in range(1000):rs = t[j]rx = fx(rs)ry = fy(rs)rtheta = atan2(dfy(rs), dfx(rs))s_condition = np.array([rs])d_condition = np.array([d[j]])x, y = frenet_to_cartesian1D(rs, rx, ry, rtheta, s_condition, d_condition)traj.append(np.array([x, y]))traj = np.array(traj)plt.plot(traj[:,0], traj[:,1], 'g')
plt.plot()
plt.show()

图中绿色曲线簇为cartesian坐标系下的候选轨迹：

例4. 如果想要知道候选轨迹上各点的角度、曲率等信息，首先得计算出base frame曲线的曲率与曲率导数等信息

import numpy as np
from math import *
import matplotlib.pyplot as plttheta = np.linspace(0, np.pi, 10)
x = 5.0*np.cos(theta)
y = 5.0*np.sin(theta)
s = theta*5.0fx = np.poly1d(np.polyfit(s,x,5))
dfx = fx.deriv()
ddfx = dfx.deriv()
dddfx = ddfx.deriv()fy = np.poly1d(np.polyfit(s,y,5))
dfy = fy.deriv()
ddfy = dfy.deriv()
dddfy = ddfy.deriv()snew = np.linspace(0, 5*np.pi, 1000)newx = fx(snew)
dnewx = dfx(snew)
ddnewx = ddfx(snew)newy = fy(snew)
dnewy = dfy(snew)
ddnewy = ddfy(snew)# alpha = np.arctan2(dnewy, dnewx)
# kappa = (ddnewx*dnewy-ddnewy*dnewx)/(dnewx*dnewx+dnewy*dnewy)**(3.0/2.0)
# norm to [-pi, pi]
def NormalizeAngle(angle):a = fmod(angle+np.pi, 2*np.pi)if a < 0.0:a += (2.0*np.pi)        return a - np.pidef polyfit(coeffs, t, order):if order == 0:return coeffs[0] + coeffs[1]*t + coeffs[2]*t*t + coeffs[3]*t*t*tif order == 1:return coeffs[1] + 2*coeffs[2]*t+3*coeffs[3]*t*tif order == 2:return 2*coeffs[2]+6*coeffs[3]*tif order == 3:return 6*coeffs[3]else:return 0.0def cubicPolyCurve1d(x0, dx0, x1, dx1, T):coeffs = np.zeros(4)coeffs[0] = x0coeffs[1] = dx0T2 = T*Tcoeffs[2] = (3*x1 - T*dx1 - 3*coeffs[0] - 2*coeffs[1]*T)/T2coeffs[3] = (dx1 - coeffs[1] - 2*coeffs[2]*T)/(3.0*T2)return coeffsdef ComputeCurvature(dx, ddx, dy, ddy):a = dx*ddy - dy*ddxnorm_square = dx*dx+dy*dynorm = sqrt(norm_square)b = norm*norm_squarereturn a/bdef ComputeCurvatureDerivative(dx, ddx, dddx, dy, ddy, dddy):a = dx*ddy-dy*ddxb = dx*dddy-dy*dddxc = dx*ddx+dy*ddyd = dx*dx+dy*dyreturn (b*d-3.0*a*c)/(d*d*d)**(2.5)def CalculateTheta(rtheta, rkappa, l, dl):return NormalizeAngle(rtheta + atan2(dl, 1-l*rkappa))def CalculateKappa(rkappa, rdkappa, l, dl, ddl):denominator = (dl * dl + (1 - l * rkappa) * (1 - l * rkappa))if fabs(denominator) < 1e-8:return 0.0denominator = pow(denominator, 1.5)numerator = (rkappa + ddl - 2 * l * rkappa * rkappa -l * ddl * rkappa + l * l * rkappa * rkappa * rkappa +l * dl * rdkappa + 2 * dl * dl * rkappa)return numerator / denominatorsnew = np.linspace(0, 5*np.pi, 1000)def frenet_to_cartesian1D(rs, rx, ry, rtheta, s_condition, d_condition):if fabs(rs - s_condition[0])>= 1.0e-6:print("The reference point s and s_condition[0] don't match")cos_theta_r = cos(rtheta)sin_theta_r = sin(rtheta)x = rx - sin_theta_r * d_condition[0]y = ry + cos_theta_r * d_condition[0]    return x, y
t = np.linspace(0, 10, 1000)
left_bound = []
right_bound = []for i in range(1000):rs = snew[i]rx = fx(rs)ry = fy(rs)drx = dfx(rs)dry = dfy(rs)rtheta = atan2(dry, drx)l_s_condition = np.array([rs])l_d_condition = np.array([1.5])lx, ly = frenet_to_cartesian1D(rs, rx, ry, rtheta, l_s_condition, l_d_condition)left_bound.append(np.array([lx, ly]))r_s_condition = np.array([rs])r_d_condition = np.array([-1.5])rx, ry = frenet_to_cartesian1D(rs, rx, ry, rtheta, r_s_condition, r_d_condition)   right_bound.append(np.array([rx, ry]))
left_bound = np.array(left_bound)
right_bound = np.array(right_bound)plt.plot(newx, newy, 'y')
plt.plot(left_bound[:,0],left_bound[:,1], 'b')
plt.plot(right_bound[:,0],right_bound[:,1], 'b')targets = []
for i in range(11):x0 = 0dx0 = 0 #np.tan(np.pi/6)x1 = -1.5 + i * 0.3dx1 = 0    targets.append(np.array([x0, dx0, x1, dx1]))thetas_buf = []
kappas_buf = []
for i in range(11):tar = targets[i]coeffs = cubicPolyCurve1d(tar[0], tar[1], tar[2], tar[3], 10.0)traj = []theta_buf = []kappa_buf = []for j in range(1000):rs = t[j]rx = fx(rs)ry = fy(rs)drx = dfx(rs)dry = dfy(rs)ddrx = ddfx(rs)ddry = ddfy(rs)dddrx = dddfx(rs)dddry = dddfy(rs)rtheta = atan2(dry, drx)rkappa = ComputeCurvature(drx, ddrx, dry, ddry)rdkappa = ComputeCurvatureDerivative(drx, ddrx, dddrx, dry, ddry, dddry)l = polyfit(coeffs, t[j], 0)dl = polyfit(coeffs, t[j], 1)theta = CalculateTheta(rtheta, rkappa, l, dl)theta_buf.append(theta)ddl = polyfit(coeffs, t[j], 2)kappa = CalculateKappa(rkappa, rdkappa, l, dl, ddl)kappa_buf.append(kappa)s_condition = np.array([rs])d_condition = np.array([l])x, y = frenet_to_cartesian1D(rs, rx, ry, rtheta, s_condition, d_condition)traj.append(np.array([x, y]))thetas_buf.append(np.array(theta_buf))kappas_buf.append(np.array(kappa_buf))traj = np.array(traj)plt.plot(traj[:,0], traj[:,1], 'g')
plt.show()
for i in range(11):plt.plot(thetas_buf[i])
plt.show()for i in range(11):plt.plot(kappas_buf[i])
plt.show()

各候选轨迹的每个点的角度：

各候选轨迹各点的曲率：

补充`CalculateKappa(rkappa, rdkappa, l, dl, ddl)`中求曲率的公式

在公式（9）中给出kxk_{x}kx的计算公式：kx=((l′′+(kr′l+krl′)tan⁡(θx−θr))cos⁡2(θx−θr)1−krl+kr)cos⁡(θx−θr)1−krlk_{x}=((l''+(k_{r}'l+k_{r}l')\tan(\theta_{x}-\theta_{r}))\frac{\cos^{2}(\theta_{x}-\theta_{r})}{1-k_{r}l}+k_{r})\frac{\cos(\theta_{x}-\theta_{r})}{1-k_{r}l}kx=((l′′+(kr′l+krl′)tan(θx−θr))1−krlcos2(θx−θr)+kr)1−krlcos(θx−θr)

但是，在上面例子代码中，已知base frame的参考点的曲率与曲率导数求待转换点的曲率公式为：

kx=kr+l′′−2lkr2−ll′′kr+l2kr3+ll′kr′+2l′2kr(l′2+(1−lkr)2)32k_{x}=\frac{k_{r}+l''-2lk_{r}^{2}-ll''k_{r}+l^2k_{r}^{3}+ll'k_{r}'+2l'^{2}k_{r}}{(l'^2+(1-lk_{r})^2)^{\frac{3}{2}}}kx=(l′2+(1−lkr)2)23kr+l′′−2lkr2−ll′′kr+l2kr3+ll′kr′+2l′2kr

下面给出简单推导过程：

从公式(8)、(9)抽出，s˙=vxcos⁡(θx−θr)1−lkr\dot{s}=\frac{v_x \cos(\theta_{x}-\theta_{r})}{1-lk_r}s˙=1−lkrvxcos(θx−θr)与vx=s˙(1−lkr)2+l′2v_{x}=\dot{s}\sqrt{(1-lk_r)^2+l'^2}vx=s˙(1−lkr)2+l′2,由这两式可得：
cos⁡(θx−θr)=1−lkr(1−lkr)2+l′2(10)\cos(\theta_{x}-\theta_{r})=\frac{1-lk_r}{\sqrt{(1-lk_{r})^{2}+l'^2}} \tag{10}cos(θx−θr)=(1−lkr)2+l′21−lkr(10)

另外有ddsx=ddsdsdtdtdsx=ddss˙vx=1(1−lkr)2+l′2dds\frac{d}{ds_{x}}=\frac{d}{ds}\frac{ds}{dt}\frac{dt}{ds_{x}}=\frac{d}{ds}\frac{\dot{s}}{v_{x}}=\frac{1}{\sqrt{(1-lk_{r})^{2}+l'^2}}\frac{d}{ds}dsxd=dsddtdsdsxdt=dsdvxs˙=(1−lkr)2+l′21dsd

kx=dθxdsx=d(arctan(l′1−lkr)+θr)dsx=11+(l′21−lkr)d(l′1−lkr)dsx+dθrdsx=(1−lkr)2l′2+(1−lkr)2dl′dsx(1−lkr)−d(1−lkr)dsxl′(1−lkr)2+dθrdsx=dl′dsx(1−lkr)−d(1−lkr)dsxl′l′2+(1−lkr)2+dθrdsx=dl′dsdsdtdtdsx(1−lkr)+dldsdsdtdtdsxl′kr+dkrdsdsdtdtdsxll′l′2+(1−lkr)2+θrdsdsdtdtdsx=dsdtdtdsx(l′′(1−lkr)+l′2kr+kr′ll′l′2+(1−lkr)2+kr)=1(1−lkr)2+l′2(l′′(1−lkr)+l′2kr+kr′ll′+krl′2+kr(1−lkr)2l′2+(1−lkr)2)=kr+l′′−2lkr2−ll′′kr+l2kr3+ll′kr′+2l′kr(l′2+(1−lkr)2)32k_{x}=\frac{d \theta_{x}}{d s_{x}}=\frac{d (arctan(\frac{l'}{1-lk_r})+\theta_{r})}{ds_{x}}=\frac{1}{1+(\frac{l'^2}{1-lk_r})}\frac{d(\frac{l'}{1-lk_r})}{ds_{x}}+\frac{d \theta_{r}}{d s_x}=\frac{(1-lk_r)^{2}}{l'^2+(1-lk_r)^2}\frac{\frac{d l'}{d s_{x}}(1-lk_r)-\frac{d(1-lk_r)}{d s_x}l'}{(1-lk_r)^2}+\frac{d \theta_{r}}{d s_x}=\frac{\frac{d l'}{d s_{x}}(1-lk_r)-\frac{d(1-lk_r)}{d s_x}l'}{l'^2+(1-lk_r)^2}+\frac{d \theta_{r}}{d s_x}=\frac{\frac{dl'}{ds}\frac{ds}{dt}\frac{dt}{ds_x}(1-lk_r)+\frac{dl}{ds}\frac{ds}{dt}\frac{dt}{ds_{x}}l'k_r+\frac{dk_r}{ds}\frac{ds}{dt}\frac{dt}{ds_{x}}ll'}{l'^2+(1-lk_r)^2}+\frac{\theta_{r}}{ds}\frac{ds}{dt}\frac{dt}{ds_{x}}=\frac{ds}{dt}\frac{dt}{ds_x}\left(\frac{l''(1-lk_r)+l'^2k_r+k_r'll'}{l'^2+(1-lk_r)^2}+k_{r}\right)=\frac{1}{\sqrt{(1-lk_{r})^{2}+l'^2}}\left(\frac{l''(1-lk_r)+l'^2k_r+k_r'll'+k_rl'^2+k_r(1-lk_r)^2}{l'^2+(1-lk_r)^2}\right)=\frac{k_r+l''-2lk_r^2-ll''k_r+l^2k_r^3+ll'k_r'+2l'k_r}{(l'^2+(1-lk_r)^2)^{\frac{3}{2}}}kx=dsxdθx=dsxd(arctan(1−lkrl′)+θr)=1+(1−lkrl′2)1dsxd(1−lkrl′)+dsxdθr=l′2+(1−lkr)2(1−lkr)2(1−lkr)2dsxdl′(1−lkr)−dsxd(1−lkr)l′+dsxdθr=l′2+(1−lkr)2dsxdl′(1−lkr)−dsxd(1−lkr)l′+dsxdθr=l′2+(1−lkr)2dsdl′dtdsdsxdt(1−lkr)+dsdldtdsdsxdtl′kr+dsdkrdtdsdsxdtll′+dsθrdtdsdsxdt=dtdsdsxdt(l′2+(1−lkr)2l′′(1−lkr)+l′2kr+kr′ll′+kr)=(1−lkr)2+l′21(l′2+(1−lkr)2l′′(1−lkr)+l′2kr+kr′ll′+krl′2+kr(1−lkr)2)=(l′2+(1−lkr)2)23kr+l′′−2lkr2−ll′′kr+l2kr3+ll′kr′+2l′kr

得出以下公式：
kx=kr+l′′−2lkr2−ll′′kr+l2kr3+ll′kr′+2l′kr(l′2+(1−lkr)2)32(11)k_x=\frac{k_r+l''-2lk_r^2-ll''k_r+l^2k_r^3+ll'k_r'+2l'k_r}{(l'^2+(1-lk_r)^2)^{\frac{3}{2}}} \tag{11}kx=(l′2+(1−lkr)2)23kr+l′′−2lkr2−ll′′kr+l2kr3+ll′kr′+2l′kr(11)