网站制作策划方案,做网站银川,传统营销和网络营销的区别,北京人力资源网站目录 0 专栏介绍1 最优控制理论2 线性二次型问题3 LQR的价值迭代推导4 基于差速模型的LQR控制5 仿真实现5.1 ROS C实现5.2 Python实现5.3 Matlab实现 0 专栏介绍
#x1f525;附C/Python/Matlab全套代码#x1f525;课程设计、毕业设计、创新竞赛必备#xff01;详细介绍全… 目录 0 专栏介绍1 最优控制理论2 线性二次型问题3 LQR的价值迭代推导4 基于差速模型的LQR控制5 仿真实现5.1 ROS C实现5.2 Python实现5.3 Matlab实现 0 专栏介绍
附C/Python/Matlab全套代码课程设计、毕业设计、创新竞赛必备详细介绍全局规划(图搜索、采样法、智能算法等)局部规划(DWA、APF等)曲线优化(贝塞尔曲线、B样条曲线等)。
详情图解自动驾驶中的运动规划(Motion Planning)附几十种规划算法 1 最优控制理论
最优控制理论是一种数学和工程领域的理论旨在寻找如何使系统在给定约束条件下达到最佳性能的方法。它的基本思想是通过选择合适的控制输入以最小化或最大化某个性能指标来优化系统的行为。其中系统的动态行为通常用状态方程描述状态表示系统在某一时刻的内部状态。状态空间表示将系统的状态和控制输入表示为向量通常用微分方程或差分方程来描述系统的演化。在最优控制理论中会设置代价函数或者目标函数用来衡量系统行为的好坏的函数。性能指标可以是各种形式如最小化路径长度、最小化能量消耗、最大化系统稳定性等。最优控制理论在许多领域都有广泛的应用包括航空航天、机器人学、经济学、生态学等。
2 线性二次型问题
若系统动力学特性可以用一组线性微分方程表示且性能指标为状态变量和控制变量的二次型函数则此类最优控制问题称为线性二次型问题。线性二次调节器(Linear Quadratic Regulator, LQR)是求解线性二次型问题常用的求解方法之一其假设系统零输入且期望状态为零。 如图所示的全状态反馈控制系统。设控制误差 x k z k − z k ∗ \boldsymbol{x}_k\boldsymbol{z}_k-\boldsymbol{z}_{k}^{*} xkzk−zk∗其中 z k \boldsymbol{z}_k zk、 z k ∗ \boldsymbol{z}_{k}^{*} zk∗分别是第 k k k个控制时间步的实际状态和期望状态则全反馈控制律由误差驱动 v k v k ∗ − K x k ⇔ u v − v ∗ u k − K x k \boldsymbol{v}_k\boldsymbol{v}_{k}^{*}-\boldsymbol{Kx}_k\xLeftrightarrow{\boldsymbol{u}\boldsymbol{v}-\boldsymbol{v}^*}\boldsymbol{u}_k-\boldsymbol{Kx}_k vkvk∗−Kxkuv−v∗ uk−Kxk
上式表明可以通过选取状态变量和输入变量使系统等效为零输入(跟踪期望输入)且期望状态为零(消除状态误差)满足应用LQR进行最优控制的条件。定义代价函数 J ∑ k 0 N ( x k T Q x k u k T R u k ) J\sum_{k0}^N{\left( \boldsymbol{x}_{k}^{T}\boldsymbol{Qx}_k\boldsymbol{u}_{k}^{T}\boldsymbol{Ru}_k \right)} Jk0∑N(xkTQxkukTRuk)
其中 Q \boldsymbol{Q} Q与 R \boldsymbol{R} R是用户设定的半正定矩阵前者衡量了系统状态向期望轨迹的收敛速度后者衡量了系统能量消耗的相对大小二者互相制约——希望系统快速收敛往往需要加强控制力度导致能量耗散。因此 与 需要结合具体场景进行调节。
3 LQR的价值迭代推导
针对 J J J进行优化引入价值迭代策略价值迭代的理论基础请看Pytorch深度强化学习1-4策略改进定理与贝尔曼最优方程详细推导 J k ( x k , u k ) min u k [ x k T Q x k u k T R u k J k 1 ( x k 1 ) ] J_k\left( \boldsymbol{x}_k,\boldsymbol{u}_k \right) \underset{\boldsymbol{u}_k}{\min}\left[ \boldsymbol{x}_{k}^{T}\boldsymbol{Qx}_k\boldsymbol{u}_{k}^{T}\boldsymbol{Ru}_kJ_{k1}\left( \boldsymbol{x}_{k1} \right) \right] Jk(xk,uk)ukmin[xkTQxkukTRukJk1(xk1)]
即第 k k k步到终端的代价等于当前步的代价与第 k 1 k1 k1步到终端的代价之和。根据 J J J的定义其一定能表示成二次型 J k x k T P k x k J_k\boldsymbol{x}_{k}^{T}\boldsymbol{P}_k\boldsymbol{x}_k JkxkTPkxk对于优化问题 u k a r g min u k J k ( x k , u k ) \boldsymbol{u}_k\mathrm{arg}\min _{\boldsymbol{u}_k}J_k\left( \boldsymbol{x}_k,\boldsymbol{u}_k \right) ukargminukJk(xk,uk)令 ∂ J k ( x k , u k ) ∂ u k ∂ ∂ u k ( x k T P k x k u k T R u k J k 1 ( A x k B u k ) ) ∂ ∂ u k ( u k T R u k ( A x k B u k ) T P k 1 ( A x k B u k ) ) 2 ( R B T P k 1 B ) u k 2 B T P k 1 A x k 0 \begin{aligned}\frac{\partial J_k\left( \boldsymbol{x}_k,\boldsymbol{u}_k \right)}{\partial \boldsymbol{u}_k}\frac{\partial}{\partial \boldsymbol{u}_k}\left( \boldsymbol{x}_{k}^{T}\boldsymbol{P}_k\boldsymbol{x}_k\boldsymbol{u}_{k}^{T}\boldsymbol{Ru}_kJ_{k1}\left( \boldsymbol{Ax}_k\boldsymbol{Bu}_k \right) \right) \\\frac{\partial}{\partial \boldsymbol{u}_k}\left( \boldsymbol{u}_{k}^{T}\boldsymbol{Ru}_k\left( \boldsymbol{Ax}_k\boldsymbol{Bu}_k \right) ^T\boldsymbol{P}_{k1}\left( \boldsymbol{Ax}_k\boldsymbol{Bu}_k \right) \right) \\2\left( \boldsymbol{R}\boldsymbol{B}^T\boldsymbol{P}_{k1}\boldsymbol{B} \right) \boldsymbol{u}_k2\boldsymbol{B}^T\boldsymbol{P}_{k1}\boldsymbol{Ax}_k\\0\end{aligned} ∂uk∂Jk(xk,uk)∂uk∂(xkTPkxkukTRukJk1(AxkBuk))∂uk∂(ukTRuk(AxkBuk)TPk1(AxkBuk))2(RBTPk1B)uk2BTPk1Axk0
则 u k ∗ − ( R B T P k 1 B ) − 1 B T P k 1 A x k \boldsymbol{u}_{k}^{*}-\left( \boldsymbol{R}\boldsymbol{B}^T\boldsymbol{P}_{k1}\boldsymbol{B} \right) ^{-1}\boldsymbol{B}^T\boldsymbol{P}_{k1}\boldsymbol{Ax}_k uk∗−(RBTPk1B)−1BTPk1Axk对比 u k − K x k \boldsymbol{u}_k-\boldsymbol{Kx}_k uk−Kxk可得 K k ( R B T P k 1 B ) − 1 B T P k 1 A \boldsymbol{K}_k\left( \boldsymbol{R}\boldsymbol{B}^T\boldsymbol{P}_{k1}\boldsymbol{B} \right) ^{-1}\boldsymbol{B}^T\boldsymbol{P}_{k1}\boldsymbol{A} Kk(RBTPk1B)−1BTPk1A
将 u k − K x k \boldsymbol{u}_k-\boldsymbol{Kx}_k uk−Kxk代入 J k J_k Jk可得 J k x k T P k x k x k T ( Q K k T R K k ( A − B K k ) P k 1 ( A − B K k ) ) x k J_k\boldsymbol{x}_{k}^{T}\boldsymbol{P}_k\boldsymbol{x}_k\boldsymbol{x}_{k}^{T}\left( \boldsymbol{Q}\boldsymbol{K}_{k}^{T}\boldsymbol{RK}_k\left( \boldsymbol{A}-\boldsymbol{BK}_k \right) \boldsymbol{P}_{k1}\left( \boldsymbol{A}-\boldsymbol{BK}_k \right) \right) \boldsymbol{x}_k JkxkTPkxkxkT(QKkTRKk(A−BKk)Pk1(A−BKk))xk
从而 P k Q A T P k 1 A − A T P k 1 B ( R B T P k 1 B ) − 1 B T P k 1 A \boldsymbol{P}_k\boldsymbol{Q}\boldsymbol{A}^T\boldsymbol{P}_{k1}\boldsymbol{A}-\boldsymbol{A}^T\boldsymbol{P}_{k1}\boldsymbol{B}\left( \boldsymbol{R}\boldsymbol{B}^T\boldsymbol{P}_{k1}\boldsymbol{B} \right) ^{-1}\boldsymbol{B}^T\boldsymbol{P}_{k1}\boldsymbol{A} PkQATPk1A−ATPk1B(RBTPk1B)−1BTPk1A
称为离散迭代黎卡提方程。根据贝尔曼最优原理在迭代过程中 P k \boldsymbol{P}_k Pk会逐步收敛。
4 基于差速模型的LQR控制
根据差分机器人运动学模型 p ˙ [ x ˙ y ˙ θ ˙ ] [ v cos θ v sin θ ω ] [ f 1 f 2 f 3 ] \boldsymbol{\dot{p}}\left[ \begin{array}{c} \dot{x}\\ \dot{y}\\ \dot{\theta}\\\end{array} \right] \left[ \begin{array}{c} v\cos \theta\\ v\sin \theta\\ \omega\\\end{array} \right] \left[ \begin{array}{c} f_1\\ f_2\\ f_3\\\end{array} \right] p˙ x˙y˙θ˙ vcosθvsinθω f1f2f3
选择状态量 p [ x y θ ] T \boldsymbol{p}\left[ \begin{matrix} x y \theta\\\end{matrix} \right] ^T p[xyθ]T和状态误差量 x [ x − x r y − y r θ − θ r ] T \boldsymbol{x}\left[ \begin{matrix} x-x_r y-y_r \theta -\theta _r\\\end{matrix} \right] ^T x[x−xry−yrθ−θr]T控制量 s [ v ω ] T \boldsymbol{s}\left[ \begin{matrix} v \omega\\\end{matrix} \right] ^T s[vω]T和控制误差量 u [ v − v r ω − ω r ] T \boldsymbol{u}\left[ \begin{matrix} v-v_r \omega -\omega _r\\\end{matrix} \right] ^T u[v−vrω−ωr]T可得 x ( k 1 ) ( T A I ) x ( k ) T B u ( k ) \boldsymbol{x}\left( k1 \right) \left( T\boldsymbol{A}\boldsymbol{I} \right) \boldsymbol{x}\left( k \right) T\boldsymbol{Bu}\left( k \right) x(k1)(TAI)x(k)TBu(k)
其中 A [ 0 0 − v r sin θ r 0 0 v r cos θ r 0 0 0 ] , B [ cos θ r 0 sin θ r 0 0 1 ] \boldsymbol{A}\left[ \begin{matrix} 0 0 -v_r\sin \theta _r\\ 0 0 v_r\cos \theta _r\\ 0 0 0\\\end{matrix} \right] , \boldsymbol{B}\left[ \begin{matrix} \cos \theta _r 0\\ \sin \theta _r 0\\ 0 1\\\end{matrix} \right] A 000000−vrsinθrvrcosθr0 ,B cosθrsinθr0001
接着按照LQR算法求解即可。
5 仿真实现
5.1 ROS C实现
核心代码如下所示
Eigen::Vector2d LQRPlanner::_lqrControl(Eigen::Vector3d s, Eigen::Vector3d s_d, Eigen::Vector2d u_r)
{Eigen::Vector2d u;Eigen::Vector3d e(s - s_d);e[2] regularizeAngle(e[2]);// state equation on errorEigen::Matrix3d A Eigen::Matrix3d::Identity();A(0, 2) -u_r[0] * sin(s_d[2]) * d_t_;A(1, 2) u_r[0] * cos(s_d[2]) * d_t_;Eigen::MatrixXd B Eigen::MatrixXd::Zero(3, 2);B(0, 0) cos(s_d[2]) * d_t_;B(1, 0) sin(s_d[2]) * d_t_;B(2, 1) d_t_;// discrete iteration Ricatti equationEigen::Matrix3d P, P_;P Q_;for (int i 0; i max_iter_; i){Eigen::Matrix2d temp R_ B.transpose() * P * B;P_ Q_ A.transpose() * P * A - A.transpose() * P * B * temp.inverse() * B.transpose() * P * A;if ((P - P_).array().abs().maxCoeff() eps_iter_)break;P P_;}// feedbackEigen::MatrixXd K -(R_ B.transpose() * P_ * B).inverse() * B.transpose() * P_ * A;u u_r K * e;return u;
}5.2 Python实现
核心代码如下所示
def lqrControl(self, s: tuple, s_d: tuple, u_r: tuple) - np.ndarray:dt self.params[TIME_STEP]# state equation on errorA np.identity(3)A[0, 2] -u_r[0] * np.sin(s_d[2]) * dtA[1, 2] u_r[0] * np.cos(s_d[2]) * dtB np.zeros((3, 2))B[0, 0] np.cos(s_d[2]) * dtB[1, 0] np.sin(s_d[2]) * dtB[2, 1] dt# discrete iteration Ricatti equationP, P_ np.zeros((3, 3)), np.zeros((3, 3))P self.Q# iterationfor _ in range(self.lqr_iteration):P_ self.Q A.T P A - A.T P B np.linalg.inv(self.R B.T P B) B.T P Aif np.max(P - P_) self.eps_iter:breakP P_# feedbackK -np.linalg.inv(self.R B.T P_ B) B.T P_ Ae np.array([[s[0] - s_d[0]], [s[1] - s_d[1]], [self.regularizeAngle(s[2] - s_d[2])]])u np.array([[u_r[0]], [u_r[1]]]) K ereturn np.array([[self.linearRegularization(float(u[0]))], [self.angularRegularization(float(u[1]))]])5.3 Matlab实现
核心代码如下所示
function u lqrControl(s, s_d, u_r, robot, param)dt param.dt;% state equation on errorA eye(3);A(1, 3) -u_r(1) * sin(s_d(3)) * dt;A(2, 3) u_r(1) * cos(s_d(3)) * dt;B zeros(3, 2);B(1, 1) cos(s_d(3)) * dt;B(2, 1) sin(s_d(3)) * dt;B(3, 2) dt;% discrete iteration Ricatti equationP param.Q;% iterationfor i1:param.lqr_iterationP_ param.Q A * P * A - A * P * B / (param.R B * P * B) * B * P * A;if max(P - P_) param.eps_iterbreak;endP P_;end% feedbackK -inv(param.R B * P_ * B) * B * P_ * A;e [s(1) - s_d(1); s(2) - s_d(2); regularizeAngle(s(3) - s_d(3))];u [u_r(1); u_r(2)] K * e;u [linearRegularization(robot, u(1), param), angularRegularization(robot, u(2), param)];
end完整工程代码请联系下方博主名片获取 更多精彩专栏
《ROS从入门到精通》《Pytorch深度学习实战》《机器学习强基计划》《运动规划实战精讲》… 源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系
