| | | 1 | | using UnityEngine; |
| | | 2 | | |
| | | 3 | | // The proportional navigation controller applies an input that is proportional to the line of |
| | | 4 | | // sight's rotation rate to steer the agent towards its target. |
| | | 5 | | // The controller uses pure proportional navigation, where the acceleration input is normal to |
| | | 6 | | // the agent's velocity. |
| | | 7 | | // Refer to "Basic Principles of Homing Guidance" by N. F. Palumbo (2010) for more information. |
| | | 8 | | public class PnController : ControllerBase { |
| | | 9 | | private const float _epsilon = 1e-3f; |
| | | 10 | | |
| | | 11 | | // Multiplication factor applied to the closing velocity when the target is moving away or abeam |
| | | 12 | | // to force an aggressive turn and prevent spiraling or divergence. |
| | | 13 | | private const float _strongTurnFactor = 100f; |
| | | 14 | | |
| | | 15 | | // Cosine threshold for determining whether the target is abeam to the agent and preventing spiral |
| | | 16 | | // behavior. A value of 0.2 corresponds to roughly +-10 degrees around 90 degrees. |
| | | 17 | | private const float _abeamThreshold = 0.2f; |
| | | 18 | | |
| | | 19 | | // Minimum line-of-sight rate used for clamping when the target is abeam to the agent. |
| | | 20 | | private const float _minLosRate = 0.2f; |
| | | 21 | | |
| | 35 | 22 | | public float Gain { get; set; } |
| | | 23 | | |
| | 30 | 24 | | public PnController(IAgent agent, float gain) : base(agent) { |
| | 15 | 25 | | Gain = gain; |
| | 15 | 26 | | } |
| | | 27 | | |
| | | 28 | | // Controller-dependent implementation of the control law. |
| | 14 | 29 | | protected override Vector3 Plan(in Transformation relativeTransformation) { |
| | 14 | 30 | | Vector3 relativePosition = relativeTransformation.Position.Cartesian; |
| | 14 | 31 | | Vector3 relativeVelocity = relativeTransformation.Velocity.Cartesian; |
| | 14 | 32 | | Vector3 normalRelativeVelocity = Vector3.ProjectOnPlane(relativeVelocity, relativePosition); |
| | | 33 | | |
| | 14 | 34 | | float distance = relativeTransformation.Position.Range; |
| | 14 | 35 | | if (distance < _epsilon) { |
| | 0 | 36 | | return Vector3.zero; |
| | | 37 | | } |
| | | 38 | | |
| | | 39 | | // Line-of-sight unit vector. |
| | 14 | 40 | | Vector3 losRHat = relativePosition / distance; |
| | | 41 | | // Line-of-sight rate vector. |
| | 14 | 42 | | Vector3 losVHat = normalRelativeVelocity / distance; |
| | 14 | 43 | | Vector3 losRotation = Vector3.Cross(losRHat, losVHat); |
| | | 44 | | |
| | | 45 | | // The closing velocity is negative because the closing velocity is opposite to the range rate. |
| | 14 | 46 | | float closingVelocity = -relativeTransformation.Velocity.Range; |
| | 14 | 47 | | float closingSpeed = Mathf.Abs(closingVelocity); |
| | | 48 | | // Set the turn factor, which is equal to the closing velocity by default. |
| | 14 | 49 | | float turnFactor = closingVelocity; |
| | | 50 | | // Handle a negative closing velocity. If the target is moving away from the agent, negate the |
| | | 51 | | // turn factor and apply a stronger turn as the agent most likely passed the target already and |
| | | 52 | | // should turn around. |
| | 15 | 53 | | if (closingVelocity < 0) { |
| | 1 | 54 | | turnFactor = Mathf.Max(1f, closingSpeed) * _strongTurnFactor; |
| | 1 | 55 | | } |
| | | 56 | | // If the target is abeam to the agent, apply a stronger turn and clamp the line-of-sight rate |
| | | 57 | | // to avoid spiral behavior. |
| | 14 | 58 | | bool isAbeam = Mathf.Abs(Vector3.Dot(Agent.Velocity.normalized, relativePosition.normalized)) < |
| | | 59 | | _abeamThreshold; |
| | 18 | 60 | | if (isAbeam) { |
| | 4 | 61 | | turnFactor = Mathf.Max(1f, closingSpeed) * _strongTurnFactor; |
| | 4 | 62 | | float clampedLosRate = Mathf.Max(losRotation.magnitude, _minLosRate); |
| | 4 | 63 | | losRotation = losRotation.normalized * clampedLosRate; |
| | 4 | 64 | | } |
| | 14 | 65 | | return Gain * turnFactor * Vector3.Cross(losRotation, Agent.Velocity.normalized); |
| | 14 | 66 | | } |
| | | 67 | | } |