欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页  >  移动技术

简易雷达避障效果实现

程序员文章站 2022-03-19 22:38:21
先上图:绿色为轨迹,每个绿色小球表示一个路标点;方块用来当做障碍物。车上应挂载的代码:using System;using System.Collections;using System.Collections.Generic;using TrackedSensorCar;using UnityEngine;[RequireComponent(typeof(WayPath))]public class CarEngine2 : MonoBehaviour{ [Tooltip("...

效果视频:

简易雷达避障车

绿色为轨迹,每个绿色小球表示一个路标点;方块用来当做障碍物。
车上应挂载的代码:

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);
    //    }
    //}

}

unity Editor中设置如下:
简易雷达避障效果实现

最后设置车:
简易雷达避障效果实现

全套资源链接:https://download.csdn.net/download/weixin_48592526/12789688

本文地址:https://blog.csdn.net/weixin_48592526/article/details/109259290