劉銳軍　謝廣明　羅文廣

摘??? 要：針對(duì)平衡車啟動(dòng)及停止時(shí)發(fā)生抖動(dòng)的問題開展了相應(yīng)的研究.通過建立系統(tǒng)動(dòng)力學(xué)模型，設(shè)計(jì)了系統(tǒng)的改進(jìn)型有限時(shí)間LQR（LQR， Linear Quadratic Regulator）控制器;對(duì)系統(tǒng)矩陣及控制矩陣進(jìn)行改進(jìn)，增加小范圍增量矩陣，系統(tǒng)能夠?qū)崿F(xiàn)自適應(yīng)調(diào)節(jié)，并使最優(yōu)控制率在小范圍內(nèi)可調(diào)，進(jìn)而使性能指標(biāo)達(dá)到最佳;通過仿真計(jì)算并選取最佳的加權(quán)矩陣[Q]和[R]，使性能指標(biāo)達(dá)到最優(yōu).仿真實(shí)驗(yàn)表明該方法具有可行性和實(shí)用性，解決了小范圍不穩(wěn)定情況，有效克服了超調(diào)量過大而引起的不穩(wěn)定.

關(guān)鍵詞：線性二次型;加權(quán)矩陣;最優(yōu)控制器;MATLAB仿真

中圖分類號(hào)：U489;TP273??????????????? DOI：10.16375/j.cnki.cn45-1395/t.2019.02.005

0??? 引言

平衡車是電動(dòng)車的一種，市場(chǎng)上的平衡車主要分為獨(dú)輪和雙輪兩種，其關(guān)鍵技術(shù)是動(dòng)態(tài)穩(wěn)定控制.就雙輪平衡車而言，傳統(tǒng)的平衡車是靠陀螺儀和加速傳感器來檢測(cè)車體的姿態(tài)變化，同時(shí)用伺服控制系統(tǒng)來驅(qū)動(dòng)電機(jī)以確保系統(tǒng)的穩(wěn)定.1985年日本提出了兩輪平衡車的模型，經(jīng)過21世紀(jì)初的一個(gè)快速發(fā)展期，多個(gè)國(guó)家對(duì)該模型進(jìn)行優(yōu)化改進(jìn)研究，主要有美國(guó)、日本、瑞士和法國(guó)等，直到2005年日本發(fā)布了“村田頑童”自行車機(jī)器人，該模型有了質(zhì)的飛躍.國(guó)內(nèi)關(guān)于平衡車的研究起步較晚，主要集中在高校，如：清華大學(xué)、西北工業(yè)大學(xué)等高校.目前平衡車的快速發(fā)展，也大力支持高校與企業(yè)之間合作及共同開展項(xiàng)目研究.

平衡車是在新產(chǎn)品的迭代創(chuàng)新過程中改進(jìn)的.雙輪平衡車在啟動(dòng)和停止時(shí)會(huì)出現(xiàn)抖動(dòng)不穩(wěn)的現(xiàn)象，需要一種控制方法來解決該問題，以確保其安全性.經(jīng)典的線性二次型調(diào)節(jié)器LQR是一種具有較好魯棒性的控制器，其研究對(duì)象是現(xiàn)代控制理論中以狀態(tài)空間形式給出的線性系統(tǒng)，而性能指標(biāo)函數(shù)為對(duì)象的狀態(tài)和控制輸入的二次型函數(shù).LQR問題之所以受到普遍重視是因?yàn)樗膽?yīng)用不局限于某種物理系統(tǒng)，具有普遍意義且魯棒性較強(qiáng).LQR最優(yōu)控制器設(shè)計(jì)主要是求出狀態(tài)反饋控制的增益[K]，使二次型性能指標(biāo)函數(shù)J取最小值，而[K]由加權(quán)矩陣[Q]和[R]唯一決定，故此[Q]和[R]的選擇尤為重要.本文將LQR控制器進(jìn)行改進(jìn)，以解決小范圍不穩(wěn)定情況.該方法應(yīng)用到雙輪平衡車上可以精確地檢測(cè)車身偏移角度，并結(jié)合精密的中央處理器下達(dá)指令使陀螺儀能夠及時(shí)調(diào)整車身角度，從而達(dá)到平衡控制[1].

1??? 雙輪平衡車系統(tǒng)建模

