Autopilot/SIASUN.Autopilot.BLL/Controler/BlDecisionControler.cs

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;
        }
    }
}