npc wp planner in ADS simulation

Posted on |

background

a ADS simulation platform should have the three components:

  • vehicle dynamics model

for game engine(e.g. unReal, Unity3D) based ADS simulation platform, there has simple game-level vehicel dynamics(VD), and in engineering side, there are more precise VD, such as Carsim, CarMaker, and a few smaller ones.

  • sensor models

currently camera and Lidar model are pretty good, for mm Radar, which has a frequency around 24G hz or 77G hz, which is far more beyond the digital computer can sample, so to simulate Radar(or Radar model) is not that practicalable.

as can find in famous simulator, e.g. lg-sim and Carla, they are puting a lot efforts in sensor models, which should be more useful in future.

another good way of sensor model simulation, is to test and verify the vendor’s sensors. most time, the performance test from vendors are not fit well. and as the physical model of the sensors is not visible, to simualate as a black box is useful then.

  • scenario describe

PEGSUS project used OpenScenario xml language; in other case, the scenario can be described directly in Python.

basically what need to define during scenario descibtion, includes:

  • scenario static env
  • scenario npc actors movement

in more advanced solutions, the process of builing virtual envs directly start from dronze/cars images or cloud point images.

what’s in this post is one part of npc movement control: lane change.

lane_change_done event

at default lg-sim, there is a lane-change event, which describes when lane change happens; for a more precise npc control, here adds a lane-chagne-done event, basically to describe when the lane change is done.

similar to lane-change event, here defines the lane-change-done event in both server and client side.

Unity.transform.position vs local position

here is the math part. Unity has plenty APIs to handle these, which is good to study in.

  • dot product 

    Vector3.Dot()

  • cross product

  • p2p distance

    Vector3.Distance()

  • interpolate

    Mathf.Lerp()

  • world 2 local transfer

    Transform.InverseFromPoint(position)

  • camera matrix

  • equations of lines

waypoints described lane

NPC agents are drived by waypoints embedded in the virtual roads, in lg-sim, waypoints is inside LaneSegmentBuilder. but in general the waypoints are not ideal:

  • their distance is not unique
  • their index in neighboring lanes doesn’t keep consistent
  • they don’t care of curves of real roads

for waypoints based planner, rather AI-based planner, e.g. lane-change planner, the processes are as following:

  • get currentTarget in target(current) lane

which is usually not pointed by the same currentIndex as the previous lane, so need to figure out the closeset wp from the waypoint list in current lane, and this closest wp should be in front of NPC (no consider NPC retrograde).

  • keep currentTarget update

during NPC (lane-change) operation, there is case when the currentTarget is behind NPC position, if it’s not expected, it’s always a error planner, leading NPC header turn over. so need to check currentTarget is always in front of NPC, if not, update currentTarget to next wp.

1) driving direction vs currentTarget2NPCposition

driving direction in global dot product currentTarget2NPCposition should greater than zero, if not, update currentTarget

2) NPC in currentTarget local position

NPC local position should always in negative axis, if not, update currentTarget. the trick here, can’t use currentTarget in nPC local position, as when NPC is head of currentTarget, NPC will be pointed in reverse, which makes the local coordinate reverse as well. but currentTarget is the fixed wp always.

  • lane-change-done criteria

ideally when the whole NPC occupied in the target lane, that’s when the lane-change operation done. but that’s never the reality. as then, if there is the next next lane, the NPC is partly in the next next lane, so can’t keep lane change in only one neighboring lane.

but in reality, highway or express way, the vehicle can’t across two lane in one time. so to keep lane-change-done and done in just the next lane, the criteria is when the NPC position in Z or X direction projection to currentTarget X or Z direction projection is more than half of the lane width:

Mathf.Abs(frontCenter.position.z - currentTarget.z) < laneWidth/2.0

usually in game engine, NPC can be controlled by AI, will discuss later.