使用Kotlin簡單分析PID算法

前言

老師佈置了個任務,用編程實現PID調節,鑑於我們專業都學過C語言和VB,於是我就想拿Kotlin練練手.
上網搜索一番別人怎麼用C實現的,學習一番後,自己用Kotlin實現了下,並將PID算法的數據可視化,可以直觀的感受到各種算法的優點.

Gradle

buildscript {
    ext.kotlin_version = '1.2.10'

    repositories {
        mavenCentral()
    }
    dependencies {
        classpath "org.jetbrains.kotlin:kotlin-gradle-plugin:$kotlin_version"
    }
}

group 'bishisimo'
version '1.0-SNAPSHOT'

apply plugin: 'java'
apply plugin: 'kotlin'
apply plugin: 'application'

sourceCompatibility = 1.8
mainClassName='motionControl.PIDKt'
repositories {
    mavenCentral()
    maven { url "https://dl.bintray.com/kotlin/kotlinx" }
}

dependencies {
    compile "org.jetbrains.kotlin:kotlin-stdlib-jdk8:$kotlin_version"
    testCompile group: 'junit', name: 'junit', version: '4.12'
    compile 'org.jetbrains.kotlinx:kotlinx-coroutines-core:0.16'
    /*圖表*/
    compile 'org.jfree:jfreechart-fx:1.0.1'
    compile 'org.jfree:jfreechart:1.5.0'
}

compileKotlin {
    kotlinOptions.jvmTarget = "1.8"
}
compileTestKotlin {
    kotlinOptions.jvmTarget = "1.8"
}
kotlin {
    experimental {
        coroutines "enable"
    }
}

簡單的可視化功能

package mJFreeChart

import org.jfree.chart.ChartFactory
import org.jfree.chart.ChartPanel
import org.jfree.chart.StandardChartTheme
import org.jfree.chart.title.TextTitle
import org.jfree.chart.ui.ApplicationFrame
import org.jfree.data.category.DefaultCategoryDataset
import java.awt.BasicStroke
import java.awt.Color
import java.awt.Font
import java.util.*


data class LineDataSet(val value: Double, val xTitle: String, val xPosition: Comparable<Int>)

class MJFreeChart : ApplicationFrame("666") {
    var seriesCount=0

    fun lineChart(list: ArrayList<ArrayList<LineDataSet>>) {
        val font = Font("宋體", Font.ITALIC, 15)
        val chartTheme = StandardChartTheme("CN")
        // 設置標題字體
        chartTheme.extraLargeFont = font
        // 設置圖例的字體
        chartTheme.regularFont = font
        // 設置軸向的字體
        chartTheme.largeFont = font
        chartTheme.smallFont = font
        chartTheme.titlePaint = Color(51, 51, 51)//Paint 可以理解爲繪製顏色；標題字體顏色

        chartTheme.legendBackgroundPaint = Color.LIGHT_GRAY// 設置標註背景色
        chartTheme.legendItemPaint = Color.BLACK//設置字體顏色
        chartTheme.chartBackgroundPaint = Color.WHITE//圖表背景色
        chartTheme.domainGridlinePaint = Color(120, 255, 201)// X座標軸垂直網格顏色
//        chartTheme.rangeGridlinePaint = Color(25, 255, 255)// Y座標軸水平網格顏色
        ChartFactory.setChartTheme(chartTheme)//設置主題樣式

        val lineDataSet = DefaultCategoryDataset()
        list.flatMap { it }
                .forEach {
                    seriesCount++
                    lineDataSet.addValue(it.value, it.xTitle, it.xPosition)//arg1爲y值,arg2爲線條類,arg3爲在x軸上的位置
                }

        val chart = ChartFactory.createLineChart("Title", "Category", "value", lineDataSet)
        val plot = chart.categoryPlot
        plot.domainAxis.isTickLabelsVisible = false
        plot.isDomainGridlinesVisible=true
        plot.isRangeGridlinesVisible = true //是否顯示格子線
//        plot.backgroundAlpha = 0.2f //設置背景透明度
        for (i in 0 until  seriesCount){
            plot.renderer.setSeriesStroke(i, BasicStroke(2f))
        }
        //設置主標題
        chart.title = TextTitle("PID調節比較", font)
        chart.antiAlias = true//抗鋸齒
//        chart.setTextAntiAlias(false)


        val chartPanne = ChartPanel(chart)
        contentPane = chartPanne
        this.pack()
        this.isVisible = true
    }
}

fun main(args: Array<String>) {
    val ml1 = mutableListOf<Double>()
    val random = Random()
    val ml2 = mutableListOf<Double>()
    val ml3 = mutableListOf<Double>()
    for (i in 1..100) {
        ml1.add(random.nextDouble())
        ml2.add(random.nextDouble())
        ml3.add(random.nextDouble())
    }
//    val list= listOf(ml1,ml2,ml3)
//    val mj=mJFreeChart.MJFreeChart()
//    mj.lineChart(list)
//    while (true){
//
//    }
}

