自动驾驶决策规划算法-Frenet坐标系与笛卡尔坐标系的转换

本文为学习B站视频《自动驾驶决策规划算法第一章第三节(上) 直角坐标与自然坐标转换》的学习笔记。

1 预备知识

（1）如下图黑色曲线为车辆轨迹点，则有：

$$\begin{array}{c}\dot{\mathbf{r}}=\frac{d\mathbf{r}}{ds}\cdot \frac{ds}{dt}=\mathbf{\tau }\cdot v=\mathbf{v}\end{array}$$
（2）如下图黑色曲线为车辆轨迹点，绿色曲线为参考线，则有：

$$\begin{array}{c}\dot{{\mathbf{r}}_{\mathbf{c}}}=\mathbf{v}\end{array}$$
$$\begin{array}{c}\dot{{\mathbf{r}}_{\mathbf{r}}}=\frac{d{\mathbf{r}}_{\mathbf{r}}}{d{s}_r}\cdot \frac{d{s}_r}{dt}=\dot{s_r}\cdot {\mathbf{\tau }}_{\mathbf{r}}\end{array}$$
其中，下标$c$表示车辆轨迹上的点，下标$r$表示在参考线上的投影点。
注意，这里$s_r$对应的是参考线，而不是车辆的实际轨迹线，注意区分，其中车辆轨迹点与在参考线上的投影点的连线垂直与投影点的切线。
（3）frenet公式

由于$\mathbf{\tau }$是单位向量，当$\theta$趋于0时，$d\mathbf{\tau }$的方向垂直于$\mathbf{\tau }$，即为$\mathbf{n}$，且
$$\begin{array}{c}|d\mathbf{\tau }|=2sin\frac{\theta }{2}=\theta \end{array}$$
因此
$$\begin{array}{c}\frac{d\mathbf{\tau }}{ds}=\frac{\theta }{ds}\mathbf{n}=k\mathbf{n}\end{array}$$
其中$k$是曲率。
由于$\mathbf{n}$是单位向量，当$\alpha$趋于0时，$d\mathbf{n}$的方向垂直于$\mathbf{n}$，方向为$\mathbf{\tau }$的反方向，且
$$\begin{array}{c}|d\mathbf{n}|=2sin\frac{\alpha }{2}=\alpha \end{array}$$
因此
$$\begin{array}{c}\frac{d\mathbf{n}}{ds}=-\frac{\alpha }{ds}\mathbf{\tau }=-k\mathbf{\tau }\end{array}$$
同样，可得如下公式：
$$\begin{array}{c}\dot{{\mathbf{\tau }}_{\mathbf{c}}}=\frac{d{\mathbf{\tau }}_{\mathbf{c}}}{dt}=\frac{d{\mathbf{\tau }}_{\mathbf{c}}}{d{s}_c}\cdot \frac{d{s}_c}{dt}=k_c{\mathbf{n}}_{\mathbf{c}}{v}_c\end{array}$$
$$\begin{array}{c}\dot{{\mathbf{n}}_{\mathbf{c}}}=\frac{d{\mathbf{n}}_{\mathbf{c}}}{dt}=\frac{d{\mathbf{n}}_{\mathbf{c}}}{d{s}_c}\cdot \frac{d{s}_c}{dt}=-k_c{\mathbf{\tau }}_{\mathbf{c}}{v}_c\end{array}$$
$$\begin{array}{c}\dot{{\mathbf{\tau }}_{\mathbf{r}}}=\frac{d{\mathbf{\tau }}_{\mathbf{r}}}{dt}=\frac{d{\mathbf{\tau }}_{\mathbf{r}}}{d{s}_r}\cdot \frac{d{s}_r}{dt}=k_r{\mathbf{n}}_{\mathbf{r}}\dot{s_r}\end{array}$$
$$\begin{array}{c}\dot{{\mathbf{n}}_{\mathbf{r}}}=\frac{d{\mathbf{n}}_{\mathbf{r}}}{dt}=\frac{d{\mathbf{n}}_{\mathbf{r}}}{d{s}_r}\cdot \frac{d{s}_r}{dt}=-k_r{\mathbf{\tau }}_{\mathbf{r}}\dot{s_r}\end{array}$$

2 坐标转换

2.1 自车状态