1.1?? 系統(tǒng)模型建立

雙輪平衡車簡(jiǎn)稱移動(dòng)機(jī)器人.近幾年研究的移動(dòng)機(jī)器人有很多種類，包括：仿生的爬行機(jī)器人、類似動(dòng)物運(yùn)動(dòng)的四腳機(jī)器人以及裝甲車式的履帶爬樓機(jī)器人等.這些機(jī)器人的發(fā)展為人們帶來了實(shí)際效益，同時(shí)也帶來了很大的方便.雙輪平衡車在實(shí)際生活當(dāng)中應(yīng)用最廣泛，例如，大型運(yùn)動(dòng)場(chǎng)、大型商場(chǎng)的治安員等都會(huì)用到雙輪平衡車來快速達(dá)到短距離目的地[2-3].

由于輪式機(jī)器人要比足式或爬行機(jī)器人的速度要高，因此需要輪式機(jī)器人在運(yùn)動(dòng)控制上具有較高的精度.這就要求控制器在設(shè)計(jì)上采用高性能控制策略，使其能夠在運(yùn)行過程中進(jìn)行準(zhǔn)確的定位.以下根據(jù)平衡車結(jié)構(gòu)建立系統(tǒng)模型，分析其動(dòng)力學(xué)模型，并建立動(dòng)力學(xué)方程[4-5].

1.1.1?? 對(duì)車輪建模

左、右車輪力學(xué)方程及轉(zhuǎn)動(dòng)慣量為：

[MLXL=fL-TL]， [JLφL=CL-fLr]?????????????????????????????????????????????????????????? （1）

[MRXR=fR-TR]， [JRφR=CR-fRr]??????????????????????????????????????????????????????????? （2）

1.1.2?? 對(duì)擺桿建模

①水平方向的平衡方程為：

[mxP=TR+TL]??????????????????????????????????????????????????????????????????????????? （3）

其中，[xp=xm+Ssinθ]，[xp=xm+S（θcosθ-θ2sinθ）]

[xm=xL+xR2]????????????????????????????????????????????????????????????????????????????????? （4）

②轉(zhuǎn)矩方程為：

[JPθ=S（PR+PL）sinθ-S（TR+TL）cosθ]?????;????????????????????????????????????????? （5）

③垂直方向的平衡方程為：

[mxZ=-mg+PR+PL]??????????????????????????????????????????????????????????????? （6）

其中，[xZ=S（cosθ-1）]，[xZ=S（θ2cosθ-θsinθ）]

④平衡車在轉(zhuǎn)彎時(shí)的平衡方程為：

[Jψψ=（TR+TL）D2]???????????????????????????????????????????????????????????????????????? （7）

[ψ=-xR+xLD]???????????????????????????????????????????????????????????????????????????????? （8）

⑤兩輪平衡車的數(shù)學(xué)模型為：

[CR+CLr=xm（2J?r2+2M）-mSθ]???????????????????????????????????????????????????????????? （9）

[-mLxm=θ（mS2+JP）-mgSθ]????????????????????????????????????????????????????? （10）

[-CR+CLr=ψ（DM+2JψD+DJ?r2）]????????????????????????????????????????????????????????? （11）

由式（9）—式（11）可得平衡車系統(tǒng)方程如下：

[xxθθ=0001a210000010a41000θxθθ+b11b12b21b220000CRCL]??????????????????????????????????????? （12）

[ψψ=0100ψψ+b11b1200CRCL]????????????????????????????????????????????????? （13）

由于本動(dòng)力學(xué)方程有兩個(gè)角度，必須滿足一定的條件才能成立，式（9）—式（11）是在假設(shè)當(dāng)[θ]在[±5°]變化時(shí)（[±5°]變化是指平衡車內(nèi)部IMU檢測(cè)擺桿內(nèi)部機(jī)械的偏差，而非平衡車擺桿的傾斜度或車身的傾斜度.因IMU模塊檢測(cè)比較靈敏，故若檢測(cè)值偏離太大，反映在擺桿及車身的角度遠(yuǎn)大于偏離值，因此[±5°]符合要求），[sinθ≈θ]，[cosθ≈1]成立，滿足該條件，從而[θ2≈0].

