[Python|自动驾驶|ADAS]曲线轨迹设计

文章仅限交流使用,软件专利受法律保护,请勿侵权

内容介绍

下面的图是去年做的一个小软件，主要用于规划和编辑轨迹，提供给驾驶机器人或无人平台小车用。采用最基本的匀变速直线、弧线、螺旋线、双移线等基本曲线类型或者组合的方式来构建目前ADAS测试标准里面要求的测试场景。但是原来的软件架构没有做好，尤其是曲线轨迹的模式没有设计好，导致后面扩展困难、维护成本很高。

现在在湖北家里被困了一个多月了，之前一直在自学设计模式，疫情期间就着手从新架构一版，方便以后根据试验标准扩展曲线轨迹。

新版软件大致效果如下，起始主要是参考Prescan这个软件做的修改，相比于原来软件版本，现在的每种基本曲线轨迹类型都有自己的单独的属性，并且也是实现了标准测试轨迹与基本轨迹的统一（原来的版本没有统一，标准测试轨迹在“Openstd”按钮中单独打开，每次做一种新的标准轨迹就要重新设计一个对话窗口，很烦）：

曲线描述

AbstractDescription

用于一段轨迹的描述。

• 成员属性

private id: int
该段轨迹的id, 同名属性访问和修改

private state: str
该段轨迹的状态, 同名属性访问和修改

private keywords: List[str]
实际用于描述该段轨迹的可选参数序列, 同名属性访问和修改

private validFlag: bool
该段轨迹描述的有效标志位, 同名属性访问和修改

public duration: float
轨迹从起点到终点消耗时间

• 接口函数

public abstract str typename():
返回当前Description类型；

public abstract int optionalnumbers():
需要描述Description可选参数的个数；

public abstract void checkrequired(**kwargs):
对描述Description的必须参数进行校核；

public abstract void checkoptional(**kwargs):
对描述Description的可选参数进行校核；

public abstract float calculatespeedstart():
计算当前轨迹的初始速度；

public abstract float calculatespeedend():
计算当前轨迹的终止速度；

public abstract float calculateacceleration():
计算当前轨迹的加速度或用于描述当前轨迹的加速度关键参数；

public abstract float calculatecurvelength():
计算当前轨迹的路程；

public abstract float calculateduration():
计算轨迹起点到终点消耗时间；

public abstract float calculateangle():
计算轨迹的圆心角；

public abstract str calculatestate():
计算轨迹状态，默认实现匀变速状态；

public abstract void checklateralacceleration():
校核侧向加速度；

public abstract dict requiredUItransfer():
必选参数与在UI显示接口信息；

public abstract dict optionalUItransfer():
可选参数与在UI显示接口信息；

public abstract numpy.ndarray calculatecoordinate(int id, float timeStepSize):
id: 当前轨迹id号；
timeStepSize: 计算时间步长；
核心方法，计算给定的Description的轨迹坐标信息，返回numpy矩阵。各列信息[x, y, velocity, heading, headingx, headingy, time, acceleration, distance, id]依次为：x坐标，y坐标，航向角，航向角x轴分量，航向角y轴分量，时间，加速度，距离，当前轨迹id号。默认实现一个匀变速运动的轨迹坐标计算；

public abstract tuple headingandxy(float distance):
distance: 轨迹上从起始点到当前点距离；
计算距离起始点给定距离处的点的坐标和航向角

public abstract Trigger gettrigger():
获得当前Description轨迹的触发模式

public void checkmovingforward():
校核当前轨迹无倒退运动

public void checklongitudinalacceleration():
校核纵向加速度；

private void setrequired(**kwargs):
设置必选参数；

private void setoptional(**kwargs):
设置可选参数；

public dict getrequired():
获得必选参数；

public dict getoptional():
设置可选参数；

public void clearoptional():
清除当前可选参数；

public void setattributes(dict requiredKwargs, dict, optionalKwargs):
调用setrequired(**kwargs)和setoptional(**kwargs)同时设置可选参数和必选参数；

public dict getdescriptionkwargs():
获得当前Description的所有必选参数和可选参数的键值；

public str inistate():
返回初始状态；

LineDescription(AbstractDescription)

描述一条匀变速运动的直线轨迹；

• 必选成员属性

public speedStart: float
轨迹初始速度；

• 可选成员属性

public speedEnd: float
轨迹末速度；

