规控常见数学方法-C++

一、求方程的根

牛顿法

1、求数字的算术平方根

// 牛顿法求 num 的平方根
// 令f(x) = x^2 - num , 求 f(x) 的零点

// initial_num  (x0)     last_num: x(1)

#include<iostream>
using namespace std;
#include<cmath>

double getroot(const double & num){
    if(num==0||num==1)return num;

    double error = 1e-17;
    double initial_num = num/2.0;
    double last_num = (num/initial_num+initial_num)/2.0;

    while ( fabs(last_num - initial_num) > error)
    {
        initial_num = last_num;
        last_num = (num/initial_num+initial_num)/2.0;
    }
    
    return last_num;
}

int main(){
    double enter_num;
    cin>>enter_num;
    cout<< "the root is :"<<getroot(enter_num)<<endl;
    return 0;

}

2、求x 2 − ln ⁡ x = 0的根

原文链接:https://blog.csdn.net/weixin_42301220/article/details/126816206

#include<iostream>
using namespace std;

constexpr double esp = 1e-6;


int main() {
        double initial_num = 2;
        double last_num = initial_num - (initial_num * initial_num + log(initial_num)) / (2.0 * initial_num + (1 / initial_num));

        while (abs(initial_num-last_num)>=esp)
        {
                initial_num = last_num;
                last_num = initial_num - (initial_num * initial_num + log(initial_num)) / (2.0 * initial_num + (1 / initial_num));

        }
        cout << last_num << endl;
        return 0;

}

二、计算几何学

Geometry

Point:

class Point {
public:
        Point() = default;
        Point(double x_in, double y_in) :x(x_in), y(y_in) {};  //拷贝构造函数

        Point(const Point& p) :x(p.x), y(p.y) {};
        Point& operator=(const Point& p) {  //赋值运算符
                x = p.x;
                y = p.y;
                return *this;
        }

        Point operator+(const Point& p) {
                return { x + p.x, y + p.y };
        }
        Point operator-(const Point& p) const {
                return { x - p.x, y - p.y };
        }
        double operator*(const Point& p) const {
                return x * p.x + y * p.y;
        }
        Point operator*(double k)const {
                return { x * k, y * k };
        }
        friend Point operator*(double k, const Point& p) {
                return { p.x * k, p.y * k };
        }
        bool operator==(const Point& p)const {
                return p.x == x && p.y == y;
        }
        bool operator!=(const Point& p)const {
                return !(p.x == x && p.y == y);
        }

        double modulus()const {
                return sqrt(x * x + y * y);
        }

        double DistanceTo(const Point& other)const {
                double dx = x - other.x;
                double dy = y - other.y;
                return sqrt(dx * dx + dy * dy);
        }

        friend std::ostream& operator<<(std::ostream& out, const Point& p) {
                out << "(" << p.x << ", " << p.y << ")";
                return out;
        }

public:
        double x;
        double y;
};

直线line:

//Define line segment.
class Line {
public:
        Line() = default;
        Line(Point p1_in, Point p2_in) : p1(p1_in), p2(p2_in), direction(p2_in - p1_in) {
        };

        friend std::ostream& operator<<(std::ostream& out, const Line& line) {
                out << "Line: " << line.p1 << " ---> " << line.p2;
                return out;
        }

public:
        Point p1;
        Point p2;
        Point direction;
};

线段segment:

class Segment {
public:
        Segment() = default;
        Segment(Point start_in, Point end_in) : start(start_in), end(end_in), direction(end - start) {
        }

        Segment(const Segment& s) : start(s.start), end(s.end), direction(end - start) {}

        Segment& operator=(const Segment& s) {
                start = s.start;
                end = s.end;
                return *this;
        }

        //Segment operator+(const Segment& rhs) const {
        //        return { start + rhs.start, end + rhs.end };
        //}
        Segment operator-(const Segment& rhs)const {
                return { start - rhs.start, end - rhs.end };
        }

        double length()const {
                return direction.modulus();
        }

