轨迹规划 | 基于差速运动学的有模型PID算法(附ROS C++仿真)

目录

0 专栏介绍1 有模型PID算法介绍2 基于差速运动学模型的路径跟踪3 微分同胚视角下的有模型PID4 算法仿真(ROS C++)

0 专栏介绍

?附C++/Python/Matlab全套代码?课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。

?详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法

---

1 有模型PID算法介绍

在

轨迹规划 | 图解路径跟踪PID算法(附ROS C++/Python/Matlab仿真)控制原理 | PID控制的三个参数如何影响控制效果？(附参数整定方法)

中我们介绍了无模型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 t+K_d{{\left( \varDelta e\left( k \right) -\varDelta e\left( k-1 \right) \right)}/{\varDelta t}} Δu(k)=Kp​Δe(k)+Ki​e(k)Δt+Kd​(Δe(k)−Δe(k−1))/Δt

其优势在于不需要建立复杂的系统模型，便于快速部署；能在不确定性较大的环境中工作，适应系统动态变化；由于不涉及模型计算，控制响应速度快。但缺点也很明显，例如参数调整往往依赖经验，可能需要多次试验；在某些情况下，可能面临系统稳定性问题，特别是在非线性系统中

当我们明确知道系统的动力学模型时，可以考虑采用有模型的PID算法更精确地调整PID参数，优化控制性能

2 基于差速运动学模型的路径跟踪

设系统输出方程

x p = [ 1 1 ] x + G ( x ) \boldsymbol{x}_p=\left[ \begin{matrix} 1& & \\ & 1& \\\end{matrix} \right] \boldsymbol{x}+\boldsymbol{G}\left( \boldsymbol{x} \right) xp​=[1​1​​]x+G(x)

其中 x = [ x y θ ] T \boldsymbol{x}=\left[ \begin{matrix} x& y& \theta\\\end{matrix} \right] ^T x=[x​y​θ​]T， G ( x ) = [ l cos ⁡ θ l sin ⁡ θ ] T \boldsymbol{G}\left( \boldsymbol{x} \right) =\left[ \begin{matrix} l\cos \theta& l\sin \theta\\\end{matrix} \right] ^T G(x)=[lcosθ​lsinθ​]T。 x p \boldsymbol{x}_p xp​的物理意义是如图所示的前瞻点， l l l是机器人中心距离前瞻点的几何长度。

进一步求解输出的一阶微分

x ˙ p = [ 1 1 ] x ˙ + J ( x ) \boldsymbol{\dot{x}}_p=\left[ \begin{matrix} 1& & \\ & 1& \\\end{matrix} \right] \boldsymbol{\dot{x}}+\boldsymbol{J}\left( \boldsymbol{x} \right) x˙p​=[1​1​​]x˙+J(x)

其中雅克比矩阵

J ( x ) = ∂ G ( x ) ∂ x T = [ 0 0 − l θ ˙ sin ⁡ θ 0 0 l θ ˙ cos ⁡ θ ] \boldsymbol{J}\left( \boldsymbol{x} \right) =\frac{\partial \boldsymbol{G}\left( \boldsymbol{x} \right)}{\partial \boldsymbol{x}^T}=\left[ \begin{matrix} 0& 0& -l\dot{\theta}\sin \theta\\ 0& 0& l\dot{\theta}\cos \theta\\\end{matrix} \right] J(x)=∂xT∂G(x)​=[00​00​−lθ˙sinθlθ˙cosθ​]

根据建模分析 | 差速轮式移动机器人运动学建模(附Python/Matlab仿真)知道差速移动机器人的运动学方程为

x ˙ = [ x ˙ y ˙ θ ˙ ] = [ cos ⁡ θ 0 sin ⁡ θ 0 0 1 ] [ v ω ] = S ( q ) u \boldsymbol{\dot{x}}=\left[ \begin{array}{c} \dot{x}\\ \dot{y}\\ \dot{\theta}\\\end{array} \right] =\left[ \begin{matrix} \cos \theta& 0\\ \sin \theta& 0\\ 0& 1\\\end{matrix} \right] \left[ \begin{array}{c} v\\ \omega\\\end{array} \right] =S\left( \boldsymbol{q} \right) \boldsymbol{u} x˙=⎣⎡​x˙y˙​θ˙​⎦⎤​=⎣⎡​cosθsinθ0​001​⎦⎤​[vω​]=S(q)u

整理可得

x ˙ p = [ cos ⁡ θ − l sin ⁡ θ sin ⁡ θ l cos ⁡ θ ] u = R ( θ ) u \boldsymbol{\dot{x}}_p=\left[ \begin{matrix} \cos \theta& -l\sin \theta\\ \sin \theta& l\cos \theta\\\end{matrix} \right] \boldsymbol{u}=\boldsymbol{R}\left( \theta \right) \boldsymbol{u} x˙p​=[cosθsinθ​−lsinθlcosθ​]u=R(θ)u