public distance: float
轨迹距离；

public acceleration: float
轨迹加速度

ArcDescription(AbstractDescription)

用于描述一条匀变速运动的圆弧轨迹

• 必选成员属性

public speedStart: float
轨迹初速度；

public radius: float
圆弧半径；

• 可选成员属性

public speedEnd: float
轨迹末速度；

public distance: float
轨迹距离；

public acceleration: float
轨迹加速度

public angle: float
圆心角

ClothoidDescription(AbstractDescription)

用于描述一条匀变速运动的回旋曲线

• 必选成员属性

public speedStart: float
轨迹初始速度；

public curveStart: float
轨迹起点曲率

public curveEnd: float
轨迹终点曲率

• 可选成员属性

public speedEnd: float
轨迹末速度；

public distance: float
轨迹距离；

public acceleration: float
轨迹加速度

SimpleCutInDescription(AbstractDescription)

用于描述一条匀变速运动的切入切出轨迹；

• 必选成员属性

public speedStart: float
轨迹初速度；

public radius: float
圆弧半径；

public laneWidth: float
变道宽度；

• 可选成员属性

public speedEnd: float
轨迹末速度；

public acceleration: float
轨迹加速度

PauseDescription(AbstractDescription)

用于描述一条暂停轨迹；

• 必选成员属性

public pauseTime: float
暂停时间；

• 可选成员属性

无

NCAP_CCRmDescription(AbstractDescription)

用于描述一条NCAP AEB CCRm测试轨迹；

• 必选成员属性

public acceleration: float
加速段ATP加速度；

public ATPSpeed: float
ATP匀速段速度；

public deceleration: float
减速段ATP减速度；

public VUTSpeed: float
VUT测试车速；

public stayLength: float
ATP碰撞后继续匀速行驶距离；

public observation: float
ATP加速至测试速度后行驶至与VUT相撞用时；

private triggerPattern: Trigger
该测试轨迹的触发模式；

• 可选成员属性

无

NCAP_CCRbDescription(AbstractDescription)

用于描述一条NCAP AEB CCRb测试轨迹；

• 必选成员属性

public acceleration: float
加速段ATP加速度；

public deceleration: float
减速段ATP减速度；

public VUTSpeed: float
VUT测试车速；
public stayLength: float
ATP碰撞后继续匀速行驶距离；

public constDistance: float
VUT与ATP相对静止行驶时车间距；

private triggerPattern: Trigger
该测试轨迹的触发模式；

• 可选成员属性

无

曲线描述工厂

AbstractDescriptionCreater

用于创建一个Description类型

• 成员属性

无

• 接口函数

public AbstractDescription descriptioncreater():
创建一个具体的Description类

LineCreater(AbstractDescriptionCreater)

用于创建一个LineDescription类型；

ArcCreater(AbstractDescriptionCreater)

用于创建一个ArcDescription类型

ClothoidCreater(AbstractDescriptionCreater)

用于创建一个ClothoidDescription类型

SimpleCutInCreater (AbstractDescriptionCreater)

用于创建一个SimpleCutInCreater类型

PauseCreater(AbstractDescriptionCreater)

用于创建一个PauseDescription类型

ClothoidCreater(AbstractDescriptionCreater)

用于创建一个ClothoidDescription类型

NCAP_CCRmCreater(AbstractDescriptionCreater)

用于创建一个NCAP_CCRmDescription类型

NCAP_CCRbCreater(AbstractDescriptionCreater)

用于创建一个NCAP_CCRbDescription类型

触发模式

AbstractTrigger

用于描述不同的触发模式

• 接口函数

public abstract str triggername():
返回Trigger类型名称；

public abstract void checktrigger():
校核当前触发模式的参数设置；

public abstract dict triggervalues():
获取当前触发模式的所有参数；

public abstract void settrigger(dict triggerargs):
设置触发模式的所有参数；

DistanceTrigger(AbstractTrigger)

• 成员属性

public radius: float
触发半径；

public triggerSpeed: float
触发速度；

GateTrigger(AbstractTrigger)

• 成员属性

public lateralOffset: float
虚拟触发门中心点相对于ATP的侧向位移，左侧为正，右侧为负；

public longitudeOffset: float
虚拟触发门中心点相对于ATP的纵向位移，向前为正，前后为负；

public headingAngle: float
虚拟触发门朝向相对于ATP朝向的夹角，逆时针为正，顺时针为负；

