免费私人网站,广东外贸网站建设企业,承德信息港,网站 简约目录 0 专栏介绍1 纯追踪算法原理推导2 自适应纯追踪算法(APP)3 规范化纯追踪算法(RPP)4 仿真实现4.1 ROS C仿真4.2 Python仿真4.3 Matlab仿真 0 专栏介绍
#x1f525;附C/Python/Matlab全套代码#x1f525;课程设计、毕业设计、创新竞赛必备#xff01;详细介绍全局规划… 目录 0 专栏介绍1 纯追踪算法原理推导2 自适应纯追踪算法(APP)3 规范化纯追踪算法(RPP)4 仿真实现4.1 ROS C仿真4.2 Python仿真4.3 Matlab仿真 0 专栏介绍
附C/Python/Matlab全套代码课程设计、毕业设计、创新竞赛必备详细介绍全局规划(图搜索、采样法、智能算法等)局部规划(DWA、APF等)曲线优化(贝塞尔曲线、B样条曲线等)。
详情图解自动驾驶中的运动规划(Motion Planning)附几十种规划算法 1 纯追踪算法原理推导
纯追踪算法(Pure Pursuit, PP)参考了人类驾驶行为其基本思想是在待跟踪路径上设置预瞄点(goal-ahead)通过简单的几何方法驱动机器人跟踪预瞄点随着机器人运动预瞄点动态移动直至抵达目标位置。
给定路径点 P { p 0 , p 1 , ⋯ , p g } \mathcal{P} \left\{ \boldsymbol{p}_0,\boldsymbol{p}_1,\cdots ,\boldsymbol{p}_g \right\} P{p0,p1,⋯,pg}。根据纯追踪算法设置的固定预瞄距离 L L L选择预瞄点 p l p i ∈ P s . t . ∥ p i − 1 − p r ∥ 2 2 L a n d ∥ p i − p r ∥ 2 2 ⩾ L \boldsymbol{p}_l\boldsymbol{p}_i\in \mathcal{P} \,\,\mathrm{s}.\mathrm{t}. \left\| \boldsymbol{p}_{i-1}-\boldsymbol{p}_r \right\| _{2}^{2}L\,\,\mathrm{and} \left\| \boldsymbol{p}_i-\boldsymbol{p}_r \right\| _{2}^{2}\geqslant L plpi∈Ps.t.∥pi−1−pr∥22Land∥pi−pr∥22⩾L
其中 p r \boldsymbol{p}_r pr是最接近机器人当前位置的路径点。
当规划时间间隔 Δ t → 0 \varDelta t\rightarrow 0 Δt→0时可认为机器人线速度 v v v和角速度 ω \omega ω不变因此其转向半径 R v / ω R{{v}/{\omega}} Rv/ω是定值即机器人进行圆周运动。 如图所示根据几何关系可得 L sin 2 α R sin ( π / 2 − α ) ⇒ R L 2 sin α \frac{L}{\sin 2\alpha}\frac{R}{\sin \left( {{\pi}/{2}}-\alpha \right)}\Rightarrow R\frac{L}{2\sin \alpha} sin2αLsin(π/2−α)R⇒R2sinαL
确定曲率半径后对于差速轮式移动机器人而言可根据下式计算当前的控制指令 { v t v d ω t v t / R \begin{cases} v_tv_d\\ \omega _t{{v_t}/{R}}\\\end{cases} {vtvdωtvt/R
在机器人局部坐标系中设机器人与预瞄点的纵向误差为 e y e_y ey则 R L 2 2 e y ⇔ κ 2 L 2 ⋅ e y R\frac{L^2}{2e_y}\Leftrightarrow \kappa \frac{2}{L^2}\cdot e_y R2eyL2⇔κL22⋅ey
相当于一个以横向跟踪误差为系统误差的比例控制器如图所示。增大 L L L有利于降低超调但会产生稳态误差减小 L L L能够加快动态响应速度但容易引起振荡。 2 自适应纯追踪算法(APP)
为了在跟踪振荡和较慢收敛间取得可接受的权衡自适应纯追踪算法(Adaptive Pure Pursuit, APP)根据运动速度自适应调整预瞄距离 L t l t v t L 0 L_tl_tv_tL_0 LtltvtL0
其中 l t l_t lt是前瞻增益表示将 v t v_t vt向前投影的时间增量 L 0 L_0 L0是最小预瞄距离。
3 规范化纯追踪算法(RPP)
考虑到机器人始终以期望速度 运动并不合理尤其是在狭窄区域、急转弯等不完全可见工作空间动态调整机器人速度有利于提供更高质量的行为表现。规范化纯追踪算法(Regulated Pure Pursuit, RPP)引入了修正启发式等进行自适应调整。举例而言曲率启发式目的是放慢机器人在部分可观察环境的速度以提高盲转弯时的安全性。其中最大曲率阈值 提供了急转弯位置的速度缩放。 v t ′ { v t , κ ⩽ κ max κ max κ v t , κ κ max v_{t}^{}\begin{cases} v_t\,\, , \kappa \leqslant \kappa _{\max}\\ \frac{\kappa _{\max}}{\kappa}v_t\,\, , \kappa \kappa _{\max}\\\end{cases} vt′{vt,κ⩽κmaxκκmaxvt,κκmax
可以根据应用需求设计更多启发式
4 仿真实现
4.1 ROS C仿真
核心代码如下所示
bool RPPPlanner::computeVelocityCommands(geometry_msgs::Twist cmd_vel)
{...double vt std::hypot(base_odom.twist.twist.linear.x, base_odom.twist.twist.linear.y);double L getLookAheadDistance(vt);getLookAheadPoint(L, robot_pose_map, prune_plan, lookahead_pt, theta, kappa);double lookahead_k 2 * sin(_dphi(lookahead_pt, robot_pose_map)) / L;// calculate commandsif (shouldRotateToGoal(robot_pose_map, global_plan_.back())){...}else{double e_theta regularizeAngle(_dphi(lookahead_pt, robot_pose_map));// large angle, turn firstif (shouldRotateToPath(std::fabs(e_theta), M_PI_2)){cmd_vel.linear.x 0.0;cmd_vel.angular.z angularRegularization(base_odom, e_theta / d_t_);}// apply constraintselse{double curv_vel _applyCurvatureConstraint(max_v_, lookahead_k);double cost_vel _applyObstacleConstraint(max_v_);double v_d std::min(curv_vel, cost_vel);v_d _applyApproachConstraint(v_d, robot_pose_map, prune_plan);cmd_vel.linear.x linearRegularization(base_odom, v_d);cmd_vel.angular.z angularRegularization(base_odom, v_d * lookahead_k);}}return true;
}4.2 Python仿真
核心代码如下所示
def plan(self):lookahead_pts []dt self.params[TIME_STEP]for _ in range(self.params[MAX_ITERATION]):# break until goal reachedif self.shouldRotateToGoal(self.robot.position, self.goal):return True, self.robot.history_pose, lookahead_pts# get the particular point on the path at the lookahead distancelookahead_pt, _, _ self.getLookaheadPoint()# get the tracking curvature with goalahead pointlookahead_k 2 * math.sin(self.angle(self.robot.position, lookahead_pt) - self.robot.theta) / self.lookahead_dist# calculate velocity commande_theta self.regularizeAngle(self.robot.theta - self.goal[2]) / 10if self.shouldRotateToGoal(self.robot.position, self.goal):if not self.shouldRotateToPath(abs(e_theta)):u np.array([[0], [0]])else:u np.array([[0], [self.angularRegularization(e_theta / dt)]])else:e_theta self.regularizeAngle(self.angle(self.robot.position, lookahead_pt) - self.robot.theta) / 10if self.shouldRotateToPath(abs(e_theta), np.pi / 4):u np.array([[0], [self.angularRegularization(e_theta / dt)]])else:# apply constraintscurv_vel self.applyCurvatureConstraint(self.params[MAX_V], lookahead_k)cost_vel self.applyObstacleConstraint(self.params[MAX_V])v_d min(curv_vel, cost_vel)u np.array([[self.linearRegularization(v_d)], [self.angularRegularization(v_d * lookahead_k)]])# update lookahead pointslookahead_pts.append(lookahead_pt)# feed into robotic kinematicself.robot.kinematic(u, dt)return False, None, None4.3 Matlab仿真
核心代码如下所示
while iter param.max_iterationiter iter 1;% break until goal reachedif shouldRotateToGoal([robot.x, robot.y], goal, param)flag true;break;end% get the particular point on the path at the lookahead distance[lookahead_pt, ~, ~] getLookaheadPoint(robot, path, param);% get the tracking curvature with goalahead pointlookahead_k 2 * sin( ...atan2(lookahead_pt(2) - robot.y, lookahead_pt(1) - robot.x) - robot.theta ...) / getLookaheadDistance(robot, param);% calculate velocity commande_theta regularizeAngle(robot.theta - goal(3)) / 10;if shouldRotateToGoal([robot.x, robot.y], goal, param)if ~shouldRotateToPath(abs(e_theta), 0.0, param)u [0, 0];elseu [0, angularRegularization(robot, e_theta / param.dt, param)];endelsee_theta regularizeAngle( ...atan2(lookahead_pt(2) - robot.y, lookahead_pt(1) - robot.x) - robot.theta ...) / 10;if shouldRotateToPath(abs(e_theta), pi / 4, param)u [0, angularRegularization(robot, e_theta / param.dt, param)];else% apply constraintscurv_vel applyCurvatureConstraint(param.max_v, lookahead_k, param);cost_vel applyObstacleConstraint(param.max_v, map, robot, param);v_d min(curv_vel, cost_vel);u [linearRegularization(robot, v_d, param), ...angularRegularization(robot, v_d * lookahead_k, param) ...];endend% input into robotic kinematicrobot f(robot, u, param.dt);pose [pose; robot.x, robot.y, robot.theta];
end完整工程代码请联系下方博主名片获取 更多精彩专栏
《ROS从入门到精通》《Pytorch深度学习实战》《机器学习强基计划》《运动规划实战精讲》… 源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系