源碼及分析

這裏是最基本的PID抽象類

package motionControl

import kotlinx.coroutines.experimental.CommonPool
import kotlinx.coroutines.experimental.delay
import kotlinx.coroutines.experimental.launch
import mJFreeChart.LineDataSet
import mJFreeChart.MJFreeChart
import java.lang.Thread.sleep
import java.util.*

abstract class PID(private val Kp: Float, private val Ki: Float, private val Kd: Float, private val target: Float) {
    val name = this.javaClass.simpleName!!
    protected var actual = 0.0  //真實值
    protected var ierr = 0.0 //積分誤差
    protected var derr = 0.0 //微分誤差
    protected var errNow = 0.0   //當前誤差
    protected var errPrevious = 0.0  //前一次誤差
    protected var errMutableList = arrayListOf<Double>() //積分誤差集合
    //比例實現
    open fun proportion(): Any {
        return Unit
    }

    //積分實現
    open fun integral(): Any {
        return Unit
    }

    //微分實現
    open fun differential(): Any {
        return Unit
    }

    //入口方法
    open fun pid(): Any {
        return Unit
    }
}

/**
 * 位置法
 * 這裏用了最基本的算法實現形式，沒有考慮死區問題，沒有設定上下限，只是對公式的一種直接的實現
 */
open class PositionPID(private val Kp: Float, private val Ki: Float, private val Kd: Float, private val target: Float) : PID(Kp, Ki, Kd, target) {

    override fun proportion(): Double {
        return Kp * errNow
    }

    override fun integral(): Double {
        errMutableList.add(errNow)
        ierr = Ki * errMutableList.sum()
        return ierr
    }

    override fun differential(): Double {
        derr = Kd * (errNow - errPrevious)
        return derr
    }

    override fun pid(): Double {
        errPrevious = errNow
        errNow = target - actual
        actual = proportion() + integral() + differential()
        return actual
    }
}

/**
 * 增量法
 * 增量式的表達結果和最近三次的偏差有關，這樣就大大提高了系統的穩定性。
 */
class IncrementalPID(private val Kp: Float, private val Ki: Float, private val Kd: Float, private val target: Float) : PID(Kp, Ki, Kd, target) {
    var errFar = 0.0//前前次誤差
    override fun proportion(): Double {
        return Kp * (errNow - errPrevious)
    }

    override fun integral(): Double {
        ierr = Ki * errNow
        return ierr
    }

    override fun differential(): Double {
        derr = Kd * (errNow - 2 * errPrevious + errFar)
        return derr
    }

    override fun pid(): Double {
        errFar = errPrevious
        errPrevious = errNow
        errNow = target - actual
        actual += proportion() + integral() + differential()
        return actual
    }
}

/**
 * 積分分離法
 * 在啓動、結束或大幅度增減設定時，短時間內系統輸出有很大的偏差，會造成PID運算的積分積累，導致控制量超過執行機構可能允許的最大動作範圍對應極限控制量，從而引起較大的超調，甚至是震盪，這是絕對不允許的。
 * 基本思路是 當被控量與設定值偏差較大時，取消積分作用; 當被控量接近給定值時，引入積分控制，以消除靜差，提高精度。
 */
class IntegralSeparationPID(Kp: Float, private val Ki: Float, Kd: Float, private val target: Float) : PositionPID(Kp, Ki, Kd, target) {
    override fun integral(): Double {
        ierr = if (errNow < target && errNow > -1 * target) {
            errMutableList.add(errNow)
            Ki * errMutableList.sum()
        } else {
            0.0
        }
        return ierr
    }
}

/**
 * 抗積分飽和法
 * 所謂的積分飽和現象是指如果系統存在一個方向的偏差，PID控制器的輸出由於積分作用的不斷累加而加大，從而導致執行機構達到極限位置，若控制器輸出U(k)繼續增大，執行器開度不可能再增大，此時計算機輸出控制量超出了正常運行範圍而進入飽和區。一旦系統出現反向偏差，u(k)逐漸從飽和區退出。進入飽和區越深則退出飽和區時間越長。在這段時間裏，執行機構仍然停留在極限位置而不隨偏差反向而立即做出相應的改變，這時系統就像失控一樣，造成控制性能惡化，這種現象稱爲積分飽和現象或積分失控現象。
 * 防止積分飽和的方法之一就是抗積分飽和法，該方法的思路是在計算u(k)時，首先判斷上一時刻的控制量u(k-1)是否已經超出了極限範圍： 如果u(k-1)>umax，則只累加負偏差; 如果u(k-1)
 */
