归一化后,RS曲线的起点必定在车辆坐标系原点,终点转换至车辆坐标系后,剩下的就是求解起点A到终点B的路径。

另外,由于归一化,已知条件还有一条,就是半径必定是1;上一节说到,RS曲线共有48种曲线类型,通过对称性,不用写48种计算公式,只需要编写9种(可能没数对,但意思正确)。

至此,RS变成了在车辆坐标系下,已知起点,终点,旋转半径长度,固定曲线类型,而求解路径长度的平面几何计算题;

由于已经预设好了曲线类型,则不必费心去思考通过什么方式去到达终点,只需按照48种情况逐个计算就好。

具体计算均脱胎于以下三个公式,是计算从起点(x,y,φ)按L,R,S的类型,经过 t  的距离后,到达新的坐标点的坐标,此公式在RS曲线的论文中:

以L+S+R+为例,终点为B(10,10)点,设L,S,R的长度分别为t,u,v;从起点A(0,0)经过左转弧 t 后到达M1(m1,n1,φ1)点,再以M1为起点经过S直线前进u,到达M2点,最后再以M2为起点,经过右转弧前进v,到达终点B(10,10),这样就有了包含t,u,v三个未知量的三个方程,通过求解t,u,v来得到RS每种类型路径的长度。

当然,单单计算t,u,v是不够的,必须对公式加以约束,因为在某些终点下,有些类型是不一定能够到达终点的,仍以L+S+R+为例,推导求解过程。

如下图所示,红色线表示RS的L+S+R+路径,从原点到达终点。

根据上图的计算公式,依次代入L,S,R,得到三个等式,对等式进行变换,化成求终点(x,y,φ)的形式,得到以下公式,t,u,v分别代表L,S,R的长度:

由于终点(x,y,φ)已知,通过上式便可求出t,u,v,观察公式,显然还可以化简,因为(t-v)是已知的,对上式变换,可以得到新的等式:

由于图中phi是0,可以知道p点的位置(图中蓝色P点所示),这里标出P的位置,只是便于理解,知道了P的坐标(x+sin(phi),y-1-cos(phi)),现在从极坐标的角度考虑这个点,matlab中cart2pol函数可以求得极坐标系下的坐标,这样,就可以知道P点在极坐标系下的角度θ和半径r(极坐标和车辆坐标系原点相同)

由RS路径图中绿色三角形可知,求得P点的极坐标系下的半径r后,绿色三角形的斜边就知道了,同时长直角边长度必定2(两个半径和),因此,LSR中的S的长度u,就可以通过勾股定理求得结果。

而L的长度t,则通过极坐标系下P的角度θ,绿色三角形的夹角之和可以求得,至此,t,u,v就通过几何的方式求出,这个方法,也是RS曲线代码中的求解方法。

RS曲线matlab代码中求解L+S+R+的代码如下

function [isok,t,u,v] = LpSpRp(x,y,phi)
    [t1,u1] = cart2pol(x+sin(phi),y-1-cos(phi));%转换至极坐标,求解P点的半径r,和角度θ
    if u1^2 >= 4
        u = sqrt(u1^2-4);
        theta = atan2(2,u);%求绿色三角形夹角
        t = mod2pi(t1+theta);
        v = mod2pi(t-phi);
        if t >= 0  && v >= 0 % 符号代表前进和后退
            isok = true;
            return
        end
    end
    isok = false;
    t = 0;
    u = 0;
    v = 0;
end

现在是否能对应上代码中的公式来源?

最后,还需要解释下代码中的限制条件,比如u1^2 >= 4,这一条就是前边说的公式的限制条件。

仍然回到RS的曲线图中:

u1就是斜边,假设u的长度为0,那么BC点就会重合,AD的距离就是两个半径之和为2,由于u的长度最小就是0,显然,2是AD的最短距离。LSR成立的情况通过图中所示,可已判定S就是LR两个圆的公切线,如果AD小于2,那么就不会有LSR这种路径。

同理,其他类型的RS曲线如CCC,CCCC均是以平面几何的思路来求解的,OMPL库中RS曲线同样也是如此,具体思路整理如下:

1、确定曲线的类型,根据L、S、R的计算公式,计算每一步的终点坐标

2、把公式换算为计算最终终点(x,y,phi)的形式,由于终点已知,此时就可以求解

3、为了方便求解和理解,采用画图的方法,从几何意义上求解t、u、v;

4、将公式换算为极坐标系下的P点计算公式,求解P点的半径和角度

5、通过三角形,平行线夹角等方式,求解t、u、v

6、根据几何意义,确定限制条件,限制条件的分析均是从圆、直线的几何关系中得到。

通过上述方法得到某一类型的计算公式后,再利用对称性扩展,就得到了全部的RS曲线

最后,值得注意的是,t、u、v只是针对CSC。CCC这种三步的RS曲线,CCCC,CCSCC这种路线会有五步,用t、u、v、w、z来表示每步的步长,同时,可以发现,如果有五个未知量,重点只有(x,y,phi)三个值是已知的,三个公式求解五个未知量显然是不能求出的,所以必须规定两步的步长,这就是为什么CCSCC中有两步必须是pi/2的原因。