笛卡尔坐标系下车辆状态表示为$[{\mathbf{x}}_{\mathbf{c}},{\mathbf{v}}_{\mathbf{c}},{\mathbf{a}}_{\mathbf{c}},{\theta }_c,k_c]$，分别表示质心坐标，质心速度，质心加速度，质心航向角，质心实际轨迹的曲率。
在Frenet坐标系下，自车在任意时刻$t$的状态（以参考线为基准）可以表示为$[s,\dot{s},\ddot{s},l,\dot{l},\ddot{l},l^{\prime },l^{\prime \prime }]$，其中：
$s$: 坐标系下纵标下投影点的值（沿道路的距离）；
$\dot{s}=\frac{ds}{dt}$: Frenet坐标系下纵坐标对时间的导数，即Frenet坐标系下纵坐标上的速度;
$\ddot{s}=\frac{d\dot{s}}{dt}$: Frenet坐标下纵坐标上的加速度;
$l$: Frenet纵坐标下横向坐标值（即与参考线的距离）;
$\dot{l}=\frac{dl}{dt}$: Frenet坐标系下横向速度;
$\ddot{l}=\frac{d\dot{l}}{dt}$: Frenet坐标系下横向加速度;
$l^{\prime }=\frac{dl}{ds}$: Frenet坐标下横向坐标对纵向坐标的一阶导数;
$l^{\prime \prime }$: Frenet坐标系下横坐标对纵坐标的二阶导数;
自车在任意时刻$t$，车辆质心在实际轨迹某轨迹点处的单位正交向量定义为${\mathbf{\tau }}_{\mathbf{c}}$和${\mathbf{n}}_{\mathbf{c}}$，在参考线上的对应的投影点处的单位正交向量定义为${\mathbf{\tau }}_{\mathbf{r}}$和${\mathbf{n}}_{\mathbf{r}}$和, 用相应的航向角可以表示为:
$$\begin{array}{c}{\mathbf{\tau }}_{\mathbf{c}}=[cos{\theta }_c,sin{\theta }_c{]}^T,{\mathbf{n}}_{\mathbf{c}}=[-sin{\theta }_c,cos{\theta }_c{]}^T\end{array}$$
$$\begin{array}{c}{\mathbf{\tau }}_{\mathbf{r}}=[cos{\theta }_r,sin{\theta }_r{]}^T,{\mathbf{n}}_{\mathbf{r}}=[-sin{\theta }_r,cos{\theta }_r{]}^T\end{array}$$

2.1 笛卡尔坐标系向Frenet坐标系转换

2.1.1 计算投影点

Frenet坐标系的参考线都是由规划模块得到的一系列离散点，假设车辆当前的质心位置矢量为$(x_c,y_c)$，找到参考线上距离车辆质心最近的离散点$(x_m,y_m)$作为匹配点，匹配点至车辆质心的向量${\mathbf{l}}_{\mathbf{mc}}$，以及匹配点的单位切向量${\mathbf{\tau }}_{\mathbf{m}}$
$$\begin{array}{c}{\mathbf{l}}_{\mathbf{mc}}=(x_c-x_m,y_c-y_m),{\mathbf{\tau }}_{\mathbf{m}}=(cos{\theta }_m,sin{\theta }_m)\end{array}$$
投影点在笛卡尔坐标系下的向量值${\mathbf{r}}_{\mathbf{r}}$近似等于匹配点的向量值${\mathbf{r}}_{\mathbf{m}}$加上向量${\mathbf{l}}_{\mathbf{mc}}$在${\mathbf{\tau }}_{\mathbf{m}}$方向上的投影分量，得：
$$\begin{array}{c}{\mathbf{r}}_{\mathbf{r}}={\mathbf{r}}_{\mathbf{m}}+({\mathbf{l}}_{\mathbf{mc}}\cdot {\mathbf{\tau }}_{\mathbf{m}})\cdot {\mathbf{\tau }}_{\mathbf{m}}\end{array}$$
参考线是直线时是精确解，参考线是曲线时这是一种近似解，参考点越密，曲率越小越精确，如下图所示，其中绿色点为离散的参考点$m$，紫色点为求得的投影点：