在平衡車中控制系統(tǒng)是關(guān)鍵的部分，整個(gè)車身的運(yùn)行情況都由控制系統(tǒng)實(shí)現(xiàn)，因此，精準(zhǔn)的控制系統(tǒng)設(shè)計(jì)是平衡車安全與否的關(guān)鍵.因?yàn)樵O(shè)計(jì)簡(jiǎn)單便捷，市面上大多平衡車采用PID算法來控制.該算法應(yīng)用到平衡車上，雖然能夠很好的利用其閉環(huán)系統(tǒng)實(shí)現(xiàn)控制，但在精準(zhǔn)控制上難以保證，因此迫切需要一種控制器來實(shí)現(xiàn)精準(zhǔn)控制.LQR控制器以其易于構(gòu)成閉環(huán)系統(tǒng)、設(shè)計(jì)簡(jiǎn)單及能夠較好的實(shí)現(xiàn)最優(yōu)控制目的的優(yōu)點(diǎn)，成為平衡車設(shè)計(jì)控制器的新方法.本文對(duì)LQR控制器進(jìn)行了改進(jìn)，增加微量調(diào)節(jié)反饋矩陣，使其能夠?qū)?dòng)及停止時(shí)的微小抖動(dòng)問題實(shí)現(xiàn)良好的控制.

2??? 平衡車LQR控制器設(shè)計(jì)

LQR即線性二次型調(diào)節(jié)器，LQR可以得到狀態(tài)線性反饋的最優(yōu)控制規(guī)律，易于構(gòu)成閉環(huán)最優(yōu)控制，可在不消耗過多能量的情況下，保持系統(tǒng)狀態(tài)各分量仍接近平衡狀態(tài).LQR最優(yōu)控制充分發(fā)揮成本低這一特點(diǎn)，使其原系統(tǒng)達(dá)到較好的性能指標(biāo)，也可以對(duì)不穩(wěn)定系統(tǒng)進(jìn)行調(diào)整.線性二次型問題解出的控制率可以通過狀態(tài)反饋實(shí)現(xiàn)閉環(huán)最優(yōu)控制，成為當(dāng)今控制工程領(lǐng)域中主要設(shè)計(jì)方法之一.

由于目前很多平衡車在啟動(dòng)和停止時(shí)出現(xiàn)不可控制的抖動(dòng)現(xiàn)象，導(dǎo)致車身不穩(wěn)，對(duì)人身安全造成一定的危險(xiǎn)，因此，本設(shè)計(jì)采用自加調(diào)節(jié)式系統(tǒng)控制，利用微小調(diào)節(jié)矩陣檢測(cè)偏移角度，從而矯正偏移量，使平衡車能夠在整體傾斜角度范圍內(nèi)，通過自加調(diào)節(jié)式系統(tǒng)克服角度差，使車身能夠在偏離一定角度后立刻得到矯正.

2.1?? 傳統(tǒng)設(shè)計(jì)方案

狀態(tài)調(diào)節(jié)器的任務(wù)在于，當(dāng)系統(tǒng)狀態(tài)由于任何原因偏離了平衡狀態(tài)時(shí)，能在不消耗過多能量的情況下，保持系統(tǒng)狀態(tài)各分量仍接近于平衡狀態(tài).在研究這類問題時(shí)，通常是把初始狀態(tài)矢量看作擾動(dòng)，把零狀態(tài)取作平衡狀態(tài).因此，需要解決的問題是，尋求最優(yōu)控制率[u]矩陣，在有限時(shí)間內(nèi)將系統(tǒng)初始狀態(tài)轉(zhuǎn)移到零點(diǎn)附近.平衡車系統(tǒng)在當(dāng)前狀態(tài)由于任何原因偏離了平衡狀態(tài)時(shí)，要在最短時(shí)間內(nèi)，保持系統(tǒng)狀態(tài)各分量仍接近于平衡狀態(tài)，尤其在外部干擾情況下要求系統(tǒng)狀態(tài)轉(zhuǎn)移到零點(diǎn)附近.其系統(tǒng)滿足最優(yōu)控制的線性二次型調(diào)節(jié)器.

①系統(tǒng)狀態(tài)方程為：

[x（t）=A（t）x（t）+B（t）u（t）x（t0）=x0y（t）=C（t）x（t）]?????????????????????????????????????????????????????????? （14）

②性能指標(biāo)為：