进行变量置换 v = R ( θ ) u \boldsymbol{v}=\boldsymbol{R}\left( \theta \right) \boldsymbol{u} v=R(θ)u，可得线性的输出方程

x ˙ p = v \boldsymbol{\dot{x}}_p=\boldsymbol{v} x˙p​=v

此时只需设计线性的反馈控制律

v = − K ( x p − x d ) \boldsymbol{v}=-\boldsymbol{K}\left( \boldsymbol{x}_p-\boldsymbol{x}_d \right) v=−K(xp​−xd​)

即可获得线性非齐次微分方程的解

x p = [ C 1 e − k 1 t + x d C 2 e − k 2 t + y d ] T \boldsymbol{x}_p=\left[ \begin{matrix} C_1e^{-k_1t}+x_d& C_2e^{-k_2t}+y_d\\\end{matrix} \right] ^T xp​=[C1​e−k1​t+xd​​C2​e−k2​t+yd​​]T

其中 K = d i a g ( k 1 , k 2 ) ( k 1 > 0 , k 2 > 0 ) \boldsymbol{K}=\mathrm{diag}\left( k_1,k_2 \right) \left( k_1>0, k_2>0 \right) K=diag(k1​,k2​)(k1​>0,k2​>0)是反馈矩阵， x d \boldsymbol{x}_d xd​是系统期望位置。通过输入输出线性化， x p \boldsymbol{x}_p xp​可以收敛到 x d \boldsymbol{x}_d xd​，控制输入为

u = R − 1 ( θ ) v \boldsymbol{u}=\boldsymbol{R}^{-1}\left( \theta \right) \boldsymbol{v} u=R−1(θ)v

其中

R − 1 ( θ ) = [ cos ⁡ θ sin ⁡ θ − sin ⁡ θ / l cos ⁡ θ / l ] \boldsymbol{R}^{-1}\left( \theta \right) =\left[ \begin{matrix} \cos \theta& \sin \theta\\ -{{\sin \theta}/{l}}& {{\cos \theta}/{l}}\\\end{matrix} \right] R−1(θ)=[cosθ−sinθ/l​sinθcosθ/l​]

这是一个基于差速运动学和误差 e = x p − x d \boldsymbol{e}=\boldsymbol{x}_p-\boldsymbol{x}_d e=xp​−xd​的P控制器

3 微分同胚视角下的有模型PID

流形(manifolds)是可以局部欧几里得空间化的一个拓扑空间，是具有拓扑结构的点集，是欧几里得空间中的曲线、曲面等概念的推广。

欧几里得空间是最简单的流形实例。流形要求拓扑结构的局部为欧式空间或其他相对简单的空间。例如下面的左图所示，在一个半径极大的球面(如地球)上，若以充分大的尺度去度量一个三角形则其只能为曲边三角形，欧式距离不成立，因为位于二维平面上的点不能沿直线穿过球面到达目标点；若以充分小的尺度去度量则可以得到直边三角形——例如在地球上测量跑道长度无需考虑地球的曲率。下面右图中的一维流形同理

一个并非流形的实例是在球面上吊一根线，因为在包含了线和球连接的那一点的附近区域一定不是简单的——既不是线也不是面。

这种将两个流形局部位置的点和几何结构等同起来的方法称为同胚(homeomorphism)，形式化定义为拥有连续逆映射的连续映射；进一步地，定义具有连续可微逆映射的连续可微映射为微分同胚(diffeomorphism)。同胚的概念允许在简单结构上处理复杂问题，利用同胚将非线性问题线性化更精准。本节的输入输出反馈线性化就是微分同胚的例子：当 l > 0 l>0 l>0时 R ( θ ) \boldsymbol{R}\left( \theta \right) R(θ)与 R − 1 ( θ ) \boldsymbol{R}^{-1}\left( \theta \right) R−1(θ)是连续可微的，因此 v \boldsymbol{v} v微分同胚于 u \boldsymbol{u} u

4 算法仿真(ROS C++)

核心代码如下所示：

Eigen::Vector2d PIDController::_pidControl(Eigen::Vector3d s, Eigen::Vector3d s_d, Eigen::Vector2d u_r){  Eigen::Vector2d u;  Eigen::Vector3d e(s_d - s);  Eigen::Vector2d sx_dot(k_ * e[0], k_ * e[1]);  Eigen::Matrix2d R_inv;  R_inv << cos(s[2]), sin(s[2]), -sin(s[2]) / l_, cos(s[2]) / l_;  u = R_inv * sx_dot;  return u;}

算法效果如下：

完整工程代码请联系下方博主名片获取

---

? 更多精彩专栏：

《ROS从入门到精通》《Pytorch深度学习实战》《机器学习强基计划》《运动规划实战精讲》…
?源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系?