        Point unit_direction()const {
                int len = length();
                if (len != 0) {
                        return { len / direction.x , len / direction.y };
                }
                else throw std::runtime_error("Cannot calculate unit direction for a segment with zero length.");
        }
        friend std::ostream& operator<<(std::ostream& out, const Segment& s) {
                out << "Segment: " << s.start << " ---> " << s.end;
                return out;
        }

public:
        Point start;
        Point end;
        Point direction;
};

折线段polyline:

class Polyline {
        Polyline() = default;

        Polyline(const std::vector<Point>& pts) :points(pts) {
                for (int i = 1; i < points.size(); ++i) {
                        segs.push_back(Segment(points[i - 1], points[i]));
                }
        }

        Polyline(const std::vector<Segment>& segs_) : segs(segs_) {
                for (int i = 0; i < segs.size(); ++i) {
                        points.push_back(segs[i].start);
                }
                points.push_back(segs[segs.size() - 1].end);
        }

        void append(const Segment& seg) {
                if (!segs.empty() && segs.back().end != seg.start) {
                        throw std::invalid_argument("Disconnected Segment");
                }
                segs.push_back(seg);
                points.push_back(seg.end);
        }
        void append(const Point& p) {
                const auto seg = Segment(points.back(), p);
                points.push_back(p);
                segs.push_back(seg);
        }

        Polyline operator+(const Polyline& other) const {
                Polyline result;
                result.segs = this->segs;
                result.points = this->points;
                for (auto& seg : other.segs) {
                        result.append(seg);
                }

                return result;
        }

        Segment GetSegmentByIndex(int index) {
                if (index < 0 || index >= segs.size()) {
                        throw std::out_of_range("Index out of range");
                }
                return segs[index];
        }
        std::vector<Point> Points() const {
                return points;
        }
        std::vector<Segment> Segments()const {
                return segs;
        }

private:
        vector<Point> points;
        vector<Segment> segs;
};

Algorithm

基本运算

点积:

constexpr double Epsilon = 1e-6;


using Vec = Point;   //类型别名

// 点积运算
double DotProduct(const Vec& v1, const Vec& v2) {
        return v1.x * v2.x + v1.y + v2.y;
}

叉乘:

// 叉乘运算
double CrossProduct(const Vec& v1, const Vec& v2) {
        return v1.x * v2.y - v2.x * v1.y;
}

int Sign(double x) {
        return x < 0 ? -1 : x>0 ? 1 : 0;
}

Projection

点到线段的距离:

// 计算点到线段的投影长度 x   (p1p*cos(theta))
double ComputeProjectionLength(const Point& p, const Segment& seg) {
        const auto& p1p = p - seg.start;
        return DotProduct(p1p, seg.unit_direction());
}

点到线段的投影点:

// Compute projection point of point p.
Point ComputeProjection(const Point& p, const Segment& segment) {
    double projection_length = ComputeProjectionLength(p, segment);
    Point ans;
    ans.x = segment.start.x + segment.unit_direction().x * projection_length;
    ans.y = segment.start.y + segment.unit_direction().y * projection_length;
    return ans;
 
}

Distance-求距离

point to point:

// Get distance between point p1 and point p2.
double GetDistance(const Point& p1, const Point& p2) {
        return p1.DistanceTo(p2);
}

point to line:

// Get distance between point p and a straight line.
double GetDistance(const Point & p, const Line& line) {
        Segment p1p2(line.p1, line.p2);
        Segment p1p(line.p1, p);

        /*p1p_.x = p1p.end.x - p1p.start.x;
        p1p_.y = p1p.end.y - p1p.start.y;*/
        return abs(CrossProduct(p1p.direction, p1p2.unit_direction()));
}

point to segment:

// Get distance between point p and segment(p1,p2).
double GetDistance(const Point& p, const Segment& seg) {
        Segment p1p(seg.start, p);
        Segment p2p(seg.end, p);
        const auto c1 = DotProduct(p1p.direction,seg.direction);
        const auto c2 = DotProduct(p2p.direction,seg.direction);
        if (c1 <= 0)return GetDistance(p, seg.start);
        if (c2 >= 0)return GetDistance(p, seg.end);
        return abs(CrossProduct(p1p.direction, seg.unit_direction()));// 投影点在线段上
}

