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));
}
}
}
}
效果图，保证整个路径上的位置平滑，速度平滑，加速度无跳变
标签：profile,return,jerk,C#,高次方程,tu,近似,double,public 来源： https://blog.csdn.net/qianshishangdi/article/details/116401747