CN111766883B - Small celestial body collaborative attachment robust obstacle avoidance control method - Google Patents

Small celestial body collaborative attachment robust obstacle avoidance control method Download PDF

Info

Publication number
CN111766883B
CN111766883B CN202010639213.4A CN202010639213A CN111766883B CN 111766883 B CN111766883 B CN 111766883B CN 202010639213 A CN202010639213 A CN 202010639213A CN 111766883 B CN111766883 B CN 111766883B
Authority
CN
China
Prior art keywords
detector
obstacle avoidance
state
celestial body
attachment
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010639213.4A
Other languages
Chinese (zh)
Other versions
CN111766883A (en
Inventor
崔平远
葛丹桐
朱圣英
梁子璇
徐瑞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202010639213.4A priority Critical patent/CN111766883B/en
Publication of CN111766883A publication Critical patent/CN111766883A/en
Application granted granted Critical
Publication of CN111766883B publication Critical patent/CN111766883B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/17—Function evaluation by approximation methods, e.g. inter- or extrapolation, smoothing, least mean square method
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q10/00—Administration; Management
    • G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q10/00—Administration; Management
    • G06Q10/06—Resources, workflows, human or project management; Enterprise or organisation planning; Enterprise or organisation modelling
    • G06Q10/063—Operations research, analysis or management
    • G06Q10/0639—Performance analysis of employees; Performance analysis of enterprise or organisation operations
    • G06Q10/06393—Score-carding, benchmarking or key performance indicator [KPI] analysis

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Business, Economics & Management (AREA)
  • General Physics & Mathematics (AREA)
  • Human Resources & Organizations (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Strategic Management (AREA)
  • Computational Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Economics (AREA)
  • Mathematical Analysis (AREA)
  • Development Economics (AREA)
  • Operations Research (AREA)
  • Entrepreneurship & Innovation (AREA)
  • General Business, Economics & Management (AREA)
  • Databases & Information Systems (AREA)
  • Game Theory and Decision Science (AREA)
  • Tourism & Hospitality (AREA)
  • Algebra (AREA)
  • Quality & Reliability (AREA)
  • Marketing (AREA)
  • Educational Administration (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention discloses a small celestial body collaborative attachment robust obstacle avoidance control method, and belongs to the technical field of deep space exploration. The implementation method of the invention comprises the following steps: aiming at the multi-obstacle terrain on the surface of a small celestial body, considering the influence of state uncertainty on obstacle avoidance in the process of multi-detector collaborative attachment, adopting a fully-symmetrical multi-cell shape to quickly estimate the detector reachable set boundary, constructing a collaborative detection collision prediction model, and quantitatively evaluating collision threats suffered by a collaborative detection system; further designing an obstacle avoidance performance index based on the state of the safety target, and realizing autonomous adjustment of the control target from accurate attachment to obstacle avoidance in the effective interval range; and finally, solving the prediction control problem of the dynamic index model on line to obtain a collaborative attachment robust obstacle avoidance control law and improve the safety and reliability of the task under the condition of uncertain state.

Description

Small celestial body collaborative attachment robust obstacle avoidance control method
Technical Field
The invention relates to a small celestial body collaborative attachment robust obstacle avoidance control method, and belongs to the technical field of deep space exploration.
Background
The small celestial body has complex dynamic environment, rugged surface appearance and poor prior information, and is very difficult to realize safe and accurate attachment on the surface. In a future small celestial body detection task, the reliability and the autonomy of the task can be effectively improved by adopting a multi-detector cooperative detection mode, and the observation and control precision is improved through the mutual cooperation between detectors, so that more complex deep space maneuvering operation and scientific tasks are completed. When the cooperative adhesion detection is carried out, the diverse heterogeneous morphology on the surface of the small celestial body threatens the safety of the detector. In order to avoid collision between the detector and a small celestial body surface obstacle in the descending process, methods such as a potential function method, obstacle avoidance track optimization, track curvature design and the like are respectively proposed in documents to improve the safety of an attachment task. On the basis, the characteristics of the cooperative detection mode are combined, the configuration constraint among detectors is considered, the distance between each detector and nearby obstacles is comprehensively considered, and the safety threat suffered by the cooperative detection system is accurately evaluated. In addition, aiming at the influence of the uncertainty of the detector state on obstacle avoidance in the descending process, a safe expansion ellipsoid is constructed to estimate the potential collision probability, and the attachment risk is reduced by optimizing a track. When uncertainty exists in the position and the speed of the multi-detector at the same time, the collision threat brought by the evaluation environment becomes more complex and troublesome, and a control method with stronger robustness is needed to realize the safe and accurate task attachment under the uncertainty of the state of the multi-detector.
Disclosure of Invention
The invention discloses a small celestial body collaborative attachment robust obstacle avoidance control method, which aims to solve the technical problems that: aiming at a small celestial body surface multi-obstacle terrain, the safety threat of a multi-detector system under the condition of uncertain state is considered, the potential collision risk is timely found and avoided by constructing an on-satellite collision prediction mechanism, the multi-detector control strategy is adjusted on line, the autonomous switching between accurate attachment and obstacle avoiding targets is realized under the condition of satisfying configuration constraint, a collaborative obstacle avoidance robust control law is generated, the robust obstacle avoidance control under the collaborative mode is realized, and the safety and reliability of tasks under the condition of uncertain state are improved.
The purpose of the invention is realized by the following technical scheme.
The invention discloses a small celestial body collaborative attachment robust obstacle avoidance control method, which considers the influence of state uncertainty on obstacle avoidance in a multi-detector collaborative attachment process, adopts a fully-symmetrical multi-cell shape to quickly estimate a detector reachable set boundary, constructs a collaborative detection collision prediction model, and quantitatively evaluates collision threats suffered by a collaborative detection system; further designing an obstacle avoidance performance index based on the state of the safety target, and realizing autonomous adjustment of the control target from accurate attachment to obstacle avoidance in the effective interval range; and finally, solving the prediction control problem of the dynamic index model on line to obtain a collaborative attachment robust obstacle avoidance control law and improve the safety and reliability of the task under the condition of uncertain state.
The invention discloses a small celestial body collaborative attachment robust obstacle avoidance control method, which comprises the following steps:
the method comprises the steps of firstly, considering multi-detector state uncertainty, combining a cooperative detection consistency algorithm, adopting a full-symmetry multi-cell shape to quickly estimate reachable set boundaries of a main detector and a following detector, constructing a cooperative detection collision prediction model, quantitatively evaluating collision threats suffered by a system, guaranteeing online estimation efficiency and improving robustness of an estimation result.
The kinetic equation of the detector is xk+1＝f(xk,uk,wk)＝g(xk,uk)+G(xk,uk)wk (1)
Wherein the detector k time state
Figure BDA0002570230200000021
From position rkAnd velocity vkThe structure of the utility model is that the material,
Figure BDA0002570230200000022
in order to control the amount of the liquid,
Figure BDA0002570230200000023
is a bounded environmental disturbance. The multi-detector system is descendingIn the process, a master-slave structure is adopted, and the control quantity u of a master detector AAOn-line solving, following detector B1,B2,...BLAnd the main detector keeps a fixed configuration and the speed is consistent during descending. Recording following detector BiThe ideal configuration relative to the main detector A is ri dThen ideally follows detector BiIn the position of
Figure BDA0002570230200000024
At a speed of
Figure BDA0002570230200000025
Wherein r isA,vARespectively the position and the speed of the main detector a. Following detector B according to a consistency algorithmiIs given by
Figure BDA0002570230200000026
Wherein gamma isi1,γi2Is a normal number, rBi,vBiRespectively follow detector BiThe true position and velocity of the vehicle. According to the state estimation result of the navigation system, the state of the main detector A at the moment k is
Figure BDA0002570230200000027
Following detector BiIn a state of
Figure BDA0002570230200000028
To construct the co-detection collision prediction model, the reachable set boundaries of the primary detector A are first estimated. Using holosymmetric polytope to pair main detector state uncertainty XAAre described in detail
Figure BDA0002570230200000029
Wherein
Figure BDA00025702302000000210
Estimate mean, H, for the primary detector stateAEstimating an error interval matrix for the state, BmIs a square grid formed by m unit intervals, and z is BmThe Minkowski sum between two members of an arbitrary vector is defined as
Figure BDA0002570230200000031
Bounded environmental disturbance W suffered by main detector AAIs expressed as
Figure BDA0002570230200000032
Wherein c iswAIs the mean value of environmental disturbances, CwAIs a matrix of the interval of the environmental disturbance,
Figure BDA0002570230200000033
is mwA square grid formed by unit intervals. For any fully symmetric polytope set
Figure BDA0002570230200000034
Where p is an n-dimensional real number vector, M is a real number interval matrix, BmFor a square grid of m unit intervals, the outer envelope is defined as
Figure BDA0002570230200000035
Where mid (M) is the center of the interval matrix and the diagonal elements of the diagonal matrix G are
Figure BDA0002570230200000036
diam(Mij) For the interval length, there are
Figure BDA0002570230200000037
Combining the dynamic equation (1) of the detector to obtain the uncertainty X of the main detector in the current stateADisturbance W from a bounded environmentALower reachable set boundary function
Figure BDA0002570230200000038
Figure BDA0002570230200000039
In which the real number vector qAAnd interval matrix SAIs determined by
qA＝g(pA,uA)+G(pA,uA)cwA (9)
SA＝G(pA,uA)CwA (10)
Interval matrix
Figure BDA00025702302000000310
Calculated by an interval algorithm.
Following detector B according to structural constraint between formula (2) and detectoriIs estimated. Following detector state uncertainty X also using a fully symmetric multi-cell pairBiWith the received bounded environmental disturbance WBiA description will be given. For any element in the state estimation error set
Figure BDA00025702302000000311
Any element in the boundary function of reachable set with main detector
Figure BDA00025702302000000312
The following detector control law is determined by
Figure BDA00025702302000000313
Following detector BiReachable set boundary function of
Figure BDA00025702302000000314
In which the real number vector qBiInterval matrix SBiAnd MBiThe determination is made in the same manner as the main detector.
Reachable set boundary function psi in combination with main detectorA(XA,uA,WA) Reachable set boundary function psi with following detectorB1(XB1,uB1,WB1),ψB2(XB2,uB2,WB2),...,ψBL(XBL,uBL,WBL) And constructing a cooperative detection collision prediction model. Considering that obstacles are continuously and densely distributed on the surface of the small celestial body, a polyhedron model is adopted to approximate the complex terrain of the small celestial body, and each approximate plane is recorded as
Figure BDA0002570230200000041
Wherein
Figure BDA0002570230200000042
Are fitting parameters. The minimum distance between the multi-detector system and the approximate plane, namely the residual safety distance under one-step prediction is
Figure BDA0002570230200000043
Therein ΨrFor set psi ═ psiA∪ψB1∪ψB2∪...∪ψBLProjection in position space, r ═ x, y, z]TSet ΨrAny one of (1) or (b) in the composition,
Figure BDA0002570230200000044
is an approximate plane closest to the multi-detector system, and the corresponding evading direction is the normal direction of the approximate plane
Figure BDA0002570230200000045
Obtaining a cooperative detection collision prediction model according to the formulas (14) to (15) from the remaining safe distance drQuantitative evaluation is realized on environmental collision threats suffered by the multi-detector system in two aspects of avoidance direction eta, and the robustness of an estimation result is improved while the on-line estimation efficiency is ensured.
And step two, setting an obstacle avoidance effective interval according to the cooperative detection collision prediction model obtained in the step one, designing a safe cooperative target state, forming an obstacle avoidance performance index, and realizing autonomous adjustment of the weight of the obstacle avoidance index in the descending process.
On the basis of a collaborative detection collision prediction model, in order to avoid collision between a multi-detector system and a small celestial body surface obstacle, a safe collaborative target state is designed, and when a detector is close to the small celestial body surface, the multi-detector system is driven to autonomously transfer to the safe collaborative target state, so that the adhesion safety is improved.
Considering that no obstacle avoidance requirement exists when the multiple detectors are far away from the surface of the small celestial body, and the requirement of minimum obstacle avoidance transfer distance is difficult to meet when the multiple detectors are too close to the surface of the small celestial body, an obstacle avoidance effective interval is set to be [ epsilon ]l,εu]That is, the remaining safety distance d obtained if and only if equation (14)r[εl,εu]And then, the cooperative detection system carries out obstacle avoidance transfer. Since the movement of the main detector A determines the movement of the entire cooperative detection system, a safe cooperative target position r is set for the main detector AsafeAnd velocity vsafeAre respectively as
Figure BDA0002570230200000046
Figure BDA0002570230200000051
Wherein
Figure BDA0002570230200000052
Current position r of main detector AAIn an approximate plane
Figure BDA0002570230200000053
Projection ofmaxThe maximum movement speed of the cooperative detection system. Thereby obtaining a safe cooperative target state
Figure BDA0002570230200000054
Further designing obstacle avoidance performance index based on safety cooperative target state
Figure BDA0002570230200000055
Where N is the number of predicted steps, xA(k) And Q' is a dynamic weight matrix which is the state of the main detector in the t step. In order to realize autonomous adjustment of a control target from accurate attachment to obstacle avoidance in an effective interval range, a weight matrix Q' is designed as a function of a remaining safety distance
Figure BDA0002570230200000056
Wherein
Figure BDA0002570230200000057
And (3) realizing the autonomous adjustment of the obstacle avoidance index weight in the descending process according to the formulas (19) and (20).
And step three, combining the obstacle avoidance performance indexes formed in the step two, solving the mixed dynamic index model prediction control problem of the main detector on line to obtain a collaborative attachment robust obstacle avoidance control law, realizing collaborative attachment detection obstacle avoidance under the condition of uncertain state, and improving the safety and reliability of the task under the condition of uncertain state.
The main detector control vector is obtained by solving the model predictive control problem on line, and aiming at the following mixed dynamic indexes according to the obstacle avoidance performance index in the formula (19)
Figure BDA0002570230200000058
Forming optimal control problem on rolling time domain
Figure BDA0002570230200000059
s.t.
xA(t+1|j)＝g(xA(t|j),uA(t|j))+G(xA(t|j),uA(t|j))wA(t|j)
Figure BDA0002570230200000061
Figure BDA0002570230200000062
Figure BDA0002570230200000063
Where N is the predicted step number, t is the tth step of the current rolling time domain, xA(t | j) is the state vector of the primary detector from time j to t, uA(tjj) is the control vector from time j to t, P, Q, R are constant weight matrices,
Figure BDA0002570230200000064
the primary detector j time position and velocity,
Figure BDA0002570230200000065
at the end of the primary probefThe position and the speed of the robot are determined,
Figure BDA0002570230200000066
a set of states is allowed for the primary probe,
Figure BDA0002570230200000069
the invariant set is controlled for the detector system,
Figure BDA0002570230200000068
is a feasible control set for the main detector.
And solving the optimization problem, and applying the first item of the obtained optimal control sequence to the main detector. In order to obtain a cooperative attachment robust obstacle avoidance control law, the updated state of the main detector is substituted into the formula (2) to form obstacle avoidance control of the following detector under configuration constraint. And then substituting the new main detector state as an initial state into the problem (23), updating the mixed dynamic index by combining the cooperative detection collision prediction model, and solving the optimal control problem again until the cooperative detection system is attached to the surface of the small celestial body, so that the cooperative attachment detection obstacle avoidance under the uncertain state is realized, and the safety and reliability of the task under the uncertain state are improved.
Has the advantages that:
1. aiming at the influence of position and speed estimation errors on obstacle avoidance in the descending process of a multi-detector, the reachable set boundary of the multi-detector system is quickly estimated by adopting a fully-symmetrical multi-cell shape, a cooperative detection collision prediction model is constructed, an obstacle avoidance effective interval is set, the state of a safe cooperative target is designed, a mixed dynamic index with obstacle avoidance is formed, the rolling time domain optimal control problem is further solved on line, a cooperative attachment robust obstacle avoidance control law is obtained, the autonomous adjustment of a descending process control target is realized, and the safety of the small celestial body cooperative attachment system under the condition of uncertain state is improved.
2. The invention discloses a small celestial body collaborative attachment robust obstacle avoidance control method which adopts a detector master-slave structure, combines configuration and speed constraint among detectors, comprehensively considers the influence of uncertainty of the state of each detector on the safety of a collaborative detection system, and realizes multi-detector collaborative attachment robust obstacle avoidance control.
3. The small celestial body collaborative attachment robust obstacle avoidance control method disclosed by the invention adopts the full-symmetry multi-cell shape to rapidly estimate the reachable set boundary of the detector, carries out quantitative evaluation on the collision threat suffered by the collaborative detection system, ensures the online estimation efficiency and improves the robustness of the estimation result.
Drawings
FIG. 1 is a flow chart of a small celestial body collaborative attachment robust obstacle avoidance control method disclosed by the invention;
FIG. 2 is an attachment trajectory of a multi-detector system without navigational error;
FIG. 3 is an attachment trajectory of a multi-detector system in the presence of navigation errors;
FIG. 4 is a diagram showing the variation of the position, speed and control amount of the detector A during the descending process;
FIG. 5 shows a detector B during descent1The position, speed and control variable variation curve of the control unit;
FIG. 6 shows a detector B during descent2The position, speed and control variable variation curve of the control unit;
FIG. 7 is a graph showing the variation of the position estimation error of the detector A in the X, Y, and Z axes during the descent;
FIG. 8 is a graph of the error in the velocity estimation of probe A along the X, Y, and Z axes during descent.
Detailed Description
For a better understanding of the objects and advantages of the present invention, reference should be made to the following detailed description taken in conjunction with the accompanying drawings and examples.
Example 1:
in order to verify the feasibility of the method, a asteroid landing dynamic model is established based on the asteroid 433 Eros, and the asteroid rotation angular velocity omega is 3.31 multiplied by 10-4rad/s, density ρ 2.67 × 103kg/m3Gravity constant G6.67 × 10-11N·m2/kg2. Using a master-slave mode of detection, i.e. one master detector A and two follower detectors B1,B2Initial position r of main detector AA0＝[5000,7000,1000]Tm, target landing point position rAf＝[4470,4980,600.1]Tm, following detector B1Initial position rB10＝[5030,7000,1040]Tm, target landing point position rB1f＝[4480,5000,610]Tm, following detector B2Initial position rB20＝[4980,7000,1000]Tm, target landing point position rB2f＝[4460,5000,610]Tm, all detectors have the same initial velocity and the ideal end velocity is the same, i.e. vA0＝vB10＝vB20＝[1,-2,-1]Tm/s，vAf＝vB1f＝vB2f＝[0,0,0]Tm/s。
As shown in fig. 1, the small celestial body collaborative attachment robust obstacle avoidance control method disclosed in this embodiment includes the following specific implementation steps:
the method comprises the steps of firstly, considering multi-detector state uncertainty, combining a cooperative detection consistency algorithm, adopting a full-symmetry multi-cell shape to quickly estimate reachable set boundaries of a main detector and a following detector, constructing a cooperative detection collision prediction model, quantitatively evaluating collision threats suffered by a system, guaranteeing online estimation efficiency and improving robustness of an estimation result.
Establishing a kinetic equation under a small celestial body centroid fixed connection system
Figure BDA0002570230200000081
Wherein the gravitational acceleration g (r) is given by a polyhedral gravitational field model, w is the sunlight pressure borne by the detector, the third body gravitational force and other environmental disturbances, and w-N (0,1 e) is satisfied-4)。
Following detector B1,B2And the main detector keeps a fixed configuration and the speed is consistent during descending. Following detector B1The ideal configuration relative to the main detector A is r1 d＝[10,0,10]TFollowing detector B2The ideal configuration relative to the main detector A is
Figure BDA0002570230200000082
Following detectorBiControl law of (1) coefficient gamma11＝γ21＝0.01,γ12＝γ220.1. When there is no navigation error, the attachment trajectory of the multi-detector system changes as shown in fig. 2, and the three detectors maintain a strict configuration and the trajectories are substantially consistent.
To construct the co-detection collision prediction model, the reachable set boundaries of the primary detector A are first estimated. State uncertainty X of the main detector by adopting the fully-symmetrical multi-cell pair of the formula (3)ADescribing that m is 6, the mean value of the main detector state estimation at the initial moment
Figure BDA0002570230200000083
State estimation error interval matrix
Figure BDA0002570230200000084
Bounded environmental disturbance W suffered by main detector AAIs given by formula (5), wherein mwMean value of environmental disturbance c ═ 3wA0, the environmental disturbance interval matrix CwA＝3e-2I3. From this, in combination with the probe kinetics equation (23), consider [ -0.1,0.1 [ - ]]m/s2Obtaining uncertain X of the main detector in the current state according to the formula (8)ADisturbance W from a bounded environmentAReachable set boundary function psi for lower 10sA(XA,uA,WA)。
Follower detector B in this embodimenti(i ═ 1,2) the state estimate is accurate, i.e.
Figure BDA0002570230200000091
Then follows the detector BiReachable set boundary function psiBi(XBi,uBi,WBi) To follow the detector state as
Figure BDA0002570230200000092
Main detectorThe state is
Figure BDA0002570230200000093
Then, the system 10s using the control law shown in equation (11) can reach the set boundary.
Reachable set boundary function psi in combination with main detectorA(XA,uA,WA) Reachable set boundary function psi with following detectorBi(XBi,uBi,WBi) And constructing a cooperative detection collision prediction model. Adopting a polyhedral model to carry out alignment on a main detector target landing point rAf＝[4470,4980,600.1]TApproximating the complex terrain of 100m multiplied by 100m in the range of m nearby, and obtaining the cooperative detection collision prediction model according to the equations (14) to (15) from the residual safe distance drAnd quantitative evaluation is realized on the environmental collision threat suffered by the multi-detector system in the two aspects of the avoiding direction eta, so that the online estimation efficiency is ensured, and the robustness of the estimation result is improved.
And step two, setting an obstacle avoidance effective interval according to the cooperative detection collision prediction model obtained in the step one, designing a safe cooperative target state, forming an obstacle avoidance performance index, and realizing autonomous adjustment of the weight of the obstacle avoidance index in the descending process.
Considering that no obstacle avoidance requirement exists when the multiple detectors are far away from the surface of the small celestial body, and the requirement of minimum obstacle avoidance transfer distance is difficult to meet when the multiple detectors are too close to the surface of the small celestial body, an obstacle avoidance effective interval is set to be [2,20 ]]m, i.e. the remaining safety distance d obtained if and only if equation (14)r∈[2,20]And m, carrying out obstacle avoidance transfer by the cooperative detection system. Setting a safety cooperative target position r according to equations (16) to (17) for the main probe AsafeAnd velocity vsafeWherein the upper and lower boundaries of the obstacle avoidance effective interval are respectively epsilonl＝2m,εu20m, maximum movement speed v of cooperative detection systemmax5m/s, thereby obtaining a safe cooperative target state
Figure BDA0002570230200000094
Further obtaining an obstacle avoidance performance index based on the safe cooperative target state as shown in the formula (19)Where the predicted step number N is 8, the dynamic weight matrix Q' is a function of the remaining safe distance, and its value in the descent is given by equation (20), where
Figure BDA0002570230200000095
And (3) realizing the autonomous adjustment of the obstacle avoidance index weight in the descending process according to the formulas (19) and (20).
And step three, combining the obstacle avoidance performance indexes formed in the step two, solving the mixed dynamic index model prediction control problem of the main detector on line to obtain a collaborative attachment robust obstacle avoidance control law, realizing collaborative attachment detection obstacle avoidance under the condition of uncertain state, and improving the safety and reliability of the task under the condition of uncertain state.
And the main detector control vector is obtained by solving a model predictive control problem on line, and aiming at a mixed dynamic index given by a formula (21), an optimal control problem (22) is formed on a rolling time domain, wherein the predicted step number N is 8, the step length is 10s, and weight matrixes corresponding to the intermediate state and the control variable are Q and I respectively6,R＝I3The terminal state weight matrix P is obtained by solving the interior kaki equation. Solving the nonlinear model predictive control problem by adopting a YALMIP tool package, and applying the first item of the obtained optimal control sequence on the main detector. In order to obtain a cooperative attachment robust obstacle avoidance control law, the updated state of the main detector is substituted into the formula (2) to form obstacle avoidance control of the following detector under configuration constraint. And then substituting the new main detector state as an initial state into a problem (22), updating a mixed dynamic index by combining a cooperative detection collision prediction model, and solving the optimal control problem again until a cooperative detection system is attached to the surface of the small celestial body, so that cooperative attachment detection obstacle avoidance under uncertain states is realized, and the safety and reliability of tasks under uncertain states are improved.
The attachment trajectory of the multi-detector system obtained by considering the navigation error is shown in FIG. 3, in which the solid line is the attachment trajectory of the detector A, and the dotted line is the detector B1The dotted line is the probe B2The attachment trajectory of (2). FIGS. 4-6 show the position and velocity of the three detectors in the X, Y and Z axes during descent, respectivelyAnd the change of the degree and the controlled variable, wherein a solid line is a change curve of each variable on an X axis, a dotted line is a change curve of each variable on a Y axis, and a dotted line is a change curve of each variable on a Z axis. Fig. 7-8 show the variation curves of the estimated error of the position and the speed of the detector a in the X, Y and Z axes during the movement process, respectively, wherein the estimated error of the position and the speed decreases with the decrease of the distance between the detector a and the minor planet surface, which means that the navigation precision is continuously increased during the descending process, and the control precision of the following detector is also continuously increased.
The above detailed description is intended to illustrate the objects, aspects and advantages of the present invention, and it should be understood that the above detailed description is only exemplary of the present invention and is not intended to limit the scope of the present invention, and any modifications, equivalents, improvements and the like made within the spirit and principle of the present invention should be included in the scope of the present invention.