segment to segment:

四个端点到线段的最小值

double GetDistance(const Segment& seg1, const Segment& seg2) {
        const double d1 = GetDistance(seg1.start, seg2);
        const double d2 = GetDistance(seg1.end, seg2);
        const double d3 = GetDistance(seg2.start, seg1);
        const double d4 = GetDistance(seg1.end, seg1);
        return min(min(d1, d2), min(d3, d4));

}

Side-求相对位置关系

enum class Side {
    // When the segment's length is zero, it's useless to determine the side, so we use DEFAULT_NO_SIDE to show.
    DEFAULT_NO_SIDE = 0,
    LEFT,
    RIGHT,
    // The three below states mean that the point is in line.
    ON_AFTER,
    ON_BEFORE,
    WITHIN
};

也就是说点与线段的相对位置关系包括以下5种:

  • 点在线段的左边
  • 点在线段的右边
  • 点在线段所在的直线上点在线段前面点在线段后面点在线段内部

判断点跟一条线段的相对位置关系:

//判断点和线段的位置关系(5种)
// Determine which side of the segment the point is.
Side GetSide(const Point& p, const Segment& s) {
    const auto& p0 = s.start;
    const auto& p2 = s.end;
    const auto& a = p - p0;
    const auto& b = p2 - p0;
    const auto cross_product = CrossProduct(a, b);
    if (cross_product != 0) {
        // Returns LEFT if p0,p,p2 are clockwise position, returns RIGHT means p0,p,p2 are counter-clockwise position.
        return cross_product < 0 ? Side::LEFT : Side::RIGHT;
    }
    const auto dot_product = DotProduct(a, b);
    if (dot_product < 0) {
        return Side::ON_BEFORE;
    }
    else if (dot_product > 0) {
        if (b.modulus() < a.modulus()) {
            return Side::ON_AFTER;
        }
        else {
            return Side::WITHIN;
        }
    }
    else {
        if (a.modulus() == 0) {
            return Side::WITHIN;
        }
        return Side::DEFAULT_NO_SIDE;
    }
}

判断点与两条相连的线段的相对位置关系:

// Determine which side of two segments the point is.
// Method 2: through the side of p to one segment to determine, and except left/right side, we also provide other options.
Side GetSide(const Point& p, const Segment& s1,const Segment& s2) {
    //DCHECK(s1.end==s2.start)<<"please ensure the two segments are connected.";
    if (s1.end != s2.start) {
        throw std::runtime_error("Error: The two segments are not connected.");
    }
    const auto side_1 = GetSide(p,s1);
    const auto side_2 = GetSide(p,s2);
    if(side_1==side_2){
        return side_1;
    }
    if(side_1==Side::WITHIN||side_2==Side::WITHIN){
        return Side::WITHIN;
    }
    const auto& p0p = p-s1.start;
    const auto& p1p = p-s2.start;
    const auto c1 = CrossProduct(s1.direction,p0p);
    const auto c2 = CrossProduct(s2.direction,p1p);
    return std::abs(c2) > std::abs(c1) ? side_2 : side_1;
}

Intersection-相交

这里相交主要指的是线段与线段之间的相交。很显然相交分为两步:

判断是否相交:

// Determine whether segment 1 intersects segment 2.
bool IsIntersection(const Segment& s1, const Segment& s2){
    const double o1 = CrossProduct(s2.start - s1.start, s1.direction);
    const double o2 = CrossProduct(s2.end - s1.start, s1.direction);
    const double o3 = CrossProduct(s1.start - s2.start, s2.direction);
    const double o4 = CrossProduct(s1.end - s2.start, s2.direction);
    // Segments are considered intersecting if they have different orientations.
    if(o1 * o2 < 0 && o3 * o4 < 0){
        return true;
    }

    auto on_segment = [](const Point &p, const Point &q, const Point &r){
        return (q.x <= std::max(p.x, r.x) && q.x >= std::min(p.x, r.x) &&
                q.y <= std::max(p.y, r.y) && q.y >= std::min(p.y, r.y));
    };
    // Additional checks for collinear points.
    if(o1 == 0 && on_segment(s1.start, s2.start, s1.end)){
        return true;
    }

    if(o2 == 0 && on_segment(s1.start, s2.end, s1.end)){
        return true;
    }

    if(o3 == 0 && on_segment(s2.start, s1.start, s2.end)){
        return true;
    }

    if(o4 == 0 && on_segment(s2.start, s1.end, s2.end)){
        return true;
    }

    return false;
}

