使用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()
}

图表展示