C# 求高次方程的近似根

五次及以上的高次方程没有公式解,所以需要求近似解
使用MathNet.Numerics.RootFinding可以很简单的实现

实际例子如下:
小车做单向直线运动,先以恒定jerk加速,再以-jerk加速到Vmax且acc=0,再以恒定-jerk减速再以jerk减速到eVel且acc=0,路程为dist,求加减速时间(化简后是一元五次方程)

测试函数.cs

using MathNet.Numerics.Differentiation;
using MathNet.Numerics.RootFinding;
using System;
using System.Collections.Generic;
using System.Linq;

namespace 数学库测试
{
    /// <summary>
    /// 先以恒定jerk加速再以-jerk加速到Vmax,再以恒定-jerk减速再以jerk减速到eVel,路程为dist,求加减速时间
    /// 开始加速运动 spos=0, svel=sVel, sacc=0, jerk=j
    /// 加速运动 spos=0, svel=sVel, sacc=0, jerk=-j
    /// 运动到 Vmax(未知)
    /// 开始减速运动 svel=Vmax, jerk=-j
    /// 减速运动 jerk=j
    /// 到达末端位置 epos=dist, evel=evel
    /// </summary>
    public class 运动规划1
    {
        public double sVel;    // 初始速度
        public double eVel;    // 末端速度
        public double jerk;    // 加加速度
        public double dist;    // 运动距离

        public 运动规划1(double sv, double ev, double j, double d)
        {
            sVel = sv;
            eVel = ev;
            jerk = j;
            dist = d;
        }

        public double MotionProfileFunc(double x)
        {
            return 2 * sVel * x + jerk * x * x * x + (sVel + eVel + jerk * x * x) * Math.Sqrt((sVel - eVel + jerk * x * x) / jerk) - dist;
        }

        public double DerivateFunc(double x)
        {
            return new NumericalDerivative().EvaluateDerivative(MotionProfileFunc, x, 1);
        }

        public double GetUpperBound()
        {
            var roots = Cubic.RealRoots(-dist / jerk, 2 * sVel / jerk, 0);
            return new List<double>() { roots.Item1, roots.Item2, roots.Item3 }.Max();
        }

        public double GetDecTime(double accTime)
        {
            return Math.Sqrt((sVel - eVel + jerk * accTime * accTime) / jerk);
        }
    }
}

求根.cs

using MathNet.Numerics.RootFinding;
using System;

namespace 数学库测试
{
    public class 求根
    {
        private double tu, td;

        // 二分法
        public Tuple<Func<double, double>, Func<double, double>> TestBisection(double sv, double ev, double j, double d)
        {
            var profile = new 运动规划1(sv, ev, j, d);
            
            //二分法,不需要额外参数
            //tu = Bisection.FindRoot(profile.MotionProfileFunc, 0, profile.GetUpperBound(), 1E-7);
            //tu = Brent.FindRoot(profile.MotionProfileFunc, 0, profile.GetUpperBound(), 1E-7);
            
            //牛顿法,需要导函数
            //tu = NewtonRaphson.FindRoot(profile.MotionProfileFunc, profile.DerivateFunc, 0, profile.GetUpperBound(), 1E-7);
            //tu = RobustNewtonRaphson.FindRoot(profile.MotionProfileFunc, profile.DerivateFunc, 0, profile.GetUpperBound(), 1E-7);
            
            //割线法,需要两次猜测
            //tu = Secant.FindRoot(profile.MotionProfileFunc, profile.GetUpperBound() * 1f / 3, profile.GetUpperBound() * 2f / 3, 0, profile.GetUpperBound(), 1E-7);

            //Broyden法,需要猜测值
            Func<double[], double[]> func = (x) => { return new double[] { profile.MotionProfileFunc(x[0]) }; };
            var guess = new double[1] { profile.GetUpperBound() * 1f / 2 };
            tu = Broyden.FindRoot(func, guess, 1E-7)[0];

            td = profile.GetDecTime(tu);

            var v1 = sv + 0.5 * j * tu * tu;
            var v2 = sv + j * tu * tu;
            var v3 = v2 - 0.5 * j * td * td;
            var p1 = sv * tu + 1f / 6 * j * tu * tu * tu;
            var p2 = 2 * sv * tu + j * tu * tu * tu;
            var p3 = p2 + v2 * td - 1f / 6 * j * td * td * td;

            Func<double, double> posFunc = (t) => {
                if ((t >= 0) && (t < tu))
                    return sv * t + 1f / 6 * j * t * t * t;
                else if ((t >= tu) && (t < tu * 2))
                {
                    t = t - tu;
                    return p1 + v1 * t + 0.5 * j * tu * t * t - 1f / 6 * j * t * t * t;
                }
                else if ((t >= tu * 2) && (t < tu * 2 + td))
                {
                    t = t - 2 * tu;
                    return p2 + v2 * t - 1f / 6 * j * t * t * t;
                }
                else if ((t >= tu * 2 + td) && (t <= tu * 2 + td * 2))
                {
                    t = t - 2 * tu - td;
                    return p3 + v3 * t - 0.5 * j * tu * t * t + 1f / 6 * j * t * t * t;
                }
                else
                    return 0.0;
            };

            Func<double, double> velFunc = (t) => {
                if ((t >= 0) && (t < tu))
                    return sv + 0.5 * j * t * t;
                else if ((t >= tu) && (t < tu * 2))
                {
                    t = t - tu;
                    return v1 + j * tu * t - 0.5 * j * t * t;
                }
                else if ((t >= tu * 2) && (t < tu * 2 + td))
                {
                    t = t - 2 * tu;
                    return v2 - 0.5 * j * t * t;
                }
                else if ((t >= tu * 2 + td) && (t <= tu * 2 + td * 2))
                {
                    t = t - 2 * tu - td;
                    return v3 - j * td * t + 0.5 * j * t * t;
                }
                else
                    return 0.0;
            };
            return new Tuple<Func<double, double>, Func<double, double>>(posFunc, velFunc);
        }