若相交则求出相交点:

//Calculate the intersection between segment 𝑝0𝑝1 and segment 𝑝2𝑝3.
Point GetIntersectionPoint(const Segment& s1, const Segment& s2){
    if(!IsIntersection(s1, s2)){
        std::cout << "No intersection, return a deafult point value:(0,0)!";
        return Point(0, 0);
    }
    const auto& u = s1.direction;
    const auto& v = s2.direction;
    const auto& w = s1.start - s2.end;
    const auto c1 = CrossProduct(w, v);
    const auto c2 = CrossProduct(v, u);
    if(c2 != 0){
        const double t = std::abs(c1 / c2);
        return s1.start + t * u;
    }
    // Address collinear and overlapping situation. If so, we return overlaping start or end.
    const auto side_1 = GetSide(s1.start, s2);
    const auto side_2 = GetSide(s1.end, s2);
    const auto side_3 = GetSide(s2.start, s1);
    const auto side_4 = GetSide(s2.end, s1);
    if(side_1 == Side::WITHIN){
        return s1.start;
    }
    if(side_2 == Side::WITHIN){
        return s1.end;
    }
    if(side_3 == Side::WITHIN){
        return s2.start;
    }
    if(side_4 == Side::WITHIN){
        return s2.end;
    }
    throw std::runtime_error("Something is wrong, please check.");
}

Curvature-曲率

通过三点求曲率:

// Obtain curvature according to p1,p2,p3.
// NOTE : curvature has a sign, not just an unsigned value.
double GetCurvature(const Point& p1, const Point& p2, const Point& p3){
    const auto& p1p2 = p2 - p1;
    const auto& p2p3 = p3 - p2;
    const auto& p1p3 = p3 - p1;
    const auto& a = p1p2.modulus();
    const auto& b = p2p3.modulus();
    const auto& c = p1p3.modulus();
    const auto cross_product = CrossProduct(p1p2, p2p3);
    return 2 * cross_product / (a * b * c);
}

Find closest segment-求polyline上距离给定点最近的线段

//
// Created by CHH3213 on 2024/1/6.
//

#ifndef MATH_GEOMETRY_SEGMENT_SEARCH_H
#define MATH_GEOMETRY_SEGMENT_SEARCH_H

#include "utils.h"
#include "Geometry.h"
#include "Distance.h"
#include "Projection.h"

// Find the given point's closest segment in polyline using linear search.
// Option 1.
Segment FindClosestSegmentByLinearSearch(const Point& point, const Polyline& polyline) {
    const auto points = polyline.Points();
    const auto segments = polyline.Segments();
    //Compute the square distance between given point and first point in polyline.
    double min_dist_sq = GetDistance(point, points[0]);
    int closest_segment_index = 0;
    for (int i = 1; i < points.size(); ++i) {
        const auto& p1 = points[i - 1];
        const auto& p2 = points[i];
        const auto& seg = segments[i - 1];
        const auto& v = seg.unit_direction();
        const auto& w = point - p1;
        const double c1 = DotProduct(w, v);
        if (c1 <= 0) {
            continue;
        }
        double dist_sq = 0.0;
        const double c2 = seg.length();
        if (c2 <= c1) {
            dist_sq = GetDistance(point, p2);
        }
        else {
            dist_sq = GetDistance(point, seg);
        }

        if (dist_sq < min_dist_sq) {
            min_dist_sq = dist_sq;
            closest_segment_index = i - 1;
            if (min_dist_sq < Epsilon) {
                break;
            }
        }

    }
    return segments[closest_segment_index];
}