Claims (4)

1. The small celestial body collaborative attachment robust obstacle avoidance control method is characterized by comprising the following steps of: comprises the following steps of (a) carrying out,
considering multi-detector state uncertainty, combining a cooperative detection consistency algorithm, and rapidly estimating reachable set boundaries of a main detector and a following detector by adopting a full-symmetry multi-cell shape to construct a cooperative detection collision prediction model, so that collision threats suffered by a system are quantitatively evaluated, online estimation efficiency is guaranteed, and meanwhile, the robustness of an estimation result is improved;
step two, setting an obstacle avoidance effective interval according to the collaborative detection collision prediction model obtained in the step one, designing a safe collaborative target state, forming an obstacle avoidance performance index, and realizing autonomous adjustment of the weight of the obstacle avoidance index in the descending process;
on the basis of a cooperative detection collision prediction model, in order to avoid collision between a multi-detector system and a small celestial body surface obstacle, a safe cooperative target state is designed, and when a detector is close to the small celestial body surface, the multi-detector system is driven to autonomously transfer to the safe cooperative target state, so that the adhesion safety is improved;
and step three, combining the obstacle avoidance performance indexes formed in the step two, solving the mixed dynamic index model prediction control problem of the main detector on line to obtain a collaborative attachment robust obstacle avoidance control law, realizing collaborative attachment detection obstacle avoidance under the condition of uncertain state, and improving the safety and reliability of the task under the condition of uncertain state.
2. The small celestial body collaborative attachment robust obstacle avoidance control method of claim 1, characterized in that: the first implementation method comprises the following steps of,
the dynamic equation of the detector is
xk+1＝f(xk,uk,wk)＝g(xk,uk)+G(xk,uk)wk (1)
Wherein the detector k time state
Figure FDA0002570230190000011
From position rkAnd velocity vkThe structure of the utility model is that the material,
Figure FDA0002570230190000012
in order to control the amount of the liquid,
Figure FDA0002570230190000013
is a bounded environmental perturbation; the multi-detector system adopts a master-slave structure in the descending process, and the control quantity u of the master detector AAOn-line solving, following detector B1,B2,...BLThe device and the main detector keep a fixed configuration in the descending process and the speed is consistent; recording following detector BiThe ideal configuration relative to the main detector A is ri dThen ideally follows detector BiIn the position of
Figure FDA0002570230190000014
At a speed of
Figure FDA0002570230190000015
Wherein r isA,vAThe position and speed of the main detector A respectively; following detector B according to a consistency algorithmiIs given by
Figure FDA0002570230190000016
Wherein gamma isi1,γi2Is a normal number, rBi,vBiRespectively follow detector BiThe true position and velocity of the vehicle; according to the state estimation result of the navigation system, the state of the main detector A at the moment k is
Figure FDA0002570230190000017
Following detector BiIn a state of
Figure FDA0002570230190000018
In order to construct a cooperative detection collision prediction model, firstly, estimating an reachable set boundary of a main detector A; using holosymmetric polytope to pair main detector state uncertainty XAAre described in detail
Figure FDA0002570230190000019
Wherein
Figure FDA00025702301900000110
Estimate mean, H, for the primary detector stateAEstimating an error interval matrix for the state, BmIs a square grid formed by m unit intervals, and z is BmThe Minkowski sum between two members of an arbitrary vector is defined as
Figure FDA0002570230190000021
Bounded environmental disturbance W suffered by main detector AAIs expressed as
Figure FDA0002570230190000022
Wherein c iswAIs the mean value of environmental disturbances, CwAIs a matrix of the interval of the environmental disturbance,
Figure FDA0002570230190000023
is mwA grid formed by unit intervals; for any fully symmetric polytope set
Figure FDA0002570230190000024
Where p is an n-dimensional real number vector, M is a real number interval matrix, BmFor a square grid of m unit intervals, the outer envelope is defined as
Figure FDA0002570230190000025
Where mid (M) is the center of the interval matrix and the diagonal elements of the diagonal matrix G are
Figure FDA0002570230190000026
diam(Mij) For the interval length, there are
Figure FDA0002570230190000027
Combining the dynamic equation (1) of the detector to obtain the uncertainty X of the main detector in the current stateADisturbance W from a bounded environmentALower reachable set boundary function
Figure FDA0002570230190000028
Figure FDA0002570230190000029
In which the real number vector qAAnd interval matrix SAIs determined by
qA＝g(pA,uA)+G(pA,uA)cwA (9)
SA＝G(pA,uA)CwA (10)
Interval matrix
Figure FDA00025702301900000210
Calculating by an interval algorithm;
following detector B according to structural constraint between formula (2) and detectoriEstimating the state of (c); following detector state uncertainty X also using a fully symmetric multi-cell pairBiWith the received bounded environmental disturbance WBiThe description is carried out; for any element in the state estimation error set
Figure FDA00025702301900000211
Any element in the boundary function of reachable set with main detector
Figure FDA00025702301900000212
The following detector control law is determined by
Figure FDA00025702301900000213
Following detector BiReachable set boundary function of
Figure FDA00025702301900000214
In which the real number vector qBiInterval matrix SBiAnd MBiThe determination mode is the same as that of the main detector;
reachable set boundary function psi in combination with main detectorA(XA,uA,WA) Reachable set boundary function psi with following detectorB1(XB1,uB1,WB1),ψB2(XB2,uB2,WB2),...,ψBL(XBL,uBL,WBL) Constructing a cooperative detection collision prediction model; considering that obstacles are continuously and densely distributed on the surface of the small celestial body, a polyhedron model is adopted to approximate the complex terrain of the small celestial body, and each approximate plane is recorded as
Figure FDA0002570230190000031
Wherein
Figure FDA0002570230190000032
Is a fitting parameter; the minimum distance between the multi-detector system and the approximate plane, namely the residual safety distance under one-step prediction is
Figure FDA0002570230190000033
Therein ΨrFor set psi ═ psiA∪ψB1∪ψB2∪...ψBLProjection in position space, r ═ x, y, z]TSet ΨrAny one of (1) or (b) in the composition,
Figure FDA0002570230190000034
is an approximate plane closest to the multi-detector system, and the corresponding evading direction is the normal direction of the approximate plane
Figure FDA0002570230190000035
Obtaining a cooperative detection collision prediction model according to the formulas (14) to (15) from the remaining safe distance drQuantitative evaluation on environmental collision threat suffered by multi-detector system in two aspects of avoidance direction etaAnd estimating, and improving the robustness of the estimation result while ensuring the online estimation efficiency.
3. The small celestial body collaborative attachment robust obstacle avoidance control method of claim 2, characterized in that: the second step is realized by the method that,
considering that no obstacle avoidance requirement exists when the multiple detectors are far away from the surface of the small celestial body, and the requirement of minimum obstacle avoidance transfer distance is difficult to meet when the multiple detectors are too close to the surface of the small celestial body, an obstacle avoidance effective interval is set to be [ epsilon ]l,εu]That is, the remaining safety distance d obtained if and only if equation (14)r∈[εl,u]Then, the cooperative detection system carries out obstacle avoidance transfer; since the movement of the main detector A determines the movement of the entire cooperative detection system, a safe cooperative target position r is set for the main detector AsafeAnd velocity vsafeAre respectively as
Figure FDA0002570230190000036
Figure FDA0002570230190000037
Wherein
Figure FDA0002570230190000038
Current position r of main detector AAIn an approximate plane
Figure FDA0002570230190000039
Projection ofmaxThe maximum movement speed of the cooperative detection system is obtained; thereby obtaining a safe cooperative target state
Figure FDA00025702301900000310
Further designing obstacle avoidance performance index based on safety cooperative target state
Figure FDA00025702301900000311
Where N is the number of predicted steps, xA(k) Taking the state of the main detector in the t step, wherein Q' is a dynamic weight matrix; in order to realize autonomous adjustment of a control target from accurate attachment to obstacle avoidance in an effective interval range, a weight matrix Q' is designed as a function of a remaining safety distance
Figure FDA0002570230190000041
Wherein
Figure FDA0002570230190000042
And (3) realizing the autonomous adjustment of the obstacle avoidance index weight in the descending process according to the formulas (19) and (20).
4. The small celestial body collaborative attachment robust obstacle avoidance control method of claim 3, characterized in that: the third step is to realize the method as follows,
the main detector control vector is obtained by solving the model predictive control problem on line, and aiming at the following mixed dynamic indexes according to the obstacle avoidance performance index in the formula (19)
Figure FDA0002570230190000043
Forming optimal control problem on rolling time domain
Figure FDA0002570230190000044
s.t.
xA(t+1|j)＝g(xA(t|j),uA(t|j))+G(xA(t|j),uA(t|j))wA(t|j)
Figure FDA0002570230190000045
Figure FDA0002570230190000046
Figure FDA0002570230190000047
Where N is the predicted step number, t is the tth step of the current rolling time domain, xA(t | j) is the state vector of the primary detector from time j to t, uA(tjj) is the control vector from time j to t, P, Q, R are constant weight matrices,
Figure FDA0002570230190000048
the primary detector j time position and velocity,
Figure FDA0002570230190000049
at the end of the primary probefThe position and the speed of the robot are determined,
Figure FDA00025702301900000410
a set of states is allowed for the primary probe,
Figure FDA00025702301900000411
the invariant set is controlled for the detector system,
Figure FDA00025702301900000412
is a feasible control set of the main detector;
solving the optimization problem to obtain an optimal control sequence first item applied to a main detector; in order to obtain a cooperative attachment robust obstacle avoidance control law, the updated state of the main detector is substituted into the formula (2) to form obstacle avoidance control of the following detector under configuration constraint; and then substituting the new main detector state as an initial state into the problem (23), updating the mixed dynamic index by combining the cooperative detection collision prediction model, and solving the optimal control problem again until the cooperative detection system is attached to the surface of the small celestial body, so that the cooperative attachment detection obstacle avoidance under the uncertain state is realized, and the safety and reliability of the task under the uncertain state are improved.
CN202010639213.4A 2020-07-06 2020-07-06 Small celestial body collaborative attachment robust obstacle avoidance control method Active CN111766883B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010639213.4A CN111766883B (en) 2020-07-06 2020-07-06 Small celestial body collaborative attachment robust obstacle avoidance control method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010639213.4A CN111766883B (en) 2020-07-06 2020-07-06 Small celestial body collaborative attachment robust obstacle avoidance control method