投影点处的曲率可近似等于匹配点处的曲率：
$$\begin{array}{c}{k}_r=k_m\end{array}$$
这样，从匹配点到投影点的曲线可认为是圆弧，弧长近似为${\mathbf{l}}_{\mathbf{mc}}\cdot {\mathbf{\tau }}_{\mathbf{m}}$，则有：
$$\begin{array}{c}{\theta }_r={\theta }_m+k_m({\mathbf{l}}_{\mathbf{mc}}\cdot {\mathbf{\tau }}_{\mathbf{m}})\end{array}$$

2.1.2 坐标转换

参考点的起始点是$(x_0,y_0)$，往后依次编号，用两点间的直线距离近似取代曲线距离，则有：
$$\begin{array}{c}s=\sqrt{(x_1-x_0{)}^2+(y_1-y_0{)}^2}+\sqrt{(x_2-x_1{)}^2+(y_2-y_1{)}^2}+\cdots +\sqrt{(x_r-x_m{)}^2+(y_r-y_m{)}^2}\end{array}$$
这里使用直线近似曲线具有较好的鲁棒性，也可以使用圆弧$(\theta /k)$近似曲线，但是不够鲁棒（当曲率很小时，微小的误差就会导致结果偏差很大）。
根据向量关系：
$$\begin{array}{c}{\mathbf{r}}_{\mathbf{c}}={\mathbf{r}}_{\mathbf{r}}+l{\mathbf{n}}_{\mathbf{r}}\end{array}$$
可得
$$\begin{array}{c}l=({\mathbf{r}}_{\mathbf{c}}-{\mathbf{r}}_{\mathbf{r}})\cdot {\mathbf{n}}_{\mathbf{r}}\end{array}$$
这是向量形式的表达式，转化为标量表达式就是：
$$\begin{array}{c}l=sign((y_c-y_r)cos{\theta }_r-(x_c-x_r)sin{\theta }_r)\sqrt{(x_c-x_r{)}^2+(y_c-y_r{)}^2}\end{array}$$
将式(19)对时间求导：
$$\begin{array}{c}{\mathbf{v}}_{\mathbf{c}}=\dot{s}{\mathbf{\tau }}_{\mathbf{r}}+\dot{l}{\mathbf{n}}_{\mathbf{r}}+l\dot{{\mathbf{n}}_{\mathbf{r}}}=\dot{s}{\mathbf{\tau }}_{\mathbf{r}}+\dot{l}{\mathbf{n}}_{\mathbf{r}}-l{k}_r\dot{s}{\mathbf{\tau }}_{\mathbf{r}}\end{array}$$
式(22)两边同时点乘${\mathbf{n}}_{\mathbf{r}}$后化简可得：
$$\begin{array}{c}\dot{l}={\mathbf{v}}_{\mathbf{c}}\cdot {\mathbf{n}}_{\mathbf{r}}\end{array}$$
转化为标量形式就是：
$$\begin{array}{c}\dot{l}=v_{c}sin({\theta }_c-{\theta }_r)\end{array}$$
式(23)对时间求导可得：
$$\begin{array}{c}\ddot{l}={\mathbf{a}}_{\mathbf{c}}\cdot {\mathbf{n}}_{\mathbf{r}}-k_r(1-l{k}_r){\dot{s}}^2\end{array}$$
进一步展开得：
$$\begin{array}{c}\ddot{l}=a_{c\tau }sin({\theta }_c-{\theta }_r)+a_{cn}cos({\theta }_c-{\theta }_r)-k_r(1-l{k}_r){\dot{s}}^2\end{array}$$