// Find the given point's closest segment in polyline using linear search.
// Option 2: since we have implemented distance function, we can directly use it.
//Segment FindClosestSegmentByLinearSearch(const Point& point, const Polyline& polyline){
//    const auto& points = polyline.Points();
//    const auto& segments = polyline.Segments();
//    //Compute the square distance  between given point and first point in polyline.
//    double min_dist_sq = GetDistance(point,points[0]);
//    int closest_segment_index = 0;
//    for(int i=0;i<segments.size();++i){
//        const auto& seg = segments[i];
//        const double dist_sq = GetDistance(point,seg);
//        if(dist_sq<min_dist_sq){
//            min_dist_sq = dist_sq;
//            closest_segment_index=i;
//            if(min_dist_sq<Epsilon){
//                break;
//            }
//        }
//    }
//    return segments[closest_segment_index];
//}

#endif //MATH_GEOMETRY_SEGMENT_SEARCH_H

三、无约束优化

梯度下降法:

求一元函数极值点

求函数f ( x ) = x 2 − 4 x 的极值点

原文链接:https://blog.csdn.net/weixin_42301220/article/details/127628289

// 无约束优化算法
//
// Created by lg on 2024/7/12.
//
#include<iostream>
#include<cmath>

constexpr double eps = 1e-6;
constexpr double Delta = 1e-4;

//原函数
double func(const double& x) {
        return  x * x - 4 * x;
}

//原函数的一阶导
double func_prime(const double& x) {
        return (func(x + Delta) - func(x - Delta)) / (2 * Delta);
}

//原函数的一阶导
double func_prime2(const double& x) {
        return 2 * x - 4;;
}

double GradientDescent(const double& x0, const double& LearningRate) {
        double x = x0;
        double x_new = x - LearningRate * func_prime(x);
        while (abs(x_new-x)>eps)
        {
                x = x_new;
                x_new = x - LearningRate * func_prime(x);
        }
        return x;
}

int main() {
        std::cout << GradientDescent(-2, 0.01) << std::endl;
        return 0;
}

求二元函数极值点:

// 梯度下降法求二元函数极值
//
// Created by lg on 2024/7/12.
//

#include<iostream>
#include<cmath>
#include <valarray>
using namespace std;
#include<vector>

constexpr double eps = 1e-8;
constexpr double Delta = 1e-4;

/*
 * 使用梯度下降法求解多元函数极值:f(x,y)=4(x-1)^2+(y-2)^4
 */

/*
  对x的偏导:8(x-1)     对y的偏导:4*(y-2)^3
  */

double func(const double& x, const double& y) {
        return 4 * pow(x-1,2) + pow(y-2,4);
}

vector<double>func_prime(const double& x, const double& y) {
        vector<double> prime;
        prime.push_back(8 * (x - 1));
        prime.push_back(4 * pow(y - 2, 3));
        return prime;
}

vector<double>GradientDescent(const vector<double>& x0, const double LearningRate) {
        vector<double>x = x0;
        vector<double>x_new(2);
        x_new[0] = x[0] - func_prime(x[0], x[1])[0]*LearningRate;
        x_new[1] = x[1] - func_prime(x[0], x[1])[1]*LearningRate;
        while (abs(x_new[0]-x[0])>eps||abs(x_new[1]-x[1])>eps)
        {
                x[0] = x_new[0];
                x[1] = x_new[1];
                x_new[0] = x[0] - func_prime(x[0], x[1])[0] * LearningRate;
                x_new[1] = x[1] - func_prime(x[0], x[1])[1] * LearningRate;
        }
        return x;
}


int main() {
        vector<double>x0 = { 3,4 };
        double learningrate = 0.01;
        vector<double>ans = GradientDescent(x0, learningrate);
        for (auto num : ans) {
                cout << num << " ";
        }
        cout << endl;
}

牛顿法求函数极值:

x_new = x - f(x)/f'(x)

思路:

