MPC——线性化与实例（2）

非线性系统线性化方法

思想：近似线性化。假设参考系统已经在期望路径上完全通过，得到了路径上每个时刻的状态量和控制量。通过对参考系统和当前系统之间的偏差处理设计使模型预测控制器来跟踪期望路径。

一、理论推导

参考系统在任意时刻，满足以下关系：

$\dot{\xi } _{r}=f(\xi _{r},u_{r})$ （1）

在任意一点处 $(\xi _{r},u_{r})$ 进行泰勒展开，只保留一阶项，忽略高阶项，得到：

$\dot{\xi } =f(\xi _{r},u_{r})+\frac{∂f}{∂\xi } \vert _{\xi =\xi _{r} \\u=u_{r}} (\xi -\xi_{r} )+\frac{∂f}{∂u } \vert _{\xi =\xi _{r} \\u=u_{r}} (u -u_{r} )$ （2）

也可以写为：

$\dot{\xi } =f(\xi _{r},u_{r})+J_{f}(\xi )(\xi -\xi_{r} )+J_{f}(u )(u -u_{r} )$ （3）

其中 $J_{f}(\xi )$ 是 $f(\xi _{r},u_{r})$ 相对于 $\xi$ 的雅克比矩阵， $J_{f}(u)$ 是 $f(\xi _{r},u_{r})$ 相对于 $u$ 的雅克比矩阵。

将（3）与（1）相见得到：

$\dot{\tilde{\xi } } =A(t)\tilde{\xi } +B(t)\tilde{u}$

式中， $\dot{\tilde{\xi } } =\dot{\xi } -\dot{\xi }_{r}$ , $\tilde{u} =u-u_{r}$ , $A(t)=J_{f}(\xi )$ , $B(t)=J_{f}(u)$

这样就得到了新的状态方程。因为该状态方程式连续的，不能直接用于模型预测控制器的设计，故需要对其进行离散化处理。

此处采用近似的离散化处理，设置采样时间为 $T$ ：

$\tilde{\xi } (k+1)=A_{k,t} \tilde{\xi } (k)+B_{k,t} \tilde{u } _{k}$

式中， $A_{k,t}=I+TA(t)$ , $\tilde{B} _{k,t}=TB(t)$

至此得到了，非线性系统在任意一个参考点的处线性化后的系统。该系统是设计线性模型预测控制算法的基础。

二、工程实例

（1）运动模型

由于控制目标是无人驾驶车辆在低速情况下的跟踪控制，因此考虑以车辆运动学方程作为预测模型。已知，低速情况下的车辆运动学方程形式如下：

$\begin {bmatrix}\dot{x} _{r} \\\dot{y}_{r} \\\dot{\psi }\end {bmatrix}=\begin {bmatrix}cos\psi *v_{r} \\sin\psi *v_{r}\\tan\delta _{f} /l*v_{r}\end {bmatrix}$

设置状态方程的观测值是 $\xi _{r}$ ，输入量是 $u_{r}$ ，由上式可以得到，

$f(\xi _{r},u_{r})=\begin {bmatrix} cos\psi *v_{r} \\sin\psi *v_{r}\\tan\delta _{f} /l*v_{r} \end {bmatrix} \dot{\xi } = \begin {bmatrix} \dot{x} _{r} \\\dot{y}_{r} \\\dot{\psi } \end {bmatrix}$

式中：

$\xi _{r}=\begin {bmatrix}x_{r}\\y_{r} \\\psi _{r} \end {bmatrix} u _{r}=\begin {bmatrix}v_{r}\\\delta _{r} \end {bmatrix}$

其中 $x_{r},y_{r},\psi _{r}$ 分别是车辆在世界坐标系下的坐标和姿态角。

$v_{r}，\delta _{r}$ 分别是车辆的纵向速度和车轮转角。

（2）状态方程参数求解

$f(\xi _{r},u_{r})$ 对 $\xi _{r}$ 求雅克比矩阵：

$A(t)=\frac{∂f(\xi _{r},u_{r})}{∂\xi_{r}} =\begin {bmatrix}0 & 0 & -v_{r}sin\psi _{r} \\0 & 0 & v_{r}cos\psi _{r} \\0 & 0 &0 \end {bmatrix}$

$f(\xi _{r},u_{r})$ 对 $u_{r}$ 求雅克比矩阵：

$B(t)=\frac{∂f(\xi _{r},u_{r})}{∂u_{r}} =\begin {bmatrix}cos\psi _{r} & 0 \\sin \psi_{r} & 0 \\\frac{tan\delta _{f,r}}{l} & \frac{v_{r} }{lcos^2(\delta _{f,r}) } \end {bmatrix}$

因此：

$A_{kin}(k)=I +A(t)T = \begin {bmatrix}1 & 0 & -v_{r}sin\psi _{r}T \\0 & 1 & v_{r}cos\psi _{r} T\\0 & 0 &1 \end {bmatrix}$

$B_{kin}(k)=B(t)T = \begin {bmatrix}cos\psi _{r}T & 0 \\sin \psi_{r}T & 0 \\\frac{tan\delta _{f,r}T}{l} & \frac{v_{r}T }{lcos^2(\delta _{f,r}) } \end {bmatrix}$

其中 $T$ 为采样时间。

最后编辑于：2019.04.03 23:08:47

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念sama阅读 213,928评论 6赞 493
死咒
序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
沈念sama阅读 91,192评论 3赞 387
救了他两次的神仙让他今天三更去死
文/潘晓璐我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
开封第一讲书人阅读 159,468评论 0赞 349
道士缉凶录：失踪的卖姜人
文/不坏的土叔我叫张陵，是天一观的道长。经常有香客问我，道长，这世上最难降的妖魔是什么？我笑而不...
开封第一讲书人阅读 57,186评论 1赞 286
港岛之恋（遗憾婚礼）
正文为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
茶点故事阅读 66,295评论 6赞 386
恶毒庶女顶嫁案：这布局不是一般人想出来的
文/花漫我一把揭开白布。她就那样静静地躺着，像睡着了一般。火红的嫁衣衬着肌肤如雪。梳的纹丝不乱的头发上，一...
开封第一讲书人阅读 50,374评论 1赞 292
城市分裂传说
那天，我揣着相机与录音，去河边找鬼。笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。我是一名探鬼主播，决...
沈念sama阅读 39,403评论 3赞 412
双鸳鸯连环套：你想象不到人心有多黑
文/苍兰香墨我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
开封第一讲书人阅读 38,186评论 0赞 269
万荣杀人案实录
序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
沈念sama阅读 44,610评论 1赞 306
护林员之死
正文独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋以下内容为张勋视角年9月15日...
茶点故事阅读 36,906评论 2赞 328
白月光启示录
正文我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
茶点故事阅读 39,075评论 1赞 341
活死人
序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
沈念sama阅读 34,755评论 4赞 337
日本核电站爆炸内幕
正文年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
茶点故事阅读 40,393评论 3赞 320
男人毒药：我在死后第九天来索命
文/蒙蒙一、第九天我趴在偏房一处隐蔽的房顶上张望。院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
开封第一讲书人阅读 31,079评论 0赞 21
一桩弑父案，背后竟有这般阴谋
文/苍兰香墨我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。一阵脚步声响...
开封第一讲书人阅读 32,313评论 1赞 267
情欲美人皮
我被黑心中介骗来泰国打工，没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。一个月前我还...
沈念sama阅读 46,934评论 2赞 365
代替公主和亲
正文我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
茶点故事阅读 43,963评论 2赞 351