同时，式(22)两边同时点乘${\mathbf{\tau }}_{\mathbf{r}}$后化简可得：
$$\begin{array}{c}\dot{s}=\frac{{\mathbf{v}}_{\mathbf{c}}\cdot {\mathbf{\tau }}_{\mathbf{r}}}{1-l{k}_r}\end{array}$$
转化为标量形式就是：
$$\begin{array}{c}\dot{s}=\frac{v_{c}cos({\theta }_c-{\theta }_r)}{1-l{k}_r}\end{array}$$
根据定义：
$$\begin{array}{c}{l}^{\prime }=\frac{dl}{ds}=\frac{dl}{dt}\frac{dt}{ds}=v_{c}sin({\theta }_c-{\theta }_r)\frac{1-l{k}_r}{v_{c}cos({\theta }_c-{\theta }_r)}=(1-l{k}_r)tan({\theta }_c-{\theta }_r)\end{array}$$
式(27)对时间求导可得：
$$\begin{array}{c}\begin{array}{r}\ddot{s}=\frac{(\dot{{\mathbf{v}}_{\mathbf{c}}}\cdot {\mathbf{\tau }}_{\mathbf{r}}+{\mathbf{v}}_{\mathbf{c}}\cdot \dot{{\mathbf{\tau }}_{\mathbf{r}}})(1-l{k}_r)+(\dot{l}{k}_r+l\dot{k_r}){\mathbf{v}}_{\mathbf{c}}\cdot {\mathbf{\tau }}_{\mathbf{r}}}{(1-l{k}_r{)}^2}\\ =\frac{{\mathbf{a}}_{\mathbf{c}}\cdot {\mathbf{\tau }}_{\mathbf{r}}}{1-l{k}_r}+\frac{k_r\dot{s}{\mathbf{v}}_{\mathbf{c}}\cdot {\mathbf{n}}_{\mathbf{r}}}{1-l{k}_r}+\frac{1}{1-l{k}_r}\dot{s}(k_r\frac{dl}{ds}\frac{ds}{dt}+l\frac{d{k}_r}{ds}\frac{ds}{dt})\\ =\frac{{\mathbf{a}}_{\mathbf{c}}\cdot {\mathbf{\tau }}_{\mathbf{r}}}{1-l{k}_r}+\frac{k_r\dot{s}{\mathbf{v}}_{\mathbf{c}}\cdot {\mathbf{n}}_{\mathbf{r}}}{1-l{k}_r}+\frac{1}{1-l{k}_r}\dot{s}(k_r{l}^{\prime }\dot{s}+l{k}_r^{\prime }\dot{s})\end{array}\end{array}$$
而
$$\begin{array}{c}{\mathbf{a}}_{\mathbf{c}}\cdot {\mathbf{\tau }}_{\mathbf{r}}={\mathbf{a}}_{\mathbf{c\tau }}\cdot {\mathbf{\tau }}_{\mathbf{r}}+{\mathbf{a}}_{\mathbf{cn}}\cdot {\mathbf{\tau }}_{\mathbf{r}}=a_{c\tau }cos({\theta }_c-{\theta }_r)-v_c^2{k}_{c}sin({\theta }_c-{\theta }_r)\end{array}$$
代入式(30)可得标量形式：
$$\begin{array}{c}\begin{array}{r}\ddot{s}=\frac{{\mathbf{a}}_{\mathbf{c}}\cdot {\mathbf{\tau }}_{\mathbf{r}}}{1-l{k}_r}+\frac{k_r{\dot{s}}^2{l}^{\prime }}{1-l{k}_r}+\frac{{\dot{s}}^2}{1-l{k}_r}(k_r{l}^{\prime }+l{k}_r^{\prime })\\ =\frac{a_{c\tau }cos({\theta }_c-{\theta }_r)}{1-l{k}_r}+\frac{v_c^2{k}_{c}sin({\theta }_c-{\theta }_r)}{1-l{k}_r}+\frac{k_r{\dot{s}}^2{l}^{\prime }}{1-l{k}_r}+\frac{{\dot{s}}^2}{1-l{k}_r}(k_r{l}^{\prime }+l{k}_r^{\prime })\\ =\frac{a_{c\tau }cos({\theta }_c-{\theta }_r)}{1-l{k}_r}-\frac{1}{1-l{k}_r}[\frac{v_c^{2}co{s}^2({\theta }_c-{\theta }_r)}{(1-l{k}_r{)}^2}\frac{(1-l{k}_r)sin({\theta }_c-{\theta }_r)}{cos({\theta }_c-{\theta }_r)}{k}_c\frac{1-l{k}_r}{cos({\theta }_c-{\theta }_r)}]\\ +\frac{k_r{\dot{s}}^2{l}^{\prime }}{1-l{k}_r}+\frac{{\dot{s}}^2}{1-l{k}_r}(k_r{l}^{\prime }+l{k}_r^{\prime })\\ =\frac{a_{c\tau }cos({\theta }_c-{\theta }_r)-{\dot{s}}^2[l^{\prime }(k_c\frac{1-l{k}_r}{cos({\theta }_c-{\theta }_r)}-k_r)-(k_r{l}^{\prime }+l{k}_r^{\prime })]}{1-l{k}_r}\end{array}\end{array}$$
根据定义：
$$\begin{array}{c}\ddot{l}=\frac{d\dot{l}}{dt}=\frac{d(l^{\prime }\dot{s})}{dt}=\frac{d{l}^{\prime }}{dt}\dot{s}+l^{\prime }\ddot{s}=\frac{d{l}^{\prime }}{ds}\frac{ds}{dt}\dot{s}+l^{\prime }\ddot{s}=l^{\prime \prime }{\dot{s}}^2+l^{\prime }\ddot{s}\end{array}$$
可得：
$$\begin{array}{c}{l}^{\prime \prime }=\frac{\ddot{l}-l^{\prime }\ddot{s}}{{\dot{s}}^2}\end{array}$$
进一步展开可得：
$$\begin{array}{c}{l}^{\prime \prime }=-(k_r{l}^{\prime }+l{k}_r^{\prime })tan({\theta }_c-{\theta }_r)+\frac{1-l{k}_r}{co{s}^2({\theta }_c-{\theta }_r)}(\frac{1-l{k}_r}{cos({\theta }_c-{\theta }_r)}{k}_c-k_r)\end{array}$$