牛顿法是直接求方程的根(零点)的迭代算法,要想求函数的极值点,必须转换成f(x) = 0的形式求解 , 也就是求原函数的一阶导等于0的点,根据牛顿法去求,因此要求原函数的二阶导。

// 牛顿法求一元函数极值
//
// Created by lg on 2024/7/12.
//
#include<iostream>
#include<cmath>
using namespace std;

constexpr double eps = 1e-9;
constexpr double Delta = 1e-4;

//原函数
double func(const double& x) {
        return  x * x - 4 * x;
}

//原函数的一阶导
double func_prime(const double& x) {
        return (func(x + Delta) - func(x - Delta)) / (2 * Delta);
}

//原函数的一阶导
double func_prime2(const double& x) {
        return 2 * x - 4;;
}

//原函数的二阶导
double func_prime_prime(const double& x) {
        return (func_prime(x + Delta) - func_prime(x - Delta)) / (2 * Delta);
}


double Newton(const double &x0) {
        double x = x0;
        double x_new = x - func_prime(x) / func_prime_prime(x);
        while (abs(x_new-x)>eps)
        {
                x = x_new; 
                x_new = x - func_prime(x) / func_prime_prime(x);
        }

        return x;
}

int main() {
        cout << Newton(9) << endl;
        return 0;
}

模拟退火法:

// 模拟退火法求函数极值(全局最优)
//
// Created by lg on 2024/7/12.
//
//
// Created by CHH3213 on 2022/9/12.
//
#include<iostream>
#include <valarray>

using namespace std;

#define DELTA 1e-4
#define EPS 1e-6
/*
 * 使用模拟退火求函数极值点
 */
double func(double x) {
    /**
     * 构造函数
     * x:自变量
     * return:函数值
     */
    return x * x - 4 * x;
}


double rnd(double low, double high) {
    /**
     * 返回指定范围内的随机浮点数
     */
    double rd = rand() / ((double)RAND_MAX + 1.0);
    return low + rd * (high - low);
}

double Annealing(double T0, double T_min, double x0, double gamma) {
    /**
     * T0:初始温度
     * T_min:温度的下限,若温度T达到T_min,则停止搜索
     * x0:初始迭代值
     * gamma:T_{k+1} = gamma * T_k, 0<gamma<1
     * return:极值点
     */
    double T = T0;
    double x = x0;
    double x_ = 2;
    double delta_y = 1.0;
    while (abs(delta_y) > EPS) {//如果函数值满足精度,停止搜索
        //     while(T>T_min){//温度T达到T_min,则停止搜索

        double x_ = x + rnd(-1, 1);
        cout << x_ << endl;
        delta_y = func(x_) - func(x);
        if (delta_y <= 0) {
            x = x_;
        }
        else {
            if (exp(-delta_y / T) > rnd(0, 1)) {
                x = x_;
            }
        }
        T = gamma * T;
        cout << x << endl;
    }
    return x;
}

int main() {
    double T0 = 200, T_min = 1, x0 = 2, gamma = 0.9;
    cout << "extreme  value: " << Annealing(T0, T_min, x0, gamma) << endl;
    return 0;
}


遗传算法(求全局最优)

四、二次规划

常见的凸优化问题的标准形式如下:

如果约束条件或目标函数包含非线性,则为非线性优化。二次规划是一种特殊的非线性规划,也是标准的凸优化问题,能够快速求解。

在路径/轨迹优化中经常建模为二次优化问题进行求解,很多MPC的求解过程也是转化为序列二次规划进行求解。

二次规划的标准形式为:

二次规划的目标函数是二次函数,给的是二次型的形式,P是一个对称矩阵,q是一个向量,约束是线性的,当然可以是线性等式或者线性不等式。

一般二次规划问题可以分成以下几类:

  • 凸二次规划问题:G半正定,问题有全局解
  • 严格凸二次规划问题:G正定,问题有唯一全局解
  • 一般二次规划问题:G不定,问题有稳定点或局部解

求解二次规划问题一般需要Eigen库、OSQP求解器、OSQP-Eigen求解器

算例:

将目标函数改写为矩阵形式:

将上述方程改写为二次规划问题求解器默认的求解形式:

