dw内部网站链接怎么做,网页设计与网站建设专业,郑州小学班级网站建设,seo优缺点本系列文章是学习深蓝学院-移动机器人运动规划课程第五章最优轨迹生成 过程中所记录的笔记#xff0c;本系列文章共包含四篇文章#xff0c;依次介绍了微分平坦特性、无约束BVP轨迹优化、无约束BIVP轨迹优、 带约束轨迹优化等内容 本系列文章链接如下#xff1a; 最优轨迹生…   本系列文章是学习深蓝学院-移动机器人运动规划课程第五章最优轨迹生成 过程中所记录的笔记本系列文章共包含四篇文章依次介绍了微分平坦特性、无约束BVP轨迹优化、无约束BIVP轨迹优、 带约束轨迹优化等内容 本系列文章链接如下 最优轨迹生成一—— 微分平坦 最优轨迹生成二—— 无约束BVP轨迹优化 最优轨迹生成三—— 无约束BIVP轨迹优化 最优轨迹生成四—— 带约束轨迹优化 三、无约束BIVP轨迹优化 如果用BVP方法来对如下所示的折线路径进行平滑时需要对每段折线解一个BVP且需要指定每段折线起始和终末状态如果指定的状态中的速度过大会不可行所以BVP的一个缺陷是需要找到合适的指定状态那么我们能不能仅对状态中的位置进行指定让其他状态量比如速度、加速度等自己去进行优化呢 也就是对于下面的路径我们仅给定起始和终末状态位置、速度、加速度、jerk等以及中间经过状态点的位置不对速度、加速度、jerk等其他状态量进行指定即要求平滑后的路径要经过这些指定的位置点但这些这些位置点处的速度、加速度、jerk等状态量通过算法优化自行得到这样也会使得轨迹更加顺滑这就是边界中间值问题BIVP。 BIVP的解具有超出输入或所优化阶数的连续性比如当s3时状态为位置、速度、加速度、输入为jerk则优化的目标函数为 min ⁡ z ( t ) ∫ t 0 t M [ p ( 3 ) ] 2 d t , \min_{z(t)}\int_{t_0}^{t_{M}}[p^{\left(3\right)}]^2\mathrm{d}t, minz(t)​∫t0​tM​​[p(3)]2dt,即最小化jerk最优性条件表明最优解是5次多项式BIVP的解可以进一步保证snap是连续的。当s4时是最小化snap但BIVP的解可以进一步保证Pop是连续的。 下图给出了s3时的例子状态p、v、a是连续的最小化的目标量jerk也是连续的、更高阶的snap是分段的但其在中间状态点处黄色的小球处也是连续的 所以我们可以直接去施加这些轨迹上的连续性条件得到一个关于多项式系数的等式Mcb只需要求一个M的逆就可以得到我们所需要的多项式的系数c不需要去做优化也不用去求 min ⁡ z ( t ) ∫ t 0 t M v ( t ) T W v ( t ) d t , \min_{z(t)}\int_{t_0}^{t_M}v(t)^{\mathrm{T}}\mathbf{W}v(t)\mathrm{d}t, minz(t)​∫t0​tM​​v(t)TWv(t)dt,这样一个问题 那么如何构建出上述的Mcb关系式呢 首先我们知道最优解一定是2s-1的多项式构成的样条我们可以把每段多项式都先写出来当s3时下式中N2s-15 f ( t ) { f 1 ( t ) ˙ ∑ i 0 N p 1 , i t i T 0 ≤ t ≤ T 1 f 2 ( t ) ˙ ∑ i 0 N p 2 , i t i T 1 ≤ t ≤ T 2 ⋮ ⋮ f M ( t ) ˙ ∑ i 0 N p M , i t i T M − 1 ≤ t ≤ T M f(t)\begin{cases}f_1(t)\dot{}\sum_{i0}^Np_{1,i}t^i\quad T_0\le t\le T_1\\f_2(t)\dot{}\sum_{i0}^Np_{2,i}t^i\quad T_1\le t\le T_2\\\vdots\quad\vdots\\f_M(t)\dot{}\sum_{i0}^Np_{M,i}t^i\quad T_{M-1}\le t\le T_M\end{cases} f(t)⎩ ⎨ ⎧​f1​(t)˙∑i0N​p1,i​tif2​(t)˙∑i0N​p2,i​ti⋮fM​(t)˙∑i0N​pM,i​ti​T0​≤t≤T1​T1​≤t≤T2​⋮TM−1​≤t≤TM​​​ 接下来将给定的信息以约束的形式写出来,比如将给定的起始状态和终末状态分别写成第一段和最后一段的等式约束如下所示 { f j ( k ) ( T j − 1 ) x 0 , j ( k ) f j ( k ) ( T j ) x T , j ( k ) \left\{\begin{matrix}{f_{j}^{(k)}(T_{j-1})}{x_{0,j}^{(k)}}\\{f_{j}^{(k)}(T_{j})}{x_{T,j}^{(k)}}\\\end{matrix}\right. {fj(k)​(Tj−1​)fj(k)​(Tj​)​x0,j(k)​xT,j(k)​​ 中间状态点的位置信息也是给定的也可以由等式约束的形式写出此外由前面的介绍可知相邻两段多项式要经过相同的状态点(位置、速度、加速度、jerk、snap均连续也就是5个等式) f j ( k ) ( T j ) f j 1 ( k ) ( T j ) f_{j}^{(k)}(T_{j})f_{j1}^{(k)}(T_{j}) fj(k)​(Tj​)fj1(k)​(Tj​) 通过这些条件就可以得到Mcb关系式 我们还需要为每段多项式轨迹分配时间有两种不同的时间轴给定方法第一种方法是每段多项式轨迹都独立计时每段多项式轨迹的起点时间记为0末端时间记为 T i T_i Ti​如下面的第一幅坐标轴所示。另一种方法是记录距离第一段轨迹开始处的时间差从第一段轨迹的开始处计时为0每段多项式轨迹的末端时间记为 T i T_i Ti​如下面的第二幅坐标轴所示。 从数值稳定性上来看上面的第一种方法更好一些 通过上面的介绍我们可以把BIVP问题根据最优条件即给定状态信息写出每一段多项式系数的方程组Mcb其中M矩阵是带状的稀疏矩阵可以调用稀疏求解器比如带状的PLU器来把每段多项式系数构成的矩阵c在线性时间内求解出来从而得到每段多项式的表达式。 那么这些中间的位置点如何确定呢 我们可以使用RRT*等全局规划算法来找到一条全局路径在这个路径上取一些关键的点来作为中间位置点再使用上面介绍的方法生成轨迹。关键点的提取可以采用Douglas-Peukcer等算法。 道格拉斯普克算法Douglas-Peukcer算法是一种简化线状要素的经典算法。其基本思想是对每一条曲线的首末点虚连一条直线求所有点与直线的距离,并找出最大距离值dmax用dmax与限差D相比。若dmaxD这条曲线上的中间点全部舍去若dmax ≥D保留dmax对应的坐标点,并以该点为界把曲线分为两部分对这两部分重复使用该方法。 算法的详细步骤如下: (1) 在曲线首尾两点间虚连一条直线,求出其余各点到该直线的距离如下图1。 (2) 选其最大者与阈值相比较,若大于阈值,则离该直线距离最大的点保留,否则将直线两端点间各点全部舍去如下图2第4点保留。 (3) 依据所保留的点,将已知曲线分成两部分处理,重复第1、2步操作迭代操作即仍选距离最大者与阈值比较,依次取舍,直到无点可舍去最后得到满足给定精度限差的曲线点坐标如图3、图4依次保留第6点、第7点舍去其他点,即完成线的化简。 DP算法的实例程序 void BuildTree(DPNode *root, vectorpcl::PointXYZ points, pcl::PointXYZ headpoint, pcl::PointXYZ endpoint, double thres_ds) {arrayoperation ArrExample;//创建一个新的根节点root new DPNode;root-points points;root-HeadPoint headpoint;root-EndPoint endpoint;if (points.size() 2)//点数少于2个的不再进行划分{root-Left_node NULL;root-Right_node NULL;root-NodeType false;//不能再划分}else{vectordouble disvec;//计算每个点到首尾两点构成直线的距离for (int i 0; i points.size(); i){double tempds Point2Dline(points[i], headpoint, endpoint);disvec.push_back(tempds);}double maxds ArrExample.getMax_vector(disvec);double maxindex ArrExample.GetIndexOfMax(disvec);//若整个点数为10个那么maxindex一定是介于 2到9之间因为不可能取首尾两个点首尾点到直线的距离为0if (maxds thres_ds)//小于阈值的不再分割{root-Left_node NULL;root-Right_node NULL;root-NodeType false;//不能再划分}else{root-NodeType true;//可以继续划分//将点划分成2部分左边与右边vectorpcl::PointXYZ Leftpointsvec, Rightpointsvec;for (int i 0; i points.size(); i){if (i maxindex){Leftpointsvec.push_back(points[i]);//左边树包含的点}}for (int i 0; i points.size(); i){if (i maxindex){Rightpointsvec.push_back(points[i]);//右边树包含的点}}//左边子树的头部点与尾部点pcl::PointXYZ left_headpoint headpoint;pcl::PointXYZ left_endpoint points[maxindex];//右边子树的头部点与尾部点pcl::PointXYZ right_headpoint points[maxindex];pcl::PointXYZ right_endpoint endpoint;//创建左、右树root-Right_node new DPNode();BuildTree(root-Left_node, Leftpointsvec, left_headpoint, left_endpoint, thres_ds);BuildTree(root-Right_node, Rightpointsvec, right_headpoint, right_endpoint, thres_ds);}} }但前面介绍的通过Mcb方法解得的多项式轨迹只能保证在中间位置点处是不碰撞的无法保证整个轨迹是不与障碍物相交的轨迹可能会与障碍物相交如下图所示 一种解决方法是首先保证全局路径规划算法找到的初始路径是无碰撞的一但生成的多项式轨迹与障碍物相交了则可以在发生碰撞的位置附近再插入新的中间位置点来使生成的多项式轨迹更加贴合最初的全局路径如下图所示 通过上面介绍的RRT* BIVP的方案我们可以把RRT在低维空间找到的可行路径拓展到高维的空间而且比Kinodynamic RRT*算法更高效可靠。 上述方法也存在一些缺陷比如在障碍物比较多的时候可能需要加入很多中间位置点轨迹要很贴合RRT*找到的原始路径才能保证安全性无人机的飞行可能不顺滑。 参考资料 1、深蓝学院-移动机器人运动规划 2、道格拉斯普克算法简化线段点