因此可得笛卡尔向Frenet转换的公式：
$$\begin{array}{c}\{\begin{array}{l}\dot{s}=\frac{v_{c}cos({\theta }_c-{\theta }_r)}{1-l{k}_r}\\ \ddot{s}=\frac{a_{c\tau }cos({\theta }_c-{\theta }_r)-{\dot{s}}^2[l^{\prime }(k_c\frac{1-l{k}_r}{cos({\theta }_c-{\theta }_r)}-k_r)-(k_r{l}^{\prime }+l{k}_r^{\prime })]}{1-l{k}_r}\\ l=sign((y_c-y_r)cos{\theta }_r-(x_c-x_r)sin{\theta }_r)\sqrt{(x_c-x_r{)}^2+(y_c-y_r{)}^2}\\ \dot{l}=v_{c}sin({\theta }_c-{\theta }_r)\\ \ddot{l}=a_{c\tau }sin({\theta }_c-{\theta }_r)+a_{cn}cos({\theta }_c-{\theta }_r)-k_r(1-l{k}_r){\dot{s}}^2\\ l^{\prime }=(1-l{k}_r)tan({\theta }_c-{\theta }_r)\\ l^{\prime \prime }=-(k_r{l}^{\prime }+l{k}_r^{\prime })tan({\theta }_c-{\theta }_r)+\frac{1-l{k}_r}{co{s}^2({\theta }_c-{\theta }_r)}(\frac{1-l{k}_r}{cos({\theta }_c-{\theta }_r)}{k}_c-k_r)\end{array}\end{array}$$
$s$的计算参考式(18)。

2.2 Frenet坐标系向笛卡尔坐标系转换

已知$[s,\dot{s},\ddot{s},l,\dot{l},\ddot{l},l^{\prime },l^{\prime \prime }]$，求$[{\mathbf{r}}_{\mathbf{c}},{\mathbf{v}}_{\mathbf{c}},{\mathbf{a}}_{\mathbf{c}},k_c]$

2.2.1 计算投影点

在离散的参考点集上找到$(x_n,y_n)$和$(x_{n+1},y_{n+1})$，使得$s_n\le s\le s_{n+1}$，如下图所示，有：
$$\begin{array}{c}{\mathbf{r}}_{\mathbf{r}}={\mathbf{r}}_{\mathbf{n}}+{\mathbf{r}}_{\mathbf{d}}\end{array}$$

其中${\mathbf{r}}_{\mathbf{d}}$的长度为$s-s_n$，方向近似为${\mathbf{\tau }}_{\mathbf{n}}=(cos{\theta }_n,sin{\theta }_n)$，这里角度也可以取${\theta }_n$和${\theta }_{n+1}$的均值：
$$\begin{array}{c}{\mathbf{r}}_{\mathbf{d}}=(s-s_n){\mathbf{\tau }}_{\mathbf{n}}\end{array}$$
可以求出${\mathbf{r}}_{\mathbf{r}}$，进而求出$(x_r,y_r)$，同时可计算得到如下变量：
$$\begin{array}{c}{\theta }_r={\theta }_n+k_n(s-s_n)\end{array}$$
$$\begin{array}{c}{k}_r=\frac{k_n+k_{n+1}}{2}\end{array}$$
$$\begin{array}{c}{k}_r^{\prime }=\frac{d{k}_r}{ds}\approx \frac{k_{n+1}-k_n}{s_{n+1}-s_n}\end{array}$$
$$\begin{array}{c}{\mathbf{\tau }}_{\mathbf{r}}=(cos{\theta }_r,sin{\theta }_r)\end{array}$$
$$\begin{array}{c}{\mathbf{n}}_{\mathbf{r}}=(-sin{\theta }_r,cos{\theta }_r)\end{array}$$

由公式(19)可得：
$$\begin{array}{c}{x}_c=x_r-lsin{\theta }_r\end{array}$$
$$\begin{array}{c}{y}_c=y_r+cos{\theta }_r\end{array}$$
由上一章节$\dot{s}$和$\dot{l}$的表达式可得：
$$\begin{array}{c}{v}_c=\sqrt{{\dot{l}}^2+{\dot{s}}^2(1-l{k}_r{)}^2}\end{array}$$

$$\begin{array}{c}tan({\theta }_c-{\theta }_r)=\frac{\dot{l}}{(1-l{k}_r)\dot{s}}=\frac{l^{\prime }}{1-l{k}_r}\end{array}$$

$$\begin{array}{c}{\theta }_c={\theta }_r+arctan(\frac{l^{\prime }}{1-l{k}_r})\end{array}$$

同理由式(25)和(30)可得：
$$\begin{array}{c}{a}_c=\sqrt{[\ddot{l}+k_r(1-l{k}_r){\dot{s}}^2{]}^2+[\ddot{s}(1-l{k}_r)-{\dot{s}}^2{k}_r{l}^{\prime }-{\dot{s}}^2(kr{l}^{\prime }+k_r^{\prime }l){]}^2}\end{array}$$
假设加速度方向与$x$轴的夹角为${\theta }_{ac}$，有：
$$\begin{array}{c}{\theta }_{ac}=arctan(\frac{\ddot{l}+k_r(1-l{k}_r){\dot{s}}^2}{\ddot{s}(1-l{k}_r)-{\dot{s}}^2{k}_r{l}^{\prime }-{\dot{s}}^2(k_r{l}^{\prime }+k_r^{\prime }l)})+{\theta }_r\end{array}$$
由式(32)可得：

$$\begin{array}{c}{a}_{c\tau }=\ddot{s}\frac{1-l{k}_r}{cos({\theta }_c-{\theta }_r)}+\frac{{\dot{s}}^2}{cos({\theta }_c-{\theta }_r)}[l^{\prime }(k_c\frac{1-l{k}_r}{cos({\theta }_c-{\theta }_r)}-k_r)-(k_r{l}^{\prime }+k_r^{\prime })]\end{array}$$

由式(35)可得：

$$\begin{array}{c}{k}_c=((l^{\prime \prime }+(k_r{l}^{\prime }+k_r^{\prime }l)tan({\theta }_c-{\theta }_r))\frac{co{s}^2({\theta }_c-{\theta }_r)}{1-l{k}_r}+k_r)\frac{cos({\theta }_c-{\theta }_r)}{1-l{k}_r}\end{array}$$

因此可得Frenet向笛卡尔转换的公式：

$$\begin{array}{c}\{\begin{array}{l}{x}_c=x_r-lsin{\theta }_r\\ y_c=y_r+cos{\theta }_r\\ {\theta }_c={\theta }_r+arctan(\frac{l^{\prime }}{1-l{k}_r})\\ v_c=\sqrt{{\dot{l}}^2+{\dot{s}}^2(1-l{k}_r{)}^2}\\ a_{c\tau }=\ddot{s}\frac{1-l{k}_r}{cos({\theta }_c-{\theta }_r)}+\frac{{\dot{s}}^2}{cos({\theta }_c-{\theta }_r)}[l^{\prime }(k_c\frac{1-l{k}_r}{cos({\theta }_c-{\theta }_r)}-k_r)-(k_r{l}^{\prime }+k_r^{\prime })]\\ k_c=((l^{\prime \prime }+(k_r{l}^{\prime }+k_r^{\prime }l)tan({\theta }_c-{\theta }_r))\frac{co{s}^2({\theta }_c-{\theta }_r)}{1-l{k}_r}+k_r)\frac{cos({\theta }_c-{\theta }_r)}{1-l{k}_r}\end{array}\end{array}$$