[J=12eT（tf）Fe（tf）+12t0tf[eT（t）Q（t）e（t）+uT（t）R（t）u（t）]dt]???????????????????? （15）

其中，[A（t）]稱為狀態(tài)矩陣，[B（t）]稱為控制輸入矩陣，[C（t）]稱為輸出矩陣，[x（t）∈Rm];[u（t）∈Rm]，無約束;[y（t）∈Rl]，[0<l≤m≤n];輸出誤差向量[e（t）=z（t）-y（t）]，[z（t）∈Rl]，為理想輸出向量，要求確定最優(yōu)控制[u?（t）]，使性能指標(biāo)[J]取得極小值[6-7].在系統(tǒng)方程（14）和二次性能指標(biāo)式（15）中，如果[C（t）∈I]，[z（t）=0]，則有：

[e（t）=-y（t）=-x（t）]???????????????????????????????????????????????????????????? （16）

那么將式（16）代入式（15）得性能指標(biāo)為：

[J=12xT（tf）Fx（tf）+12t0tf[xT（t）Q（t）x（t）+uT（t）R（t）u（t）]dt]?????????????????????? （17）

式（17）即為狀態(tài)調(diào)節(jié)器的性能泛函.

構(gòu)造哈密頓函數(shù)（Hamilton）

[H[x，u，λ，t]=12[xT（t）Q（t）x（t）+uT（t）R（t）u（t）]+λT[A（t）x（t）+B（t）u（t）]]??? （18）

從而得到：

[u?（t）=-R-1（t）BT（t）P（t）x（t）=-K（t）x（t）]?????????????????????????????????? （19）

其中，[K（t）=R-1（t）BT（t）P（t）]，[P（t）]可以通過黎卡提（Riccati）微分方程來計(jì)算.

[P（t）=-P（t）A（t）-AT（t）P（t）+P（t）B（t）R-1（t）BT（t）×P（t）-Q（t）]????????????????????????? （20）

式（17）中加權(quán)矩陣[Q是n×n]維半正定矩陣，[R是r×r]維正定矩陣;第一個(gè)積分項(xiàng)表示系統(tǒng)動(dòng)態(tài)跟蹤誤差加權(quán)平均和的積分;第二個(gè)積分項(xiàng)表示系統(tǒng)控制能耗總量. LQR最優(yōu)控制器設(shè)計(jì)主要是求出狀態(tài)反饋控制的增益[K]，使二次型性能指標(biāo)函數(shù)[J]取最小值，而[K]由加權(quán)矩陣[Q]和[R]唯一決定，故此[Q]和[R]的選擇尤為重要.在本設(shè)計(jì)平衡車控制器中[Q]和[R]的選取主要取決于工程上的調(diào)試及結(jié)構(gòu)設(shè)計(jì)，一般根據(jù)設(shè)計(jì)要求的不同來選擇[Q]和[R]的大小從而加以約束狀態(tài)量及控制量.

2.2?? 改進(jìn)型的設(shè)計(jì)方案

①系統(tǒng)狀態(tài)方程為：

[x（t）=A+ΔA1（t）x（t）+B+ΔB1（t）u（t）x（t0）=x0y（t）=C1（t）x（t）]???????????????????????????????????????? （21）

其中，[A]和[ΔA1]為相同維數(shù)的矩陣，[B]和[ΔB1]也為相同維數(shù)的矩陣，[ΔA1]與[ΔB1]為不確定性矩陣，而這兩個(gè)矩陣分別是在外界干擾時(shí)所產(chǎn)生的，通過調(diào)節(jié)[ΔA1]與[ΔB1]來確定系統(tǒng)的穩(wěn)定[8].增量矩陣[ΔA1]與[ΔB1]是通過平衡車的IMU檢測(cè)后反饋系統(tǒng)的差值，根據(jù)差值適當(dāng)增加[ΔA1]與[ΔB1]矩陣以達(dá)到穩(wěn)定的目標(biāo).增量矩陣不是無限隨意增加，根據(jù)平衡車抖動(dòng)的范圍設(shè)定增量矩陣的界限，因此在檢測(cè)到偏差值后反饋給系統(tǒng)即可以確定增量矩陣大小，從而調(diào)節(jié)系統(tǒng)穩(wěn)定性.

②相應(yīng)的[u?（t）]及[P（t）]如下：

