123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460 |
- using ProjectBase.Data.BaseDAL;
- using ProjectBase.Data.Logs;
- using ProjectBase.Data.Redis;
- using ProjectBase.Util;
- using SIASUN.Autopilot.GNSS;
- using SIASUN.Autopilot.Model;
- using System;
- using System.Collections.Generic;
- using System.Threading;
- using System.Threading.Tasks;
- namespace SIASUN.Autopilot.BLL
- {
- public class BlDecisionControler
- {
- /// <summary>
- /// 参考路径起始点
- /// </summary>
- private WGS84Points startPointRef = new WGS84Points();
- /// <summary>
- /// 当前路径起始点
- /// </summary>
- private WGS84Points startPointCur = new WGS84Points();
- /// <summary>
- /// 当前点
- /// </summary>
- private WGS84Points pointCur = new WGS84Points();
- // 当前速度
- private double SpeedCur;
- /// <summary>
- /// 地球的半径
- /// </summary>
- private const double EARTH_RADIUS = 6378.137;
- RedisHelper redis = new RedisHelper(0);
- List<MoLocationRuleInfo> locationList = new List<MoLocationRuleInfo>();
- private string PointsToOrigin = "pointstoorigin";
- /// <summary>
- /// 角度误差
- /// </summary>
- private const double angleError = 0.102;
- /// <summary>
- /// 速度误差
- /// </summary>
- private const double speedError = 0.103;
- /// <summary>
- /// 参考距离--用于动作指令判断
- /// </summary>
- private double distanceRefAction = 0;
- /// <summary>
- /// 参考角度--用于动作指令判断
- /// </summary>
- private double angleRefAction = 0;
- /// <summary>
- /// 参考速度--用于动作指令判断
- /// </summary>
- private double speedRefAction = 0;
- /// <summary>
- /// 执行到第几个动作指令
- /// </summary>
- private int count = 1;
- private bool bLock = true;
- //public event EventStartPointHandler StartPointHandler;
- public event EventHandler<StartPointEventArgs> StartPointHandler;
- public BlDecisionControler()
- {
- InitPlanRoute();
- BlGNSSPerception.Instance.GNSSHandler += Instance_GNSSHandler;
- }
- private void Instance_GNSSHandler(object sender, GNSSInfoEventArgs e)
- {
- LogHelper.log.Debug("控制指令算法执行");
- string msg = "";
- string start = redis.GetString(SysEnvironment.Start);
- if (start.Equals("1"))
- {
- if (Math.Abs(((double)e.Nmea.gpgga.Latitude - startPointRef.y)) > 0.0000001)// 验证起始点是否在10cm范围内
- {
- LogHelper.log.Debug("通过纬度验证起始点不在范围内");
- return;
- }
- if (Math.Abs(((double)e.Nmea.gpgga.Longitude - startPointRef.x)) > 0.0000001)
- {
- LogHelper.log.Debug("通过经度验证起始点不在范围内");
- return;
- }
- msg = "车辆运行到起始点。请开始测试!";
- LogHelper.log.Debug(msg);
- StartPointEventArgs message = new StartPointEventArgs(msg);
- if (StartPointHandler != null)
- {
- StartPointHandler(sender,message);
- }
- startPointCur.x = (double)e.Nmea.gpgga.Longitude;
- startPointCur.y = (double)e.Nmea.gpgga.Latitude;
- redis.SetString(SysEnvironment.Start, "0");
- }
- pointCur.x = (double)e.Nmea.gpgga.Longitude;
- pointCur.y = (double)e.Nmea.gpgga.Latitude;
- SpeedCur = (double)e.Nmea.gprmc.Speed;
- UpdateDirective();
- if (bLock)
- {
- bLock = false;
- Task task = new Task(() =>
- {
- ExcutePid();
- bLock = true;
- });
- task.Start();
- }
- }
- /// <summary>
- /// 获取规划路径的各个点到起始点的距离
- /// </summary>
- private double GetDistance(WGS84Points startPoint, WGS84Points endPoint)
- {
- double lat = startPoint.y - endPoint.y;
- double lon = startPoint.x - endPoint.x;
- double dis = 2 * Math.Asin(Math.Sqrt(Math.Pow(Math.Sin(lat / 2), 2) +
- Math.Cos(startPoint.y) * Math.Cos(endPoint.y) * Math.Pow(Math.Sin(lon / 2), 2)));
- dis = dis * EARTH_RADIUS;
- dis = Math.Round(dis * 1e4) / 1e4;
- return dis;
- }
- /// <summary>
- /// 求出任何一点到起始点的角度的tan值
- /// </summary>
- /// <param name="startPoint"></param>
- /// <param name="endPoint"></param>
- /// <returns></returns>
- private double GetTanAngle(WGS84Points startPoint, WGS84Points endPoint)
- {
- WGS84Points endPointX = new WGS84Points();
- endPointX.x = endPoint.x;
- endPointX.y = startPoint.y;
- double distanceX = GetDistance(startPoint, endPointX);
- WGS84Points endPointY = new WGS84Points();
- endPointY.x = startPoint.x;
- endPointY.y = endPoint.y;
- double distanceY = GetDistance(startPoint, endPointY);
- return distanceY / distanceX;
- }
- /// <summary>
- /// 计算点到线段的距离
- /// </summary>
- /// <param name="point">点的坐标</param>
- /// <param name="startPoint">线段起始点坐标</param>
- /// <param name="endPoint">线段终止点坐标</param>
- /// <returns></returns>
- private double GetPointLineDistance(WGS84Points point,WGS84Points startPoint, WGS84Points endPoint)
- {
- double a = GetDistance(startPoint,endPoint);
- double b = GetDistance(endPoint,point);
- double c = GetDistance(startPoint,point);
- if (b*b >= c*c + a*a)
- {
- return c;
- }
- if (c*c >= b*b + a*a)
- {
- return b;
- }
- double p = (a + b + c) / 2; //周长的一半
- double s = Math.Sqrt(p * (p - a) * (p - b) * (p - c)); //海伦公式求面积
- double dis = 2.0 * s / a;
- dis = Math.Round(dis * 1e4) / 1e4;
- return dis;
- }
- /// <summary>
- /// 计算各点到原点的距离和角度
- /// </summary>
- public void InitPlanRoute()
- {
- try
- {
- string condition = string.Format("rule_name = '{0}'", SysEnvironment.CurrentRuleName);
- locationList = BLLFactory<BlLocationRuleInfo>.Instance.Find(condition);
- startPointRef.x = locationList[0].LocationLon;
- startPointRef.y = locationList[0].LocationLat;
- startPointRef.z = locationList[0].LocationHeight;
- for (int i = 1; i < locationList.Count; i++)
- {
- WGS84Points endPoint = new WGS84Points();
- endPoint.x = locationList[i].LocationLon;
- endPoint.y = locationList[i].LocationLat;
- PointsToOrigin origin = new PointsToOrigin();
- origin.Height = endPoint.z = locationList[i].LocationHeight;
- origin.distance = GetDistance(startPointRef, endPoint);
- origin.TanAngle = GetTanAngle(startPointRef, endPoint);
- origin.Speed = locationList[i].LocationSpeed;
- redis.SetHash(PointsToOrigin, i.ToString(), origin);
- }
- PointsToOrigin Reference = new PointsToOrigin();
- Reference = redis.GetHash<PointsToOrigin>(PointsToOrigin, count.ToString());
- distanceRefAction = Reference.distance;
- angleRefAction = Reference.TanAngle;
- speedRefAction = Reference.Speed;
- }
- catch (Exception ex)
- {
- LogHelper.log.Error("计算规划的路径距离原点距离和Tan角度出现错误", ex);
- }
- }
- private int i = 1;
- /// <summary>
- /// 执行加减速和方向控制的PID算法
- /// </summary>
- private void ExcutePid()
- {
- double distanceCur = GetDistance(startPointCur, pointCur);
- double tan = GetTanAngle(startPointCur, pointCur);
- PointsToOrigin Reference = new PointsToOrigin();
- int A = 0, B = 0;
- do
- {
- Reference = redis.GetHash<PointsToOrigin>(PointsToOrigin, i.ToString());
- B = i;
- A = i - 1;
- i++;
- }
- while (Reference.distance >= distanceCur && Math.Abs(Reference.TanAngle - tan) <= angleError);
- WGS84Points pointA = new WGS84Points();
- pointA.x = locationList[A].LocationLon;
- pointA.y = locationList[A].LocationLat;
- WGS84Points pointB = new WGS84Points();
- pointB.x = locationList[B].LocationLon;
- pointB.y = locationList[B].LocationLat;
- double accFactor = 0, befAccFactor = 0;
- PidAlgorithm accPid = new PidAlgorithm(1, 1, 1); // TODO传入比例系数
- double tempSpeed = 0, speedLast = 0, curSpeedError = 0, befSpeedError1 = 0, befSpeedError2 = 0;
-
- double angFactor = 0, befAangFactor = 0;
- PidAlgorithm angPid = new PidAlgorithm(1, 1, 1); // TODO传入比例系数
- double tempAng = 0, curAngError = 0, befAngError1 = 0, befAngError2 = 0;
- do
- {
- double curSpeed = BlCanBusPerception.Instance.GetRealSpeed; // 调用Can总线接口,获取实时速度
- double acceleration = (curSpeed - speedLast) / timeSpan / PreNumbers;
- double preSpeed = PreSpeed(curSpeed, acceleration);
- curSpeedError = preSpeed - Reference.Speed;
- speedLast = curSpeed;
- double factorx = PrePointFactorX(Math.Atan(tan), angFactor, curSpeed, acceleration, startPointCur, pointCur);
- double factory = PrePointFactorY(Math.Atan(tan), angFactor, curSpeed, acceleration, startPointCur, pointCur);
- pointCur.x += pointCur.x * factorx;
- pointCur.y += pointCur.y * factory;
- curAngError = GetPointLineDistance(pointCur, pointA, pointB);
- accFactor = accPid.Pid(befAccFactor, curSpeedError, befSpeedError1, befSpeedError2);
- // 调用加速接口
- //BeckhoffDrive.Accelerator((float)accFactor);
- string condition = string.Format("当前油门系数值{0}", accFactor);
- LogHelper.log.Debug(condition);
- befAccFactor = accFactor;
- befSpeedError1 = curSpeedError;
- befSpeedError2 = tempSpeed;
- tempSpeed = befSpeedError1;
- angFactor = angPid.Pid(befAangFactor, curAngError, befAngError1, befAngError2);
- // 调用转盘方向角,基于当前角度调整
- //BeckhoffDrive.SteerAngCt((float)angFactor);
- condition = string.Format("当前转盘方向角{0}", angFactor);
- LogHelper.log.Debug(condition);
- befAangFactor = angFactor;
- befAngError1 = curAngError;
- befAngError2 = tempAng;
- tempAng = befAngError1;
- Thread.Sleep(50);
- }
- while ((accFactor != 0) && (angFactor != 0));
- }
- /// <summary>
- /// 更新指令集执行结果
- /// </summary>
- private void UpdateDirective()
- {
- try
- {
- double distance = GetDistance(startPointCur, pointCur);
- double angle = GetTanAngle(startPointCur, pointCur);
- if (distanceRefAction <= distance && Math.Abs(angleRefAction - angle) <= angleError)
- {
- string speed = "00";
- MoActionDirective mo = BLLFactory<BlActionDirective>.Instance.FindByID(new object[] { SysEnvironment.CurrentRuleName, count });
- //更新数据库
- if (Math.Abs(Convert.ToDouble(speed) - speedRefAction) <= speedError)
- {
- mo.DirectiveResult = 1;
- }
- else
- {
- mo.DirectiveResult = 0;
- }
- BLLFactory<BlActionDirective>.Instance.Update(mo, new object[] { SysEnvironment.CurrentRuleName, count });
- //下一个指令信息
- count++;
- PointsToOrigin Reference = new PointsToOrigin();
- Reference = redis.GetHash<PointsToOrigin>(PointsToOrigin, count.ToString());
- distanceRefAction += Reference.distance;
- angleRefAction = Reference.TanAngle;
- speedRefAction = Reference.Speed;
- }
- }
- catch (Exception ex)
- {
- LogHelper.log.Error(string.Format("更新动作指令执行结果失败:{0}", ex.ToString()));
- }
- }
- #region 预测模型
- public double timeSpan = 0.05; // 预测时间间隔
- public double longf = 2.0; // 前轮到重心的距离
- public double longr = 2.0; // 后轮到重心的距离
- public int PreNumbers = 10;
- /// <summary>
- /// 预测模型中的速度值
- /// </summary>
- /// <param name="speedCur"></param>
- /// <param name="acceleration"></param>
- /// <returns></returns>
- private double PreSpeed(double speedCur, double acceleration)
- {
- double speedPre = speedCur;
- for (int i = 0; i < PreNumbers; i++)
- {
- speedPre += acceleration * timeSpan;
- }
- return speedPre;
- }
- /// <summary>
- /// 预测模型中的前一个点的角度值
- /// </summary>
- /// <param name="angleRef"></param>
- /// <param name="anglePid"></param>
- /// <param name="speedCur"></param>
- /// <param name="acceleration"></param>
- /// <returns></returns>
- private double PreAngle(double angleCur, double anglePid, double speedCur, double acceleration)
- {
- double b = Math.Atan(longr / (longf + longr) * Math.Tan(anglePid));
- double anglePre = angleCur;
- double speedPre = speedCur;
- for (int i = 0; i < PreNumbers - 1; i++)
- {
- speedPre += acceleration * timeSpan;
- anglePre += speedPre / longr * Math.Sin(b) * timeSpan;
- }
- return anglePre;
- }
- /// <summary>
- /// 预测模型中X坐标比例系数,相对于当前坐标值的比例系数
- /// </summary>
- /// <param name="angleCur"></param>
- /// <param name="anglePid"></param>
- /// <param name="preAngle"></param>
- /// <param name="speedCur"></param>
- /// <param name="acceleration"></param>
- /// <param name="startPoint"></param>
- /// <param name="endPoint"></param>
- /// <returns></returns>
- private double PrePointFactorX(double angleCur, double anglePid, double speedCur,
- double acceleration, WGS84Points startPoint, WGS84Points endPoint)
- {
- double b = Math.Atan(longr / (longf + longr) * Math.Tan(anglePid));
- double speedPre = speedCur;
- double anglePre = angleCur;
- double tempx = 0;
- for (int i = 0; i < PreNumbers -1; i++)
- {
- anglePre += speedPre / longr * Math.Sin(b) * timeSpan;
- tempx += speedPre * Math.Cos(anglePre + b) * timeSpan;
- speedPre += acceleration * timeSpan;
- }
- WGS84Points endPointX = new WGS84Points();
- endPointX.x = endPoint.x;
- endPointX.y = startPoint.y;
- double distanceX = GetDistance(startPoint, endPointX);
- return tempx / distanceX;
- }
- /// <summary>
- /// 预测模型中X坐标比例系数,相对于当前坐标值的比例系数
- /// </summary>
- /// <param name="angleCur"></param>
- /// <param name="anglePid"></param>
- /// <param name="preAngle"></param>
- /// <param name="speedCur"></param>
- /// <param name="acceleration"></param>
- /// <param name="startPoint"></param>
- /// <param name="endPoint"></param>
- /// <returns></returns>
- private double PrePointFactorY(double angleCur, double anglePid, double speedCur,
- double acceleration, WGS84Points startPoint, WGS84Points endPoint)
- {
- double b = Math.Atan(longr / (longf + longr) * Math.Tan(anglePid));
- double speedPre = speedCur;
- double anglePre = angleCur;
- double tempy = 0;
- for (int i = 0; i < PreNumbers - 1; i++)
- {
- anglePre += speedPre / longr * Math.Sin(b) * timeSpan;
- tempy += speedPre * Math.Sin(anglePre + b) * timeSpan;
- speedPre += acceleration * timeSpan;
- }
- WGS84Points endPointY = new WGS84Points();
- endPointY.x = startPoint.x;
- endPointY.y = endPoint.y;
- double distanceY = GetDistance(startPoint, endPointY);
- return tempy / distanceY;
- }
- #endregion
- }
- /// <summary>
- /// 起始点事件
- /// </summary>
- public delegate void EventStartPointHandler(StartPointEventArgs e);
-
- public class StartPointEventArgs : EventArgs
- {
- public string Mesg { get; set; }
- public StartPointEventArgs(string value)
- {
- this.Mesg = value;
- }
- }
- }
|