简易雷达避障效果实现
程序员文章站
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