Publications (2)

Publication Number Publication Date
CN111766883A CN111766883A (en) 2020-10-13
CN111766883B true CN111766883B (en) 2021-06-15

Family

ID=72723764

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010639213.4A Active CN111766883B (en) 2020-07-06 2020-07-06 Small celestial body collaborative attachment robust obstacle avoidance control method

Country Status (1)

Country Link
CN (1) CN111766883B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112269390B (en) * 2020-10-15 2021-09-21 北京理工大学 Small celestial body surface fixed-point attachment trajectory planning method considering bounce
CN112631285B (en) * 2020-12-08 2021-11-23 北京理工大学 Method for quickly generating small celestial body attachment autonomous obstacle avoidance track
CN114003015B (en) * 2021-10-11 2023-06-23 清华大学深圳国际研究生院 Active fault diagnosis method and device under mixed uncertainty and storage medium

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6182011B1 (en) * 1996-04-01 2001-01-30 The United States Of America As Represented By The Administrator Of National Aeronautics And Space Administration Method and apparatus for determining position using global positioning satellites
CN1851597A (en) * 2006-05-17 2006-10-25 哈尔滨工业大学 Deep space detector soft landing autonomic obstruction evasion control method
CN102173313A (en) * 2010-12-24 2011-09-07 北京控制工程研究所 Soft landing relay obstacle avoiding method
CN104267734A (en) * 2014-08-01 2015-01-07 北京理工大学 Mars complex terrain region safe landing trajectory generation method with minimum fuel consumption
CN105091801A (en) * 2014-05-06 2015-11-25 上海新跃仪表厂 Landing detecting sensor and landing area obstacle detecting method for planetoid detection
CN105739537A (en) * 2016-03-29 2016-07-06 北京理工大学 Active control method for adhesion motion on small celestial body surface
CN108594802A (en) * 2018-02-28 2018-09-28 北京控制工程研究所 The determination of detector target touchdown area and avoidance method of guidance and device
CN110104219A (en) * 2019-04-24 2019-08-09 中国人民解放军63920部队 A kind of method and device controlling detector landing objects outside Earth

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6182011B1 (en) * 1996-04-01 2001-01-30 The United States Of America As Represented By The Administrator Of National Aeronautics And Space Administration Method and apparatus for determining position using global positioning satellites
CN1851597A (en) * 2006-05-17 2006-10-25 哈尔滨工业大学 Deep space detector soft landing autonomic obstruction evasion control method
CN102173313A (en) * 2010-12-24 2011-09-07 北京控制工程研究所 Soft landing relay obstacle avoiding method
CN105091801A (en) * 2014-05-06 2015-11-25 上海新跃仪表厂 Landing detecting sensor and landing area obstacle detecting method for planetoid detection
CN104267734A (en) * 2014-08-01 2015-01-07 北京理工大学 Mars complex terrain region safe landing trajectory generation method with minimum fuel consumption
CN105739537A (en) * 2016-03-29 2016-07-06 北京理工大学 Active control method for adhesion motion on small celestial body surface
CN108594802A (en) * 2018-02-28 2018-09-28 北京控制工程研究所 The determination of detector target touchdown area and avoidance method of guidance and device
CN110104219A (en) * 2019-04-24 2019-08-09 中国人民解放军63920部队 A kind of method and device controlling detector landing objects outside Earth

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
Probability-based hazard avoidance guidance for planetary landing;Xu Yuan等;《acta astronautic》;20170119;全文 *
Prudent small celestial body landing strategy with risk precautions;Pingyuan Cui等;《Acta Astronautica》;20190918;全文 *
Recent development of autonomous GNC technologies for small celestial body descent and landing;Dantong Ge等;《Progress in Aerospace Sciences》;20190705;全文 *
基于Lyapunov函数的小天体软着陆;胡海静等;《深空探测学报》;20150630;全文 *
小天体安全着陆与表面探测控制方法研究;刘延杰等;《深空探测学报》;20161031;全文 *
小天体自主附着多滑模面鲁棒制导方法研究;袁旭;《深空探测学报》;20151231;全文 *

