网站除了做流量还需要什么软件,镇江手机网站建设,wordpress中文转拼音,百石网怎么做网站目录 0 专栏介绍1 PID控制基本原理2 基于PID的路径跟踪3 仿真实现3.1 ROS C实现3.2 Python实现3.3 Matlab实现 0 专栏介绍
#x1f525;附C/Python/Matlab全套代码#x1f525;课程设计、毕业设计、创新竞赛必备#xff01;详细介绍全局规划(图搜索、采样法、智能算法等)实现3.2 Python实现3.3 Matlab实现 0 专栏介绍
附C/Python/Matlab全套代码课程设计、毕业设计、创新竞赛必备详细介绍全局规划(图搜索、采样法、智能算法等)局部规划(DWA、APF等)曲线优化(贝塞尔曲线、B样条曲线等)。
详情图解自动驾驶中的运动规划(Motion Planning)附几十种规划算法 1 PID控制基本原理
PID控制是一种常用的经典控制算法其应用背景广泛例如
工业自动化控制温度控制、压力控制、流量控制、液位控制等过程控制系统多采用PID闭环可以帮助维持系统参数在设定值附近以提高生产过程的稳定性和效率机械工程PID算法可用于实现精确的运动控制包括控制位置、速度和力。这包括机器人控制、电机控制、汽车巡航控制等农业自动化PID算法可用于控制温室环境包括温度、湿度和光照以促进植物的生长和提高农业生产…
PID代表比例(Proportional)、积分(Integral)和微分(Derivative)它通过根据误差信号的大小和变化率来调整控制器的输出以使系统的输出尽可能接近期望值其控制框图如下所示 连续型PID控制律如下 u ( t ) K p e ( t ) K i ∫ t 0 t e ( τ ) d τ K d e ˙ ( t ) u\left( t \right) K_pe\left( t \right) K_i\int_{t_0}^t{e\left( \tau \right) \mathrm{d}\tau}K_d\dot{e}\left( t \right) u(t)Kpe(t)Ki∫t0te(τ)dτKde˙(t)
其中 K p K_p Kp、 K i K_i Ki、 K d K_d Kd分别称为比例、积分与微分增益系数
位置式离散型PID控制律如下 u ( k ) K p e ( k ) K i ∑ i 0 k e ( i ) Δ t K d ( e ( k ) − e ( k − 1 ) ) / Δ t u\left( k \right) K_pe\left( k \right) K_i\sum_{i0}^k{e\left( i \right) \varDelta t}K_d{{\left( e\left( k \right) -e\left( k-1 \right) \right)}/{\varDelta t}} u(k)Kpe(k)Kii0∑ke(i)ΔtKd(e(k)−e(k−1))/Δt
由于位置式PID算法需要计算累计偏差占用存储单元可以通过 u ( k ) − u ( k − 1 ) u\left( k \right) -u\left( k-1 \right) u(k)−u(k−1)
计算增量式PID控制律 Δ u ( k ) K p Δ e ( k ) K i e ( k ) Δ t K d ( Δ e ( k ) − Δ e ( k − 1 ) ) / Δ t \varDelta u\left( k \right) K_p\varDelta e\left( k \right) K_ie\left( k \right) \varDelta tK_d{{\left( \varDelta e\left( k \right) -\varDelta e\left( k-1 \right) \right)}/{\varDelta t}} Δu(k)KpΔe(k)Kie(k)ΔtKd(Δe(k)−Δe(k−1))/Δt
其中 Δ u ( k ) u ( k ) − u ( k − 1 ) Δ e ( k ) e ( k ) − e ( k − 1 ) \varDelta u\left( k \right) u\left( k \right) -u\left( k-1 \right) \\ \varDelta e\left( k \right) e\left( k \right) -e\left( k-1 \right) Δu(k)u(k)−u(k−1)Δe(k)e(k)−e(k−1)
2 基于PID的路径跟踪
在基于PID的局部路径规划中希望机器人能快速跟踪上预设的轨迹设误差量为 e k e_k ek。 e k e_k ek可以根据实际的控制目标进行选择例如线速度误差、角速度误差、轨迹跟踪误差等 以轨迹跟踪误差为例如图所示根据几何关系可得 e k sin ( θ k , d − θ k ) ⋅ d k e_k\sin \left( \theta _{k,d}-\theta _k \right) \cdot d_k eksin(θk,d−θk)⋅dk
其中 θ k , d a tan ( y k , d − y k , x k , d − x k ) d k ( x k , d − x k ) 2 ( y k , d − y k ) 2 \theta _{k,d}\mathrm{a}\tan \left( y_{k,d}-y_k,x_{k,d}-x_k \right) \\ d_k\sqrt{\left( x_{k,d}-x_k \right) ^2\left( y_{k,d}-y_k \right) ^2} θk,datan(yk,d−yk,xk,d−xk)dk(xk,d−xk)2(yk,d−yk)2
接着以该误差作为反馈测量值通过PID控制器生成控制量机器人基于控制量和运动学模型运动循环往复直到机器人完成控制目标
3 仿真实现
3.1 ROS C实现
核心的线速度PID控制和角速度PID控制代码如下
double PIDPlanner::LinearPIDController(nav_msgs::Odometry base_odometry, double b_x_d, double b_y_d)
{double v std::hypot(base_odometry.twist.twist.linear.x, base_odometry.twist.twist.linear.y);double v_d std::hypot(b_x_d, b_y_d) / d_t_;if (std::fabs(v_d) max_v_)v_d std::copysign(max_v_, v_d);double e_v v_d - v;i_v_ e_v * d_t_;double d_v (e_v - e_v_) / d_t_;e_v_ e_v;double v_inc k_v_p_ * e_v k_v_i_ * i_v_ k_v_d_ * d_v;if (std::fabs(v_inc) max_v_inc_)v_inc std::copysign(max_v_inc_, v_inc);double v_cmd v v_inc;if (std::fabs(v_cmd) max_v_)v_cmd std::copysign(max_v_, v_cmd);else if (std::fabs(v_cmd) min_v_)v_cmd std::copysign(min_v_, v_cmd);return v_cmd;
}double PIDPlanner::AngularPIDController(nav_msgs::Odometry base_odometry, double e_theta)
{regularizeAngle(e_theta);double w_d e_theta / d_t_;if (std::fabs(w_d) max_w_)w_d std::copysign(max_w_, w_d);double w base_odometry.twist.twist.angular.z;double e_w w_d - w;i_w_ e_w * d_t_;double d_w (e_w - e_w_) / d_t_;e_w_ e_w;double w_inc k_w_p_ * e_w k_w_i_ * i_w_ k_w_d_ * d_w;if (std::fabs(w_inc) max_w_inc_)w_inc std::copysign(max_w_inc_, w_inc);double w_cmd w w_inc;if (std::fabs(w_cmd) max_w_)w_cmd std::copysign(max_w_, w_cmd);else if (std::fabs(w_cmd) min_w_)w_cmd std::copysign(min_w_, w_cmd);return w_cmd;
}3.2 Python实现
主体控制流程如下
def plan(self):plan_idx 0for _ in range(self.max_iter):# break until goal reachedif math.hypot(self.robot.px - self.goal[0], self.robot.py - self.goal[1]) self.p_precision:return True, self.robot.history_pose# find next tracking pointwhile plan_idx len(self.path):...# calculate velocity commandif math.hypot(self.robot.px - self.goal[0], self.robot.py - self.goal[1]) self.p_precision:if abs(self.robot.theta - self.goal[2]) self.o_precision:u np.array([[0], [0]])else:u np.array([[0], [self.angularController(self.goal[2])]])elif abs(theta_d - self.robot.theta) np.pi / 2:u np.array([[0], [self.angularController(theta_d)]])else:v_d math.hypot(b_x_d, b_y_d) / self.dt / 10u np.array([[self.linearController(v_d)], [self.angularController(theta_d)]])# feed into robotic kinematicself.robot.kinematic(u, self.dt)return False, None3.3 Matlab实现
核心的线速度PID控制和角速度PID控制代码如下
function [v, e_v_, i_v_] linearController(robot, b_x_d, b_y_d, dt, e_v_, i_v_)v_d norm([b_x_d, b_y_d]) / dt / 10;e_v v_d - robot.v;i_v_ i_v_ e_v * dt;d_v (e_v - e_v_) / dt;e_v_ e_v;k_v_p 1.00;k_v_i 0.00;k_v_d 0.00;v_inc k_v_p * e_v_ k_v_i * i_v_ k_v_d * d_v;v robot.v v_inc;
endfunction [w, e_w_, i_w_] angularController(robot, theta_d, dt, e_w_, i_w_)e_theta theta_d - robot.theta;if (e_theta pi)e_theta e_theta - 2 * pi;elseif (e_theta -pi)e_theta e_theta 2 * pi;endw_d e_theta / dt / 10;e_w w_d - robot.w;i_w_ i_w_ e_w * dt;d_w (e_w - e_w_) / dt;e_w_ e_w;k_w_p 1.00;k_w_i 0.00;k_w_d 0.01;w_inc k_w_p * e_w_ k_w_i * i_w_ k_w_d * d_w;w robot.w w_inc;
end完整工程代码请联系下方博主名片获取 更多精彩专栏
《ROS从入门到精通》《Pytorch深度学习实战》《机器学习强基计划》《运动规划实战精讲》… 源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系