public gateWidth: float
虚拟触发门宽度；

public triggerSpeed: float
触发速度；

触发模式工厂

AbstractTriggerCreater

用于创建一个Trigger类型

• 接口函数

public AbstractTrigger createtrigger():
创建一个具体的Trigger类

DistanceTriggerCreater(AbstractTriggerCreater)

用于创建一个DistanceTrigger类型；

• 类属性

无

• 成员属性

无

GateTriggerCreater(AbstractTriggerCreater)

用于创建一个GateTrigger类型；

轨迹管理者

AbstractTrajectoryHandler

采用责任链模式校核一条轨迹

• 成员属性

private nextHandler: AbstractTrajectoryHandler
下一个处理者；

• 接口函数

public AbstractTrajectoryHandler nextHandler ():
返回下一个责任人；

public abstract dict currentHandler():
返回当前责任人id与名称;

public abstract bool process(HandlerInput handlerInput, func)
handlerInput: 处理者输入，见HandlerInput类；
func: 用于处理异常信息的函数地址；
责任人对当前输入进行处理，处理成功返回真；

CheckAttributesHandler(AbstractTrajectoryHandler)

第一位责任人，处理轨迹参数设置，对接Description::setattributes()；

CheckDurationHandler(AbstractTrajectoryHandler)

第二位责任人，处理轨迹耗时计算，对接Description::calculateduration()；

CheckMovingForwardHandler(AbstractTrajectoryHandler)

第三位责任人，校核轨迹前进，对接Description:: checkmovingforward()；

CheckAccelerationHandler (AbstractTrajectoryHandler)

第四位责任人，校核加速度，对接Description:: checklongitudinalacceleration()和Description:: checklateralacceleration()；

管理输入信息

HandlerInput

TrajectoryHandler的输入信息

• 成员属性

public description: AbstractDescription
描述的轨迹；

public requiredKwargs: dict
描述轨迹的必选参数键值对

public optionalKwargs: dict
描述轨迹的可选参数键值对

轨迹报头

Header

用于记录轨迹中的一些附加信息

• 必选成员属性

public timeStepSize: float
指定计算轨迹坐标信息时采用的时间步长

• 可选成员属性

public trajectoryName: str
轨迹名称

public maxRec: str
修改版本

public minRec: str
修改版本

public author: str
发布者

public date: str
发布日期

public additional: str
附加信息

轨迹合成

Trajectory

用于将多个Description合成为一条完整的轨迹

• 成员属性

private header: Header
轨迹报头, 同名属性访问

private descriptions: Dict[int, AbstractDescription]
用于存放各段轨迹的字典, 以描述轨迹的id为区分,同名属性访问

private id: Set
用于存放所有轨迹描述的id的集合,同名属性访问

private trajectoryData: numpy.ndarray
完整轨迹的所有解算信息[x, y, velocity, heading, headingx, headingy, time, acceleration, distance, id],依次为x坐标,y坐标,航向角,航向角x分量,y分量,时间,加速度,距离和描述轨迹id,同名属性访问

private triggerPattern: AbstractTrigger
完整轨迹的触发模式,同名属性访问

private validFlag: bool
完整轨迹的有效标志位, 同名属性访问

• 接口函数

public void cleartrajectory():
清除当前Trajectory存储的所有的description的信息

public void adddescription():
往当前Trajectory增加一条description

public void removedescription(int id):
将Trajectory中指定id的description清除掉

public void replacedescription(AbstractDescription description):
将当前在Trajectory中description.id位置的轨迹描述替换为description

public numpy.ndarray clonedtrajectoryData():
返回trajectoryData的副本

public bool alternextspeedstart(int id):
判断是否需要更改(id+1)的description的初速度,用于保证id和id+1的两条description的速度不发生突变

public bool alterpreviousspeedend(int id):
判断是否需要更改(id-1)条描述轨迹的末速度,用于保证id和id-1的两条description的速度不发生突变

public List[int] checkspeedconsensus():
返回完整轨迹中所有速度发生突变的description的id

public List[int] checkvalidflag():
返回完整轨迹中所有有效标志位为假的description的id

public numpy.ndarray trajectoriescoordinate():
将第一个状态为initialstate之前的所有非initialstate的description通过坐标变换组合成完整的轨迹,计算,修改并返回trajectoryData