Also Published As

Publication number Publication date
CN111766883A (en) 2020-10-13

Similar Documents

Publication Publication Date Title
CN111766883B (en) Small celestial body collaborative attachment robust obstacle avoidance control method
CN111413966B (en) Progressive model prediction unmanned planning tracking cooperative control method
CN108326852B (en) Multi-objective optimization space manipulator trajectory planning method
CN109807886A (en) A kind of space non-cooperative target based on prediction arrests strategy
Baek et al. Optimal path planning of a target-following fixed-wing UAV using sequential decision processes
CN112947572B (en) Terrain following-based four-rotor aircraft self-adaptive motion planning method
CN110686683B (en) Planetary landing trajectory online optimization method based on uneven expansion ellipsoid
Jing Behavior dynamics based motion planning of mobile robots in uncertain dynamic environments
CN112000132A (en) Spacecraft obstacle avoidance control method based on ellipsoid description
CN110466808B (en) Convex polygon spacecraft safety control method based on multi-equal collision probability line method
Boivin et al. UAV collision avoidance using cooperative predictive control
CN109765919A (en) Based on etc. collision probabilities collimation method spacecraft close-distance safety method of controlling operation thereof
Kim Control laws to avoid collision with three dimensional obstacles using sensors
CN115081343B (en) Space-based passive detection orbit determination method based on neural network combined with genetic algorithm
Taghavifar et al. A novel optimal path-planning and following algorithm for wheeled robots on deformable terrains
Sakayori et al. Energy-aware trajectory planning for planetary rovers
US11679759B2 (en) System and method for adaptive control of vehicle dynamics
Elmi et al. Path planning using model predictive controller based on potential field for autonomous vehicles
Nieuwenhuisen et al. Local multiresolution trajectory optimization for micro aerial vehicles employing continuous curvature transitions
Alzorgan et al. Actuator trajectory planning for uavs with overhead manipulator using reinforcement learning
Kanjanawanishkul Coordinated path following for mobile robots using a virtual structure strategy with model predictive control
CN110703792B (en) Underwater robot attitude control method based on reinforcement learning
Bellini et al. Information driven path planning and control for collaborative aerial robotic sensors using artificial potential functions
Bae et al. A fuzzy compensated PID controller for formation control of mobile robots
Wang et al. A hierarchical controller for path planning and path following based on model predictive control

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant