简易雷达避障效果实现

程序员文章站 2022-05-21 23:17:25

...

先上图：简易雷达避障效果实现
绿色为轨迹，每个绿色小球表示一个路标点；方块用来当做障碍物。
车上应挂载的代码：

using System;
using System.Collections;
using System.Collections.Generic;
using TrackedSensorCar;
using UnityEngine;

[RequireComponent(typeof(WayPath))]
public class CarEngine2 : MonoBehaviour
{
    [Tooltip("雷达所在")]
    [SerializeField] private LidarScan lidar;
    [Tooltip("存放规划路径目标点")]
    [SerializeField] private WayPath wayPath;
    [Tooltip("4个车轮碰撞体")]
    [SerializeField] public WheelCollider[] m_WheelColliders = new WheelCollider[4];
    [Tooltip("4个车轮")]
    [SerializeField] private GameObject[] m_Wheels = new GameObject[4];
    [Tooltip("驱动扭矩")]
    [SerializeField] private float m_FullTorqueOverAllWheels;
    [Tooltip("最大转向角")]
    public float m_MaxSteerAngle = 30f;
    [Tooltip("躲避障碍物的转向速度")]
    [SerializeField] private float trunSpeed = 10f;    //转向速度

    [Tooltip("第一个目标点为wayPoint的第几个点？（从0开始）")]
    [SerializeField] private int m_PosPoint = 0;
    [Tooltip("距离目标位置为多少时切换下一个目标")]
    [SerializeField] private float AcceptableDistance;
    private Vector3 targetPos;
    private float m_CurrentSteerAngle { get; set; }
    private Vector3 relativePos { get; set; }
    private void Start()
    {
        //transform.position = wayPath.wayPoints[0].position;
        //transform.LookAt(wayPath.wayPoints[1].position);
        targetPos = wayPath.wayPoints[m_PosPoint].position; // 根据自主设置的位标确定第一目标点
    }
    // Update is called once per frame
    void Update()
    {
        //保持轮胎网络跟随wheelcollider转动
        for (int i = 0; i < m_Wheels.Length; i++)
        {
            m_WheelColliders[i].GetWorldPose(out Vector3 position, out Quaternion rotation);
            m_Wheels[i].transform.position = position;
            m_Wheels[i].transform.rotation = rotation;
        }
    }
    private void FixedUpdate()
    {
        ApplySteer();
        HeadForTarget();    //朝目标行驶
        ChangeTarget(); //行驶到目标位置后切换下一目标
    }
    void ApplySteer()
    {
        //获取水平面方向上的相对位置
        relativePos = transform.InverseTransformPoint(targetPos);
        relativePos = new Vector3(relativePos.x, 0, relativePos.z); //去除y方向(垂直方向)上的偏移
        Debug.Log(relativePos);
        if (lidar.avoid)    //躲避障碍物
        {
            float lerpToSteerAngle = Mathf.Lerp(m_CurrentSteerAngle, m_MaxSteerAngle * lidar.steer, Time.deltaTime * trunSpeed);    //是转向平滑
            m_CurrentSteerAngle = lerpToSteerAngle;
        }
        else   //如果未检测到障碍物
        {
            //计算相对角度，并转向
            m_CurrentSteerAngle = Mathf.Asin(relativePos.x / relativePos.magnitude) / Mathf.PI * 180;
        } 

        m_WheelColliders[0].steerAngle = m_CurrentSteerAngle;
        m_WheelColliders[1].steerAngle = m_CurrentSteerAngle;
        //Debug.Log($"Avoid:{lidar.avoid},m_CurrentSteerAngle:{m_CurrentSteerAngle}");

    }
    void HeadForTarget()
    {
        //四轮驱动
        float thrustTorque = m_FullTorqueOverAllWheels / 4f;
        for (int i = 0; i < 4; i++)
        {
            if (lidar.avoid)
            {
                m_WheelColliders[i].motorTorque = thrustTorque / 2; //躲避障碍时驱动减半
            }
            else
            {
                m_WheelColliders[i].motorTorque = thrustTorque;
            }
            //m_WheelColliders[i].motorTorque = thrustTorque;
        }
    }
    void ChangeTarget()
    {
        if (wayPath != null && relativePos.magnitude< AcceptableDistance)
        {
            if (++m_PosPoint > wayPath.wayPoints.Length - 1)    //当到达最后一个目标点时
            {
                if (wayPath.looped)     //如果设置为loop循环
                {
                    m_PosPoint = 0; //则重新从第一个路点开始
                }
                else
                {
                    m_FullTorqueOverAllWheels = 0;  //否则熄火关驱动
                    foreach(WheelCollider wheelCollider in m_WheelColliders)
                    {
                        wheelCollider.brakeTorque = float.MaxValue;
                    }
                }
            }
            targetPos = wayPath.wayPoints[m_PosPoint].position; //设置下一个目标点
        }
    }

    private void OnDrawGizmos()
    {
        //以前车轮中心为中心，AcceptableDistance为半径画球，显示探测范围（不是雷达）
        Gizmos.color = Color.red;
        Gizmos.DrawWireSphere(transform.position+transform.up*0.4f, AcceptableDistance);
        Gizmos.DrawSphere(targetPos, 0.15f);
    }
}

Hierarchy中单独建一个空物体命名为”WayPoints“，以后所有路标点都作为其子物体：
简易雷达避障效果实现
WayPoints的代码如下：

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class WayPath : MonoBehaviour
{
    public Transform[] wayPoints = null;
    //public List<Transform> pathNotes = new List<Transform>();

    [Tooltip("连线颜色")]
    [SerializeField] private Color lineColor = Color.blue;
    [Tooltip("是否首位相接")]
    [SerializeField] public bool looped;

    private void OnDrawGizmos()
    {
        Gizmos.color = lineColor;
        wayPoints = GetComponentsInChildren<Transform>();
        for (int i = 0; i < wayPoints.Length; i++)
        {
            Gizmos.DrawWireSphere(wayPoints[i].position, 0.1f);
            if (i > 0)
            {
                Gizmos.DrawLine(wayPoints[i - 1].position, wayPoints[i].position);
            }
        }
        if (looped)
        {
            Gizmos.DrawLine(wayPoints[wayPoints.Length - 1].position, wayPoints[0].position);
        }
    }

    //public Transform target;

    //void OnDrawGizmosSelected()
    //{
    //    if (target != null)
    //    {
    //        // Draws a blue line from this transform to the target
    //        Gizmos.color = Color.blue;
    //        Gizmos.DrawLine(transform.position, target.position);
    //        Gizmos.DrawSphere(transform.position, 0.1f);
    //    }
    //}

}