其线性不等式约束写为矩阵形式:

// osqp-eigen
#include "OsqpEigen/OsqpEigen.h"
 
// eigen
#include <Eigen/Dense>
#include <iostream>
 
int main()
{
    // allocate QP problem matrices and vectores
    Eigen::SparseMatrix<double> hessian(2, 2);      //P: n*n正定矩阵,必须为稀疏矩阵SparseMatrix
    Eigen::VectorXd gradient(2);                    //Q: n*1向量
    Eigen::SparseMatrix<double> linearMatrix(2, 2); //A: m*n矩阵,必须为稀疏矩阵SparseMatrix
    Eigen::VectorXd lowerBound(2);                  //L: m*1下限向量
    Eigen::VectorXd upperBound(2);                  //U: m*1上限向量
 
    hessian.insert(0, 0) = 2.0; //注意稀疏矩阵的初始化方式,无法使用<<初始化
    hessian.insert(1, 1) = 2.0;
    // std::cout << "hessian:" << std::endl
    //           << hessian << std::endl;
    gradient << -2, -2;
    linearMatrix.insert(0, 0) = 1.0; //注意稀疏矩阵的初始化方式,无法使用<<初始化
    linearMatrix.insert(1, 1) = 1.0;
    // std::cout << "linearMatrix:" << std::endl
    //           << linearMatrix << std::endl;
    lowerBound << 1, 1;
    upperBound << 1.5, 1.5;
 
    // instantiate the solver
    OsqpEigen::Solver solver;
 
    // settings
    solver.settings()->setVerbosity(false);
    //将verbosity设置为false通常意味着求解器在运行时不会输出额外的调试信息或进度信息,仅输出最终结果
    solver.settings()->setWarmStart(true);
   //热启动是一种优化策略,它允许求解器从上一次求解的解开始,而不是每次都从初始解开始。 
    // set the initial data of the QP solver
    solver.data()->setNumberOfVariables(2);   //变量数n
    solver.data()->setNumberOfConstraints(2); //约束数m
    if (!solver.data()->setHessianMatrix(hessian))
        return 1;
    if (!solver.data()->setGradient(gradient))
        return 1;
    if (!solver.data()->setLinearConstraintsMatrix(linearMatrix))
        return 1;
    if (!solver.data()->setLowerBound(lowerBound))
        return 1;
    if (!solver.data()->setUpperBound(upperBound))
        return 1;
 
    // instantiate the solver
    if (!solver.initSolver())
        return 1;
 
    Eigen::VectorXd QPSolution;
 
    // solve the QP problem
    if (!solver.solve())
    {
        return 1;
    }
 
    QPSolution = solver.getSolution();
    std::cout << "QPSolution" << std::endl
              << QPSolution << std::endl; //输出为m*1的向量
    return 0;
}

二次规划数值求解理论

【数学与算法理论】二次规划的数学模型-CSDN博客

二次规划其实是在求解一个多元变量的目标函数在多元变量的约束下的最小值,最后得到使得在线性约束条件下目标函数最小的变量向量以及最小值。一般有消元法拉格朗日法求解。

注意:

当目标函数的hessian矩阵是正定矩阵时,是凸二次规划问题,局部最优解就是全局最优解。

#自动驾驶##机器人##C++##数值计算##优化算法#
自动驾驶/机器人C++八股精选 文章被收录于专栏

在自动驾驶和机器人领域,C++因其高性能、内存管理高效和跨平台兼容性等特性,被广泛应用。本专栏整理了C++面试中常遇到的八股问题,可私信作者要飞书文档,不论是嵌入式软开、算法、软件开发都可以阅读,包括了C++的虚函数、C++11新特性、C++的STL库、Linux常见命令......

全部评论
写得好!!!
1 回复 分享
发布于 08-13 18:05 湖南

相关推荐

点赞 评论 收藏
分享
08-18 22:48
已编辑
门头沟学院 算法工程师
查看5道真题和解析 投递大疆等公司10个岗位
点赞 评论 收藏
分享
4 17 评论
分享
牛客网
牛客企业服务