carmaker/matlab联合仿真(三) 生成车道线点，拟合多项式方程
carmaker/matlab联合仿真(三) 生成车道线点，拟合多项式方程
win10
matlab 2020b
carmaker 10.2.2
在carmaker/matlab仿真中，carmaker提供了摄像头、雷达等传感器模型，能够将场景中在传感器覆盖范围内的目标传送到simulink中供给算法模块利用。但是对于车道线信息，Line Sensor只能将车道类型、距离等属性信息，缺少对于车道线本身点的位置信息。而对于有横向控制的算法功能如LKA等就需要提供车道线的点或者车道线拟合成的多项式方程。对于这种情况，可以通过carmaker的拓展功能将当前车道左右侧车道线的点以UAQ的形式传送到simulink中，再做进一步的拟合，以便后续功能模块使用。
还是以前本系列前两篇内容中生成的工程文件为基础做演示。
1 由于在第一篇中创建工程时只勾选了Carmaker for Simulink Extras的选项，文件夹中缺少拓展的模块。所以需要重新在创建一次工程以补充缺少的模块。在工程文件夹下打开的simulink模型中通过点击GUI图标打开carmaker主界面。点击File -> Project Folder -> Create Project，将工程文件夹切换到之前建立的文件夹处，然后勾选所有五个选项，点击OK。警告框中选择Continue。此时在项目文件夹下会多出来一个src文件夹。
2 打开工程文件夹下的src文件夹，发现这是一个visual studio的工程文件。使用visual studio打开CarMaker.sln文件。选择项目->重定目标解决方案。在跳出的重定向目标中点击确定。点击生成->生成解决方案，在输出框中会出现生成成功的字样。
3 将下面三个文件放到src文件夹下，替换原来的文件。
User.c
/*
******************************************************************************
** CarMaker - Version 10.2.2
** Vehicle Dynamics Simulation Toolkit
**
** Copyright (C) IPG Automotive GmbH
** Bannwaldallee 60 Phone +49.721.98520.0
** 76185 Karlsruhe Fax +49.721.98520.99
** Germany WWW www.ipg-automotive.com
******************************************************************************
**
** Functions
** ---------
**
** Initialization
**
** User_Init_First ()
** User_PrintUsage ()
** User_ScanCmdLine ()
**
** User_AppLogFilter ()
**
** User_Init ()
** User_Register ()
** User_DeclQuants ()
**
** User_Param_Add ()
** User_Param_Get ()
**
**
** Main TestRun Start/End:
**
** User_TestRun_Start_atBegin ()
** User_TestRun_Start_atEnd ()
** User_TestRun_Start_StaticCond_Calc ()
** User_TestRun_Start_Finalize ()
** User_TestRun_RampUp ()
**
** User_TestRun_End_First ()
** User_TestRun_End ()
**
**
** Main Cycle:
**
** User_In ()
**
** User_DrivMan_Calc ()
** User_Traffic_Calc ()
** User_VehicleControl_Calc ()
** User_Brake_Calc () in Vhcl_Calc ()
** User_Calc ()
** User_Check_IsIdle ()
**
** User_Out ()
**
**
** APO Communication:
**
** User_ApoMsg_Eval ()
** User_ApoMsg_Send ()
**
** User_ShutDown ()
** User_End ()
** User_Cleanup ()
**
**
******************************************************************************
*/
#include )
*/
int
User_TestRun_End (void)
{
return 0;
}
/*
** User_In ()
**
** Assign quantities of the i/o vector to model variables
**
** Call:
** - in the main loop
** - pay attention to realtime condition
** - just after IO_In()
*/
void
User_In (const unsigned CycleNo)
{
if (SimCore.State != SCState_Simulate)
return;
}
/*
** User_DrivMan_Calc ()
**
** called
** - in RT context
** - after DrivMan_Calc()
*/
int
User_DrivMan_Calc (double dt)
{
/* Rely on the Vehicle Operator within DrivMan module to get
the vehicle in driving state using the IPG's
PowerTrain Control model 'Generic' or similar */
if (Vehicle.OperationState != OperState_Driving)
return 0;
return 0;
}
/*
** User_VehicleControl_Calc ()
**
** called
** - in RT context
** - after VehicleControl_Calc()
*/
int
User_VehicleControl_Calc (double dt)
{
/* Rely on the Vehicle Operator within DrivMan module to get
the vehicle in driving state using the IPG's
PowerTrain Control model 'Generic' or similar */
if (Vehicle.OperationState != OperState_Driving)
return 0;
return 0;
}
/*
** User_Brake_Calc ()
**
** called
** - in RT context
** - after Brake_Calc() in Vhcl_Calc()
*/
int
User_Brake_Calc (double dt)
{
/* Modify the total brake torque from the brake system model Brake.Trq_tot[]
or the target drive source torque from the brake control unit
Brake.HydBrakeCU_IF.Trq_DriveSrc_trg[]
*/
return 0;
}
/*
** User_Traffic_Calc ()
**
** called
** - in RT context
** - after Traffic_Calc()
*/
int
User_Traffic_Calc (double dt)
{
if (SimCore.State != SCState_Simulate)
return 0;
return 0;
}
/*
** User_Calc ()
**
** called in RT context
*/
int
User_Calc (double dt)
{
/* Starting with CM 6.0 User_Calc() will be invoked in EVERY simulation
state. Uncomment the following line in order to restore the behaviour
of CM 5.1 and earlier. */
/*if (!UserCalcCalledByAppTestRunCalc) return 0;*/
int rv;
rv = Lineds_Calc(dt);
return rv;
}
/*
** User_Check_IsIdle ()
**
** Checking, if the simulation model is in idle conditions (stand still,
** steeringwheel angle zero, cluch pedal pressed, ...).
** If reached idle state, the calculation of vehicle model and driving
** manoevers is stopped.
** Ready for start new simulation.
**
** Return:
** 1 idle state reached
** 0 else
**
** Call:
** - in main task, in the main loop
** - pay attention to realtime condition
** - while SimCore.State==SCState_EndIdleGet
*/
int
User_Check_IsIdle (int IsIdle)
{
double val;
/*** ECU / carmodel signals */
/* vehicle and wheels: stand still */
val = 0.5*kmh2ms;
if (Vehicle.v > val
|| fabs(Vehicle.Wheel[0]->vBelt) > val || fabs(Vehicle.Wheel[1]->vBelt) > val
|| fabs(Vehicle.Wheel[2]->vBelt) > val || fabs(Vehicle.Wheel[3]->vBelt) > val) {
IsIdle = 0;
}
/* SteerAngle: drive straight forward position */
val = 1.0*deg2rad;
if (Vehicle.Steering.Ang > val || Vehicle.Steering.Ang < -val)
IsIdle = 0;
return IsIdle;
}
/*
** User_Out ()
**
** Assigns model quantities to variables of the i/o vector
**
** call:
** - in the main loop
** - pay attention to realtime condition
** - just before IO_Out();
*/
void
User_Out (const unsigned CycleNo)
{
RBS_OutMap(CycleNo);
if (SimCore.State != SCState_Simulate)
return;
}
/*
** User_ApoMsg_Eval ()
**
** Communication between the application and connected GUIs.
** Evaluate messages from GUIs
**
** Call:
** - in the main loop
** - pay attention to realtime condition
** - near the end of the main loop, if the function SimCore_ApoMsg_Eval()
** skips the message
**
** Return:
** 0 : message evaluated
** -1 : message not handled
*/
int
User_ApoMsg_Eval (int Ch, char *Msg, int len, int who)
{
#if defined(CM_HIL)
/*** FailSafeTester */
if (Ch == ApoCh_CarMaker) {
if (FST_ApoMsgEval(Ch, Msg, len) <= 0)
return 0;
}
#endif
return -1;
}
/*
** User_ApoMsg_Send ()
**
** Communication between the application and connected GUIs.
** Sends messages to GUIs
**
** Call:
** - near the end of the main loop, in MainThread_FinishCycle()
** - pay attention to realtime condition
*/
void
User_ApoMsg_Send (double T, const unsigned CycleNo)
{
}
/*
** User_ShutDown ()
**
** Prepare application for shut down
**
** Call:
** - at end of program
** - no realtime conditions
*/
int
User_ShutDown (int ShutDownForced)
{
int IsDown = 0;
/* Prepare application for shutdown and return that
shutdown conditions are reached */
if (1) {
IsDown = 1;
}
return IsDown;
}
/*
** User_End ()
**
** End all models of the user module
**
** Call:
** - one times at end of program
** - no realtime conditions
*/
int
User_End (void)
{
return 0;
}
/*
** User_Cleanup ()
**
** Cleanup function of the User module
**
** Call:
** - one times at end of program, just before exit
** - no realtime conditions
*/
void
User_Cleanup (void)
{
}
Lineds.h
typedef struct tLds tLds;
struct tLds {
struct {
struct {
double x;
double y;
double z;
} p1, p2,p3,p4,p5,p6,p7,p8,p9,p10, p11, p12, p13, p14, p15, p16, p17, p18, p19, p20;
} L1, L2, R1, R2;
};
extern tLds Lds;
int Lineds_TestRun_Start_atEnd (void);
void Lineds_DeclQuants (void);
int Lineds_Calc (double dt);
Lineds.c
#include "Lineds.h"
#include 4 打开src文件夹下的Makefile文件，将
OBJS = CM_Main.o CM_Vehicle.o User.o
改为
OBJS = CM_Main.o CM_Vehicle.o User.o Lineds.o
5 打开工程文件CarMaker.sln，将Lineds.c加入到工程源文件列表中，点击生成->重新生成解决方案，成功的话会生成CarMaker.win64.exe文件。
6 通过快捷方式或者开始菜单打开carmaker,点击Application->Configuration/Status,在Command(executable)下面的框中选择生成的CarMaker.win64.exe文件，点击start&connect,然后点击close
7 回到主界面后，点击Application -> Direct variable access,按住Quantity下的向下箭头就能看到L1\L2\R1\R2四条车道线的x,y,z的坐标点，可以选取几个进行观察。关闭carmaker。
8 通过simulink模型中的CM图标打开carmaker，点击File->Open打开之前我们新建的testrun。
9 打开IPGMovie观察动画，打开Direct Variable Access观察变量。点击Start开始运行仿真。可以看到已经能够读取车道线的坐标点。
10 在simulink模型中接收车道线点的数据。simulink模型中集成控制算法的位置为generic/CarMaker/VehicleControl/CreateBus VhclCtrl，所以在这里接受车道线的数据，通过Read CM Dict模块读取carmaker变量。
11 拟合左右车道线数据，生成车道线方程
使用matlab自带的拟合函数，一般采用三次拟合的方式，即可输出车道线方程。
function [l1c,r1c] = fcn(l1x,l1y,r1x,r1y)
l1c = polyfit(l1x,l1y,3);
r1c = polyfit(r1x,r1y,3);
end
运行效果如图