        public Tuple<double, double> GetBoundaries()
        {
            return new Tuple<double, double>(0, 2 * (tu + td));
        }
    }
}

接下来用LiveChart在WPF下绘图
MainWindow.xaml

<Window x:Class="函数图表.MainWindow"
        xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
        xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
        xmlns:d="http://schemas.microsoft.com/expression/blend/2008"
        xmlns:mc="http://schemas.openxmlformats.org/markup-compatibility/2006"
        xmlns:lvc="clr-namespace:LiveCharts.Wpf;assembly=LiveCharts.Wpf"
        xmlns:local="clr-namespace:函数图表"
        mc:Ignorable="d"
        Title="函数图表" Height="450" Width="800">
    <Grid>
        <!--LegendLocation 表示图例的位置在右边-->
        <lvc:CartesianChart Series="{Binding SC}" LegendLocation="Right" >
            <lvc:CartesianChart.AxisX>
                <!-- Labels X轴坐标值-->
                <lvc:Axis Name="xAxis" Title="时间(s)" Labels="{Binding Xs}"></lvc:Axis>
            </lvc:CartesianChart.AxisX>
            <lvc:CartesianChart.AxisY>
                <!-- YFormatter Y轴坐标值-->
                <lvc:Axis Title="y"></lvc:Axis>
            </lvc:CartesianChart.AxisY>
        </lvc:CartesianChart>
    </Grid>
</Window>

MainWindow.xaml.cs

using LiveCharts;
using LiveCharts.Wpf;
using System;
using System.Collections.Generic;
using System.Windows;
using 数学库测试;

namespace 函数图表
{
    /// <summary>
    /// MainWindow.xaml 的交互逻辑
    /// </summary>
    public partial class MainWindow : Window
    {
        private int steps = 128;

        public SeriesCollection SC { get; set; }
        public IList<string> Xs { get; set; }
        public List<double> Pos { get; set; }
        public List<double> Vel { get; set; }

        public MainWindow()
        {
            InitializeComponent();

            ShowBisectionResult();
            DataContext = this;
        }

        public void ShowBisectionResult()
        {
            var opt = new 求根();
            var func = opt.TestBisection(200, 60, 1000, 800);
            var bound = opt.GetBoundaries();
            GenerateXY(func, bound);

            SC = new SeriesCollection
            {
                // 匿名新建LineSeries对象
                new LineSeries { Title = "位置(mm)", Values = new ChartValues<double>(Pos) },
                new LineSeries { Title = "速度(mm/s)", Values = new ChartValues<double>(Vel) },
            };
        }

        public void GenerateXY(Tuple<Func<double, double>, Func<double, double>> func, Tuple<double, double> bound)
        {
            var interval = (bound.Item2 - bound.Item1) / steps;

            var time = 0.0;
            Xs = new List<string>() { time.ToString("F4") };
            Pos = new List<double>() { 0.0 };
            Vel = new List<double>() { func.Item2(0.0) };

            for (int i = 1; i < steps; i++)
            {
                time += interval;
                Xs.Add(time.ToString("F4"));
                Pos.Add(func.Item1(time)) ;
                Vel.Add(func.Item2(time));
            }
        }
    }
}

效果图,保证整个路径上的位置平滑,速度平滑,加速度无跳变

C# 求高次方程的近似根

上一篇:大失败!莫名其妙被卡之列出连通集


下一篇:python调libclang如何打印编译报错与调试信息(采用translation unit的diagnostic)