unity 简易雷达避障效果实现

效果视频:

简易雷达避障车

绿色为轨迹，每个绿色小球表示一个路标点；方块用来当做障碍物。

车上应挂载的代码：

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 framevoid 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中设置如下：

最后设置车：

全套资源链接：/download/weixin_48592526/12789688