PID 控制器即使对于自然角度也返回正值

Question

我的总体目标是编写一个 PID 控制器，允许刚体根据玩家位置使用扭矩旋转到特定角度（Aim/shoot 非常基本的东西）。

问题来了。它会掉下来，然后转圈。代码如下所示：

public class EnemyBehavour : MonoBehaviour {
    //speed of enemy
    public float speed = .5f;
    //base distance enemy will attack player
    public float range = 3f;
    //how far back enemy will stand
    public float stopDistance;
    //what the enemy will pursue
    private GameObject target;
    //targetable Enemies
    private GameObject[] targetable;
    //itself
    private Rigidbody self;
    //distance to check against
    private float saveDistance;
    //get the mass for physics stuff
    float mass;
    //gets drag for physics
    float drag;
    //gets angular drag for physics
    float adrag;
    //checks distance
    float distance; 
    //the player
    GameObject player;
    GameObject[] turrets;
    //the base  
    GameObject playerBase;
    EnemyShoot Shoot;
    //torque holder;
    float torque;
//used for PID
public float maxAccPID = 180f;
public float maxSpeedPID = 90f;
public float pGain = 20f;
public float dGain = 10f;
float Accel;
float anglSpeed;


float error;
float lastError = 0f;
float diff;




void Start () {
    //get own rigidbody
    self = GetComponent<Rigidbody> ();
    //playerbase should be obviouse
    playerBase = GameObject.FindGameObjectWithTag ("Base");
    target = playerBase;

    Shoot = GetComponentInChildren<EnemyShoot> ();

    mass = self.mass;
    drag = self.drag;
    adrag = self.angularDrag;
    PIDRotation ();
}


void FixedUpdate () {
    target = FindFoe ();

    //checks range and 
    if (Vector3.Distance (transform.position, target.transform.position) > range || target == playerBase) {
        target = playerBase;
    }
    torque = PIDRotation ();
    //Debug.Log (torque.ToString ());



    torque *= .1f;
    //look at the pursueing target
    self.AddTorque (Vector3.up * torque);
    //move towards the pursuing target
    self.AddForce (transform.forward * speed);
    Debug.Log (torque.ToString ());
}

float PIDRotation () {

    //this gets the difference between facing forward and facing target
    //this creates the vector facing the target
    Vector3 relativePos = self.position - target.transform.position;
    //this gets the crossproduct or the sin of angle between the facing vector and the vector to face
    Vector3 crossAngle = Vector3.Cross (transform.forward.normalized, relativePos.normalized);
    //this gets the current angle
    float currentAngle = transform.eulerAngles.y;
    //this gets the angle in euler to turn; positive is 
    float deltaAngle = Mathf.Asin (crossAngle.y)/Mathf.PI * 180;
    Debug.Log (crossAngle.ToString () + " " + deltaAngle.ToString());



    error = currentAngle - deltaAngle; //generate error signal
    diff = (lastError - error) / Time.deltaTime; //calculate differentail
    Debug.Log ("diff " + diff.ToString ());
    lastError = error;
    Debug.Log ("error " + error.ToString ());
    //calculate acceleration

    Accel = error * pGain + diff * dGain;

    Debug.Log ("Accel1 " + Accel.ToString ());
    Accel = Mathf.Clamp (Accel, -1f * maxAccPID, maxAccPID);
    Debug.Log ("Accel2 " + Accel.ToString ());
    anglSpeed = Accel * Time.deltaTime;
    anglSpeed = Mathf.Clamp (anglSpeed, -maxSpeedPID, maxSpeedPID);

    return anglSpeed;












}






//checks if turrets or player is within range
GameObject FindFoe() {
    distance = range;
    targetable = GameObject.FindGameObjectsWithTag ("ETarget");

    //sets player base as default target


    foreach (GameObject test in targetable) {
        GameObject testing = test.transform.parent.gameObject;
        print (testing.ToString());
        saveDistance = Vector3.Distance (transform.position, testing.transform.position);
        if (saveDistance < distance) {
            distance = saveDistance;
            print (saveDistance.ToString());
            target = testing;
        }



    }
    return target;


}

}

调试看起来像这样： http://puu.sh/j9Df2/52a2b2d07b.jpg

我知道问题出在哪里了。（一些我如何获得正值的加速度，即使是负角。）但我不知道出了什么问题或如何解决它。

Answer 1

查看这一行：

error = currentAngle - deltaAngle; //generate error signal

直接减去两个角不太可能是你想做的。假设您的角度都来自 0-360.

currentAngle = 5
deltaAngle = 355

逻辑上，你会说从 currentAngle 到 deltaAngle 你需要移动 -10 度，因为 355 是圆上的同一个地方作为 -5。但是如果你只做 currentAngle - deltaAngle，你会得到 -350!

那么如何获得正确的 10 度差？谁在乎，Unity can do it for you with Mathf.DeltaAngle(a, b)！

PID 控制器即使对于自然角度也返回正值

PID controller returning a positive value even for nagetive angles

c#

pid

unity3d

game-physics