[u*（t）=-R-1（t）B+ΔB1T（t）P（t）x（t）=-K1（t）x（t）]???????????????????????????????????? （22）

[P（t）=-P（t）A+ΔA1（t）-A+ΔA1T（t）P（t）+P（t）B+ΔB1（t）R-1（t）B+ΔB1T（t）×P（t）-Q（t）]???&nbsp;?????????????????? （23）

3??? 仿真實(shí)驗(yàn)及分析

3.1?? 仿真實(shí)驗(yàn)數(shù)據(jù)選擇

式（24）—式（25）中，系統(tǒng)矩陣：[A+ΔA1];輸入矩陣：[B+ΔB1];輸出矩陣：[C1];輸入向量：[u];狀態(tài)向量：[X];輸出向量：[Y].

在實(shí)際仿真中各參數(shù)值的設(shè)定如表2所示.選取各參數(shù)值確定系統(tǒng)方程式（24）、式（25），使用數(shù)據(jù)分析計(jì)算軟件MATLAB進(jìn)行仿真實(shí)驗(yàn).仿真結(jié)果分別以可視化圖形展示，圖2—圖6分別表示不同情況下的仿真結(jié)果[9-12].

本仿真實(shí)驗(yàn)根據(jù)設(shè)計(jì)要求及所要達(dá)到目標(biāo)為依據(jù)，選擇不同[Q]和[R]的大小對(duì)狀態(tài)量及控制量進(jìn)行約束.在不同的[Q]和[R]值下進(jìn)行仿真對(duì)比并得出結(jié)論，如圖2—圖6所示.

3.2?? 仿真結(jié)果分析

由以上仿真實(shí)驗(yàn)得知：解決雙輪平衡車在啟動(dòng)與停止時(shí)小范圍抖動(dòng)的問題，關(guān)鍵在控制器的設(shè)計(jì)，控制器的參數(shù)值是控制效果的直接決定因素.仿真中可以得出，改進(jìn)型的控制器只要選定合理的參數(shù)就可以實(shí)現(xiàn)較短時(shí)間內(nèi)達(dá)到穩(wěn)定狀態(tài).圖2和圖4代表加權(quán)矩陣[Q]相同而加權(quán)矩陣[R]不同的情況.從圖中可以看出在加權(quán)矩陣[Q]相同[R]不同時(shí)：當(dāng)[R]越小，擺桿的擺幅偏離中心方向越小，而且到達(dá)穩(wěn)定時(shí)間較短，并且[R]越小，會(huì)出現(xiàn)小范圍的超調(diào)量，但很快就恢復(fù)正常.圖2和圖3代表加權(quán)矩陣[R]相同[Q]不同的情況.這種情況下改變[Q]的單一向量會(huì)消除超調(diào)量，并且在控制擺幅相等時(shí)，穩(wěn)定時(shí)間會(huì)更短.圖5表明同時(shí)選擇較大的[Q]和[R]會(huì)使平衡車出現(xiàn)明顯的超調(diào)量，而且穩(wěn)定性無法得到控制.圖6是經(jīng)過調(diào)試后確定的[Q]和[R]值，并得到的狀態(tài)響應(yīng)及最優(yōu)控制曲線.圖中可以看到平衡車在發(fā)生抖動(dòng)時(shí)能夠很快得到控制并趨于穩(wěn)定，控制效果很好，在該條件下[K]=[-12.541 9， -1.000 0， 20.282 2， -22.626 8].通過實(shí)驗(yàn)仿真驗(yàn)證了該方案的可行性，即：增加小范圍偏移防抖動(dòng)矩陣有效解決了雙輪平衡車在啟動(dòng)及停止時(shí)刻出現(xiàn)小范圍抖動(dòng)的問題.

4??? 結(jié)束語

