{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Chapter 6"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"using ControlSystems\n",
"using Plots; gr()\n",
"using LinearAlgebra"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## ナイキストの安定判別"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"TransferFunction{Continuous, ControlSystems.SisoRational{Float64}}\n",
" 1.0\n",
"----------------------------\n",
"1.0s^3 + 1.0s^2 + 1.5s + 1.0\n",
"\n",
"Continuous-time transfer function model\n",
"ComplexF64[-0.12040192275078712 + 1.1413527165187305im, -0.12040192275078712 - 1.1413527165187305im, -0.7591961544984255 + 0.0im]\n"
]
}
],
"source": [
"P = tf([0, 1], [1, 1, 1.5, 1])\n",
"println(P)\n",
"println(pole(P))"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 3,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"P = tf([0,1],[1,1,1.5,1])\n",
"# 位相が 180[deg] 遅れる周波数を取得\n",
"wpc, _, _, _ = margin(P);\n",
"\n",
"t = 0:0.1:30;\n",
"u = sin.(wpc[]*t);\n",
"y = 0 .* u;\n",
"\n",
"p = [ plot() plot(); plot() plot() ]\n",
"\n",
"for i in [1,2]\n",
" for j in [1,2]\n",
" # 出力をネガティブフィードバックして次の時刻の入力を生成\n",
" u = sin.(wpc[]*t) - vec(y)\n",
" y, t, x, uout = lsim(P, u', t)\n",
" plot!(p[i,j], t, u, label=\"u\");\n",
" plot!(p[i,j], t, vec(y), label=\"y\");\n",
" end\n",
"end\n",
"\n",
"plot( p[1,1], p[1,2], p[2,1], p[2,2], layout=(2,2))"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"TransferFunction{Continuous, ControlSystems.SisoRational{Float64}}\n",
" 1.0\n",
"-------------------------------\n",
"1.0s^3 + 2.0s^2 + 1.9999s + 1.0\n",
"\n",
"Continuous-time transfer function model\n",
"ComplexF64[-1.0000999999990001 + 0.0im, -0.4999500000004993 + 0.8659965401198194im, -0.4999500000004993 - 0.8659965401198194im]\n"
]
}
],
"source": [
"P = tf([0, 1], [1, 2, 1.9999, 1])\n",
"println(P)\n",
"println(pole(P))"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 5,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"P = tf([0,1],[1,2,1.9999,1])\n",
"# 位相が 180[deg] 遅れる周波数を取得\n",
"wpc, _, _, _ = margin(P);\n",
"\n",
"t = 0:0.1:30;\n",
"u = sin.(wpc[]*t);\n",
"y = 0 .* u;\n",
"\n",
"p = [ plot() plot(); plot() plot() ]\n",
"\n",
"for i in [1,2]\n",
" for j in [1,2]\n",
" # 出力をネガティブフィードバックして次の時刻の入力を生成\n",
" u = sin.(wpc[]*t) - vec(y)\n",
" y, t, x, uout = lsim(P, u', t)\n",
" plot!(p[i,j], t, u, label=\"u\");\n",
" plot!(p[i,j], t, vec(y), label=\"y\");\n",
" end\n",
"end\n",
"\n",
"plot( p[1,1], p[1,2], p[2,1], p[2,2], layout=(2,2))"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 7,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"# 閉ループ系が不安定になる場合\n",
"P = tf([0, 1],[1, 1, 1.5, 1])\n",
"\n",
"nyquistplot(P, gaincircle=true, lw = 2, xlims=(-2.5,2.5), ylims=(-2.5,2.5), size=(300,300))"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 8,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"# 閉ループ系が安定になる場合\n",
"P = tf([0, 1],[1, 2, 1.9999, 1])\n",
"\n",
"nyquistplot(P, gaincircle=true, lw = 2, xlims=(-2.5,2.5), ylims=(-2.5,2.5), size=(300,300))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## アームの角度制御（PID制御）"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"30"
]
},
"execution_count": 3,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"g = 9.81 # 重力加速度[m/s^2]\n",
"l = 0.2 # アームの長さ[m]\n",
"M = 0.5 # アームの質量[kg]\n",
"mu = 1.5e-2 # 粘性摩擦係数\n",
"J = 1.0e-2 # 慣性モーメント\n",
"\n",
"P = tf( [0,1], [J, mu, M*g*l] )\n",
"\n",
"ref = 30 # 目標角度 [deg]"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### P制御"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 4,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"kp = (0.5, 1, 2)\n",
"\n",
"H = [ P*tf([0, kp[i]], [0, 1]) for i = 1:length(kp) ]\n",
"\n",
"setPlotScale(\"dB\")\n",
"bodeplot(H, lw=2, size=(450,400),\n",
" label=[\"kₚ=$(kp[1])\" \"kₚ=$(kp[1])\" \"kₚ=$(kp[2])\" \"kₚ=$(kp[2])\" \"kₚ=$(kp[3])\" \"kₚ=$(kp[3])\"],\n",
" legend=:best, title=\"\" )"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"kP=0.5\n",
"(wpc, GM, wgc, PM)\n",
"([NaN;;], [Inf;;], [12.048359830388655;;], [21.007168728281016;;])\n",
"-----------------\n",
"kP=1\n",
"(wpc, GM, wgc, PM)\n",
"([NaN;;], [Inf;;], [14.00755547266918;;], [12.0877310471827;;])\n",
"-----------------\n",
"kP=2\n",
"(wpc, GM, wgc, PM)\n",
"([NaN;;], [Inf;;], [17.231985129715717;;], [7.406517464524029;;])\n",
"-----------------\n"
]
}
],
"source": [
"for i in 1:length(kp)\n",
" K = tf([0, kp[i]], [0, 1]) # P制御\n",
" H = P * K # 開ループ系\n",
" \n",
" println(\"kP=\", kp[i])\n",
" println(\"(wpc, GM, wgc, PM)\")\n",
" println(margin(H))\n",
" println(\"-----------------\")\n",
"end"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### PI制御"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 6,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"kp = 2\n",
"ki = (0, 5, 10)\n",
"\n",
"H = [ P*tf([kp, ki[i]], [1, 0]) for i = 1:length(ki) ]\n",
"\n",
"setPlotScale(\"dB\")\n",
"bodeplot(H, lw=2, size=(450,400),\n",
" label=[\"ki=$(ki[1])\" \"ki=$(ki[1])\" \"ki=$(ki[2])\" \"ki=$(ki[2])\" \"ki=$(ki[3])\" \"ki=$(ki[3])\"],\n",
" legend=:best, title=\"\" )"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"kI=0\n",
"(wpc, GM, wgc, PM)\n",
"([NaN;;], [Inf;;], [17.231985129715717;;], [7.406517464524029;;])\n",
"-----------------\n",
"kI=5\n",
"(wpc, GM, wgc, PM)\n",
"([15.671318145841404;;], [0.7374341834142752;;], [17.29090924820676;;], [0.8699461934346004;;])\n",
"-----------------\n",
"kI=10\n",
"(wpc, GM, wgc, PM)\n",
"([11.849199557862834;;], [0.2113800049851713;;], [17.452928304499043;;], [8.761143190659993;;])\n",
"-----------------\n"
]
}
],
"source": [
"for i in 1:length(ki)\n",
" K = tf([kp, ki[i]], [1, 0]) # PI制御\n",
" H = P * K # 開ループ系\n",
" \n",
" println(\"kI=\", ki[i])\n",
" println(\"(wpc, GM, wgc, PM)\")\n",
" println(margin(H))\n",
" println(\"-----------------\")\n",
"end"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 8,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"plt = plot()\n",
"\n",
"for i in 1:1:length(ki)\n",
" K = tf([kp, ki[i]], [1, 0]) # PI制御\n",
" Gyr = feedback(P*K,1) # 閉ループ系\n",
" y, t = step( Gyr, 0:0.01:2 )\n",
" plot!(plt, t, ref*y',\n",
" xlabel=\"t\", #X軸のラベル\n",
" ylabel=\"y\", #Y軸のラベル\n",
" lw=2, #線幅\n",
" ls=:solid, #線種\n",
" label=\"kI=$(ki[i])\",\n",
" legend=:bottomright,\n",
" size=(300,230) #プロットのサイズ \n",
" )\n",
"end\n",
" \n",
"plot(plt)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### PID制御"
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 9,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"kp = 2\n",
"ki = 5\n",
"kd = (0, 0.1, 0.2)\n",
"\n",
"H = [ P*tf([kd[i], kp, ki], [1,0]) for i = 1:length(kd) ]\n",
"\n",
"setPlotScale(\"dB\")\n",
"bodeplot(H, lw=2, size=(450,400),\n",
" label=[\"kd=$(kd[1])\" \"kd=$(kd[1])\" \"kd=$(kd[2])\" \"kd=$(kd[2])\" \"kd=$(kd[3])\" \"kd=$(kd[3])\"],\n",
" legend=:best, title=\"\" )"
]
},
{
"cell_type": "code",
"execution_count": 10,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"kD=0\n",
"(wpc, GM, wgc, PM)\n",
"([15.671318145841404;;], [0.7374341834142752;;], [17.29090924820676;;], [0.8699461934346004;;])\n",
"-----------------\n",
"kD=0.1\n",
"(wpc, GM, wgc, PM)\n",
"([NaN;;], [Inf;;], [18.809759446133874;;], [45.220116699008315;;])\n",
"-----------------\n",
"kD=0.2\n",
"(wpc, GM, wgc, PM)\n",
"([NaN;;], [Inf;;], [24.730502425798765;;], [71.27203690036902;;])\n",
"-----------------\n"
]
}
],
"source": [
"for i in 1:length(kd)\n",
" K = tf([kd[i], kp, ki], [1,0]) # PID制御\n",
" H = P * K # 開ループ系\n",
" \n",
" println(\"kD=\", kd[i])\n",
" println(\"(wpc, GM, wgc, PM)\")\n",
" println(margin(H))\n",
" println(\"-----------------\")\n",
"end"
]
},
{
"cell_type": "code",
"execution_count": 11,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 11,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"plt = plot()\n",
"\n",
"for i in 1:1:length(kd)\n",
" K = tf([kd[i],kp,ki],[1,0]) # PID制御\n",
" Gyr = feedback(P*K,1) # 閉ループ系\n",
" y, t = step( Gyr, 0:0.01:2 )\n",
" plot!(plt, t, ref*y',\n",
" xlabel=\"t\", #X軸のラベル\n",
" ylabel=\"y\", #Y軸のラベル\n",
" lw=2, #線幅\n",
" ls=:solid, #線種\n",
" label=\"kD=$(kd[i])\",\n",
" legend=:bottomright,\n",
" size=(300,230) #プロットのサイズ \n",
" )\n",
"end\n",
" \n",
"plot(plt)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"開ループ系の比較"
]
},
{
"cell_type": "code",
"execution_count": 12,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 12,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"kp = (2, 1)\n",
"ki = (5, 0)\n",
"kd = (0.1, 0)\n",
"Label = (\"After\", \"Before\")\n",
"\n",
"H = [ P*tf([kd[i], kp[i], ki[i]], [1,0]) for i = 1:length(kp) ]\n",
"\n",
"setPlotScale(\"dB\")\n",
"bodeplot(H, lw=2, size=(450,400), \n",
" label=[\"After\" \"After\" \"Before\" \"Before\"] , title=\"\" )"
]
},
{
"cell_type": "code",
"execution_count": 13,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"kP=2, kI=5, kD=0.1(wpc, GM, wgc, PM)\n",
"([NaN;;], [Inf;;], [18.809759446133874;;], [45.220116699008315;;])\n",
"-----------------\n",
"kP=1, kI=0, kD=0(wpc, GM, wgc, PM)\n",
"([NaN;;], [Inf;;], [14.00755547266918;;], [12.0877310471827;;])\n",
"-----------------\n"
]
}
],
"source": [
"for i in 1:length(kp)\n",
" K = tf([kd[i], kp[i], ki[i]], [1,0]) # PID制御\n",
" H = P * K # 開ループ系\n",
" \n",
" print(\"kP=\", kp[i], \", kI=\", ki[i], \", kD=\", kd[i])\n",
" println(\"(wpc, GM, wgc, PM)\")\n",
" println(margin(H))\n",
" println(\"-----------------\")\n",
"end"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"閉ループ系の比較"
]
},
{
"cell_type": "code",
"execution_count": 14,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"After\n",
"定常偏差 =2.220446049250313e-16\n",
"------------------\n",
"Before\n",
"定常偏差 =0.49520444220090865\n",
"------------------\n"
]
},
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 14,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"kp = (2, 1)\n",
"ki = (5, 0)\n",
"kd = (0.1, 0)\n",
"Label = (\"After\", \"Before\")\n",
"\n",
"plt = plot()\n",
"\n",
"for i in 1:1:length(kd)\n",
" K = tf( [kd[i], kp[i], ki[i]], [1, 0])\n",
" Gyr = feedback(P*K, 1)\n",
" Gyr = minreal(Gyr)\n",
" y, t = step( Gyr, 0:0.01:2 )\n",
" plot!(plt, t, y',\n",
" xlabel=\"t\", #X軸のラベル\n",
" ylabel=\"y\", #Y軸のラベル\n",
" lw=2, #線幅\n",
" ls=:solid, #線種\n",
" label=\"$(Label[i])\",\n",
" legend=:bottomright,\n",
" size=(300,230) #プロットのサイズ \n",
" )\n",
" \n",
" println(Label[i])\n",
" e_std = ( 1 - dcgain(Gyr)[])\n",
" println(\"定常偏差 =\", e_std) \n",
" println(\"------------------\")\n",
"end\n",
"\n",
"plot(plt)"
]
},
{
"cell_type": "code",
"execution_count": 15,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 15,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"H = [ P*tf([kd[i], kp[i], ki[i]], [1,0]) for i = 1:length(kp) ]\n",
"Gyr = [ H[i]/(1+H[i]) for i = 1:length(kp) ]\n",
"\n",
"setPlotScale(\"dB\")\n",
"bodeplot(Gyr, lw=2, size=(450,400),\n",
" label=[\"After\" \"After\" \"Before\" \"Before\"] , title=\"\" )"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## 位相遅れ・進み補償"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### 位相遅れ"
]
},
{
"cell_type": "code",
"execution_count": 16,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"TransferFunction{Continuous, ControlSystems.SisoRational{Float64}}\n",
"1.0s + 10.0\n",
"-----------\n",
"1.0s + 1.0\n",
"\n",
"Continuous-time transfer function model"
]
},
"execution_count": 16,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"α = 10\n",
"T1 = 0.1\n",
"K1 = tf([α *T1, α ], [α *T1, 1])"
]
},
{
"cell_type": "code",
"execution_count": 17,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 17,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"setPlotScale(\"dB\")\n",
"bodeplot(K1, lw=2, size=(450,400), legend=false, title=\"\" )"
]
},
{
"cell_type": "code",
"execution_count": 18,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"ωₘ=3.162277660168379\n",
"ϕₘ=-54.90319877241541\n"
]
}
],
"source": [
"ωₘ = 1/T1/√α\n",
"ϕₘ = asin( (1-α)/(1+α ) ) * 180/π\n",
"println(\"ωₘ=\", ωₘ)\n",
"println(\"ϕₘ=\", ϕₘ)"
]
},
{
"cell_type": "code",
"execution_count": 19,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"TransferFunction{Continuous, ControlSystems.SisoRational{Float64}}\n",
"10000.0s + 100000.0\n",
"-------------------\n",
" 10000.0s + 1.0\n",
"\n",
"Continuous-time transfer function model"
]
},
"execution_count": 19,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"α = 100000\n",
"T1 = 0.1\n",
"K1 = tf([α *T1, α ], [α *T1, 1])"
]
},
{
"cell_type": "code",
"execution_count": 20,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 20,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"setPlotScale(\"dB\")\n",
"bodeplot(K1, lw=2, size=(450,400), legend=false, title=\"\" )"
]
},
{
"cell_type": "code",
"execution_count": 21,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"ωₘ=0.03162277660168379\n",
"ϕₘ=-89.63763088074153\n"
]
}
],
"source": [
"ωₘ = 1/T1/√α\n",
"ϕₘ = asin( (1-α)/(1+α ) ) * 180/π\n",
"println(\"ωₘ=\", ωₘ)\n",
"println(\"ϕₘ=\", ϕₘ)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### 位相進み"
]
},
{
"cell_type": "code",
"execution_count": 22,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"TransferFunction{Continuous, ControlSystems.SisoRational{Float64}}\n",
"1.0s + 1.0\n",
"----------\n",
"0.1s + 1.0\n",
"\n",
"Continuous-time transfer function model"
]
},
"execution_count": 22,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"β = 0.1\n",
"T2 = 1\n",
"K2 = tf([T2, 1],[β*T2, 1])"
]
},
{
"cell_type": "code",
"execution_count": 23,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 23,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"setPlotScale(\"dB\")\n",
"bodeplot(K2, lw=2, size=(450,400), legend=false, title=\"\" )"
]
},
{
"cell_type": "code",
"execution_count": 24,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"ωₘ=3.162277660168379\n",
"ϕₘ=54.9031987724154\n"
]
}
],
"source": [
"ωₘ = 1/T2/√β\n",
"ϕₘ = asin( (1-β)/(1+β ) ) * 180/π\n",
"println(\"ωₘ=\", ωₘ)\n",
"println(\"ϕₘ=\", ϕₘ)"
]
},
{
"cell_type": "code",
"execution_count": 25,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"TransferFunction{Continuous, ControlSystems.SisoRational{Float64}}\n",
" 1.0s + 1.0\n",
"-------------\n",
"1.0e-6s + 1.0\n",
"\n",
"Continuous-time transfer function model"
]
},
"execution_count": 25,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"β = 0.000001\n",
"T2 = 1\n",
"K2 = tf([T2, 1],[β*T2, 1])"
]
},
{
"cell_type": "code",
"execution_count": 26,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 26,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"setPlotScale(\"dB\")\n",
"w = exp10.( -2:0.01:3 )\n",
"bodeplot(K2, w, lw=2, size=(450,400), legend=false, title=\"\" )"
]
},
{
"cell_type": "code",
"execution_count": 27,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"ωₘ=1000.0\n",
"ϕₘ=89.88540847917132\n"
]
}
],
"source": [
"ωₘ = 1/T2/√β\n",
"ϕₘ = asin( (1-β)/(1+β ) ) * 180/π\n",
"println(\"ωₘ=\", ωₘ)\n",
"println(\"ϕₘ=\", ϕₘ)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## アームの角度制御（位相遅れ・進み補償）"
]
},
{
"cell_type": "code",
"execution_count": 28,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"30"
]
},
"execution_count": 28,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"g = 9.81 # 重力加速度[m/s^2]\n",
"l = 0.2 # アームの長さ[m]\n",
"M = 0.5 # アームの質量[kg]\n",
"mu = 1.5e-2 # 粘性摩擦係数\n",
"J = 1.0e-2 # 慣性モーメント\n",
"\n",
"P = tf( [0,1], [J, mu, M*g*l] )\n",
"\n",
"ref = 30 # 目標角度 [deg]"
]
},
{
"cell_type": "code",
"execution_count": 29,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 29,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"setPlotScale(\"dB\")\n",
"w = exp10.( -2:0.001:3 )\n",
"bodeplot(P, w, lw=2, size=(450,400), legend=false, title=\"\" )"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"制御対象のボード線図．\n",
"低周波ゲインが０[dB]なので，このままフィードバック系を構築しても定常偏差が残る．"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### 位相遅れ補償の設計"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"** 定常偏差を小さくするために，位相遅れ補償から設計する **\n",
"\n",
"低周波ゲインを上げるために，$\\alpha=20$とする．そして，ゲインを上げる周波数は，$T_1$で決めるが，最終的なゲイン交差周波数（ゲイン交差周波数の設計値）の１０分の１程度を$1/T_1$にするために，$T_1=0.25$とする（$1/T_1=40/10=4$）．"
]
},
{
"cell_type": "code",
"execution_count": 30,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"TransferFunction{Continuous, ControlSystems.SisoRational{Float64}}\n",
"5.0s + 20.0\n",
"-----------\n",
"5.0s + 1.0\n",
"\n",
"Continuous-time transfer function model\n"
]
}
],
"source": [
"α = 20\n",
"T1 = 0.25\n",
"K1 = tf([α*T1, α], [α*T1, 1])\n",
"println(K1)"
]
},
{
"cell_type": "code",
"execution_count": 31,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 31,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"H1 = P*K1\n",
"setPlotScale(\"dB\")\n",
"bodeplot(H1, lw=2, size=(450,400), legend=false, title=\"\" )"
]
},
{
"cell_type": "code",
"execution_count": 32,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"-----------------------\n",
"phase at 40rad/s =-183.1364012726378\n"
]
}
],
"source": [
"sys = freqresp(H1, [40])\n",
"phaseH1at40 = atan(imag(sys)[],real(sys)[]) * (180/π)\n",
"println(\"-----------------------\")\n",
"println(\"phase at 40rad/s =\", phaseH1at40-360)\n",
"#（注意）freqrespは -180 を下回っているときに +360 を加えて出力されるため phaseH1at40-360 を表示している\n",
"# ただし，phaseH1at40 の値は，このあとの計算ではそのまま用いても問題ない"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"最終的にゲイン補償によって，ゲイン交差周波数を設計値の40[rad/s]まで上げるが，あげてしまうと，位相余裕が60[dB]を下回る．実際， 40[rad/s]における位相は -183[deg]程度なので，位相余裕は -3[deg]程度になってしまう．したがって，40[rad/s]での位相を -120[deg] まであげておく．"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### 位相進み補償の設計"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"** 位相進み補償の設計 ** \n",
"\n",
"40[rad/s]において位相を進ませる量は 60 - (180-183) = 63[deg]程度とする．"
]
},
{
"cell_type": "code",
"execution_count": 33,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"TransferFunction{Continuous, ControlSystems.SisoRational{Float64}}\n",
"0.10468126899830243s + 1.0\n",
"---------------------------\n",
"0.005970504618263038s + 1.0\n",
"\n",
"Continuous-time transfer function model\n"
]
}
],
"source": [
"ϕₘ= (60- (180 + phaseH1at40 ) ) * π/180\n",
"β = (1-sin(ϕₘ))/(1+sin(ϕₘ))\n",
"T2 = 1/40/√β\n",
"K2 = tf([T2, 1],[β*T2, 1])\n",
"println(K2)"
]
},
{
"cell_type": "code",
"execution_count": 34,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 34,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"H2 = P*K1*K2\n",
"setPlotScale(\"dB\")\n",
"bodeplot(H2, lw=2, size=(450,400), legend=false, title=\"\" )"
]
},
{
"cell_type": "code",
"execution_count": 35,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"-----------------------\n",
"gain at 40rad/s =-11.058061395752679\n",
"phase at 40rad/s =-119.99999999999997\n"
]
}
],
"source": [
"sys = freqresp(H2, [40])\n",
"magH2at40 = abs(sys[])\n",
"phaseH2at40 = atan(imag(sys)[],real(sys)[]) * (180/π)\n",
"println(\"-----------------------\")\n",
"println(\"gain at 40rad/s =\", 20*log10(magH2at40))\n",
"println(\"phase at 40rad/s =\", phaseH2at40)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"位相進み補償により，40[rad/s]での位相が -120[deg]となっている．\n",
"あとは，ゲイン補償により，40[rad/s]のゲインを 0[dB] にすればよい．"
]
},
{
"cell_type": "code",
"execution_count": 36,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"0.27996060941679224"
]
},
"execution_count": 36,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"magH2at40"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### ゲイン補償の設計"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"** ゲイン補償の設計 **\n",
"\n",
"40[rad/s] におけるゲインが -11.06[dB] 程度なので， 11.06[dB]分上に移動させる．\n",
"そのために，$k = 1/magL2at40$ をゲイン補償とする．\n",
"これにより，40[rad/s]がゲイン交差周波数になり，位相余裕もPM=60[deg]となる．"
]
},
{
"cell_type": "code",
"execution_count": 37,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"k=3.571931073029088"
]
}
],
"source": [
"k = 1/magH2at40\n",
"print(\"k=\", k)"
]
},
{
"cell_type": "code",
"execution_count": 38,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 38,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"H = P*k*K1*K2\n",
"setPlotScale(\"dB\")\n",
"w = exp10.(-1:0.01:2)\n",
"bodeplot([H, P], w, lw=2, size=(450,400),\n",
" label=[\"After\" \"After\" \"Before\" \"Before\"] , title=\"\" )"
]
},
{
"cell_type": "code",
"execution_count": 39,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"(wpc, GM, wgc, PM)\n",
"([NaN;;], [Inf;;], [40.018325278977585;;], [60.00128104275365;;])\n"
]
}
],
"source": [
"println(\"(wpc, GM, wgc, PM)\")\n",
"println(margin(H))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### 閉ループ系の応答"
]
},
{
"cell_type": "code",
"execution_count": 40,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"error=0.013546052578222278\n",
"------------------\n",
"error=0.49520444220090865\n",
"------------------\n"
]
},
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 40,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"plt = plot()\n",
"\n",
"Gyr_H = feedback(H, 1)\n",
"y, t = step( Gyr_H, 0:0.01:2 )\n",
"plot!(plt, t, ref*y',\n",
" xlabel=\"t\", #X軸のラベル\n",
" ylabel=\"y\", #Y軸のラベル\n",
" lw=2, #線幅\n",
" ls=:solid, #線種\n",
" label=\"H\",\n",
" legend=:bottomright,\n",
" size=(300,230) #プロットのサイズ \n",
")\n",
"\n",
"e_std = 1 - dcgain(Gyr_H)[]\n",
"println(\"error=\", e_std) \n",
"println(\"------------------\")\n",
"\n",
"Gyr_P = feedback(P, 1)\n",
"y, t = step( Gyr_P, 0:0.01:2 )\n",
"plot!(plt, t, ref*y',\n",
" xlabel=\"t\", #X軸のラベル\n",
" ylabel=\"y\", #Y軸のラベル\n",
" lw=2, #線幅\n",
" ls=:solid, #線種\n",
" label=\"P\",\n",
" legend=:bottomright,\n",
" size=(300,230) #プロットのサイズ \n",
")\n",
"\n",
"e_std = 1 - dcgain(Gyr_P)[]\n",
"println(\"error=\", e_std) \n",
"println(\"------------------\")\n",
"\n",
"\n",
"plot(plt)"
]
},
{
"cell_type": "code",
"execution_count": 41,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"\n",
"\n"
]
},
"execution_count": 41,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"Gyr = [ H/(1+H), P/(1+P)]\n",
"\n",
"setPlotScale(\"dB\")\n",
"bodeplot(Gyr, lw=2, size=(450,400),\n",
" label=[\"After\" \"After\" \"Before\" \"Before\"] , title=\"\" )"
]
},
{
"cell_type": "code",
"execution_count": 42,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"直流ゲイン =-0.11846369931437545\n",
"------------------\n",
"直流ゲイン =-5.937689510770942\n",
"------------------\n"
]
}
],
"source": [
"println(\"直流ゲイン =\", 20*log10(dcgain(Gyr_H)[]) ) \n",
"println(\"------------------\")\n",
"println(\"直流ゲイン =\", 20*log10(dcgain(Gyr_P)[]) ) \n",
"println(\"------------------\")"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Julia 1.7.0",
"language": "julia",
"name": "julia-1.7"
},
"language_info": {
"file_extension": ".jl",
"mimetype": "application/julia",
"name": "julia",
"version": "1.7.0"
}
},
"nbformat": 4,
"nbformat_minor": 4
}