class IntegralSaturationPID(Kp: Float, private val Ki: Float, Kd: Float, private val target: Float) : PositionPID(Kp, Ki, Kd, target) {
    private val uMax = 2 * target
    private val uMin = -1 * target
    override fun integral(): Double {
        ierr = if (actual > uMax) {
            if (errNow >= -1 * target && errNow < 0) {      //-t<=err<0
                errMutableList.add(errNow)
                Ki * errMutableList.sum()
            } else if (errNow in 0.0..target.toDouble()) {    //0<=err<=t
                Ki * errMutableList.sum()
            } else {                                    //err<-t||err>t
                0.0
            }
        } else if (actual < uMin) {
            if (errNow > 0 && errNow <= target) {         //0<err<=t
                errMutableList.add(errNow)
                Ki * errMutableList.sum()
            } else if (errNow >= -1 * target && errNow <= 0) {     //-t<=err<=0
                Ki * errMutableList.sum()
            } else {                                    //err<-t||err>t
                0.0
            }
        } else {
            if (errNow >= -1 * target && errNow <= target) {         //-t<=err<t
                errMutableList.add(errNow)
                Ki * errMutableList.sum()
            } else {                                    //err<-t||err>t
                0.0
            }

        }
        return ierr
    }
}

/**
 * 梯形積分法
 * 作爲PID控制律的積分項，其作用是消除餘差，爲了儘量減小余差，應提高積分項運算精度，爲此可以將矩形積分改爲梯形積分
 */
class TrapezoidalIntegralPID(Kp: Float, private val Ki: Float, Kd: Float, target: Float) : PositionPID(Kp, Ki, Kd, target) {
    override fun integral(): Double {
        errMutableList.add(errNow)
        ierr = Ki * errMutableList.sum() / 2
        return ierr
    }
}

/**
 * 變積分法
 * 變積分PID可以看成是積分分離的PID算法的更一般的形式。在普通的PID控制算法中，由於積分系數ki是常數，所以在整個控制過程中，積分增量是不變的。但是，系統對於積分項的要求是，系統偏差大時，積分作用應該減弱甚至是全無，而在偏差小時，則應該加強。積分系數取大了會產生超調，甚至積分飽和，取小了又不能短時間內消除靜差。因此，根據系統的偏差大小改變積分速度是有必要的。
 * 變積分PID的基本思想是設法改變積分項的累加速度，使其與偏差大小相對應：偏差越大，積分越慢; 偏差越小，積分越快。
 */
class VariableIntegralPID(Kp: Float, private val Ki: Float, Kd: Float, private val target: Float) : PositionPID(Kp, Ki, Kd, target) {
    override fun integral(): Double {
        ierr = if (errNow < -target && errNow > target) {
            0.0
        } else if (actual > -0.8 * target && errNow < 0.8 * target) {
            errMutableList.add(errNow)
            Ki * errMutableList.sum()
        } else {
            if (errNow >= -1 * target && errNow <= target) {         //-t<=err<t
                errMutableList.add(errNow)
                Ki * errMutableList.sum()
            } else {                                    //err<-t||err>t
                errMutableList.add(errNow)
                Ki * errMutableList.sum() * (target - Math.abs(errNow)) / 10
            }
        }
        return ierr
    }
}

fun comparePID() {//比較各種算法的函數
    var kp = 0.2f
    var ki = 0.15f
    var kd = 0.2f
    var target = 100f
    var testCount = 100

    val sc = Scanner(System.`in`)
    println("是否輸入參數y/n,默認kp=$kp,ki=$ki,kd=$kd,目標值=$target,調節次數=$testCount")
    if (sc.nextLine().startsWith('y')) {
        print("請輸入Kp,：")
        kp = sc.nextFloat()
        print("請輸入Ki：")
        ki = sc.nextFloat()
        print("請輸入Kd：")
        kd = sc.nextFloat()
        print("請輸入目標值：")
        target = sc.nextFloat()
        print("請輸入調節次數：")
        testCount = sc.nextInt()
    }

    val pidList = arrayListOf<PID>()
    //註冊待測試的PID調節方式
    pidList.add(PositionPID(kp, ki, kd, target))
    pidList.add(IncrementalPID(kp, ki, kd, target))
    pidList.add(IntegralSeparationPID(kp, ki, kd, target))
    pidList.add(IntegralSaturationPID(kp, ki, kd, target))
    pidList.add(TrapezoidalIntegralPID(kp, ki, kd, target))
    pidList.add(VariableIntegralPID(kp, ki, kd, target))


    val dataList = arrayListOf<ArrayList<LineDataSet>>()
    for (i in 1..testCount) {//設置PID調節次數
        for ((count, item) in pidList.withIndex()) {//獲取6種調節方式的數據
            if (i == 1) dataList.add(arrayListOf())//初始化儲存列表
            dataList[count].add(LineDataSet(item.pid() as Double, item.name.split("motionControl.PID")[0], i))//填充數據
        }
    }

    val mj = MJFreeChart()
    mj.lineChart(dataList)
}

fun main(args: Array<String>) {
    comparePID()
}

圖表展示