大量的仿真結(jié)果表明，當(dāng)在系統(tǒng)中引入可調(diào)節(jié)控制矩陣時(shí)控制效果比傳統(tǒng)的效果好，并且可以較為簡(jiǎn)單的實(shí)現(xiàn)調(diào)節(jié)功能.本文利用力學(xué)知識(shí)對(duì)平衡車進(jìn)行系統(tǒng)建模，計(jì)算出系統(tǒng)方程;通過黎卡提（Riccati）微分方程來計(jì)算，從而得出P（t），進(jìn)而計(jì)算出反饋矩陣.另外，在控制過程中[Q]和[R]的選取對(duì)控制效果有明顯的影響，而且二者是相互制約的關(guān)系，改變其中任何一個(gè)都會(huì)有較大影響，在多次試驗(yàn)后選取了最優(yōu)控制的加權(quán)矩陣[Q]和[R].文中雖然達(dá)到了控制效果，但還需改善達(dá)到最優(yōu)控制的時(shí)間，才能使系統(tǒng)更加快速的達(dá)到穩(wěn)定狀態(tài)，該方案將在實(shí)際工程中具有很好的參考與指導(dǎo)價(jià)值.

參考文獻(xiàn)

[1]???? 朱年華.兩輪自平衡電動(dòng)車控制系統(tǒng)設(shè)計(jì)與研究[D].南昌：華東交通大學(xué)，2016.

[2]???? 牛伯冕.線性最優(yōu)控制方法及其在旋轉(zhuǎn)倒立擺中的應(yīng)用分析[J].自動(dòng)化與儀器儀表，2018（2）：183-186.

[3]???? 楊正才，呂科.基于模糊PID控制方法的兩輪直立自平衡電動(dòng)車研究[J].控制工程，2016，23（3）：366-370.

[4]???? 薛凡，孫京誥，嚴(yán)懷成.兩輪平衡車的建模與控制研究[J].化工自動(dòng)化及儀表，2012，39（11）：1450-1454.

[5]???? 申鐵龍，梅生偉，王宏，等.魯棒控制基準(zhǔn)設(shè)計(jì)問題：倒立擺控制[J].控制理論與應(yīng)用，2003，20（6）：974-975.

[6]???? 段廣仁.線性系統(tǒng)理論[M].2版.哈爾濱：哈爾濱工業(yè)大學(xué)出版社，2004.

[7]???? 張曉亮，羅文廣.汽車主動(dòng)懸架系統(tǒng)的線性二次最優(yōu)控制研究[J].廣西工學(xué)院學(xué)報(bào)，2011，22（4）：44-48.

[8]???? 劉愛民，梁亞茹.基于修正型線性二次最優(yōu)控制的PID參數(shù)優(yōu)化方法及其應(yīng)用[J].組合機(jī)床與自動(dòng)化加工技術(shù)，2007（11）：36-39.

[9]???? 周加全，羅文廣，李亮，等.仿生機(jī)器魚位姿模糊控制研究[J].廣西科技大學(xué)學(xué)報(bào)，2017，28（2）：17-22.

[10]?? 謝超藝，羅文廣，張午昀，等.基于AD5435的永磁同步電機(jī)矢量控制系統(tǒng)仿真[J].廣西科技大學(xué)學(xué)報(bào)，2015，26（2）：8-13.

[11]?? PRASAD L B，TYAGI B，GUPTA H O.Optimal control of nonlinear inverted pendulum system using PID controller and LQR： performance analysis without and with disturbance input[J].國(guó)際自動(dòng)化與計(jì)算雜志（英文版），2014，11（6）：661-670.

[12]?? ENDERLE J D，WOLFE J W. Time-optimal control of saccadic eye-movements[J].IEEE Transactions on Biomedical Engineering，1987，34（1）：43-55.

Design of optimal controller for balanced vehicle based

on improved LQR

LIU Ruijun1， XIE Guangming2， LUO Wenguang*1

（1.School of Electric and Information Engineering， Guangxi University of Science and Technology， Liuzhou 545006， China;2.Engineering School， Peking University， Beijing 100871， China）

Abstract： In this paper， a systematic study is made on the jitter problem of balancing vehicle when starting and stopping. The improved LQR （LQR， Linear Quadratic Regulator） controller is designed by establishing the system dynamics model. The system matrix and control matrix are improved to?????????? increase the matrix of small range increment， the system realizes the adaptive adjustment， and the??????? optimal control rate is adjustable in a small range so as to achieve the best performance index. The???? improved method solves the small-scale instability and effectively overcomes the instability of the?? overshoot. We can find the Q and R values that make the best performance by the simulation????????????? calculation of different Q and R. Through the simulation calculation and selecting the appropriate weight matrix Q and R， the performance index is optimized. Simulation experiments show that the method is feasible and practical.

Key words： linear quadratic; weighted matrix; optimal controller; MATLAB simulation