Simulation method and apparatus for manipulator apparatus, simulation and control method and apparatus for manipulator apparatus, and control method and apparatus for manipulator apparatus

This application is a 371 of PCT/JP93/00751 filed Jun. 3, 1993.
BACKGROUND OF THE INVENTION
This invention relates to a simulation method and apparatus for performing simulation, a method and apparatus for performing simulation and control, and a method and apparatus for performing control of a manipulator apparatus for use with a spacecraft such as an artificial satellite or a space robot having a manipulator for working on an orbit around the earth or in the space or a robot for hazardous environments having a manipulator for working on the bottom of the sea or in an underground passage or a tunnel.
Investigations for spacecrafts (space robots, artificial satellites and so forth) having a manipulator which performs various operations by an astronaut outside the spacecraft in place of the astronaut actively in order to perform various operations on an orbit, which are forecast in construction and application of a space infrastructure such as a space station or a platform, are proceeded efficiently with safety.
In this instance, it is convenient if an emulation apparatus in the process of development of on-board software (OBS) or an operator supporting apparatus in the process of application of a spacecraft is available.
This similarly applies to the case wherein a manipulator which has a dynamic distance from an operator such as on the bottom of the sea or in an underground passage or a tunnel is operated by programmed operation or remote control.
Conventionally, one of very popular techniques of performing a design analysis or simulation regarding, for example, an attitude control system for an artificial satellite is to construct a dynamic model of an artificial satellite on a computer and perform necessary calculations off-line. In such off-line simulation, simulation proceeds by way of the steps of preparations of input data, dynamics calculation and display of output data (graphic display, time series graph display). And, in this instance, in the off-line simulation, the steps are performed separately one by one.
Further, since a conventional space robot, artificial satellite or the like does not include such a large attitude disturbance source as a manipulator, naturally a cooperative control system between an attitude control system and a manipulator control system or the like is not developed as yet. Consequently, for an attitude control system of the body of a space robot, an artificial satellite or the like, only control systems separate for different axes for rolling, pitching and yawing are designed and applied.
For example, where a manipulator is installed on a space shuttle, such a special system as a system for suppressing a disturbance originating from the manipulator to the attitude control system of the space shuttle is not adopted, and a variation in attitude of the spacecraft involved in driving of the manipulator is ignored.
In a conventional spacecraft having such a manipulator as described above, where the spacecraft does not include means for controlling a disturbance arising from the manipulator, there is a subject in that, when the manipulator is operated, control against an attitude disturbance which arises each time the manipulator is driven cannot be taken into consideration and consequently the variation in attitude of the body of the spacecraft cannot be regulated.
Further, in the development and application of a spacecraft, an emulation apparatus (for simulating the dynamics of a spacecraft) in the process of development and an operator supporting apparatus (for driving a manipulator of the spacecraft efficiently with safety) in the process of application are required essentially. However, the following subjects to be solved are involved in application of a conventional off-line simulator to an emulation apparatus or an operator supporting apparatus.
First, an off-line simulator does not have a real time performance which is required for an emulation apparatus or an operator supporting apparatus. In particular, it cannot sequentially repeat the three steps (refer to FIG. 56) wherein preparations for input data are performed at step CA1 and then dynamics calculation is performed at step CA2, whereafter display of output data (graphic display, time series graph display) is performed at step CA3.
Further, the off-line simulator has another subject in that results of calculation cannot be displayed on the real time basis in the form of graphics or a time series graph, which significantly degrades the efficiency of the development procedure or the supporting faculty for an operator.
Further, in such a system wherein there is a spatial distance between an operator and a manipulator as described above, it is difficult for the operator to work while directly observing the manipulator near by. Consequently, the operator sometimes delivers an excessive position moving instruction or force instruction to the manipulator, and there is the possibility that the manipulator or the body on which the manipulator is mounted may be damaged.
Furthermore, in a system wherein there is a dynamic distance between an operator and a manipulator such as a robot for hazardous environments, if an operator continues to work based only on vital feelings of the operator itself, it sometimes occurs that the operator develops an excessive position moving instruction OF force instruction, and there is the possibility that the manipulator or the body on which the manipulator is mounted may be damaged.
Conventionally, a countermeasure for the possibility that the manipulator or the body on which the manipulator is mounted may be damaged relies entirely upon a delicate operation technique of an operator based on a learning effect or practice of the operator on the knack of operation.
Accordingly, there is a subject to improve the operability of a manipulator by an operator to eliminate the possibility that the body on which the manipulator is mounted may be damaged.
The present invention has been made in view of the subjects described above, and it is a first object of the present invention to provide a simulation method and apparatus for a manipulator apparatus for a spacecraft, a robot for hazardous environments or a like apparatus with a manipulator wherein three-dimensional dynamics of the manipulator apparatus can be calculated on the real time basis and a result of the calculation can be outputted on the real time basis on a display.
It is a second object of the present invention to provide a control method and apparatus for a manipulator apparatus wherein a disturbance provided, upon driving of a manipulator, from the manipulator to a control system for a manipulator body on which the manipulator is supported can be suppressed and the controlling performance of the manipulator apparatus can be kept high and also to provide a simulation and control apparatus for a manipulator apparatus wherein three-dimensional dynamics of the manipulator apparatus can be calculated on the real time basis and a result of the calculation can be outputted on the real time basis on a display and besides, a disturbance provided, upon driving of a manipulator, from the manipulator to a control system for a manipulator body on which the manipulator is supported can be suppressed and the controlling performance of the manipulator apparatus can be kept high.
Further, it is a third object of the present invention to provide a simulation method and apparatus for a manipulator apparatus, a simulation and control method and apparatus for a manipulator apparatus, and a control method and apparatus for a manipulator apparatus wherein a work can be performed efficiently with safety when a manipulator which has a spatial distance or a dynamic distance from an operator therefor is operated by programmed operation or remote control.
DISCLOSURE OF THE INVENTION
Thus, in order to attain the first object described above, according to the present invention as set forth in claim 1, a simulation method for a manipulator apparatus which includes a manipulator and a manipulator body for supporting the manipulator thereon and whose attitude can be controlled by rotation of wheels is characterized in that information of an articulation angle, an articulation angular velocity and an articulation angular acceleration of the manipulator is inputted, and three-dimensional dynamics of the entire manipulator apparatus including an attitude control system is calculated on the real time basis based on the information using the laws of conservation of momentum/angular momentum of the entire system, reverse dynamics or normal dynamics of the wheel system to display motion of the manipulator apparatus by simulation display on a display.
Accordingly, there is an advantage in that three-dimensional dynamics of the manipulator apparatus with a manipulator can be calculated on the real time basis and a result of the calculation can be outputted on the real time basis on the graphic display.
Further, in order to attain the first object described above, according to the present invention as set forth in claim 2, a simulation apparatus for a manipulator apparatus which includes a manipulator and a manipulator body for supporting the manipulator thereon and whose attitude can be controlled by rotation of wheels is characterized in that it comprises a manipulator articulation information inputting section for inputting information of an articulation angle, an articulation angular velocity and an articulation angular acceleration of the manipulator, and a display for displaying motion of the manipulator apparatus by simulation display thereon, and that it further comprises a three-dimensional dynamics real time simulation section for receiving information from the manipulator articulation information inputting section and calculating three-dimensional dynamics of the entire manipulator apparatus including an attitude control system on the real time basis using the laws of conservation of momentum/angular momentum of the entire system, reverse dynamics or normal dynamics of the wheel system to cause the display to display motion of the manipulator apparatus by simulation display.
Meanwhile, according to the present invention as set forth in claim 3, a simulation apparatus for a manipulator apparatus is characterized in that the three-dimensional dynamics real time simulation section includes a manipulator apparatus velocity and angular velocity calculation section for inputting position attitude information and wheel angular velocity information of the manipulator apparatus and manipulator articulation angle and articulation angular velocity information from the manipulator articulation information inputting section and calculating velocity and angular velocity information of the manipulator apparatus using the laws of conservation of momentum/angular momentum of the entire system, a manipulator apparatus position information calculation section for inputting the position information of the manipulator apparatus and velocity and angular velocity information obtained by the manipulator apparatus velocity and angular velocity calculation section at two certain points of time, performing integration of first order for the input information to obtain position information of the manipulator apparatus at a next point of time and outputting the position information to the display, a wheel feedback control torque calculation section for receiving the manipulator apparatus position information from the manipulator apparatus position information calculation section and calculating a wheel feedback control torque, an internal torque and wheel system inertia information calculation section for inputting the manipulator articulation angle and articulation angular velocity information and calculating an internal torque and wheel system inertia information using the normal dynamics of the wheel system, a wheel feed forward torque calculation section for inputting the manipulator articulation angle and articulation angular velocity information and calculating a wheel feed forward torque using binding reverse dynamics, and a wheel dynamics integration section for inputting the wheel rotational angles, wheel angular velocity information, feedback control torque information, internal torque information, wheel system inertia information and feed forward torque information at two points of time and outputting wheel rotational angle and wheel angular velocity information at a next point of time.
Meanwhile, according to the present invention as set forth in claim 4, a simulation apparatus for a manipulator apparatus according to the present invention as set forth in claim 3 is characterized in that the wheel dynamics integration section is constructed as a first order integration section such that, when it is to numerically solve an equation of motion of the entire system, it performs only numerical integration of first order for the wheels from the rotational symmetry of the wheels while it does not require rotational angle information of the wheels.
Further, according to the present invention as set forth in claim 5, a simulation apparatus for a manipulator apparatus is characterized in that the wheel dynamics integration section is constructed as a second order integration section such that wheel angular velocity information is obtained by calculation of integration of first order and then wheel rotational angle information is obtained by calculation of integration of second order.
Meanwhile, according to the present invention as set forth in claim 6, a simulation apparatus for a manipulator apparatus according to the present invention as set forth in claim 5 is characterized in that, when an equation of motion of the entire system is to be solved, from the rotational symmetry of the wheels, the step sizes in numerical integration of first order and numerical integration of second order of the wheels are made different from each other such that the numerical integration of second order is processed by rougher calculation than the numerical integration of first order.
Further, according to the present invention as set forth in claim 7, a simulation apparatus for a manipulator apparatus according to the present invention as set forth in claim 5 is characterized in that, when an equation of motion of the entire system is to be solved, from the rotational symmetry of the wheels, the integration degree numbers in numerical integration of first order and numerical integration of second order of the wheels are made different from each other such that the numerical integration of second order is processed by rougher calculation than the numerical integration of first order.
Meanwhile, according to the present invention as set forth in claim 8, a simulation apparatus for a manipulator apparatus according to the present invention as set forth in claim 5 is characterized in that, when an equation of motion of the entire system is to be solved, from the rotational symmetry of the wheels, the step sizes and the integration degree numbers in numerical integration of first order and numerical integration of second order of the wheels are made different from each other such that the numerical integration of second order is processed by rougher calculation than the numerical integration of first order.
Further, according to the present invention as set forth in claim 9, a simulation apparatus for a manipulator apparatus according to the present invention as set forth in claim 3 is characterized in that, in order to calculate a rotational angle of the wheels at a high speed with a high degree of accuracy, inertial moments of all of the wheels are proportionally multiplied.
Further, according to the present invention as set forth in claim 10, a simulation apparatus for a manipulator apparatus is characterized in that, when rotation of the wheels is to be displayed on the display, an angular velocity of the wheels is displayed in the form wherein it is proportionally multiplied immediately before it is displayed.
Accordingly, by inputting information of an articulation angle, an articulation angular velocity and an articulation angular acceleration of the manipulator of the manipulator apparatus which includes the manipulator and the manipulator body for supporting the manipulator thereon and whose attitude can be controlled by rotation of wheels and calculating three-dimensional dynamics of the entire manipulator apparatus including the attitude control system on the real time basis based on the information using the laws of conservation of momentum/angular momentum of the entire system, reverse dynamics or normal dynamics of the wheel system, motion of the manipulator apparatus can be displayed by simulation display on the display. Consequently, there is an advantage in that three-dimensional dynamics of the entire manipulator apparatus including the control system can be calculated on the real time basis and a result of the calculation can be outputted on the real time basis on the display.
Further, an equation of motion of the entire system can be solved at a high speed and with a high degree of accuracy, and consequently, there is another advantage in that real time calculation of three-dimensional dynamics of the entire manipulator apparatus can be realized readily.
Meanwhile, in order to attain the second object described above, according to the present invention as set forth in claim 11, a simulation and control method for a manipulator apparatus which includes a manipulator and a manipulator body for supporting the manipulator thereon is characterized in that the manipulator apparatus inputs information of an articulation angle, an articulation angular velocity and an articulation angular acceleration of the manipulator and calculates three-dimensional dynamics of the entire manipulator apparatus including an attitude control system on the real time basis based on the information using the laws of conservation of momentum/angular momentum of the entire system, reverse dynamics or normal dynamics of the wheel system to display motion of the manipulator apparatus by simulation display on a display, and the manipulator apparatus includes an attitude control system for controlling an attitude of the manipulator apparatus using the wheels, a manipulator control system for inputting information of an articulation angle, an articulation angular velocity and an articulation angular acceleration of the manipulator and controlling the manipulator, and a cooperative control system for calculating a disturbance originating from the manipulator based on information from the manipulator control system using a required algorithm and supplying the disturbance as a feed forward signal to the attitude control system, and that the cooperative control system takes, in order to compensate for the disturbance originating from the manipulator, an angular momentum or a wheel angular velocity obtained by dividing the angular momentum by a mass of each of the wheels or a torque as a feed forward amount to be delivered to the wheel control system of the attitude control system and calculates an aimed angular momentum or an aimed angular velocity of the wheels from the laws of conservation of momentum/angular momentum or reverse dynamics for the universe to control the wheels.
Further, according to the present invention as set forth in claim 12, a simulation and control method for a manipulator apparatus according to the present invention as set forth in claim 11 is characterized in that, upon application of the laws of conservation of momentum/angular momentum, a constraint that the angular velocity of a manipulator body is 0 is applied to the angular velocity of the manipulator body.
Meanwhile, according to the present invention as set forth in claim 13, a simulation and control method for a manipulator apparatus according to the present invention as set forth in claim 11 is characterized in that, upon application of the laws of conservation of momentum/angular momentum, a constraint that the angular velocity of a manipulator body is a constant is applied to the angular velocity of the manipulator body.
Further, according to the present invention as set forth in claim 14, a simulation and control method for a manipulator apparatus according to the present invention as set forth in claim 11 is characterized in that, upon application of the laws of conservation of momentum/angular momentum, a constraint that the angular velocity of a manipulator body is an arbitrary function of the time is applied to the angular velocity of the manipulator body.
Meanwhile, according to the present invention as set forth in claim 15, a simulation and control method for a manipulator apparatus according to the present invention as set forth in claim 11 is characterized in that, upon application of the reverse dynamics for the universe, a constraint that the angular acceleration of a manipulator body is 0 is applied to the angular acceleration of the manipulator body.
Further, according to the present invention as set forth in claim 16, a simulation and control method for a manipulator apparatus according to the present invention as set forth in claim 11 is characterized in that, upon application of the reverse dynamics for the universe, a constraint that the angular velocity and the angular acceleration of a manipulator body are both 0 is applied to the angular velocity and the angular acceleration of the manipulator body.
Meanwhile, according to the present invention as set forth in claim 17, a simulation and control method for a manipulator apparatus is characterized in that, upon application of the reverse dynamics for the universe, a constraint that the angular velocity of a manipulator body is a constant and the angular acceleration of the manipulator body is 0 is applied to the angular velocity and the angular acceleration of the manipulator body.
Further, according to the present invention as set forth in claim 18, a simulation and control method for a manipulator apparatus according to the present invention as set forth in claim 11 is characterized in that, upon application of the reverse dynamics for the universe, a constraint that the angular acceleration of a manipulator body is an arbitrary function of the time is applied to the angular acceleration of the manipulator body.
Meanwhile, according to the present invention as set forth in claim 19, a simulation and control method for a manipulator apparatus is characterized in that, upon application of the reverse dynamics for the universe, a constraint that the angular velocity and the angular acceleration of a manipulator body are both arbitrary functions of the time is applied to the angular velocity and the angular acceleration of the manipulator body.
Further, according to the present invention as set forth in claim 20, a simulation and control method for a manipulator apparatus according to the present invention as set forth in claim 11 is characterized in that the degree of freedom of the entire system is set to a sum L+6 of the number L of articulations of the manipulator and the degree of freedom of 6 of the position and attitude of the manipulator body.
Meanwhile, according to the present invention as set forth in 21, a simulation and control method for a manipulator apparatus according to the present invention as set forth in claim 11 is characterized in that the attitude control system performs PD feedback control based on an attitude angle of the manipulator apparatus and differentiation of the attitude angle in order to compensate for a disturbance originating from the manipulator.
Further, according to the present invention as set forth in claim 22, a simulation and control method for a manipulator apparatus is characterized in that the attitude control system performs PID feedback control based on an attitude angle of the manipulator apparatus, integration of the attitude angle and differentiation of the attitude angle in order to compensate for a disturbance originating from the manipulator.
Accordingly, there is an advantage in that three-dimensional dynamics of the entire manipulator apparatus can be calculated on the real time basis and a result of the calculation can be outputted on the real time basis on the graphic display and besides a disturbance provided to the control system of the manipulator body upon driving of the manipulator can be suppressed to keep the controlling performance of the manipulator apparatus high.
Meanwhile, in order to attain the second object described above, according to the present invention as set forth in claim 23, a simulation and control apparatus for a manipulator apparatus which includes a manipulator and a manipulator body for supporting the manipulator thereon and whose attitude can be controlled by rotation of wheels is characterized in that it comprises a manipulator articulation information inputting section for inputting information of an articulation angle, an articulation angular velocity and an articulation angular acceleration of the manipulator, a display for displaying motion of the manipulator apparatus by simulation display thereon, and a three-dimensional dynamics real time simulation section for receiving information from the manipulator articulation information inputting section and calculating three-dimensional dynamics of the entire manipulator apparatus including an attitude control system on the real time basis using the laws of conservation of momentum/angular momentum of the entire system, reverse dynamics or normal dynamics of the wheel system to cause the display to display motion of the manipulator apparatus by simulation display, and it further comprises an attitude control system for controlling an attitude of the manipulator apparatus using the wheels, a manipulator control system for inputting information of an articulation angle, an articulation angular velocity and an articulation angular acceleration of the manipulator and controlling the manipulator, and a cooperative control system for calculating a disturbance originating from the manipulator based on the information from the manipulator control system using a required algorithm and supplying the disturbance as a feed forward signal to the attitude control system, and that the cooperative control system takes, in order to compensate for the disturbance originating from the manipulator, an angular momentum or a wheel angular velocity obtained by dividing the angular momentum by a mass of each of the wheels or a torque as a feed forward amount to be delivered to the wheel control system of the attitude control system and calculates an aimed angular momentum or an aimed angular velocity of the wheels from the laws of conservation of momentum/angular momentum or reverse dynamics for the universe to control the wheels.
Further, according to the present invention as set forth in claim 24, a simulation and control apparatus for a manipulator apparatus according to the present invention as set forth in claim 23 is characterized in that the three-dimensional dynamics real time simulation section includes a manipulator apparatus velocity and angular velocity calculation section for inputting position attitude information and wheel angular velocity information of the manipulator apparatus and manipulator articulation angle and articulation angular velocity information from the manipulator articulation information inputting section and calculating velocity and angular velocity information of the manipulator apparatus using the laws of conservation of momentum/angular momentum of the entire system, a manipulator apparatus position information calculation section for inputting the position information of the manipulator apparatus and velocity and angular velocity information obtained by the manipulator apparatus velocity and angular velocity calculation section at two certain points of time, performing integration of first order for the input information to obtain position information of the manipulator apparatus at a next point of time and outputting the position information to the display, a wheel feedback control torque calculation section for receiving the manipulator apparatus position information from the manipulator apparatus position information calculation section and calculating a wheel feedback control torque, an internal torque and wheel system inertia information calculation section for inputting the manipulator articulation angle and articulation angular velocity information and calculating an internal torque and wheel system inertia information using the normal dynamics of the wheel system, a wheel feed forward torque calculation section for inputting the manipulator articulation angle and articulation angular velocity information and calculating a wheel feed forward torque using binding reverse dynamics for the universe, and a wheel dynamics integration section for inputting the wheel rotational angles, wheel angular velocity information, feedback control torque information, internal torque information, wheel system inertia information and feed forward torque information at two points of time and outputting wheel rotational angle and wheel angular velocity information at a next point of time.
Accordingly, there is an advantage in that three-dimensional dynamics of the entire spacecraft including the attitude control system can be calculated on the real time basis and a result of the calculation can be outputted on the real time basis on the graphic display and besides a disturbance provided to the attitude control system of the spacecraft body upon driving of the manipulator can be suppressed to keep the attitude controlling performance of the spacecraft high.
Meanwhile, in order to attain the second object described above, according to the present invention as set forth in claim 25, a control method for a manipulator apparatus which includes a manipulator and a manipulator body for supporting the manipulator thereon and whose attitude can be controlled by rotation of wheels is characterized in that the manipulator apparatus comprises an attitude control system for controlling an attitude of the manipulator apparatus using the wheels, a manipulator control system for controlling the manipulator provided on the manipulator apparatus, and further comprises a cooperative control system for calculating a disturbance originating from the manipulator based on information from the manipulator control system using a required algorithm and supplying the disturbance as a feed forward signal to the attitude control system, and that the cooperative control system takes, in order to compensate for the disturbance originating from the manipulator, an angular momentum or a wheel angular velocity obtained by dividing the angular momentum by a mass of each of the wheels as a feed forward amount to be delivered to the wheel control system of the attitude control system and calculates an aimed angular momentum or an aimed angular velocity of the wheels from the laws of conservation of momentum/angular momentum to control the wheels.
Further, in order to attain the second object described above, according to the present invention as set forth in claim 26, a control method for a manipulator apparatus which includes a manipulator and a manipulator body for supporting the manipulator thereon and whose attitude can be controlled by rotation of wheels is characterized in that the manipulator apparatus comprises an attitude control system for controlling an attitude of the manipulator apparatus using the wheels, a manipulator control system for controlling the manipulator provided on the manipulator apparatus, and further comprises a cooperative control system for calculating a disturbance originating from the manipulator based on information from the manipulator control system using a required algorithm and supplying the disturbance as a feed forward signal to the attitude control system, and that the cooperative control system takes, in order to compensate for the disturbance originating from the manipulator, a torque as a feed forward amount to be delivered to the wheel control system of the attitude control system and calculates a value of the torque from reverse dynamics for the universe to control the wheels.
Accordingly, there is an advantage in that a disturbance provided to the attitude control system of the spacecraft body upon driving of the manipulator can be suppressed and the attitude controlling performance of the spacecraft can be kept high.
Meanwhile, in order to attain the second object described above, according to the present invention as set forth in claim 27, a control method for a manipulator apparatus which includes a manipulator and a manipulator body for supporting the manipulator thereon and whose attitude can be controlled by rotation of wheels is characterized in that the manipulator apparatus comprises an attitude control system for controlling an attitude of the manipulator apparatus using the wheels, a manipulator control system for controlling the manipulator provided on the manipulator apparatus, and further comprises a cooperative control system for calculating a disturbance originating from the manipulator based on information from the manipulator control system using a required algorithm and supplying the disturbance as a feed forward signal to the attitude control system, and that the cooperative control system takes, in order to compensate for the disturbance originating from the manipulator, an angular momentum or a wheel angular velocity obtained by dividing the angular momentum by a mass of each of the wheels or a torque as a feed forward amount to be delivered to the wheel control system of the attitude control system and calculates an aimed angular momentum or an aimed angular velocity of the wheels from the laws of conservation of momentum/angular momentum or reverse dynamics for the universe to control the wheels.
Accordingly, there is an advantage in that a disturbance provided to the control system of the manipulator body upon driving of the manipulator can be suppressed and the controlling performance of the manipulator apparatus can be kept high.
Further, in order to attain the third object described above, according to the present invention as set forth in claim 28, a simulation method for a manipulator apparatus which includes a manipulator and, a manipulator body for supporting the manipulator thereon is characterized in that route instruction value information of the manipulator is inputted as an initial instruction value and information of a disturbance which is produced when the manipulator operates in accordance with the initial instruction value is forecast on the real time basis as an excessive instruction value control index based on a dynamic model regarding the manipulator, and scale conversion is performed for the initial instruction value based on the excessive instruction value control index to obtain an intermediate instruction value, and thereafter, motion of the manipulator is displayed by simulation display in accordance with the intermediate instruction value obtained by the scale conversion.
Meanwhile, according to the present invention as set forth in claim 29, a simulation method for a manipulator apparatus is characterized in that a momentum/angular momentum are forecast on the real time basis as the excessive instruction value control index using the laws of conservation of momentum/angular momentum.
Further, according to the present invention as set forth in claim 30, a simulation method for a manipulator apparatus according to the present invention as set forth in claim 28 is characterized in that a torque/force are forecast on the real time basis as the excessive instruction, value control index using reverse dynamics.
Meanwhile, according to the present invention as set forth in claim 31, a simulation method for a manipulator apparatus according to the present invention as set forth in claim 28 is characterized in that a momentum/angular momentum and a torque/force are forecast on the real time basis as the excessive instruction value control indices using the laws of conservation of momentum/angular momentum and reverse dynamics.
Further, according to the present invention as set forth in claim 32, a simulation method for a manipulator apparatus according to the present invention as set forth in claim 28 is characterized in that, as differentiation information of the initial instruction value which is to be used upon calculation of the excessive instruction value control index, a differential value of the initial instruction value is used after it is processed by averaging processing.
Meanwhile, according to the present invention as set forth in claim 33, a simulation method for a manipulator apparatus is characterized in that exponential conversion calculation is performed when the scale conversion is to be performed.
Further, according to the present invention as set forth in claim 34, a simulation method for a manipulator apparatus is characterized in that the scale conversion processing and spline interpolation calculation processing are combined with each other to produce an aimed route for the manipulator.
Meanwhile, according to the present invention as set forth in claim 35, a simulation method for a manipulator apparatus according to the present invention as set forth in claim 34 is characterized in that spline interpolation calculation processing continuous to differentiation of first order is used as the spline interpolation calculation processing.
Further, according to the present invention as set forth in claim 36, a simulation method for a manipulator apparatus according to the present invention as set forth in claim 34 is characterized in that spline interpolation calculation processing continuous to differentiation of second order is used as the spline interpolation calculation processing.
Accordingly, there is an advantage in that, when the manipulator which has a spatial distance or a dynamic distance from the operator therefor is to be operated by programmed operation or remote control, the present invention can be helpful to operation training of the operator which is performed in order to allow the operator to work efficiently with safety.
Meanwhile, in order to attain the third object described above, according to the present invention as set forth in claim 37, a simulation apparatus for a manipulator apparatus which includes a manipulator and a manipulator body for supporting the manipulator thereon is characterized in that it comprises initial instruction value inputting means for inputting route instruction value information of the manipulator as an initial instruction value, excessive instruction value control index forecasting means for receiving the initial instruction value from the initial instruction value inputting means and forecasting information of a disturbance, which is produced when the manipulator operates in accordance with the initial instruction value, on the real time basis as an excessive instruction value control index based on a dynamic model regarding the manipulator, scale conversion means for performing scale conversion for the initial instruction value based on the excessive instruction value control index forecast by the excessive instruction value control index forecasting means to obtain an intermediate instruction value, and display means for displaying motion of the manipulator by simulation display in accordance with the intermediate instruction value obtained by the conversion of the scale conversion means.
Accordingly, there is an advantage in that, when the manipulator which has a spatial distance or a dynamic distance from the operator therefor is to be operated by programmed operation or remote control, the present invention can be helpful to operation training of the operator which is performed in order to allow the operator to work efficiently with safety.
Further, in order to attain the third object described above, according to the present invention as set forth in claim 38, a simulation and control method for a manipulator apparatus which includes a manipulator and a manipulator body for supporting the manipulator thereon is characterized in that route instruction value information of the manipulator is inputted as an initial instruction value and information of a disturbance, which is produced when the manipulator operates in accordance with the initial instruction value, is forecast on the real time basis as an excessive instruction value control index based on a dynamic model regarding the manipulator, and scale conversion is performed for the initial instruction value based on the excessive instruction value control index to obtain an intermediate instruction value, and then motion of the manipulator is displayed by simulation display and the manipulator is controlled in accordance with the intermediate instruction value obtained by the scale conversion.
Meanwhile, according to the present invention as set forth in claim 39, a simulation and control method for a manipulator apparatus according to the present invention as set forth in claim 38 is characterized in that, in order to control the manipulator, controlling processing is performed for a first intermediate instruction value obtained by the scale conversion of the initial instruction value based on an actual disturbance of the manipulator system to obtain a second intermediate instruction value, and then the manipulator is controlled in accordance with the second stage intermediate instruction value.
Further, according to the present invention as set forth in claim 40, a simulation and control method for a manipulator apparatus according to the present invention as set forth in claim 38 is characterized in that, in addition to motion of the manipulator based on the intermediate instruction value, motion of the manipulator based on the initial instruction value and a reached value representative of a current position of the manipulator is displayed by simulation display.
Meanwhile, according to the present invention as set forth in claim 41, a simulation and control method for a manipulator apparatus according to the present invention as set forth in claim 40 is characterized in that, if a difference greater than a predetermined value is produced among the initial instruction value, the intermediate instruction value and the reached value, the initial instruction value and the intermediate instruction values later than the current reached point of the manipulator are reset and the initial instruction value of the manipulator is re-set to the current reached point.
Accordingly, there is an advantage in that, when the manipulator which has a spatial distance or a dynamic distance from the operator therefor is to be operated by programmed operation or remote control, a work can be performed efficiently with safety and a disturbance provided to the control system of the manipulator body upon driving of the manipulator can be suppressed to keep the controlling performance of the manipulator apparatus high.
Further, in order to attain the third object described above, according to the present invention as set forth in claim 42, a simulation and control apparatus for a manipulator apparatus which includes a manipulator and a manipulator body for supporting the manipulator thereon is characterized in that it comprises initial instruction value inputting means for inputting route instruction value information of the manipulator as an initial instruction value, excessive instruction value control index forecasting means for receiving the initial instruction value from the initial instruction value inputting means and forecasting information of a disturbance, which is produced when the manipulator operates in accordance with the initial instruction value, on the real time basis as an excessive instruction value control index based on a dynamic model regarding the manipulator, scale conversion means for performing scale conversion for the route instruction value information based on the excessive instruction value control index forecast by the excessive instruction value control index forecasting means to obtain an intermediate instruction value, display means for displaying motion of the manipulator by simulation display in accordance with the intermediate instruction value obtained by the conversion of the scale conversion means, and control means for controlling the manipulator in accordance with the intermediate instruction value obtained by the conversion of the scale conversion means.
Accordingly, there is an advantage in that, when the manipulator which has a spatial distance or a dynamic distance from the operator therefor is to be operated by programmed operation or remote control, a work can be performed efficiently with safety and a disturbance provided to the control system of the manipulator body upon driving of the manipulator can be suppressed to keep the controlling performance of the manipulator apparatus high.
Meanwhile, in order to attain the third object described above, according to the present invention as set forth in claim 43, a simulation and control method for a manipulator apparatus which includes a manipulator and a manipulator body for supporting the manipulator thereon is characterized in that route instruction value information of the manipulator is inputted as an initial instruction value and information of a disturbance, which is produced when the manipulator operates in accordance with the initial instruction value, is forecast on the real time basis as an excessive instruction value control index based on a dynamic model regarding the manipulator, and scale conversion is performed for the initial instruction value based on the excessive instruction value control index to obtain an intermediate instruction value, and then the manipulator is controlled in accordance with the intermediate instruction value obtained by the scale conversion.
Accordingly, there is an advantage in that, when the manipulator which has a spatial distance or a dynamic distance from the operator therefor is to be operated by programmed operation or remote control, a work can be performed efficiently with safety.
Further, in order to attain the third object described above, according to the present invention as set forth in claim 44, a simulation and control apparatus for a manipulator apparatus which includes a manipulator and a manipulator body for supporting the manipulator thereon is characterized in that it comprises initial instruction value inputting means for inputting route instruction value information of the manipulator as an initial instruction value, excessive instruction value control index forecasting means for receiving the initial instruction value from the initial instruction value inputting means and forecasting information of a disturbance, which is produced when the manipulator operates in accordance with the initial instruction value, on the real time basis as an excessive instruction value control index based on a dynamic model regarding the manipulator, scale conversion means for performing scale conversion for the route instruction value information based on the excessive instruction value control index forecast by the excessive instruction value control index forecasting means to obtain an intermediate instruction value, and control means for controlling the manipulator in accordance with the intermediate instruction value obtained by the conversion of the scale conversion means.
Accordingly, there is an advantage in that, when the manipulator which has a spatial distance or a dynamic distance from the operator therefor is to be operated by programmed operation or remote control, a work can be performed efficiently with safety.

BRIEF DESCRIPTION OF THE DRAWINGS
FIG. 1 is a block diagram showing a first embodiment of the present invention; FIG. 2 is a perspective view showing a spacecraft having a manipulator: FIG. 3 is a diagrammatic view illustrating a coordinate system of the spacecraft having a manipulator; FIG. 4 is a diagrammatic view illustrating the coordinate system of the spacecraft having a manipulator; FIG. 5 is a diagrammatic view illustrating the coordinate system of the spacecraft having a manipulator; FIG. 6 is a flow chart for calculation of the velocity and the angular velocity of a satellite body by a spacecraft velocity and angular velocity calculation section; FIG. 7 is a flow chart illustrating a processing procedure by a spacecraft position information calculation section; FIG. 8 is a flow chart illustrating a technique based on reverse dynamics for the universe; FIG. 9 is a flow chart illustrating a calculation technique for a feed forward torque; FIGS. 10(a) and 10(b) are diagrammatic views illustrating that wheels are treated as branching links from a satellite body; FIG. 11 is a block diagram showing another embodiment of the present invention; FIG. 12 is a diagrammatic view illustrating a real time data monitoring system; FIG. 13 is a diagrammatic view illustrating the real time data monitoring system; FIG. 14 is a diagram illustrating an interpolation technique for .THETA..sub.w ; FIG. 15 is a flow chart illustrating an efficiency enhancing method based on an inertial moment proportional multiplication method; FIG. 16 is a flow chart illustrating the efficiency enhancing method based on the inertial moment proportional multiplication method; FIG. 17 is a flow chart illustrating the efficiency enhancing method based on the inertial moment proportional multiplication method; and FIG. 18 is a flow chart illustrating the efficiency enhancing method based on the inertial moment proportional multiplication method.
FIG. 19 is a perspective view showing another spacecraft having a manipulator; FIG. 20 is a block diagram showing a second embodiment of the present invention; FIG. 21 is a block diagram showing several control systems of the spacecraft in the second embodiment of the present invention; FIG. 22 is a schematic view showing the construction of three-axis momentum wheels in the second embodiment of the present invention; FIG. 23 is a diagrammatic view illustrating a disturbance provided to a base in the second embodiment of the present invention; FIG. 24 is a view showing a block diagram in the second embodiment of the present invention; FIG. 25 is a view showing another block diagram in the second embodiment of the present invention; FIG. 26 is a view showing a further block diagram in the second embodiment of the present invention; FIG. 27 is view showing a still further block diagram in the second embodiment of the present invention; FIG. 28 is a diagrammatic view illustrating a coordinate system of a spacecraft having a manipulator; FIG. 29 is a diagrammatic view illustrating the coordinate system of the spacecraft having a manipulator; FIG. 30 is a diagrammatic view illustrating the coordinate system of the spacecraft having a manipulator; FIG. 31 is a view illustrating an algorithm AA in the second embodiment of the present invention; FIG. 32 is a view illustrating an algorithm AB in the second embodiment of the present invention; FIG. 33 is a view illustrating an algorithm AC in the second embodiment of the present invention; FIG. 34 is a view illustrating general-purpose reverse dynamics for the universe of the second embodiment of the present invention; FIG. 35 is a view illustrating binding reverse dynamics for the universe of the second embodiment of the present invention; FIG. 36 is a view illustrating the binding reverse dynamics for the universe of the second embodiment of the present invention; FIG. 37 is a view illustrating the binding reverse dynamics for the universe of the second embodiment of the present invention; FIG. 38 is a view illustrating the binding reverse dynamics for the universe of the second embodiment of the present invention; and FIG. 39 is a view illustrating the binding reverse dynamics for the universe of the second embodiment of the present invention.
FIG. 40 is a block diagram showing a simulation and control apparatus for a manipulator apparatus as a third embodiment of the present invention; FIG. 41 is a perspective view showing a further spacecraft having a manipulator; FIG. 42 is a diagrammatic view illustrating a coordinate system of the spacecraft having a manipulator; FIG. 43 is a diagrammatic view illustrating the coordinate system of the spacecraft having a manipulator: FIG. 44 is a diagrammatic view illustrating the coordinate system of the spacecraft having a manipulator: FIG. 45 is a view illustrating an algorithm BA in the third embodiment of the present invention: FIG. 46 is a view illustrating another algorithm BB in the third embodiment of the present invention: FIG. 47 is a flow chart illustrating a method of calculation of a differentiation term of a manipulator articulation angle: FIG. 48 is a view illustrating an example of on-the-ground system/on-the-orbit system velocity scale conversion: FIG. 49 is a view illustrating another example of on-the-ground system/on-the-orbit system velocity scale conversion; FIG. 50 is a block diagram showing several control systems of the spacecraft in the third embodiment of the present invention; FIG. 51 is a view illustrating spline interpolation calculation; FIG. 52 is a view illustrating a displaying manner of a graphic display; FIG. 53 is a view illustrating another displaying manner of the graphic display; FIG. 54 is a block diagram showing a simulation and control apparatus for a manipulator apparatus as a fourth embodiment of the present invention; FIG. 55 is a block diagram showing a simulation apparatus for a manipulator apparatus as a fifth embodiment of the present invention; and FIG. 56 is a flow chart illustrating a processing procedure of a conventional off-line simulator.

BEST FORMS IN EMBODYING THE INVENTION
In the following, embodiments of the present invention will be described with reference to the drawings.
(a) Description of the First Embodiment
Now, the present embodiment is directed to a three-dimensional dynamics real time simulation system for an attitude control system for a spacecraft 12 such as a space robot or an artificial satellite having such manipulators 11 as shown in FIG. 2. It is to be noted that, in FIG. 2, reference numeral 13 denotes a paddle.
By the way, while FIG. 1 is a block diagram showing the present embodiment, the three-dimensional dynamics real time simulation system for a satellite having a serial link manipulator of an 1 degree (the character 1 is el of the small character) of freedom shown in FIG. 1 is constructed including a manipulator articulation information inputting section 1, a graphic display 2, and a three-dimensional dynamics real time simulation section 10.
First, with the manipulator articulation information inputting section 1, an operator 1A will input initial values of an articulation angle, an articulation angular velocity, an articulation angular acceleration and a position attitude of the body of the satellite (spacecraft) of each of the manipulators 11 (refer to FIG. 2) and an initial value of a wheel angular velocity by way of a master arm, a joy-stick, a keyboard or a like inputting apparatus while observing the graphic display 2.
It is to be noted that only an articulation angle .theta.1 (t) (i=1 to n) of each of the manipulators 11 (refer to FIG. 2) is delivered to the graphic display 2.
Further, the graphic display 2 supports the operability of the operator 1A, and on the graphic display 2, a satellite in the graphics starts to move from a start point defined by an initial value of the position and attitude of the satellite body and an initial value of the wheel angular velocity designated by the operator 1A. In particular, on the graphic display 2, motion of the spacecraft can be displayed by simulation display.
The three-dimensional dynamics real time simulation section 10 receives information from the manipulator articulation information inputting section 1 and calculates, on the real time basis, three dimensional dynamics of the entire spacecraft including an attitude control system using the laws of conservation of momentum/angular momentum of the entire system, reverse dynamics for the universe and normal dynamics of the wheel system so that motion of the spacecraft may be displayed by simulation display on the graphic display 2. To this end, the three-dimensional dynamics real time simulation section 10 includes a manipulator articulation information reception section 3, a spacecraft velocity and angular velocity calculation section 4, a spacecraft position information calculation section 5, a wheel feedback control torque calculation section 6, an internal torque and wheel system inertia information calculation section 7, a wheel feed forward torque calculation section 8 and a wheel dynamics integration section 9.
Here, the manipulator articulation information reception section 3 receives articulation angles (.theta..sub.i (t)), articulation angular velocities (obtained by differentiation of first order of the articulation angles) and articulation angular accelerations (obtained by differentiation of second order of the articulation angles) of each of the manipulators 11 which are inputted for each step of dynamics calculation.
The spacecraft velocity and angular velocity calculation section 4 performs calculation using the laws of conservation of momentum/angular momentum of the entire system. The routine at the spacecraft velocity and angular velocity calculation section 4 receives, as inputs, body position attitude values (X(t), Y(t), Z(t), .alpha.(t), .beta.(t), .gamma.(t)) at the point of time t, wheel rotational angles (.theta..sub.wj (t): j=1 to m), wheel angular velocities (obtained by differentiation of first order of wheel rotational angles), manipulator articulation angles (.THETA.i(t): i=1 to n) and articulation angular velocities (obtained by differentiation of first order of the manipulator articulation angles) and outputs a velocity and an angular velocity (V.sub.0 (t), .OMEGA..sub.0 (t)) of the satellite body at the point of time t.
The spacecraft position information calculation section 5 performs differentiation of first order. In particular, the spacecraft position information calculation section 5 receives, as inputs, the positions of the satellite body at the the points of time t and t-.DELTA.t, transformation matrices (R.sub.0 (t), .sup.A A.sub.0 (t)) and (R.sub.0 (t-.DELTA.t), .sup.A A.sub.0 (t-.DELTA.t)) and the velocities and angular velocities (V.sub.0 (t), .OMEGA..sub.0 (t)) and (V.sub.0 (t-.DELTA.t), .OMEGA..sub.0 (t-.DELTA.t)) and outputs a position transformation matrix (R.sub.0 (t+.DELTA.t), .sup.A A.sub.0 (t+.DELTA.t)) of the satellite body at the point of time t+.DELTA.t. It is to be noted that, as for an attitude variation, Eulerian angles (.alpha.(t), .beta.(t), .gamma.(t)) may be used in place of the transformation matrices.
The wheel feedback control torque calculation section 6 receives, as inputs, Eulerian angles (.alpha.(t+.DELTA.t), .beta.(t+.DELTA.t), .gamma.(t+.DELTA.t)) and the angular velocity .OMEGA..sub.0 (t+.DELTA.t) of the satellite body at the point of time t+.DELTA.t and calculates a wheel feedback control torque (wheel control torque) .tau..sub.WC (t+.DELTA.t) at the point of time t+.DELTA.t.
The internal torque and wheel system inertia information calculation section 7 receives, as inputs, a manipulator articulation angle (.THETA.i(t+.DELTA.t)) and a manipulator articulation angular velocity (obtained by differentiation of first order of .THETA.i(t+.DELTA.t)) at the point of time t+.DELTA.t and calculates an internal torque (.tau..sub.W (t+.DELTA.t)) and a wheel system inertia matrix (H.sub.W (t+.DELTA.t)) at the point of time t+.DELTA.t using the wheel system normal dynamics.
The wheel feed forward torque calculation section 8 receives, as inputs, a manipulator articulation angle (.THETA..sub.i (t+.DELTA.t)) and a manipulator articulation angular velocity (obtained by differentiation of first order of .THETA..sub.i (t+.DELTA.t)) at the point of time t+.DELTA.t and calculates a wheel feed forward torque (.tau..sub.FF (t+.DELTA.t)) at the point of time t+.DELTA.t using binding reverse dynamics for the universe.
The wheel dynamics integration section 9 receives, as inputs, wheel rotational angles (.theta..sub.wj (t): j=1 to m), wheel angular velocities (obtained by differentiation of first order of .theta..sub.wj (t)), feedback control torques .tau..sub.WC, internal torques .tau..sub.W, a wheel system inertia matrix H.sub.W and feed forward torque .tau..sub.FF at the points of time t and t+.DELTA.t and outputs a wheel rotational angle (.theta..sub.wj (t+2 .DELTA.t): j=1 to m) and a wheel angular velocity (obtained by differentiation of first order of .theta..sub.wj (t+2 .DELTA.t)) at the point of time t+2 .DELTA.t. Accordingly, the wheel dynamics integration section 9 determines wheel angular velocity information by differentiation of first order and then determines wheel rotational angle information by differentiation calculation of second order.
Further, while details of the blocks (refer to the portions of the reference numerals 4 to 9 of FIG. 1) described above are such as described hereinbelow, notations which will be referred to hereinbelow are taken in the following manner before description of the details is given. It is to be noted that each capital letter denotes a vector as viewed from the inertial coordinate system .SIGMA..sub.A while each small letter denotes a vector as viewed from the base fixed coordinate system .SIGMA..sub.0.
.sub.i : velocity of the origin of the coordinate system .SIGMA..sub.i relative to the inertial coordinate system .SIGMA..sub.A
.sub.i : acceleration of the origin of the coordinate system .SIGMA..sub.i relative to the inertial coordinate system .SIGMA..sub.A
.sub.i : velocity of the center of gravity of the link i relative to the inertial coordinate system .SIGMA..sub.A
: acceleration of the center of gravity of the link i relative to the inertial coordinate system .SIGMA..sub.A
.sub.i : angular velocity of the link i relative to the inertial coordinate system .SIGMA..sub.A
.sub.i : angular acceleration of the link i relative to the inertial coordinate system .SIGMA..sub.A
.sub.i : time differentiation of the vector .sub.i with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i .tbd.d.sub.0 .sub.i /dt= .sub.i '
.sub.i ': time differentiation of the vector .sub.i with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i '.tbd.d.sub.0 .sub.i /dt= .sub.i "
: time differentiation of the vector with respect to the base fixed coordinate system .SIGMA..sub.0
.tbd.d.sub.0 /dt= "
: time differentiation of the vector with respect to the base fixed coordinate system .SIGMA..sub.0
'.tbd.d.sub.0 /dt= "
.sub.i : angular velocity of the link i with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i ': time differentiation of .sub.i with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i '.tbd.d.sub.0 .sub.i /dt
Note 1) The time differentiation with respect to the inertial coordinate system .SIGMA..sub.A and the time differentiation with respect to the base fixed coordinate system .SIGMA..sub.0 are distinguished definitely from each other.
For a vector
time differentiation with respect to .SIGMA..sub.A :
=d /dt
time differentiation with respect to .SIGMA..sub.0 :
'=d.sub.0 /dt
The relationship between and ' is given by the following equation:
= '+.OMEGA..sub.0 .times.
Meanwhile, as a coordinate system for the entire system, such a Denavit-Hartenberg coordinate system as shown in FIG. 3 is adopted.
Here, when vectors represented in the coordinate systems .SIGMA..sub.i and .SIGMA..sub.i-1 of FIG. 3 are defined as .sup.i a and .sup.i-1 a (while .sup.i a and .sup.i-1 a should actually be represented in vector representation, they are represented in scalar representation for the convenience of representation. In the following description, a vector amount, which should be represented in vector representation, is sometimes represented in scalar representation for the convenience of representation), respectively, .sup.i a and .sup.i-1 a have a relationship given by the following equation (refer to FIG. 4). (The character l in a representation is sometimes represented in a writing style.)
.sup.i-1 =.sup.i-1 A; .sup.i +.sup.i-1 .sub.i
Here, .sup.i-1 A.sub.i is an orthogonal matrix representative of rotation between different coordinate systems, and .sup.i-1 l.sub.i (vector) is a representation of a vector from the coordinate origin O.sub.i-1 to O.sub.i in the .SIGMA..sub.i-1 system.
It is to be noted that .sup.i-1 A.sub.i and .sup.i-1 l.sub.i (vectors) are represented in the following manner when standard parameters are used on the Denavit-Hartenberg coordinate system representation: ##EQU1##
Meanwhile, definitions of parameters of the Denavit-Hartenberg coordinate system are such as illustrated in FIG. 5, and this coordinate system is set in accordance with the following criteria:
z.sub.i axis: axis of the articulation .sub.i+1
x.sub.i axis: common perpendicular line from z.sub.i-1 toward z.sub.i
y.sub.i : constructed so as to make a right-hand coordinate system
b.sub.i : length of the common perpendicular line to z.sub.i and z.sub.i-1
d.sub.i : length between x.sub.i and x.sub.i-1 (z.sub.i-1 coordinate value)
.alpha.i: angle between z.sub.i and z.sub.i-1 (the z.sub.i-1 direction coincides with the z.sub.i direction when it is rotated by .alpha..sub.i around the x.sub.i axis)
.theta..sub.i : angle between x.sub.i and x.sub.i-1 (the x.sub.i-1 direction coincides with the x.sub.i direction when it is rotated by .theta..sub.1 around the z.sub.i-1 axis)
Further, when i=-1, .sup.i a represents an inertia system representation. Accordingly, the following equations are obtained. ##EQU2##
In this instance, the prefix A on the left top denotes the initial letter of Absolute coordinate, and particularly, the equation below represents a transformation matrix between the inertia system and base reference coordinate system. ##EQU3##
Now, the spacecraft velocity and angular velocity calculation section 4 which performs calculation using the laws of conservation of momentum/angular momentum of the entire system will be described in more detail.
In this instance, the laws of conservation of momentum and angular momentum of the entire system are given by the following equations: ##EQU4## law of conservation of momentum (1) ##EQU5## law of conservation of angular momentum (2)
Here, the suffix i runs following all of the links (also each of the wheels and the body is regarded as a link).
And, the velocity of the center of gravity of the link i with respect to the inertial coordinate system .SIGMA..sub.A and the angular velocity of the link i with respect to the inertial coordinate system .SIGMA..sub.A are given by the following equations: ##EQU6##
Accordingly, the equations (1) and (2) can be rewritten in the following manner with regard to the velocity and the angular velocity (V.sub.0 and .OMEGA..sub.0) of the satellite body: ##EQU7##
And, from the equation (5), the velocity and the angular velocity (V.sub.0 and .OMEGA..sub.0) of the satellite body can be determined.
It is to be noted that a detailed flow is such as illustrated in FIG. 6. In particular, at step A1, initial value setting processing is performed, and at step A2, normal order repetitive calculation processing is performed. Then at step A3, base velocity and angular velocity processing is performed.
Here, the procedure of the processing (initial value setting processing) at step A1 of FIG. 6 is such as follows: ##EQU8##
Meanwhile, the procedure of the processing (normal order repetitive calculation processing) at step A2 of FIG. 6 is such as follows: ##EQU9##
Here. i=1, 2, . . . l, w.sub.1, w.sub.2, and w.sub.3.
Further, the procedure of the processing (base velocity and angular velocity processing) at step A3 of FIG. 6 is such as follows ##EQU10##
Further, the spacecraft position information calculation section 5 for performing calculation of integration of first order of the laws of conservation of quantum/angular quantum of the entire system will be described in more detail below.
In particular, in this instance, the position and the attitude of the satellite body are calculated by integration of first order of the equation (5) given hereinabove. However, the integration method may be any integration method in principle, and the Euler's method, the Adams-Bashforth method, the Runge-Kutta method and so forth may be used.
Here, while the Adams-Bashforth method of second order is taken as an example, the equation which makes a base for integration then is such as follows:
f (t+.delta.t)=f (t)+{3f (t)-f (t-.delta.t)}.delta.t/2+O (.delta.t.sup.3)(15)
And, the calculation flow in this instance is such as illustrated in FIG. 7. In particular, at step B1, processing of calculating the position of the origin of .SIGMA..sub.0 is performed, and at step B2, processing of calculating a .SIGMA..sub.0 to .SIGMA..sub.A transformation matrix is performed.
Here, the processing (processing of calculating the position of the origin of .SIGMA..sub.0) at step B1 of FIG. 7 is given by the following equation: ##EQU11##
Meanwhile, the processing (processing of calculating a .SIGMA..sub.0 to .SIGMA..sub.A transformation matrix) at step B2 in FIG. 7 is such as shown below: ##EQU12##
Further, detailed description of the calculation of a wheel control torque is such as follows.
First, a conversion matrix represented in the following equation is calculated from the equation (17). ##EQU13##
And, from this value, the Eulerian angle of X-Y-Z (or an arbitrary array of X, Y and Z) is calculated.
For example, if E.sub.x (.alpha.) E.sub..gamma. (.beta.) E.sub.z (.gamma.) (turned by .alpha. around the x-axis, by .beta. around the y-axis and by .gamma. around the z-axis) is taken as the definition of the Eulerian angle, then it is given by the following equation: ##EQU14##
Accordingly, in the present case, the Eulerian angles are calculated in accordance with the following equations: ##EQU15##
When the Eulerian angles are given as above, feedback control torques for maintaining the axes are given as follows: ##EQU16##
Further, detailed description of the internal torque and wheel system inertia information calculation section 7 in which the wheel system normal dynamics are employed will be given below.
(a) Motion Equation of the Wheel System
Here, assuming that the number of wheels is three for simplification, the following definition is assumed. ##EQU17##
With such definition, the motion equation which describes the entire system is given as follows:
() + (, ) = (21)
Here, each of H and C (both vectors) appearing above is a (1+3).times.(1+3) matrix called inertia matrix and centrifugal force+Corioli's force matrix, respectively. ##EQU18##
Consequently, the following motion equation for the wheel system is obtained from the equation (21):
.sub.w.THETA. () .THETA..sub.m + .sub.w () .THETA..sub.w + .sub.w.THETA. (, ) .THETA..sub.m + .sub.w (, ) .THETA..sub.w = .sub.w (23)
Now, the following equation is defined:
.tbd. .sub.w.THETA. () /.sub.m .sub.w.THETA. (, ) .THETA..sub.m + .sub.w (, ) .THETA..sub.w
Then, the following equation is obtained:
.sub.w (q) .theta..sub.w = .sub.w - .sub.w (24)
Then, values of individual components of .tau..sub.W and H.sub.W (q) in the equation (24) are calculated based on the reverse dynamics for the universe described below.
(b) Reverse Dynamics for the Universe
(b-1) Body Acceleration and Angular Acceleration
In order to determine the individual components of .tau..sub.W and H.sub.W (q) in the equation (24) above, a system of reverse dynamics which inputs q(t), differentiation of first order of q(t) and differentiation of second order of q(t) and outputs .tau..sub.q (t).
Here, the equations which makes a base are the following equations representative of sum totals of internal forces and internal torques obtained by time differentiation of the laws of conservation of momentum/angular momentum [equations (1) and (2)]: ##EQU19##
By arranging the equations (25) and (26) using the equations given above, the following equations are obtained: ##EQU20##
(b-2) Torque Calculation
First, the calculation algorithm for an acceleration and an angular acceleration of the body is such as illustrated in FIG. 8, and in this instance, the calculation algorithm is calculation subsequent to step A3 of the laws of conservation of momentum/angular momentum. In particular, subsequently to step A3 described above, calculation processing for an acceleration and angular acceleration of the base is performed at step A4, and then at step A5, reverse order repetitive calculation processing is performed.
Here, the processing (calculation processing for an acceleration and an angular acceleration of the base) at step A4 of FIG. 8 described above is such as given below: ##EQU21##
Meanwhile, the processing (reverse order repetitive calculation processing) at step A5 of FIG. 8 is such as given below: ##EQU22##
(c) Component Calculation of .tau.W and H.sub.W
The algorithm for calculating components of .tau.W and H.sub.W described above using the algorithm of the reverse dynamics is such as described below.
In particular, the routine for calculating .tau..sub.q based on the algorithm in the section (b) above is represented by the following equation:
SUB 1 (.THETA..sub.m, .THETA..sub.w, .THETA..sub.m, .THETA..sub.w, .THETA..sub.m, .THETA..sub.w, .sub.q) (34)
Meanwhile, the routine wherein those terms of the algorithm of (b) which increase In proportion to the velocity (time differentiation of .THETA..sub.m, and time differentiation of .THETA..sub.W) and the acceleration (time differentiation of second order of .THETA..sub.m) are omitted from the beginning is represented by the following equation:
SUB 2 (.THETA..sub.m, .THETA..sub.w, .THETA..sub.w, .sub.q)(35)
(c-1) Calculation of .tau.W
Current values are used for .THETA..sub.m, differentiation of first order of .THETA..sub.m, differentiation of second order of .THETA..sub.m, .THETA..sub.W and differentiation of first order of .THETA..sub.W in the equation (34) given above and the following equation is used:
.THETA..sub.w .tbd.0
(c-2) Calculation of H.sub.W
Current values are used for .THETA..sub.m and .THETA..sub.W in the equation (35) given above, and the following equation is used:
.THETA..sub.w = .sub.i (i=1 to 3), ei denotes a unit vector)
It is to be noted that .tau..sub.W is the sum of a feedback torque .tau..sub.WC and a feed forward torque .tau..sub.FF. In other words. .tau..sub.W is given by the following equation:
.sub.w = .sub.we + .sub.FF (36)
Subsequently, the wheel feed forward torque calculation section 8 in which the binding reverse dynamics for the universe will be described.
First, the feed forward torque .tau..sub.FF to be calculated here is used to always keep the attitude of the satellite body fixed, and the present feed forward torque .tau..sub.FF is calculated in the following manner using the reverse dynamics for the universe.
In the present case, the following equation is adopted as a constraint:
.sub.0 = .sub.0 =0 (37)
In this instance, the acceleration of the body is given by the following equation from the equation of the sum total of the internal forces=0: ##EQU23##
Meanwhile, the internal force and the internal torque F.sub.i and N.sub.i are given by the following equations: ##EQU24##
Accordingly, the feed forward torque in a binding condition in the equation (37) can be arranged into such an algorithm as illustrated in FIG. 9. In particular, setting of initial values is performed at step C1, and forward repetitive calculation is performed at step C2, whereafter reverse repetitive calculation is performed at step C3.
And, the processing (setting processing of initial values) at step C1 in FIG. 9 is performed in the following manner: ##EQU25##
Meanwhile, the processing (forward order repetitive calculation processing) at step C2 in FIG. 9 is performed in the following manner: ##EQU26##
Here. i=1, 2, . . . l, w.sub.1, w.sub.2, w.sub.3.
Further, the processing (reverse order repetitive calculation processing) at step C3 in FIG. 9 is performed in the following manner: ##EQU27##
Here, i=l, l-1, . . . , 0, and w=w.sub.1 to w.sub.3.
Subsequently, the wheel dynamics integration section 9 which performs differentiation of second order of the wheel dynamics will be described in detail.
First, the equation describing the wheel dynamics is given, from the equations (24) and (36), as follows:
.sub.w .THETA..sub.w = .sub.wc - + .sub.FF (40)
Accordingly, the following equation is obtained:
.THETA..sub.w = .sub.w.sup.-1 ( .sub.wc - + .sub.FF) (41)
Now, if the Adams-Bashforth method of second order is employed as the integration method, then the integration of first order of the equation (40) is given by the following equation:
.THETA..sub.w (t+2 .delta.t)=.THETA..sub.w (t+.delta.t)+{3 .THETA..sub.w (t+.delta.t)-.THETA.(t) } .delta.t/2 (42)
Accordingly, the integration of second order of the equation (40) is given by the following equation:
.THETA..sub.w (t+2 .delta.t)=.THETA..sub.w (t+.delta.t)+{3 .THETA..sub.w (t+.delta.t)-.THETA..sub.w (t) } .delta.t/2 (43)
Accordingly, the wheel angular velocity and the wheel rotational angle can be calculated in accordance with the equations (42) and (43).
It is to be noted that each wheel is treated as a branching link from the body of the satellite 12 as shown in FIGS. 10(a) and 10(b). Accordingly, in a recursive formulation which appears in the reverse dynamics and the normal dynamics, calculation proceeds in such a form that the wheel system is adjacent to the body.
Further, in the present program, the equations of the entire system are described by the reverse dynamics having the degree of freedom of (the number of links of a manipulator+the number of wheels) and the normal dynamics having the degree of freedom of (the number of wheels). Accordingly, as can be read from the article "Efficient Calculation Method of the Kinetics and Dynamics of a Manipulator for the Universe" (Lecture No. 2, 104, the Eight Lecture Meeting of the Robot Engineering Society of Japan"), the number of product sums of the present program increases in proportion to (the number of links of the manipulator).
Subsequently, a method wherein, when the motion equation of the entire system is to be solved numerically based on the method which is based on integration of first order in the wheel normal dynamics, that is, the method described in detail already, taking notice of the rotational symmetry of a wheel, only numerical integration of first order is used for a wheel and the rotational angle of the wheel is not calculated positively.
Since a wheel is symmetrical with respect to a rotational axis thereof, the inertia matrix H(q) and the centrifugal force+Corioli's force matrix C have no dependency upon the rotational angle of the wheel.
Accordingly, the wheel dynamics integration section 9 is required to perform only integration of first order of differentiation of second order of .THETA..sub.W as illustrated in FIG. 11.
In particular, the following equation is considered:
.THETA..sub.w = .sup.-1 ( .sub.wc - + .sub.FF) (44)
And, if the Adams-Bashforth method of second order is adopted as the integration method for the equation (44) above, differentiation of first order of .THETA..sub.W is calculated in accordance with the following equation:
.THETA..sub.w (t+2 .delta.t)=.THETA..sub.w (t+.delta.t)+{3.THETA..sub.w (t+.delta.t)-.THETA. (t)} .delta.t/2 (45)
It is to be noted that the other portion in FIG. 11 is substantially same as that of FIG. 1.
Subsequently, a method of displaying a result of calculation of the dynamics of the spacecraft on the real time basis in graphics and a time sequential graph will be described.
First, the object of it is as follows. It is to monitor, on the real time basis, angular velocities .omega. (i=1 to 3) of the satellite wheels, Eulerian angles .alpha., .beta. and .gamma. of the satellite body, disturbance torques .tau..sub.i (i=1 to 3) acting on the satellite body and so forth.
And, as a system construction in this instance, a system wherein data are communicated between CPUs CPU-1 and CPU-2 by way of an ETHER NET or the like as shown in FIG. 12, another system wherein data are communicated between CPUs CPU-1 and CPU-2 by way of a common memory 141 as shown in FIG. 13 and some other system may be available.
Subsequently, a method wherein integration of second order in the wheel normal dynamics is performed by very simple numerical integration in order to display rotation of the wheels graphically will be described.
First, a method will be described wherein, in order to numerically solve the motion equation of the entire system based on the method described in detail already, taking notice of the rotational symmetry of a wheel, the step size of numerical integration of first order and numerical integration of second order for a wheel is varied such that numerical integration of second order is processed by rougher calculation than numerical integration of first order.
In this instance, since a wheel is symmetrical with respect to an axis of rotation thereof, the inertia matrix H(q) and the centrifugal force+Corioli's force matrix C have no dependency upon the rotational angle of the wheel.
Accordingly, in the dynamics, the necessity for calculation of a rotational angle of a wheel system is low.
However, it is very effective to graphically display rotation of a wheel in order for an operator of the real time simulator to determine the degree of operation of the control system.
In this instance, it is required that it can be visually discriminated whether the wheel is rotating (control ON) or at rest (control OFF), and dynamically severe accuracy is not required for the rotational angle.
Based on the investigations described so far, in the present method, the integration step size of numerical integration of second order of a wheel is set rougher than that of numerical integration of first order so that numerical integration of second order is processed by calculation rougher than that for numerical integration of first order.
In the following, description will be given taking the Adams-Bashforth method as an example.
In this instance, the ordinary differential equation which makes the base is such as given below:
.THETA..sub.w = .sub.w.sup.-1 ( .sub.wc - +.GAMMA..sub.FF) (46)
Further, by integration of first order of the equation above adopting the Adams-Bashforth method of second order, the following equation is obtained:
.THETA..sub.w (t+2 .delta.t)=.THETA..sub.w (t+.delta.t)+{3.THETA..sub.w (t+.delta.t)-.THETA. (t)} .delta.t/2 (47)
And, when the rotational angle .THETA..sub.W of a wheel is to be calculated based on the present equation (47), the integration step size is made rougher than that of the equation (47). In particular, it is given by the following equation:
.THETA..sub.w (t+2 S.delta.t)=.THETA..sub.w (t+S.delta.t)+{3 .THETA..sub.w (t+S.delta.t)-.THETA..sub.w (t)} S.delta.t/2 (48)
Here, s is an arbitrary integral number equal to or greater than 2.
Further, for t+(s+1).delta.t, t+(s+2).delta.t, . . . , and t+(2s-1).delta.t between (t+s.delta.t and t+2s.delta.t), the value of .THETA..sub.w (t+s.delta.t) is used as it is as shown in FIG. 14.
Subsequently, in order to graphically display rotation of a wheel similarly, as another method wherein integration of second order in the wheel normal dynamics is performed by very simple numerical integration, a method wherein, when a motion equation of the entire system is to be solved numerically, taking notice of the rotational symmetry of a wheel, the integration degree number is varied between numerical integration of first order and numerical integration of second order for a wheel such that numerical integration of second order is processed by rougher calculation than numerical integration of first order.
Also in this instance, numerical integration of second order of a wheel is processed by rougher calculation than that of numerical integration of first order because of a similar reason to that of the case described above.
It is to be noted that, as a method of the rough calculation, a method wherein the integration degree number of numerical integration of second order is made lower than that of numerical integration of first order is adopted.
Now, description is given taking the Adams-Bashforth method as an example.
In this instance, the ordinary differential equation which makes the base is given as the following equation:
.THETA..sub.w = .sub.w.sup.-1 ( .sub.wc - + .sub.FF) (49-1)
Further, when integration of second order is performed based on the Adams-Bashforth method of second order, it is given by the following equation:
.THETA..sub.w (t+2 .delta.t)=.THETA..sub.w (t+.delta.t)+{3.THETA..sub.w (t+.delta.t)-.THETA..sub.w (t)} .delta.t/2 (49-2)
Meanwhile, integration of first order is performed based on the Adams-Bashforth method (same as in the Euler's method) of first order:
.THETA..sub.w (t+3 .delta.t)=.THETA..sub.w (t+2 .delta.t)+.THETA..sub.w (t+2 .delta.t).multidot..delta.t (50)
Generally, when integration of second order is performed based on the Adams-Bashforth method of sth order, integration of first order may be performed based on the Adams-Bashforth method of degree of (s-1)th or less.
It is to be noted that the integration method is not limited to the Adams-Bashforth method but may be any arbitrary method such as the Runge-Kutta method. However, integration of first order which is higher in speed than the integration of second order should be adopted for the integration of first order.
Further, in order to graphically display rotation of a wheel, as a method wherein integration of second order in the wheel normal dynamics is performed by very simple numerical integration, a method wherein the step size and the integration degree number are varied between numerical integration of first order and numerical integration of second order of a wheel such that integration of second order is processed by rougher calculation than that for integration of first order will be described.
Also in this instance, numerical integration of second order of a wheel is processed by rougher calculation than that for numerical integration of first order because of a similar reason to that of the case described above.
It is to be noted that, as a method of the rough calculation, a method wherein the integration step size of numerical integration of second order is made lower than that of numerical integration of first order to make the integration degree number of the numerical integration of second order lower than that of the numerical integration of first order is adopted.
Now, if description is given taking the Adams-Bashforth method as an example, then the ordinary differential equation which makes the base is given as the following equation:
.THETA..sub.w = .sub.w.sup.-1 ( .sub.wc - + .sub.FF) (51)
Further, by integration of first order of the equation above adopting the Adams-Bashforth method of second order, the following equation is obtained:
.THETA..sub.w (t+2 .delta.t)=.THETA..sub.w (t+.delta.t)+{3.THETA..sub.w (t+.delta.t)-.THETA..sub.w (t)} .delta.t/2 (52)
And, when the rotational angle .THETA..sub.W of a wheel is to be calculated based on the present equation (52), the integration step size is set rougher than that of the case of the equation (52) and besides the integration degree number is lowered. In particular, the following equation is employed:
.THETA..sub.w (t+2 S.delta.t)=.THETA..sub.w (t+S.delta.t)+.THETA..sub.w (t+S.delta.t) .delta.t (53)
Here, s is an arbitrary integral number equal to or greater than 2.
Meanwhile, for t+(s+1).delta.t, t+(s+2).delta.t, . . . , and t+(2s-1).delta.t between (t+s.delta.t and t+2s.delta.t), the value of .THETA..sub.W (t+s.delta.t) is used as it is (refer to FIG. 14).
Subsequently, a method wherein inertia moments of all of the wheels are multiplied proportionally in order to calculate the rotational angles of the wheels as high in speed and accuracy as possible.
Generally, when a motion equation of a dynamic system is numerically integrated, the accuracy of the integration depends significantly upon the step size of integration.
And, while the step size of integration must necessarily be set fine in order to raise the accuracy, if the step size is excessively fine, then the time required for calculation becomes long.
In the present method, in order to maintain both of the calculation accuracy and the calculation speed at appropriate values, notice is taken of a part in the entire system which rotates very fast and the inertia moments there are all multiplied proportionally (for example, by 1,000 time) to lower the speed of rotation of that part to the order of the speed of the other part to raise the calculation accuracy without decreasing the step size of integration.
It is to be noted that the step size of integration is set with reference to a portion which is moving at a most high speed in the entire system. Accordingly, in the case of dynamics of a satellite, according to an ordinary numerical integration method, the step size of integration is determined in conformity with the wheels.
Meanwhile, the present method is applied only to a component which is axis-symmetrical around an axis of rotation such as a wheel.
If the dynamics of a satellite having wheels is taken as an example, since a portion which rotates at a most high speed in the entire system is the wheels, all of inertial moments of the wheels are multiplied by s (for example, s=1,000.0). In this instance, while the angular velocity of the wheels is reduced to 1/s, little influence is had on the other portion, and no variation is involved as the dynamics.
The proof of this will be given below.
First, an investigation is made for a manipulator in a stopping condition. And, if notice is taken of the satellite body portion (satellite body+paddle+manipulators) and the wheels to establish an Euler's equation of the satellite body portion in the form wherein the satellite body portion takes in reactions undergoing from the wheels, the following equation is obtained: ##EQU28##
Here,
.sub.B : inertia matrix of the satellite body portion
.sub.wi : inertia matrix of the wheels, represented in wheel coordinates (j.sub.wi, j.sub.wi, I.sub.wi)
.sub.B : angular velocity of the body portion
.sub.wi : unit vector in the direction of the axis of rotation of the wheel represented in wheel coordinates (0, 0, 1).sup.T
I.sub.wi : inertia moment around the axis of rotation of the wheel
.tau..sub.wi : wheel driving torque
Accordingly, when the inertial mass of the satellite body portion is sufficiently great comparing with that of the wheels and the angular velocity .OMEGA..sub.B of the satellite body portion is sufficiently small through the entire motion, the equation (54) is converted into the equation given below: ##EQU29##
And, from the equation (55), it can be seen that only the angular momentum L.sub.wi of each wheel relies upon the dynamics which describes the satellite body portion, but the value of the moment of inertia of each wheel does not rely directly upon the dynamics. Accordingly, if the value of the moment of inertia of each wheel in the equation (55) is multiplied by s, then the angular velocity is reduced to a 1/s time, and it is possible to reduce the angular velocity of each wheel to an appropriate value (an angular velocity similar to that of the other links) by suitably selecting the value of s.
Further, a graphic display will be described below.
In particular, if the motion of a wheel whose angular velocity is reduced to a 1/s time is graphically displayed as it is, then since the motion is slow, it is difficult to discriminate the degree of rotation. Therefore, when integration of second order in an equation of motion of a wheel system is to be performed, the angular velocity of each wheel is multiplied by s. Consequently, the motion of the wheel becomes faster relatively comparing with the other portions of the satellite, and the graphic display effect is not deteriorated.
In particular, the equation (43) given hereinabove is modified as given by the following equation:
.THETA..sub.w (t+2 .delta.t)=.THETA..sub.w (t+.delta.t)+{3 S.THETA..sub.w (t+.delta.t)-S .THETA..sub.w (t)} .delta.t/2 (57)
A flow of the processing described in the present section so far is illustrated in FIG. 15. In particular, after initialization is performed at step D1, dynamics calculation is performed at step D2, and then at step D3, graphic display is performed.
Subsequently, in order to graphically display rotation of a wheel, as a method wherein integration of second order in the wheel normal dynamics is performed by very simple numerical integration, a method, in the method wherein the step sizes of numerical integration of first order and numerical integration of second order of a wheel are varied such that the numerical integration of second order is processed by rougher calculation than that for numerical integration of first order, wherein the moments of inertia of all of the wheels are multiplied proportionally in order to calculate the rotational angles of the wheels as high in speed and accuracy as possible will be described.
In this instance, similarly as described hereinabove, for the algorithm of the method wherein the step sizes of numerical integration of first order and numerical integration of second order of each wheel are varied such that the numerical integration of second order is processed by rougher calculation than that for the numerical integration of first order, such processing as illustrated in FIG. 16 is performed. In particular, after initialization is performed at step E1, dynamics calculation is performed at step E2, and then integration processing of second order of the wheel system is performed at step E3. It is to be noted that, at the present step E3, the equation (48) given hereinabove is modified as given by the following equation to effect calculation:
.THETA..sub.w (t+2 S.delta.t)=.THETA..sub.w (t+S.delta.t)+{3U.THETA..sub.w (t+S.delta.t)-U.THETA..sub.w (t)}.times.S.delta.t/2 (58)
Subsequently, in order to graphically display rotation of a wheel, as a method wherein integration of second order in the wheel normal dynamics is performed by very simple numerical integration, a method, in the method wherein the integration degree numbers of numerical integration of first order and numerical integration of second order of a wheel are varied such that the numerical integration of second order is processed by rougher calculation than that for the numerical integration of first order, wherein the moments of inertia of all of the wheels are multiplied proportionally in order to calculate the rotational angles of the wheels as high in speed and accuracy as possible will be described.
In this instance, similarly as described hereinabove, for the algorithm of the method wherein the integration degree numbers of numerical integration of first order and numerical integration of second order of each wheel are varied such that the numerical integration of second order is processed by rougher calculation than that for the numerical integration of first order, such processing as illustrated in FIG. 17 is performed. In particular, after initialization is performed at step F1, dynamics calculation is performed at step F2, and then integration processing of second order of the wheel system is performed at step F3. It is to be noted that, at the present step F3, the following equation is calculated:
.THETA..sub.w (t+3 .delta.t)=.THETA..sub.w (t+2 .delta.t)+S.THETA..sub.w (t+2 .delta.t) .delta.t (59)
Subsequently, in order to graphically display rotation of a wheel, as a method wherein integration of second order in the wheel normal dynamics is performed by very simple numerical integration, a method, in the method wherein the step sizes and the integration degree numbers of numerical integration of first order and numerical integration of second order of a wheel are varied such that the numerical integration of second order is processed by rougher calculation than that for the numerical integration of first order, wherein the moments of inertia of all of the wheels are multiplied proportionally in order to calculate the rotational angles of the wheels as high in speed and accuracy as possible will be described.
In this instance, similarly as described hereinabove, for the algorithm of the method wherein the step numbers and the integration degree numbers of numerical integration of first order and numerical integration of second order of each wheel are varied such that the numerical integration of second order is processed by rougher calculation than that for the numerical integration of first order, such processing as illustrated in FIG. 18 is performed. In particular, after initialization is performed at step G1, dynamics calculation is performed at step G2, and then integration processing of second order of the wheel system is performed at step G3. It is to be noted that, at the present step G3, the following equation is calculated:
.THETA..sub.w (t+2 S.delta.t)=.THETA..sub.w (t+S.delta.t)+U.THETA..sub.w (t+S.delta.t) .delta.t (60)
In this manner, in the present embodiment, the dynamics including the attitude control system of the satellite or space robot having a manipulator can be calculated at a high speed, that is, the three-dimensional dynamics of the entire spacecraft including the attitude control system can be calculated on the real time basis, and a result of the calculation can be outputted on the real time bases on the display.
Further, the equation of motion of the entire system can be solved at a high speed and with a high degree of accuracy, and consequently, real time calculation of the three-dimensional dynamics of the entire spacecraft can be realized readily.
(2) Description of the Second Embodiment
Subsequently, a second embodiment of the present invention will be described. In the present embodiment, three-dimensional dynamics real time simulation of an attitude control system of a spacecraft 12 such as a space robot or an artificial satellite having such manipulators 11 as shown in FIG. 19 or attitude control by wheels is performed. It is to be noted that reference numeral 13 in FIG. 19 denotes a paddle.
The present apparatus includes, as shown in FIGS. 20 and 21, a three-dimensional dynamics real time simulation system for a satellite having serial link manipulators having a degree of freedom 1 (the 1 is the el of a small letter) and includes a control system 210 necessary for the operation.
It is to be noted that the three-dimensional dynamics real time simulation system shown in FIG. 20 includes a manipulator articulation information inputting section 1, a graphic display 2 and a three-dimensional dynamics real time simulation section 10 which are similar to those of the first embodiment described above, and accordingly, can perform simulation similar to that of the first embodiment.
Meanwhile, the control system 210 in FIG. 21 roughly includes three parts including an attitude control system 24, a manipulator control system 25 and a cooperative control system 26.
Here, the manipulator control system 25 is a part which receives articulation angle time series data from the manipulator articulation information inputting section. 1 (refer to FIG. 20) (1 in FIG. 3) and effects servoing control of the manipulators 11 provided on the spacecraft 12 so that they operate along routes of the articulation angle time series data.
To this end, the manipulator control system 25 includes a manipulator driving actuator 25.multidot.1 and further includes, commonly with the cooperative control system 26, a manipulator driving logic calculation section 25.multidot.2.
The manipulator driving logic calculation section 25.multidot.2 calculates, based on articulation angle time series data from the manipulator articulation information inputting section 1 (1 in FIG. 3) and an attitude angle and an attitude angular velocity received from the attitude control system 24 by way of the cooperative control system 26, an articulation angle, an angular velocity and an angular acceleration of each of the manipulators necessary to drive the manipulators 1 and converts them into and outputs voltage signals.
The manipulator driving actuator 25.multidot.1 drives the manipulators 11 so that they may move along aimed routes (articulation angles, angular velocities and angular accelerations of the manipulators) indicated by voltage signals from the manipulator driving logic calculation section 25.multidot.2.
Then, here, before the cooperative control system 26 and the attitude control system 24 are described, symbols used in various equations used to describe the constructions of the individual apparatus and a coordinate system will be described.
In particular, notations will be hereinafter taken as follows:
.sub.i : velocity of the origin of the coordinate system .SIGMA..sub.i relative to the inertial coordinate system .SIGMA..sub.A
.sub.i : acceleration of the origin of the coordinate system .SIGMA..sub.i relative to the inertial coordinate system .SIGMA..sub.A
: velocity of the center of gravity of the link i relative to the inertial coordinate system .SIGMA..sub.A
: acceleration of the center of gravity of the link i relative to the inertial coordinate system .SIGMA..sub.A
.sub.i : angular velocity of the link i relative to the inertial coordinate system .SIGMA..sub.A
.sub.i : angular acceleration of the link i relative to the inertial coordinate system .SIGMA..sub.A
.sub.i : time differentiation of the vector .sub.i with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i .tbd.d.sub.0 .sub.i /dt= .sub.i '
.sub.i ': time differentiation of the vector v.sub.i with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i '.tbd.d.sub.0 .sub.i /dt= .sub.i "
: time differentiation of the vector r.sub.i with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i .tbd.d.sub.0 .sub.i /dt= .sub.i '
.sub.i ': time differentiation of the vector v.sub.i with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i '.tbd.d.sub.0 .sub.i /dt= .sub.i "
.sub.i : angular velocity of the link i with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i ': time differentiation of .omega.i with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i '.tbd.d.sub.0 .sub.i /dt
Capital letter: vector as viewed from the inertial coordinate system .SIGMA..sub.A
Small letter: vector as viewed from the base fixed coordinate system .SIGMA..sub.0
Note 1) The time differentiation with respect to the inertial coordinate system .SIGMA..sub.A and the time differentiation with respect to the base fixed coordinate system .SIGMA..sub.0 are distinguished definitely from each other.
For a vector ,
time differentiation with respect to .SIGMA..sub.A :
.tbd.d /dt
Time differentiation with respect to .SIGMA..sub.0 :
'.tbd.d.sub.0 /dt
The relationship between and ' is given by the following equation:
= '+ .sub.0 .times.
Meanwhile, FIG. 28 shows a Denavit-Hartenberg coordinate system. And, as can be recognized from FIG. 29, when vectors represented in the coordinate systems .SIGMA..sub.i and .SIGMA..sub.i-1 of FIG. 28 are defined as .sup.i a and .sup.i-1 a (the character a is represented in scalar representation for the convenience of representation and actually represents a vector. In the following description, in such a case, a scalar representation sometimes indicates a vector), respectively, .sup.i a and .sup.i-1 a have a relationship given by the following equation (A1). (The character l in a representation is represented in a writing style.)
.sup.i-1 =.sup.i-1 A; .sup.i +.sup.i-1 .sub.i (A 1)
Here, .sup.i-1 A.sub.i in the equation (A1) is an orthogonal matrix representative of rotation between different coordinate systems, and .sup.i-1 l.sub.i (also this l is represented in scalar representation for the convenience of representation and is handled similarly as in the case of a) is a representation of a vector from the coordinate origin O.sub.i-1 to O.sub.i in the .SIGMA..sub.i-1 system representation.
Those .sup.i-1 A.sub.i and .sup.i-1 l.sub.i are represented in such a manner as given by the following equations (A2) and (A3) when standard parameters are used on the Denavit-Hartenberg coordinate system representation: ##EQU30##
And, definitions of parameters on the Denavit-Hartenberg coordinate system of FIG. 29 are illustrated in FIG. 30.
The present coordinate system is set in accordance with the following standards:
z.sub.i axis: axis of the articulation i+1
xi axis: common perpendicular line from z.sub.i-1 toward z.sub.i
y.sub.i axis: constructed so as to make a right-hand coordinate system
b.sub.i : length of the common perpendicular line to z.sub.i and z.sub.i-1
di: length between x.sub.i and x.sub.i-1 (z.sub.i-1 coordinate value)
.alpha..sub.i : angle between z.sub.i and z.sub.i-1 (the z.sub.i-1 direction coincides with the zi direction when it is rotated by .alpha..sub.i around the x.sub.i axis)
.THETA..sub.i : angle between x.sub.i and x.sub.i-1 (the x.sub.i-1 direction coincides with the x.sub.i direction when it is rotated by .THETA..sub.i around the z.sub.i-1 axis)
Further, when i=-1 in .sup.i a, .sup.i a represents an inertia system representation. Accordingly, the following equations (A4) and (A5) are obtained. ##EQU31##
The prefix A at the left shoulder denotes the initial letter of Absolute coordinate. Particularly, the equation (A5) represents a conversion matrix between the inertia system and the base reference coordinate system.
It is to be noted that, when differentiation of first and second orders of time is to be indicated in any other portion than the expressions and the figures of the present embodiment, d/dt and d.sup.2 /dt.sup.2 are used.
Now, description of the cooperative control system 26 and the attitude control system 24 is started here.
The cooperative control system 26 establishes cooperation between the manipulator control system 25 and the attitude control system 24 to keep the attitude controlling performance of the satellite good and is constructed such that it includes the manipulator driving logic calculation section 25.multidot.2 described hereinabove commonly with the manipulator control system 25 and it further includes a feed forward amount calculation section 26.multidot.1.
The feed forward amount calculation section 26.multidot.1 calculates and outputs a feed forward amount to be delivered to the wheel control system 27 of the attitude control system 24 in order to compensate for a disturbance originating from the manipulators 11 based on the output of the manipulator driving logic calculation section 25.multidot.2 described hereinabove.
In particular, the feed forward amount calculation section 26.multidot.1 calculates a disturbance originating from each of the manipulators 11 based on a particular calculation algorithm (such algorithm will be hereinafter described in detail) for each case and delivers a value of the disturbance as a feed forward term to the attitude control system 24.
And, the feed forward amount calculation section 26.multidot.1 takes, as such feed forward amount, an angular momentum or a wheel angular velocity obtained by dividing the angular momentum by the mass of momentum wheels 23.multidot.1 to 23.multidot.3 (wheels 3) and calculates an aimed angular momentum or an aimed angular velocity of the momentum wheels 23.multidot.1 to 23.multidot.3 based on the laws of conservation of momentum/angular momentum.
Further, the feed forward amount calculation section 26.multidot.1 applies, upon application of the laws of conservation of momentum/angular momentum described above, to the angular velocity of the body of the satellite 12, one of a constraint that the angular velocity of the body of the spacecraft 12 is equal to 0, another constraint that the angular velocity of the body of the spacecraft 12 is a constant, and a further constraint that the angular velocity of the body of the spacecraft 12 is an arbitrary function of the time. It is to be noted that all of the constraints are vectors.
And, the laws of conservation of momentum/angular momentum., that is, the two equations, are given by the following expressions (A6) and (A7): ##EQU32##
Accordingly, the following definition is set for the manipulator articulation angle and the wheel articulation angle (in the present embodiment, the number of the momentum wheels 23.multidot.1 to 23.multidot.3 is set to 3 for simplified description). ##EQU33##
In this instance, the equations (A6) and (A7) can be replaced as given by the following equation (A7-1): ##EQU34## .sub.0 : velocity of satellite body .sub.0 : angular velocity of satellite body
It is to be noted that the definition of the character S (also this character S is represented in scalar representation for the convenience of representation and should be treated similarly to the character a described hereinabove) in this equation is such as given below: ##EQU35##
Note) i=(0, w.sub.1, w.sub.2, w.sub.3) signifies that summation is performed for the satellite body and the wheel 1, the wheel 2 and the wheel 3 when i=0. ##EQU36##
And, this equation (A7-1) can be transformed, in the form, into the following equation (A8) using differentiation of each articulation angle: ##EQU37##
Further, a condition based on the following description is provided to the equation (A8):
If .THETA..sub.m =a current value and .THETA..sub.w =0, then ##EQU38##
Then, from the equation (A8) above, the following equation (A9) is obtained: ##EQU39##
Accordingly, the following equation (A10) is obtained from the equation (A9) above: ##EQU40##
Further, the following definition is provided:
If ##EQU41##
Consequently, from the equation (A8) given hereinabove, the following equation (A11) is obtained: ##EQU42##
As a result, from the equations (A10) and (A11) given above, a generalized equation (A12) given below of a feed forward angular velocity to be delivered to each of the momentum wheels 23.multidot.1 to 23.multidot.3 is obtained:
.THETA..sub.w [ .sub.01 .sub.02 .sub.03 ].sup.-1 ( .sub.0 - .sub.00) (A12)
And, an algorithm AA in this instance is an algorithm for calculating the velocity and the angular velocity of the satellite body based on the equation (A8) as shown in FIG. 31, and after initial value setting (step AA1) is performed, normal order repetitive calculation (step AA2) is performed, whereafter calculation of the base velocity and angular velocity (step AA3) is performed.
Here, the calculations shown in FIG. 31 have the following constructions:
In particular, the initial value setting (step AA1) is constructed from the following calculations: ##EQU43##
Meanwhile, the normal order repetitive calculation (step AA2) is constructed from the following calculations: ##EQU44##
Further, the calculation of the base velocity and angular velocity (steps AA3) is constructed from the following calculations: ##EQU45##
It is to be noted that the following definitions are applied to the calculations of the algorithm AA: ##EQU46##
In short, the feed forward amount calculation section 26.multidot.1 repeats, when the constraint described above is applicable, the algorithm AA for calculation of the velocity and the angular velocity of the satellite body by four times based on the equation (A8), and finally takes the inverse of a 3.times.3 matrix based on an equation obtained by putting the constraint into the equation (A12) to calculate a feed forward angular velocity under this constraint.
In the following, in what manner the feed forward amount calculation section 26.multidot.1 actually calculates a feed forward angular velocity (a feed forward value to be delivered to each of the momentum wheels 23.multidot.1 to 23.multidot.3 in order to keep the attitude of the satellite body at a fixed value) for each of the constraints described above when the wheel angular velocity is taken as a feed forward amount will be described successively.
[1]. First, the case wherein the feed forward amount calculation section 26.multidot.1 applies, upon application of the laws of conservation of momentum/angular momentum described above, the constraint of 0 to the angular velocity of the body of the spacecraft 12 will be described (in other words, the case wherein the constraint is .OMEGA..sub.0 =0. It is to be noted that also those .OMEGA..sub.0 and 0 are represented in scalar representation for the convenience of representation but should be treated similarly to the case of the vector a described hereinabove).
In this instance, the feed forward amount calculation section 26.multidot.1 substitutes the constraint 0 into the angular velocity .OMEGA..sub.0 of the satellite body included in the right side of the generalized equation (A12) of the feed forward angular velocity described above to calculate the following equation (A13) representative of the feed forward angular velocity under this constraint:
.THETA..sub.w =-[ .sub.01 .sub.02 .sub.03 ].sup.-1 .sub.00 (A 13)
And, the feed forward amount calculation section 26.multidot.1 repeats the algorithm AA for calculation of the velocity and the angular velocity of the satellite body described above four times based on the equation (A8), and finally takes the inverse of a 3.times.3 matrix based on the equation (A13) to calculate a value of the left side of the equation (A13), in short, a feed forward angular velocity under this constraint.
[2]. Subsequently, the case wherein the feed forward amount calculation section 26.multidot.1 applies, upon application of the laws of conservation of momentum/angular momentum described above, to the angular velocity of the body of the spacecraft 12 the constraint that the angular velocity of the body of the spacecraft 12 is a constant (that is, the case wherein the constraint is .OMEGA..sub.0 =C (given constant). It is to be noted that also this C is represented in scalar representation for the convenience of representation but should treated similarly to the case of the vector a described hereinabove).
In this instance, the feed forward amount calculation section 26.multidot.1 substitutes the constant C as the constraint into the angular velocity .OMEGA..sub.0 of the satellite body on the right side of the equation (A12) to calculate the following equation (A14) which represents a feed forward angular velocity under this constraint:
.THETA..sub.w =-[ .sub.01 .sub.02 .sub.03 ].sup.-1 ( - .sub.00) (A14)
And, the feed forward amount calculation section 26.multidot.1 calculates the value of the left side of the equation (A14), that is, a feed forward angular velocity under this constraint in a similar manner as described hereinabove using the equation (A14) and the equation (A8) as well as the algorithm A for calculation of the velocity and the angular velocity of the satellite body.
[3]. Further, the case wherein the feed forward amount calculation section 26.multidot.1 applies, upon application of the laws of conservation of momentum/angular momentum described above, to the angular velocity of the body of the spacecraft 12 the constraint that the angular velocity of the body of the spacecraft 12 is an arbitrary function of the time t (that is, the case wherein the constraint is .OMEGA..sub.0 =u(t) (u: given function of t). In this instance, also this u is represented in scalar representation for the convenience of representation but should be treated similarly to the case of the vector a described hereinabove).
In this instance, the feed forward amount calculation section 26.multidot.1 substitutes the arbitrary function u(t) of the time as the constraint into the angular velocity .OMEGA..sub.0 of the satellite body on the right side of the equation (A12) to calculate the following equation (A15) which represents a feed forward angular velocity under this constraint:
.THETA..sub.w =-[ .sub.01 .sub.02 .sub.03 ].sup.-1 ((t)- .sub.00) (A15)
And, the feed forward amount calculation section 26.multidot.1 calculates the value of the left side of the equation (A15), that is, the feed forward angular velocity under this constraint in a similar manner as in the case wherein the constraint is that the angular velocity of the body of the spacecraft 12 is 0 again using the equation (A15) and the equation (A8) as well as the algorithm AA for calculation of the velocity and the angular velocity of the satellite body.
The foregoing describes the technique of calculating a feed forward angular velocity for each constraint by the feed forward amount calculation section 26.multidot.1 when a wheel angular velocity is taken as a feed forward amount.
Meanwhile, the feed forward amount calculation section 26.multidot.1 multiplies, when an angular momentum is taken as a feed forward amount, a feed forward angular velocity calculated for each of the constraints described above by the mass of the momentum wheels 23.multidot.1 to 23.multidot.3 and outputs the product as a feed forward angular momentum.
By the way, the feed forward amount calculation section 26.multidot.1 further can take a torque in place of an angular momentum or a wheel angular velocity as a feed forward amount to be delivered to the wheel control system 27 of the attitude control system 24 in order to compensate for a disturbance originating from the manipulators 11 and calculate the value of the torque based on the reverse dynamics for the universe to control the attitude control system actuator 24.multidot.1.
In particular, the feed forward amount calculation section 26.multidot.1 calculates, when a torque is taken in this manner, a disturbance originating from the manipulators 1 based on a particular calculation algorithm AB (the algorithm will be hereinafter described in detail) and delivers the value of the disturbance as a feed forward term to the attitude control system 24.
And, in this instance, the feed forward amount calculation section 26.multidot.1 applies, upon application of the reverse dynamics for the universe described above, to the angular acceleration of the body of the spacecraft 12 either one of the constraint that the angular acceleration of the body of the spacecraft 12 is 0 or the constraint that the angular acceleration of the body of the spacecraft 12 is an arbitrary function of the time.
Or else, the feed forward amount calculation section 26.multidot.1 applies, upon application of the reverse dynamics for the universe described above, one of the following constraints to the angular velocity and the angular acceleration of the body of the spacecraft 12.
In short, the constraints include a constraint that the angular velocity and the angular acceleration of the body of the spacecraft 12 are both equal to 0, another constraint that the angular velocity of the body of the spacecraft 12 is a constant and the angular acceleration of the body of the spacecraft 12 is 0, and a further constraint that each of the angular velocity and the angular acceleration of the body of the spacecraft 12 is an arbitrary function of the time. It is to be noted that those constraints are all vectors.
And, the equations which make the base of the reverse dynamics for the universe in the case wherein a wheel torque is taken as a feed forward amount are differentiation of the laws of conservation of momentum/angular momentum described above and indicated by the equations (A6) and (A7) given hereinabove, that is, the expression of the sum total of the internal forces and the internal torques=zero [the following equations (A16) and (A17)]. ##EQU47##
Further, those equations (A16) and (A17) can be rewritten into the following equations (A18) and (A19): ##EQU48##
Further, the equations (A18) and (A19) can be rewritten into the following equations (A20) and (A21):
.sub.i = .sub.0 + .sub.i = .sub.0 + .sub.i '+ .sub.0 .times. .sub.i (A 20)
.sub.i = .sub.0 + .sub.0 .times. .sub.i +2 .sub.0 .times. .sub.i + .sub.0 .times.( .sub.0 .times. .sub.i)+ .sub.i ' (A21)
And, if the equations (A16) and (A17) above are transformed using those equations (A20) and (A21), then the following equations (A22), (A23) and (A24) are obtained: ##EQU49##
Furthermore, from the present equation (A22), a feed forward torque to be delivered to the wheels is calculated in the following manner. In particular, by transforming the equation (A22) into a form similar to that of the equation (A8), the following equation (A25) is obtained: ##EQU50##
The following definitions are used in the equation (A25):
If .THETA..sub.w =current value
.THETA..sub.m =current value
.THETA..sub.m =current value
.THETA..sub.w =
then ##EQU51## .sub.0 : acceleration of the satellite body .sub.0 : angular acceleration of the satellite body
As a result, the following equation (A26) is obtained: ##EQU52##
Accordingly, the following equation (A27) is obtained: ##EQU53##
And, the following definition is used in the equation (A25) given hereinabove:
If ##EQU54## Consequently, the following equation (A28) is obtained: ##EQU55##
From such equations (A27) and (A28), the equation (A29) given below is obtained:
.THETA..sub.w =[ .sub.01 .sub.02 .sub.03 ].sup.-1 ( .sub.0 - .sub.00)(A29)
And, the algorithm AB for calculation of the acceleration and the angular acceleration of the satellite body illustrated in FIG. 32 and based on the equation (A22) is constructed such that the velocity and the angular velocity of the satellite body are calculated from the laws of conservation of momentum/angular momentum of the algorithm AA for the case wherein a wheel angular velocity is taken as a feed forward amount described above and then the values of them are applied to the equation (A22).
In short, the algorithm AB is constructed such that, after initial value setting (step AB1) is performed, forward normal repetitive calculation (step AB2) is performed, and then calculation of the base velocity and angular velocity (step AB3) is performed, whereafter calculation of the base acceleration and angular acceleration (step AB4) is performed.
Here, the calculations illustrated in FIG. 32 have such constructions as described below.
In particular, the initial value setting (step AB1) is constructed from the following calculations: ##EQU56##
Meanwhile, the normal order repetitive calculation (step AB2) is constructed from the following calculations: ##EQU57##
Further, the calculation of the base velocity and angular velocity (step AB3) is constructed from the following equations: ##EQU58##
And, the calculation of the base acceleration and angular acceleration (step AB4) is constructed from the following calculations: ##EQU59##
In particular, the feed forward amount calculation section 26.multidot.1 calculates, when a wheel torque is taken as a feed forward amount, a feed forward torque, which is an aimed angular velocity, making use of the equations (A22) and (A29) given hereinabove and the algorithm AB.
In the following, in what procedure the feed forward amount calculation section 26.multidot.1 actually calculates a feed forward torque for each of the constraints described above when the wheel torque is taken as a feed forward amount (a feed forward value to be delivered to each of the momentum wheels 23.multidot.1 to 23.multidot.3 in order to keep the attitude of the satellite body at a fixed value) will be described suocessively.
[4]. First, the case wherein the feed forward amount calculation section 26.multidot.1 applies, upon application of the reverse dynamics for the universe described above, the constraint of 0 to the angular acceleration of the body of the spacecraft 12 will be described (in other words, the case wherein the constraint is d.OMEGA..sub.0 /dt=0).
In this instance, the feed forward amount calculation section 26.multidot.1 substitutes the constraint 0 into the angular acceleration d.OMEGA..sub.0 /dt of the satellite body included in the right side of the generalized equation (A29) of the feed forward torque described above to calculate the following equation (A30):
.THETA..sub.w =-[ .sub.01 .sub.02 .sub.03 ].sup.-1 .sub.00 (A 30)
Further, the feed forward amount calculation section 26.multidot.1 multiplies this equation (A30) by an inertia I.sub.3wi in the direction of the axis of rotation of each wheel to obtain the following equation (A31) representative of a feed forward torque to be delivered to each wheel.
The feed forward torque ##EQU60## inertia in the direction of the axis of rotation: I.sub.3wi
And, the feed forward amount calculation section 26.multidot.1 repeats, after the processing described above, the algorithm AB for calculation of the acceleration and the angular acceleration of the satellite body four times based on the equation (A22), takes the inverse of a 3.times.3 matrix based on the equation (A30) and multiplies it by the inertia in the equation (A31) to calculate a feed forward torque under this constraint indicated by the equation (A31).
Or, the feed forward amount calculation section 26.multidot.1 places the constraint into the equation obtained by multiplying the equation (A30) by the inertia I.sub.3wi in advance to calculate a feed forward torque then.
[5]. Subsequently, the case wherein the feed forward amount calculation section 26.multidot.1 applies, upon application of the reverse dynamics for the universe described above, to the angular velocity of the body of the spacecraft 12 the constraint t;hat the angular velocity and the angular acceleration of the body of the spacecraft 12 are both equal to 0 (that is, the case wherein the constraint is .OMEGA..sub.0 =0 and d.OMEGA..sub.0 /dt=0).
By the way, in this instance, the feed forward amount calculation section 26.multidot.1 substitutes 0 as a constraint into the angular velocity .OMEGA..sub.0 of the satellite body on the right side of the equation (A12) described above to calculate the following equation (A32) which represents an angular velocity of each of the wheels under this constraint. It is to be noted that the present equation (A32) is similar to the equation (A13) given hereinabove.
.THETA..sub.w =-[ .sub.01 .sub.02 .sub.03 ].sup.-1 .sub.00 (A 32)
Further, using the equation (A25), the following equation (A33) is obtained from the equation (A32): ##EQU61##
And, the following definition is used in the present equation (A33):
If .THETA..sub.m =current value
.THETA..sub.m =current value
.THETA..sub.w =-[ .sub.01 .sub.02 .sub.03 ].sup.-1 .sub.00 (A 32)
then ##EQU62##
as a result, the equation (A33) is transformed into the equation (A34) given below: ##EQU63##
Accordingly, the following equation (A35) is obtained. ##EQU64##
And, in order to calculate a component of G.sub.w.sup.3 of the equation (A35), the following definition is used in the equation (A33). It is to be noted that also this G is merely represented in scalar representation for the convenience of illustration but should be treated similarly as in the case of the vector a described hereinabove. ##EQU65## Then, the following equation (A36) is obtained: ##EQU66##
From such equations (A35) and (A36) as given hereinabove, the following equation (A37) is obtained:
.THETA..sub.w =-[ .sub.01 .sub.02 .sub.03 ].sup.-1 ( .sub.0 - .sub.00) (A37)
Here, if another constraint of the angular acceleration of the satellite body d.OMEGA..sub.0 /dt=0 is applied, then the equation (A37) can be rewritten into the following equation (A38):
.THETA..sub.w =-[ .sub.01 .sub.02 .sub.03 ].sup.-1 .sub.00 (A 38)
In particular, a value obtained by multiplying the present equation (A38) by the inertia I.sub.3wi in the direction of the axis of rotation of each wheel is a feed forward torque to be delivered to each wheel. This is given by the following equation (A39):
Feed forward torque ##EQU67##
In particular, the feed forward amount calculation section 26.multidot.1 repeats, when the angular velocity .OMEGA..sub.0 and the angular acceleration d.OMEGA..sub.0 /dt of the constraint are both equal to zero, the algorithm AA for calculation of the velocity and the angular velocity four times based on the equation (A8) to calculate a feed forward torque, and calculates the left side of the equation (A32) using the equation (A13).
And further, the feed forward amount calculation section 26.multidot.1 repeats the algorithm AB for calculation of the acceleration and the angular acceleration four times based on the equation (A22) using a value obtained based on the equation (A32), takes an inverse 3.times.3 matrix based on the equation (A38) and multiplies the equation (A39) by the inertia to calculate a feed forward torque.
[6]. Meanwhile, the case wherein the feed forward amount calculation section 26.multidot.1 applies, upon application of the reverse dynamics for the universe described above, to the angular velocity of the body of the spacecraft 12 the constraint that the angular velocity of the body of the spacecraft 12 is a constant and the angular acceleration is 0 (that is, the case wherein the constraint is .OMEGA..sub.0 =C (constant) and d.OMEGA..sub.0 /dt=0).
In this instance, a feed forward torque can be calculated in a similar manner as in the case of [5] described above (the constraint is .OMEGA..sub.0 =0 and d.OMEGA..sub.0 /dt=0) only by changing the constraint, but for an angular velocity of a wheel, the equation (A14) of [2] above is used in place of the equation (A32).
In particular, the feed forward amount calculation section 26.multidot.1 calculates, in the case of the constraint that the angular velocity is equal to the constant C and the angular acceleration is 0, a feed forward torque in a similar manner as in the case of [5] using the equations given hereinabove.
[7]. Subsequently, the case wherein the feed forward amount calculation section 26.multidot.1 applies, upon application of the reverse dynamics for the universe described above, to the angular velocity of the body of the spacecraft 12 the constraint that the angular acceleration of the body of the spacecraft 12 is an arbitrary function of the time t (that is, the case wherein the constraint is d.OMEGA..sub.0 /dt=du(t)/dt (u: given of t).
In this instance, the feed forward amount calculation section 26.multidot.1 calculates a feed forward torque using the following equation (A40) which is obtained by applying u(t) as a constraint to the angular acceleration of the satellite body in the equation (A29) used in the case of [4] described above.
.THETA..sub.w =[ .sub.01 .sub.02 .sub.03 ].sup.-1 ((t)- .sub.00)(40)
In particular, the feed forward amount calculation section 26.multidot.1 calculates, in the case of the constraint that the angular acceleration is an arbitrary function of the time, a feed forward torque in a similar manner as in the case of [4] except only that the equation (A40) is used in place of the equation (A30) of [4] described above.
[8] And then, the case wherein the feed forward amount calculation section 26.multidot.1 applies, upon application of the reverse dynamics for the universe described above, to the angular velocity of the body of the spacecraft 12 the constraint that the acceleration and the angular acceleration of the body of the spacecraft 12 are individually arbitrary functions of the time will be described (that is, the case wherein the constraint is .OMEGA..sub.0 =u(t) and d.OMEGA..sub.0 /dt=du(t)/dt).
In this instance, several equations for calculation of a feed forward torque are obtained based on a similar principle to that of the case of [5] described above. However, due to the constraint of ([8]) then, the equation (A15) of [3] described hereinabove is adopted for an angular velocity of a wheel in place of the equation (A32) of [5].
Further, together with this, the following equation (A41) is adopted in place of the equation (A38) in the equation (A37) of [5].
.THETA..sub.w =[ .sub.01 .sub.02 .sub.03 ].sup.-1 ( (t)- .sub.00)(41)
In particular, the feed forward amount calculation section 26.multidot.1 performs similar processing as in the case of [5] using the equations obtained based on the principle described above to calculate a feed forward torque.
The foregoing describes the technique of calculating a feed forward torque for each constraint by the feed forward amount calculation section 26.multidot.1 when a wheel torque is taken as a feed forward amount.
By the way, the feed forward amount calculation section 26.multidot.1 further can truncate (truncate), when it is to calculate a feed forward angular velocity or a feed forward torque, in order to reduce the number of calculations, the degree of freedom of a wheel to set the degree of freedom of the entire system to a sum of the articulation number L of the manipulators 11 and the 6 degree of freedom of the position and posture of the body of the spacecraft 12, that is, to the L+6 degree of freedom.
[9] In the following, details of a calculation method for a feed forward term when the degree of freedom of a wheel is truncated in [1] to [8] will be described successively for the individual conditions [refer to 1) to 6) below].
1) Description of the case wherein a wheel angular velocity is adopted as a feed forward term (corresponding to [1], [2] and [3] described hereinabove).
In this instance, since the degree of freedom of a wheel is truncated, the laws of conservation of momentum/angular momentum are given by the following equation (A42) in place of the equation (A8): ##EQU68##
Meanwhile, the sum total (Summation) regarding the satellite body and the momentum wheels 23.multidot.1, 23.multidot.2 and 23.multidot.3 in the case of i=0 in the definition of S described hereinabove is only provided from the body while the momentum wheel portions are omitted.
And, from the present equation (A42), the angular momentum L.sub.B (also this L is represented in scalar representation for the convenience of representation and should be treated similarly as in the case of a described above) flowing to the body is given by the following equation (A43): ##EQU69##
The angular momentum a wheel should have [refer to the following equation (A44)] is made coincide with the equation (A43). When the wheel arrangement is particularly set to the three-axis perpendicular type. the following equation (A45) is obtained. ##EQU70##
As a result, an aimed angular velocity of each wheel given by the following equation is determined from the equation (A45) above.
(aimed angular velocity of wheel)=.THETA..sub.w ref.
From the foregoing, an algorithm AC for calculation of a feed forward wheel angular velocity when the degree of freedom of a wheel is truncated is arranged in such a manner as illustrated in FIG. 33.
In particular, the present algorithm AC is constructed such that calculation of the angular velocity of the body based on the algorithm AA is performed (step AC1) and then calculation of the angular momentum of the body based on the equation (A43) is performed (step AC2), whereafter calculation of an aimed angular velocity of the wheel based on the equation (A45) is performed (step AC3).
2)-1. Description of the case wherein a wheel torque is adopted as a feed forward term and the constraint that the angular acceleration of the body is zero is applied (corresponding to [4] above).
When the degree of freedom of a wheel is truncated, a disturbance force and a disturbance torque acting upon the satellite body are calculated in the following manner.
In particular, where a manipulator 1 and a base 13 are defined as illustrated in FIG. 23, if the manipulator 1 is driven, the following internal force f.sub.1 and internal torque n.sub.1 act from the base upon the manipulator 1, and as reactions to them, the following internal force -f.sub.1 and internal torque -n.sub.1 act from the manipulator 1 upon the base 13 (also those f and n are represented in scalar representation for the convenience of representation but should be treated similarly as in the case of a described hereinabove).
If the internal force -f.sub.1 and the internal torque -n.sub.1 are converted around the center of gravity of the base, then the disturbance force f.sub.D and the disturbance torque .tau..sub.D applied to the base are given by the following equations (A46) and (A47) (also this .tau. is represented in scalar representation but should be treated similarly as in the case of the vector a described hereinabove). ##EQU71##
From the foregoing discussion, in order to calculate a disturbance torque (the value is set as a feed forward torque of a wheel), the internal torque n.sub.i to act upon each articulation should be calculated. A most popular technique of calculating the internal torque n.sub.1 is general purpose reverse dynamics for the universe illustrated in FIG. 34.
The general purpose reverse dynamics for the universe is constructed such that, after initial value setting (step Aa1) is performed first, normal order repetitive calculation (step Aa2) is performed, and then calculation of the base velocity and angular velocity (step Aa3), calculation of the base acceleration and angular acceleration (step Aa4) and reverse order repetitive calculation (step Aa5) are performed.
Here, description of construction of each calculation illustrated in FIG. 34 will be provided.
In particular, the initial value setting (step Aa1) is constructed from the following calculations: ##EQU72##
Meanwhile, the normal order repetitive calculation (step Aa2) is constructed from the following calculations: ##EQU73##
Further, the calculation of the base velocity and angular velocity (step Aa3) is constructed from the following calculations: ##EQU74##
And, the calculation of the base acceleration and angular acceleration (step Aa4) is constructed from the following calculations: ##EQU75##
Furthermore, the reverse order repetitive calculation (step Aa5) is constructed from the following calculations: ##EQU76##
It is to be noted that the following definitions are employed in the calculations of the general purpose reverse dynamics for the universe: ##EQU77##
And, when 0 is applied as the constraint of [4], that is, as the angular acceleration of the body as in the present example, such binding reverse dynamics for the universe based on the angular acceleration d.OMEGA..sub.0 /dt=0 as illustrated in FIG. 35 is employed.
The binding reverse dynamics for the universe based on the angular acceleration d.OMEGA..sub.0 /dt=0 is constructed from setting of an initial value (step Aa1'), normal order repetitive calculation (step Aa2'), calculation of the base velocity and angular velocity (step Aa3'), calculation of the base acceleration (step Aa4') and reverse order repetitive calculation (step Aa5').
And, in the binding reverse dynamics for the universe based on the angular acceleration d.OMEGA..sub.0 /dt=0, the calculation (step Aa4) of the base acceleration and angular acceleration in the general purpose reverse dynamics for the universe is replaced by the calculation (step Aa4') of the base acceleration, and the other steps Aa1', a2', a3' and a5' are similar to them.
Here, the calculation of the base acceleration (step Aa4') illustrated in FIG. 35 has such constructions as given below. ##EQU78##
In summary, the feed forward amount calculation section 26.multidot.1 truncates the degree of freedom of a wheel, calculates, when a wheel torque is adopted for a feed forward term and 0 is applied as an angular acceleration of the body as in [4], the internal force f.sub.1 and the internal torque m using the binding reverse dynamics for the universe based on the angular acceleration d.OMEGA..sub.0 /dt=0, and calculates a wheel feed forward torque based on the equation (A47).
3)-2. Description of the case wherein a wheel torque is adopted for a feed forward term and 0 is applied to the angular velocity and angular acceleration of the body as a constraint (corresponding to [5]).
In this instance, since the angular velocity and the angular acceleration of the body are both equal to zero as a constraint, the general purpose reverse dynamics for the universe is simplified very much. In particular, the velocity of the body is given by the following equation (A48) from the law of conservation of momentum: ##EQU79##
Consequently, by differentiating the equation (A48), the following equation (A49) is obtained for the acceleration of the body: ##EQU80##
And, the algorithm A for calculation of an external force (the algorithm will be hereinafter referred to as algorithm AD) omits, as shown in FIG. 36, the calculation (step Aa3) of the base velocity and angular velocity of the general purpose reverse dynamics for the universe and the calculation (step Aa4) of the base acceleration and angular acceleration.
In short, the algorithm AD in this instance is constructed such that, after initial value setting (step AD1) is performed similarly as in the case of the general purpose reverse dynamics for the universe, normal order repetitive calculation (step AD2) is performed.
Thereafter, the present algorithm AD performs calculation (step AD3) of the base velocity and acceleration and then performs reverse order repetitive calculation (step AD4).
Here, the calculations illustrated in FIG. 36 have such constructions as described below.
In particular, the initial value setting (step AD1) is constructed from the following calculations: ##EQU81##
Meanwhile, the normal order repetitive calculation (step AD2) is constructed from the following calculations: ##EQU82##
Further, the calculation of the base velocity and acceleration (step AD3) is constructed from the following calculations: ##EQU83##
And, the reverse order repetitive calculation (step AD4) is constructed from the following calculations: ##EQU84##
In particular, this is the binding reverse dynamics for the universe when the angular velocity and the angular acceleration are both equal to 0.
In summary, the feed forward amount calculation section 26.multidot.1 truncates the degree of freedom of a wheel, calculates, when a wheel torque is adopted for a feed forward term as in [5] and 0 is applied both to the angular velocity and the angular acceleration of the body, the internal force f.sub.1 and the internal torque n.sub.1 based on the binding reverse dynamics for the universe based on the angular velocity and the angular acceleration which are both equal to 0, and calculates the disturbance torque .tau..sub.D based on the equation (A47).
4)-3. Description of the case -3 wherein a wheel torque is adopted for a feed forward term and the constraint is applied such that the angular velocity of the body is a constant and the angular acceleration of the body is 0 (corresponding to [6] above).
In this instance, since the constraint is that the angular velocity of the body is a constant [.sup.A C (given constant)] and the angular acceleration of the body is 0, the general purpose reverse dynamics for the universe is simplified significantly. In particular, the velocity of the body is given by the following equation (A50) from the law of conservation of momentum: ##EQU85##
From this equation (A50) and the equation of the sum total of the internal forces=0, the acceleration of the body is given by the following equations (A51) and (A52): ##EQU86##
And, an external force in this instance can be calculated based on an algorithm AE. The algorithm AE is constructed from, as illustrated in FIG. 37, setting of an initial value (step AE1), normal order repetitive calculation (step AE2), calculation of the base velocity and acceleration (step AE3') and reverse order repetitive calculation (step AE4).
They are the steps of, in short, the algorithm AD described hereinabove, that is, the algorithm A modified such that the calculation (step AD3) of the base velocity and acceleration in the binding reverse dynamics for the universe when the angular velocity and the angular acceleration are both equal to 0 is replaced by calculation (step AE3) of the base speed and acceleration given below, and the other steps AE1. AE2 and AE4 are similar to those of the algorithm A. ##EQU87##
In particular, this is the binding reverse dynamics for the universe when the angular velocity of the body is a constant and the angular acceleration of the body is 0.
In summary, the feed forward amount calculation section 26.multidot.1 truncates the degree of freedom of a wheel and calculates, when a wheel torque is adopted for a feed forward term as in [6] and the constraint that the angular velocity of the body is a constant and the angular acceleration of the body is 0 is applied, an external force based on the binding reverse dynamics for the universe when the angular velocity of the body is a constant and the angular acceleration of the body is 0 as described above.
5)-4. Description of the case wherein a wheel torque is adopted for a feed forward term and the constraint that the angular acceleration of the body is an arbitrary function of the time is applied (corresponding to [7] described hereinabove).
In this instance, since an arbitrary function of the time is applied to the angular velocity of the body as the constraint, the calculation (step Aa4) of the base acceleration and angular acceleration in the general purpose reverse dynamics for the universe is replaced by the following calculation of the base acceleration (step Ab4): ##EQU88##
As a result, such binding reverse dynamics for the universe when the angular acceleration of the body is an arbitrary function of the time as illustrated in FIG. 38 is obtained. The binding reverse dynamics for the universe is constructed from, as shown in FIG. 38, setting of an initial value (step Ab1), normal order repetitive calculation (step Ab2), calculation of the base velocity and angular velocity (step Ab3), calculation of the base acceleration (step Ab4) and reverse order repetitive calculation (step Ab5).
In short, they are those steps, as apparent also from the preceding description, of the general purpose reverse dynamics for the universe wherein the step Aa4 is replaced by the step Ab4 given above and the other steps Ab1, Ab2, Ab3 and Ab5 are similar to those of the general purpose reverse dynamics for the universe.
In summary, the feed forward amount calculation section 26.multidot.1 truncates the degree of freedom of a wheel and calculates, when a wheel torque is adopted for a feed forward term as in [7] and the constraint that the angular acceleration of the body is an arbitrary function of the time is applied, a wheel feed forward torque based on the binding reverse dynamics for the universe wherein the angular acceleration of the body described above is an arbitrary function of the time.
6)-5. Description of the case wherein a wheel torque is adopted for a feed forward term and the constraint that the angular velocity of the body and the angular acceleration of the body are both arbitrary functions of the time is applied (corresponding to [8] described above).
In this instance, since the constraint that the angular velocity of the body and the angular acceleration of the body are arbitrary functions of the time is applied, similarly to the case of 3) described above, the general purpose reverse dynamics for the universe is simplified significantly.
In particular, the velocity of the body is given by the following equation (A53) from the law of conservation of momentum: ##EQU89##
And, the acceleration of the body is given by the following equation (A54): ##EQU90##
As a result, the algorithm for calculation of an external force (this will be hereinafter referred to as algorithm AF) is obtained by replacing the calculation (step AD3) of the base velocity and acceleration in the algorithm AD described above with the following calculation (step AF3) of the base velocity and acceleration: ##EQU91##
In short, the present algorithm AF is constituted from, as illustrated in FIG. 39, initial value setting (step AF1), normal order repetitive calculation (step AF2), calculation of the base velocity and acceleration (step AF3) and reverse order repetitive calculation (step AF4).
And, as described hereinabove, the present algorithm AF is a modification to the algorithm AD described above in that the calculation of the base velocity and acceleration (step AD3) is replaced with the following calculation of the base velocity and acceleration (step AF3), and the other steps AF1, AF2 and AF4 are similar to those of the algorithm AD.
In particular, the present algorithm AF is the binding reverse dynamics for the uniform when the angular velocity of the body and the angular acceleration of the body are both arbitrary functions of the time.
In summary, the feed forward amount calculation section 26.multidot.1 truncates the degree of freedom of a wheel and calculates, when a wheel torque is adopted for a feed forward term as in [8] and the constraint that the angular velocity of the body and the angular acceleration of the body are both arbitrary functions of the time, a wheel feed forward torque based on the binding reverse dynamics for the universe wherein the angular velocity of the body and the angular acceleration of the body are both arbitrary functions of the time as described above.
The foregoing is a method of calculation of a feed forward term when the degree of freedom of a wheel is truncated in the cases of [1] to [8] by the feed forward amount calculation section 26.multidot.1.
By setting the degree of freedom of the entire system to the degree of freedom of the sum L+6 between the articulation number L of the manipulators 1 and the degree of freedom of 6 of the position and attitude of the body of the artificial satellite, that is, by truncating the degree of freedom of the momentum wheels 23.multidot.1 to 23.multidot.3, the number of calculations can be reduced remarkably, which can contribute to simplification of the construction of the apparatus and enhancement of the processing performance.
By the way, the attitude control system 24 of FIG. 21 includes the attitude control system actuator 24.multidot.1 (23, 211), the wheel control system 27 and the attitude control sensor system 24.multidot.3 and controls the attitude of the spacecraft 12 using the attitude control system actuator 24.multidot.1.
The attitude control system actuator 24.multidot.1 employs a three-axis momentum wheel 211 as shown in FIG. 22, and the three-axis momentum wheel 211 is constituted from the three momentum wheels 23.multidot.1 to 23.multidot.3 disposed perpendicularly to each other around the center of gravity of the body.
In particular, the attitude control system actuator 24.multidot.1 rotates the momentum wheels 23.multidot.1 to 23.multidot.3 independently of each other in accordance with an instruction of the wheel control system 27 to directly perform attitude control of the satellite body.
Meanwhile, the attitude control sensor system 24.multidot.3 of FIG. 21 detects a variation of the attitude of the satellite, and the wheel control system 27 effects, in order to compensate for a disturbance originating from the manipulators 11, PD feedback control based on the attitude angle of the satellite body and differentiation of the attitude angle and adds the output to a feed forward output.
Or, the wheel control system 27 effects, in order to compensate for a disturbance originating from the manipulators 11. PID feedback control based on one of the attitude angle of the body of the artificial satellite, integration of the attitude angle and differentiation of the attitude angle and adds the output to a feed forward output.
To this end, the wheel control system 27 is constituted from an attitude control logic calculation section 24.multidot.4 and an adder 24.multidot.2. Here, the adder 24.multidot.2 combines, in order to compensate for a disturbance originating from the manipulators 11, a feed forward signal (an angular momentum, a wheel angular velocity or a torque, in short, feed forward control of a wheel based on [1] to [9] described above) transmitted thereto from the cooperative control system 26 and a PD or PID feedback control signal from the attitude control logic calculation section 24.multidot.4 with each other, determines the combination as an actuator control signal and converts the actuator control signal into and outputs a voltage signal.
The attitude control logic calculation section 24.multidot.4 takes the following constructions described below in accordance with individual conditions when PD feedback control based on the attitude angle of the satellite body and differentiation of the attitude angle is adopted for a feedback term of the attitude control system 24.
[10] In the following, description of the case wherein the attitude control logic calculation section 24.multidot.4 adopts PD feedback control based on the attitude angle of the satellite body and differentiation of the attitude angle for a feedback term of the attitude control system 24 in [1] to [9] described hereinabove will be described successively.
a). First, description of the cases of the PD feedback+feed forward wheel angular velocity (corresponding to [1], [2], [3] and [9]-1 described hereinabove) will be given.
In this instance, the attitude control logic calculation section 24.multidot.4 produces such a block diagram as shown in FIG. 24 for each axis. And, reference characters in FIG. 24 are such as described below:
.phi..sub.i : Eulerian angle of the i axis of the body
.phi..sub.i : time differentiation of the Eulerian angle of the i axis of the body
.theta..sub.wi : wheel angular velocity of around the i axis
.theta..sub.wi ref. : feed forward wheel angular velocity around the i axis
.tau..sub.PDi : PD feedback torque around the i axis
.tau..sub.FFi : feed forward torque around the i axis
.tau..sub.woi : control torque around the i axis
G.sub.Pi : proportional gain around the i axis
G.sub.Di : differential gain around the i axis
G.sub.Wi : wheel velocity control proportional gain around the i axis
By the attitude control logic calculation section 24.multidot.4 of such construction, the Eulerian angle of the i axis of the body and the time differentiation of the Eulerian angle are processed to obtain a proportional gain and a differential gain around the i axis, respectively, and those outputs are added to each other to obtain a PD feedback torque around the i axis.
Meanwhile, an output obtained by subtracting the wheel angular velocity around the i axis from the feed forward wheel angular velocity around the i axis is used to calculate a wheel velocity control proportional gain around the i axis and outputted as a feed forward torque around the i axis.
And, the PD feedback torque and the feed forward torque around the i axis of the two outputs are added to each other, and the sum is outputted as a control torque around the i axis to the attitude,control system actuator 24.multidot.1.
b) Description of the case of the PD feedback+feed forward wheel torque (corresponding to [4], [5], [6], [7], [8] and [9]-2) to -6)).
In this instance, the attitude control logic calculation section 24.multidot.4 produces such a block diagram as shown in FIG. 25 for each axis. And, .tau..sub.ffi in FIG. 25 denotes a wheel feed forward torque around the i axis, and the other reference characters are similar to those of FIG. 24.
By the attitude control logic calculation section 24.multidot.4 of such construction, a PD feedback torque around the i axis obtained in a similar manner as in the case described hereinabove is added to a wheel feed forward torque around the i axis, and the sum is outputted as a control torque around the i axis to the attitude control system actuator 24.multidot.1.
And then, the attitude control logic calculation section 24.multidot.4 takes the construction's described below in accordance with individual conditions when PID feedback control based on the attitude angle of the satellite body, integration of the attitude angle and differentiation of the attitude angle is adopted for a feedback term of the attitude control system 24.
[11] In the following, description of the case wherein the attitude control logic calculation section 24.multidot.4 adopts PID feedback control based on the attitude angle of the satellite body, integration of the attitude angle and differentiation of the attitude angle for a feedback term of the attitude control system 24 in [1] to [9] described above will be given successively.
c) Description of the case of the PID feedback+feed forward wheel angular velocity (corresponding to [1], [2], [3] and [9]-1)).
In this instance, the attitude control logic calculation section 24.multidot.4 produces such a block diagram as shown in FIG. 26 for each axis. And, G.sub.ii in FIG. 26 denotes an integral gain around the i axis, and the other reference characters are similar to those of FIG. 24.
By the attitude control logic calculation section 24.multidot.4 of such construction, the Eulerian angle output of the i axis of the body is transformed by Laplace transformation and used to calculate an integral gain around the i axis. Meanwhile, together with this, the Eulerian angle output of the i axis of the body is used to calculate a proportional gain around the i axis.
And, those outputs are processed to calculate differential gains around the i axis and added to the output of time differentiation of the Eulerian angle of the i axis of the body to produce a PID feedback torque around the i axis.
Meanwhile, the output obtained by subtracting the wheel angular velocity around the i axis from the feed forward wheel angular velocity around the i axis is processed to calculate a wheel velocity control proportional gain around the i axis and outputted as a feed forward torque around the i axis.
And, the PID feedback torque around the i axis and the feed forward torque around the i axis of the two outputs are added to each other, and the sum of them is outputted as a control torque around the i axis to the attitude control system actuator 24.multidot.1.
d) Description of the case of the PID feedback+feed forward wheel torque (corresponding to [4], [5], [6], [7], [8] and [9]-2) to -6)).
In this instance, the attitude control logic calculation section 24.multidot.4 produces such a block diagram as shown in FIG. 27 for each axis. And, reference characters in FIG. 27 are similar to those of FIG. 26 described above.
By the attitude control logic calculation section 24.multidot.4 of such construction, a PID feedback torque around the i axis obtained in a similar manner as described above is added to a wheel feed forward torque around the i axis, and the sum of them is outputted as a control torque around the i axis to the attitude control system actuator 24.multidot.1.
The foregoing descriptions describe the processing by the attitude control logic calculation section 24.multidot.4 regarding the combinations between the feed forward term and the feedback term in the various cases.
In this manner, in order to compensate for a disturbance originating from the manipulators 11, the attitude control system 24 either can perform PD feedback control based on the attitude angle of the spacecraft 12 and differentiation of the attitude angle or perform PID feedback control based on the attitude angle of the spacecraft 12, integration of the attitude angle and differentiation of the attitude angle to drive the manipulators 11 to suppress a disturbance brought to the attitude control system 24 of the satellite body from the manipulators 11 to keep the attitude controlling performance of the spacecraft at a high level.
With the construction described above, as seen in FIG. 21, the manipulator control system 25 receives articulation angle time series data from the manipulator articulation information inputting section 1 (1 in FIG. 21) and controls the manipulators 11 by servo control so that they may follow a route of the data.
In particular, the manipulator control system 25 receives articulation angle time series data at a manipulator driving logic calculation section 25.multidot.2, in which a voltage signal for driving the manipulator driving actuator 25.multidot.1 so as to follow an aimed route is produced.
As a result, the manipulator driving actuator 25.multidot.1 drives the manipulators 1 in accordance with the voltage signal. Consequently, the disturbance (2 in FIG. 21) comes to the attitude of the spacecraft 12 through the root of the manipulators 11 so that the attitude is varied.
And, the attitude control system 24 detects the variation of the attitude of the spacecraft 12 by way of the attitude control sensor system 24.multidot.3 and delivers the value to the attitude control logic calculation section 24.multidot.4. The attitude control logic calculation section 24.multidot.4 receiving the value obtains a PD feedback control signal from the attitude angle of the spacecraft 12 and differentiation of the attitude angle or the attitude angle of the spacecraft 12 and integration of the attitude angle as well as differentiation of the attitude angle and delivers the PD feedback control signal to the adder 24.multidot.2.
And, the PD feedback control signal (6 in FIG. 21) is combined with a feed forward control signal transmitted thereto from the feed forward amount calculation section 26.multidot.1 by the adder 24.multidot.2 (3 in FIG. 21) so that it is converted into a voltage signal and then transmitted to the attitude control system actuator 24.multidot.1 (7 in FIG. 21).
As a result, cooperation is established between the manipulator control system 25 and the attitude control system 24, and the attitude control system actuator 24.multidot.1 operates so as to keep the attitude of the spacecraft 12 good.
Meanwhile, in the cooperative control system 26, in order to cause the manipulator control system 25 and the attitude control system 24 to cooperate with each other to keep the attitude controlling performance of the spacecraft 12 good, the feed forward amount calculation section 26.multidot.1 produces a feed forward angular velocity or a feed forward torque,to be delivered to the attitude control system actuator 24.multidot.1 based on time series information (5 in FIG. 21) of the manipulator articulation angle, the angular velocity and the angular acceleration (6 in FIG. 21).
In this manner, the spacecraft 12 having the manipulators 11 includes the attitude control system 24 and the manipulator control system 25 and further includes the cooperative control system 26 for supplying a feed forward signal to the attitude control system 24 based on information from the manipulator control system 25, and in order to compensate for a disturbance originating from the manipulators 11, the cooperative control system 26 takes, as a feed forward amount to be delivered to the wheel control system 27 of the attitude control system 24, an angular momentum or a wheel angular velocity obtained by dividing the angular momentum by the mass of the momentum wheels 23.multidot.1 to 23.multidot.3 and calculates an aimed angular momentum or an aimed angular velocity of the momentum wheels 23.multidot.1 to 23.multidot.3 from the laws of conservation of momentum/angular momentum to control the momentum wheels 23.multidot.1 to 23.multidot.3. Further, upon application of the laws of conservation of momentum/angular momentum, either the constraint that the angular velocity of the body of the spacecraft 12 is 0 is applied to the angular velocity of the body of the spacecraft 12 or the constraint that the angular velocity of the body of the spacecraft 12 is a constant is applied or else the constraint that the angular velocity of the body of the spacecraft 12 is an arbitrary function of the time. Consequently, a disturbance brought to the satellite body from the manipulators 11 upon driving of the manipulators 11 can be suppressed by cooperative control of the manipulator control system 25 and the attitude control system 24, and the attitude controlling performance of the spacecraft 12 can be maintained at a high level.
Meanwhile, in the spacecraft 12 having the manipulators 11, the cooperative control system 26 takes, as a feed forward amount to be delivered to the wheel control system 27 of the attitude control system 24, a torque and calculates the value of the torque based on the reverse dynamics for the universe to control the momentum wheels 23.multidot.1 to 23.multidot.3. Further, upon application of the reverse dynamics for the universe, either the constraint that the angular acceleration of the body of the spacecraft 12 is 0 is applied to the angular acceleration of the body of the spacecraft 12 or the constraint that the angular velocity and the angular acceleration of the body of the spacecraft 12 are both equal to 0 is applied to the angular velocity and the angular acceleration of the body of the spacecraft 12 or else the constraint that the angular velocity of the body of the spacecraft 12 is a constant and the angular acceleration of the body of the spacecraft 12 is 0 is applied to the angular velocity and the angular acceleration of the body of the spacecraft 12 or otherwise the constraint that the angular acceleration of the body of the spacecraft 12 is an arbitrary function of the time is applied to the angular acceleration of the body of the spacecraft 12 or alternatively the constraint that the angular velocity and the angular acceleration of the body of the spacecraft 12 are both arbitrary functions of the time is applied to the angular velocity and the angular acceleration of the body of the spacecraft 12. Consequently, similarly as in the case of a feed forward amount based on the angular momentum or the wheel angular velocity described above, in the case of a feed forward torque, a disturbance brought to the satellite body from the manipulators 11 upon driving of the manipulators 11 can be suppressed by cooperative control of the manipulator control system 25 and the attitude control system 24, and the attitude controlling performance of the spacecraft 12 can be maintained at a high level.
Further, the dynamics which includes an attitude control system in a satellite or space robot having a manipulator can be calculated at a high speed, that is, the three-dimensional dynamics for the entire spacecraft including the attitude control system can be calculated on the real time basis, and a result of the calculation can be outputted on the real time basis on a display.
Further, the equation of motion of the entire system can be solved at a high speed and with a high degree of accuracy, and consequently, real time calculation of the three-dimensional dynamics for the entire spacecraft can be realized readily.
Further, while, in the present embodiment, three-dimensional dynamic real time simulation for the attitude control system of the spacecraft 12 such as a space robot or an artificial satellite having the manipulators 11 and attitude control based on a wheel are performed, only attitude control based on a wheel may be performed.
In particular, in this instance, similarly as that shown in FIG. 21, if, for example, manipulator operation information from an operator is received as a manipulator driving pattern, then attitude control of the spacecraft is controlled by a wheel and the manipulator is driven.
(c) Description of the Third Embodiment
Subsequently, a third embodiment of the present invention will be described. The present embodiment is directed to a simulation and control apparatus for a manipulator apparatus which performs simulation to effect attitude control of such a spacecraft 12 such as an artificial satellite having manipulators 11 as shown in FIG. 41. It is to be noted that reference numeral 13 in FIG. 41 denotes a paddle.
The present simulation and control apparatus for a manipulator apparatus can be represented functionally in such a block diagram as shown in FIG. 40. In particular, the simulation and control apparatus for a manipulator apparatus is constructed from separate sections including a simulation section 310 installed at a location adjacent an operator 31A on the ground and a control section 300 installed on the spacecraft 12 on an orbit.
Here, the simulation section 310 includes a manipulator articulation information inputting section 31 serving as initial instruction value inputting means, an excessive instruction value suppressing index forecasting section (means) 310A, a scale conversion section (means) 310B, and a graphic display 32 serving as display means.
The manipulator articulation information inputting section 31 is used to input route instruction value information .theta.(t) of a manipulator as an initial instruction value, and the excessive instruction value suppressing index forecasting section 310A receives the initial instruction value .theta.(t) from the manipulator articulation information inputting section 31 and forecasts (calculates), based on a dynamic model regarding a manipulator 11, disturbance information, which is produced when the manipulator 11 operates in accordance with the initial instruction value, on the real time basis as an excessive instruction value suppressing index.
The scale conversion section 310B performs scale conversion for the initial instruction value based on the excessive instruction value suppressing index forecast by the excessive instruction value suppressing index forecasting section 310A to obtain an intermediate instruction value s.theta.(t), and the graphic display 32 displays motion of the manipulator 11 by simulation display based on the initial instruction value, the intermediate instruction value s.theta.(t) obtained from the scale conversion section 310B and a sensor value from a scale conversion section 35.multidot.2 of the control section 300 which will be hereinafter described.
Here, three methods are available to take an excessive instruction value suppressing index to be forecast by the excessive instruction value suppressing index forecasting section 310A, including [1] the method of taking an angular momentum or a momentum, [2] the method of taking a torque or a force, and [3] the method of taking a torque (force) and an angular momentum (momentum) in combination. In the following, those methods will be described.
Here, symbols used in various equations used to describe the constructions of the individual apparatus hereinbelow and coordinate systems will be described. In particular, the notations hereinbelow are taken as listed in the following table. It is to be noted that a capital letter represents a vector as viewed from an inertial coordinate system .SIGMA..sub.A while a small letter represents a vector as viewed from a base fixed coordinate system .SIGMA..sub.0.
It is to be noted that, while d/dt and d.sup.2 /dt.sup.2 are used in the text to represent differentiation of first order and differentiation of second order of time in the present embodiment, this does not apply to mathematical expressions or the drawings.
.sub.i : velocity of the origin of the coordinate system .SIGMA..sub.i relative to the inertial coordinate system .SIGMA..sub.A
.sub.i : acceleration of the origin of the coordinate system .SIGMA..sub.i relative to the inertial coordinate system .SIGMA..sub.A
: velocity of the center of gravity of the link i relative to the inertial coordinate system .SIGMA..sub.A
: acceleration of the center of gravity of the link i relative to the inertial coordinate system .SIGMA..sub.A
.sub.i : angular velocity of the link i relative to the inertial coordinate system .SIGMA..sub.A
.sub.i : angular acceleration of the link i relative to the inertial coordinate system .SIGMA..sub.A
.sub.i : time differentiation of the vector .sub.i with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i .tbd.d.sub.0 .sub.i /dt= .sub.i '
.sub.i ': time differentiation of the vector z,7 .sub.i with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i '.tbd.d.sub.0 .sub.i /dt= .sub.i "
: time differentiation of the vector with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i .tbd.d.sub.0 .sub.i /dt= .sub.i '
': time differentiation of the vector with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i '.tbd.d.sub.0 .sub.i /dt= .sub.i "
.sub.i : angular velocity of the link i with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i ': time differentiation of .sub.i with respect to the base fixed coordinate system .SIGMA..sub.0
.sub.i '.tbd.d.sub.0 .sub.i /dt
Note 1) The time differentiation with respect to the inertial coordinate system .SIGMA..sub.A and the time differentiation with respect to the base fixed coordinate system .SIGMA..sub.0 are distinguished definitely from each other.
For a vector ,
time differentiation with respect to .SIGMA..sub.A :
.tbd.d /dt
time differentiation with respect to .SIGMA..sub.0 :
'.tbd.d.sub.0 z,12 /dt
The relationship between X and X' is given by the following equation:
= '+ .sub.0 .times.
Meanwhile, as a coordinate system for the entire system, such a Denavit-Hartenberg coordinate system as shown in FIG. 42 is adopted.
Here, when vectors represented in the coordinate systems .SIGMA..sub.i and .SIGMA..sub.i-1 of FIG. 42 are defined as .sup.i a and .sup.i-1 a (while those .sup.i a and .sup.i-1 a should actually be represented in vector representation, they are represented in scalar representation for the convenience of representation. While, also in the following description, a vector should be represented in vector representation in this manner, it is sometimes represented in scalar representation for the convenience of representation), respectively, .sup.i a and .sup.i-1 a have a relationship given by the following equation (refer to FIG. 43). (The character l in a mathematical representation is sometimes represented in a writing style.)
-1 =.sup.i-1 A.sub.i .sup.i +.sup.i-1 .sub.i
Here, .sup.i-1 A.sub.i is an orthogonal matrix representative of rotation between the coordinate systems, and .sup.i-1 l.sub.i (vector) is a representation of a vector from the coordinate origin O.sub.i-1 to O.sub.i in the .SIGMA..sub.i-1 system representation.
It is to be noted that .sup.i-1 A.sub.i and .sup.i-1 l.sub.i (vectors) are represented in the following manner when standard parameters are used on the Denavit-Hartenberg coordinate system representation: ##EQU92##
And, the definitions of parameters on the Denavit-Hartenberg coordinate system are such as illustrated in FIG. 44, and the coordinate system is set in accordance with such standards as listed in the following table:
z.sub.i axis: axis of the articulation i+1
x.sub.i axis: common perpendicular line from z.sub.i-1 toward z.sub.i
y.sub.i axis: constructed so as to make a right-hand coordinate system
b.sub.i : length of the common perpendicular line to z.sub.i and z.sub.i-1
d.sub.i : length between x.sub.i and x.sub.i-1 (z.sub.i-1 coordinate value)
.alpha..sub.i : angle between z.sub.i and z.sub.i-1 (the z.sub.i-1 direction coincides with the z.sub.i direction when it is rotated by .alpha..sub.i around the x.sub.i axis)
.theta..sub.i : angle between x.sub.i and x.sub.i-1 (the x.sub.i-1 direction coincides with the x.sub.i direction when it is rotated by .theta..sub.1 around the z.sub.i-1 axis)
Further, when i=-1 in .sup.i a, .sup.i a represents an inertial system representation. Accordingly, the following equations are obtained. ##EQU93##
In this instance, the prefix A at the left shoulder denotes the initial letter of Absolute coordinate, and particularly, the following equation represents a transformation matrix between the inertia system and the base reference coordinate system. ##EQU94##
[1] The case wherein an excessive instruction value suppressing index is forecast based on an angular momentum or a momentum
A disturbance angular momentum/disturbance momentum acting upon an object by way of the finger of a manipulator 11 are reflected on the angular momentum/momentum acting upon the root of the manipulator 11 based on the principle of action and reaction.
Therefore, a momentum/angular momentum are calculated by calculation described below, and an excessive instruction value suppressing index is forecast based on the momentum/angular momentum.
Here, the laws of conservation of momentum/angular momentum, that is, the two equations, are given by the following expressions (A6) and (A7): ##EQU95##
And, the following definition is set for the manipulator articulation angle and the wheel articulation angle (in the present embodiment, the number of the momentum wheels 3.multidot.1 to 3.multidot.3 is set to 3 for simplified description). ##EQU96##
In this instance, the equations (A6) and (A7) can be replaced as given by the following equation (A7-1): ##EQU97##
It is to be noted that the definition of the character S (also this character S is represented in scalar representation for the convenience of representation and should be handled similarly to the character a described hereinabove) in this equation is such as given below: ##EQU98##
Note) i=(0, w.sub.1, w.sub.2, w.sub.3) signifies that summation is performed for the satellite body and the wheel 1, the wheel 2 and the wheel 3 when i=0. ##EQU99##
An algorithm BA for calculating ##EQU100## based on the equation (A7-1) is such as given below.
Here, the calculations illustrated in FIG. 45 have such constructions as described below.
In particular, initial value setting (step BA1) is constructed from the following calculations: ##EQU101##
Meanwhile, normal order repetitive calculation (step BA2) is constructed from the following calculations: ##EQU102##
Further, calculation of the base velocity and angular velocity (step BA3) is constructed from the following calculations: ##EQU103##
It is to be noted that the following definitions are applied to the calculations of the present algorithm BA: ##EQU104##
Therefore, ##EQU105##
Values obtained by multiplying the mass M.sub.0 and the inertia I.sub.0 of the body by ##EQU106## obtained based on the equation (A7-1) in this manner are a momentum and an angular momentum, respectively. In particular, M.sub.0 V.sub.0 : momentum
I.sub.0 .OMEGA..sub.0 : angular momentum
[2] The case wherein an excessive instruction value suppressing index is forecast based on a force or a torque
A disturbance torque/disturbance force acting upon an object by way of the finger of a manipulator 11 are reflected on the torque/force acting upon the root of the manipulator 11 based on the principle of action and reaction.
Therefore, a torque/force are calculated by calculation described below, and an excessive instruction value suppressing index is forecast based on the torque/force.
The equations which make the base are differentiation of the laws of conservation of momentum and angular momentum, that is, the equation of the sum total of the internal force and the internal torque=0. ##EQU107##
The equations (B9) and (B10) can be rewritten in the following manner, respectively. ##EQU108##
If the equations (B9) and (B10) are transformed using the equations (B13) and (B14), then ##EQU109##
.sub.OUT : sum total of the external forces,
.sub.OUT : sum total of the external torques
An algorithm BB for calculating ##EQU110## based on the equation (B15) is such as given below.
The algorithm BB is constructed such that the velocity and the angular velocity of the body of the satellite are calculated from the laws of conservation of momentum/angular momentum of the algorithm BA described above and the values of them are applied to the equation (B15).
In short, the present algorithm BB is constructed such that, after initial value setting (step BB1) is performed, normal order repetitive calculation (step BB2) is performed, and then after calculation of the base velocity and angular velocity (step BB3) is performed, calculation of the base acceleration and angular acceleration (step BB4) is performed.
Here, the calculations illustrated in FIG. 46 are constructed in the following manner.
In particular, the initial value setting (step BB1) is constructed from the following calculations: ##EQU111##
Meanwhile, the normal order repetitive calculation (step BB2) is constructed from the following calculations: ##EQU112##
Further, the calculation of the base velocity and angular velocity (step BB3) is constructed from the following calculations: ##EQU113##
And, the calculation of the base acceleration and angular acceleration (step BB4) is constructed from the following calculations: ##EQU114##
[3] The case wherein an excessive instruction value suppressing index is forecast based on a combination of an angular momentum/momentum and a torque/force.
As described hereinabove, an angular momentum/momentum and a torque/force acting upon an object by way of the finger of a manipulator 11 are reflected on the angular momentum/momentum and the torque/force acting upon the root of the manipulator 11 based on the principle of action and reaction.
Those angular momentum/momentum and torque/force are calculated by the calculation described in [1] and [2] of the foregoing description, respectively.
[4] Calculation method for a differentiation term of a manipulator articulation angle in velocity control
As described in detail in [1] and [2] of the foregoing description, in order to calculate an index for scale conversion, differentiation of first order (d.theta./dt) and differentiation of second order (d.sup.2 .theta./dt.sup.2) of an articulation angle of a manipulator 11 are required.
Since an initial instruction value of a manipulator 11 is provided in the form of an articulation angle time series .theta..sub.i (t) (i=1 to 1), it is required to calculate d.theta./dt and d.sup.2 .theta./dt.sup.2 from .theta..sub.i (t).
To this end, d.theta./dt and d.sup.2 .theta./dt.sup.2 are calculated based on such calculation equations as illustrated in FIG. 47.
In particular, for .theta..sub.i (t) as an initial instruction value (step BC1), such calculation of a displacement as shown at step BC2 is performed. Then, a result of calculation of an equation shown at step BC3 is calculated as differentiation of first Order (d.theta./dt) of .theta.(t).
Meanwhile, upon calculation of differentiation of second order, calculation of a displacement of differentiation of first order (d.theta./dt) of .theta.(t) is performed as at step BC4, and using this, a result of calculation at step BC5 is calculated as differentiation of second order (d.sup.2 .theta./dt.sup.2) of .theta.(t).
Consequently, as differentiation information of an initial instruction value to be used to calculate an excessive instruction value suppressing index, a value obtained by processing a differential value of the initial instruction value by averaging processing is used.
By such calculation, the scale conversion section 310B converts the scale based on an excessive instruction value suppressing index forecast by the excessive instruction value suppressing index forecasting section 310A to obtain an intermediate instruction value s.theta.(t), and the manner of conversion of the scale is such, for example, as illustrated in FIG. 48.
In particular, arm articulation angle time series data .THETA.(t) as an initial instruction value (step BD1) and time series data [mx(t), my(t), mz(t)] regarding the momentum/angular momentum or the torque/force as an excessive instruction value suppressing index are inputted (step BD2).
And, as shown at step BD3, threshold value functions [Sx(t), Sy(t), Sz(t)] of mx(t), my(t), mz(t) are calculated, and such high/low discrimination as shown at step BD4 is performed based on the threshold value functions to determine S(t), and then it is determined based on this S(t) whether or not the initial instruction value is an excessive instruction value.
In particular, when the threshold value function of each of the axes are equal to or lower than 1, the initial instruction value is maintained as it is as S(t), but otherwise, the maximum value of the threshold value function is set to S(t) so that, by using this S(t), the velocity of the arm articulation angle set as the initial instruction value is controlled as shown at step BD5.
Here, T.sub.S is a sampling distance of input data as an initial instruction value.
It is to be noted that, as a conversion method for a scale, in addition to such a method as illustrated in FIG. 48, an exponential function can be introduced as illustrated in FIG. 49.
In particular, according to the method illustrated in this figure, at steps BE1 to BE4, similarly as at steps BD1 to BD4 of FIG. 48, arm articulation angle time series data .THETA.(t) as an initial instruction value and time series data [mx(t), my(t), mz(t)] regarding the momentum/angular momentum or the torque/force as an excessive instruction value suppressing index are inputted to determine S(t).
And, a function obtained by processing S(t) obtained at step BE4 described above by such exponential conversion calculation as shown at step BE5 is set as S(t), and by using this S(t), s.theta.(t) as an intermediate instruction value controls the velocity of the arm articulation angle set as the initial instruction value as shown at step BE6.
As described already, based on the initial instruction value .theta.(t), s.theta.(t) as an intermediate instruction value outputted in such a manner as described above and a sensor value from the scale conversion section 35.multidot.2 on the orbit, motion of the manipulator 11 is displayed by simulation display on the graphic display 32. The simulation display is performed in such a manner as shown in FIG. 52.
In particular, referring to FIG. 52, if an initial instruction value is inputted from an operator 31A, then, for example, such a screen as shown in (a) of FIG. 52 is constructed, and then, when an intermediate instruction value is calculated and an articulation angle of a manipulator 11 on the orbit is inputted from the altitude sensor system 34.multidot.3, such a screen as shown in (b) of FIG. 52 is constructed. The screen is overlaid suitably to support a manipulator operation of the operator 31A.
Further, upon application of the manipulator 11, when a great difference is produced among the initial instruction value .theta.(t) of the manipulator 11 from the operator 31A, the intermediate instruction value s.theta.(t) and a reached value (current articulation angle from the scale conversion section 35.multidot.2 on the orbit), the intermediate instruction values are all reset, and the initial value of the manipulator 11 is re-set to the current reached point and displayed on the graphic display 32 to resume the application.
In particular, in addition to the motion of the manipulator 11 based on the intermediate instruction value s.theta.(t), motion of the manipulator 11 based on the initial instruction value .theta.(t),and the reached value representative of the current manipulator position is displayed by simulation display, and when a deviation higher than a predetermined value is produced among the initial instruction value .theta.(t), the intermediate instruction value s.theta.(t) and the reached value, the initial instruction value .theta.(t) and the intermediate instruction value s.theta.(t) after the current manipulator reached point are reset, and the initial value of the manipulator 11 is re-set to the current reached point.
By the way, the control section 300 installed on the spacecraft 12 on the orbit is divided, as shown in FIGS. 40 and 50, into an attitude control system 34, an attitude control sensor system 34.multidot.3, a manipulator control system 35 and a cooperative control system 36.multidot.1.
Here, the manipulator control system 35 is constituted from a scale conversion section (means) 35.multidot.2 serving as a manipulator driving logic calculation section which receives an intermediate instruction value obtained by processing an initial instruction value inputted from the operator 31A to the simulation section 310 by scale conversion by the scale conversion section 310B (1 in FIG. 50) and modifies the data so that the manipulator 11 may follow the route of the intermediate instruction value to control the manipulator 11 of the spacecraft 12 by servoing control, and a manipulator driving actuator (robot system) 35.multidot.1.
In particular, the scale conversion section 35.multidot.2 modifies articulation,time series data inputted from the scale conversion section 310B of the simulation section 310 based on attitude angle/wheel angular velocity detection information from the attitude control sensor system 34.multidot.3 to produce an electric signal for driving the manipulator driving actuator 35.multidot.1 to cause the manipulator 11 to follow an aimed route.
In short, in order to control a manipulator 11, a first intermediate instruction value obtained by processing an initial instruction value by scale conversion is processed by suppressing processing based on an actual disturbance of the manipulator system to obtain a second intermediate instruction value, and then the manipulator 11 is controlled in accordance with the second stage intermediate instruction value.
Further, the scale conversion section 35.multidot.2 is different from the scale conversion section 310B of the simulation section 310 on the ground in that it does not perform scale conversion based on a forecast angular momentum/momentum or torque/force or both of them but performs scale conversion based on an angular momentum/momentum detected by the attitude control sensor system 34.multidot.3 or a torque/force or both of them, and the method of the scale conversion is similar to the method by the scale conversion section 310B described hereinabove.
Accordingly, modification to articulation time series data from the simulation section 310 based on attitude angle/wheel angular velocity detection information by the scale conversion section 35.multidot.2 is performed by the method illustrated in FIG. 48 or 49 described hereinabove.
Meanwhile, the manipulator driving actuator 35.multidot.1 drives a manipulator 11 so that it may follow an aimed route (an articulation angle, an angular velocity and an angular acceleration of the manipulator) indicated by a voltage signal from the scale conversion section 35.multidot.2.
By the way, the scale conversion section 35.multidot.2 sets the period of an electric signal to be outputted to the manipulator driving actuator 35.multidot.1 shorter than the period of scale conversion so that the control period of the manipulator 11 is made short to assure smooth operation.
Here, a voltage signal from the scale conversion section 35.multidot.2 is produced by such a method as described below.
[5] Production of an aimed route of a manipulator--1
Upon production of an aimed route of a manipulator 11, the following velocity scale conversion and spline interpolation calculation are combined with each other, and such a smooth aimed route as shown in FIG. 51 is produced by differentiation of first order of the combination.
Here, the period of dynamic calculation, that is, the period in which conversion of the velocity scale is performed, is represented by T.sub.D, and the manipulator controlling period is represented by T.sub.MC.
Spline interpolation is performed between .theta..sub.i and .theta..sub.i+1 based on the following equation: ##EQU115##
Accordingly, the manipulator aimed route for each T.sub.MC is obtained from the following equation: ##EQU116##
[6] Production of an aimed route of a manipulator--2
Meanwhile, the control period of a manipulator 11 can be combined with scale conversion calculation and spline interpolation calculation which is continuous up to differentiation of second order to produce such a smooth aimed route as shown in FIG. 51.
In [8], the equation (B8-1) is rewritten with the following equation: ##EQU117##
It is to be noted that the attitude control system 34 is constituted from an attitude control system actuator 34.multidot.1 and a wheel control system 37 having functions similar to those of the attitude control system 24 of the second embodiment described above, and controls the attitude of the spacecraft 12 using the attitude control system actuator 34.multidot.1. It is to be noted that reference numerals 311, 33.multidot.1 to 33.multidot.3 denote a three-axis momentum wheel and momentum wheels having similar functions to those of the second embodiment, respectively.
Meanwhile, the attitude control sensor system 34.multidot.3 detects a variation of the attitude of the spacecraft 12 and has a function similar to that of the attitude control system 24 of the second embodiment described hereinabove.
And, the cooperative control system 36 causes the manipulator control system 35 and the attitude control system 34 to cooperate with each other to keep the attitude controlling performance of the satellite good and is constructed such that it has the scale conversion section 35.multidot.2 described above commonly with the manipulator control system 35 and includes a feed forward amount calculation section 36.multidot.1 having a function similar to that of the second embodiment.
With the construction described above, according to the present embodiment, if articulation angle time series data as an initial instruction value are inputted from the operator 31A on the ground as shown in FIG. 40, then an excessive instruction value suppressing index is forecast by calculation by the excessive instruction value suppressing index forecasting section 310A (1).
Then, the scale conversion section 310B performs conversion of the velocity scale of the articulation angle time series data .theta.(t) as an initial instruction value based on the forecast excessive instruction value suppressing index (2) to obtain an intermediate instruction value s.theta.(t) and outputs the intermediate instruction value s.theta.(t) to the control section 300 on the orbit and the graphic display 32.
And, the scale conversion section 35.multidot.2 of the control section 300 on the orbit performs velocity scale conversion based on a sensor value from the attitude sensor system 34.multidot.3 (4) and outputs the data obtained by the conversion as an electric signal to the manipulator driving actuator 35.multidot.1 to control the actuator 11.
Meanwhile, the feed forward amount calculation section 36.multidot.1 calculates the feed forward wheel angular velocity .omega..sub.FF (t) from the electric signal from the scale conversion section 35.multidot.2 and outputs it to the attitude control system 34 to control the attitude of the spacecraft 12.
Further, an image of the state of the spacecraft 12 is outputted from the scale conversion section 310B to the graphic display 32 on the ground based on the initial instruction value, the intermediate instruction value, and the sensor value from the attitude sensor system 34.multidot.3 regarding the actual articulation angle of the actuator.
Accordingly, there is an advantage in that, when a manipulator which is spaced dynamically from an operator thereof is operated by programmed operation or remote control, a work by the manipulator can be performed efficiently with safety.
It is to be noted that, while, in the present embodiment, the scale conversion section 35.multidot.2 and the feed forward amount calculation section 36.multidot.1 are provided on the control section 300 on the orbit and an intermediate instruction, for which scale conversion has been performed by the scale conversion section 310B, is received to modify data so that the manipulator may follow the route of the intermediate instruction value to control the manipulator 11 provided on the spacecraft 12 by servoing control while the attitude control system 34 performs attitude control based on information from the feed forward amount calculation section 36.multidot.1 and the attitude sensor system 34.multidot.3, in the present invention, a control section 400 on the orbit need not include the scale conversion section 35.multidot.2 or the feed forward amount calculation section 36.multidot.1 as shown in FIG. 54.
In this instance, a scale conversion section 410B of a simulation apparatus 410 on the ground functionally includes a combination of the scale conversion section 310B of the simulation section 310 of the third embodiment described above and the scale conversion section 35.multidot.2 of the control section 300 on the orbit, and consequently, it is constructed so as to receive an initial instruction value, an excessive instruction value forecast index forecast by an excessive instruction value suppressing index forecasting section 410A and a sensor value from an attitude sensor system 44.multidot.3 and calculate an intermediate instruction value.
Meanwhile, a manipulator driving actuator 45.multidot.1 of the control section 400 inputs an intermediate instruction value obtained by scale conversion by the scale conversion section 410B of the simulation apparatus 410 on the ground and an excessive instruction value suppressing index forecast by the excessive instruction value suppressing index forecasting section 410A to control a manipulator 11 while the attitude control of the spacecraft 12 is performed based only on the attitude sensor system 44.multidot.3.
In the meantime, the graphic display 32 displays an image of motion of the manipulator 11 based on an initial instruction value from the operator 31A, an intermediate instruction value obtained by scale conversion by the simulation apparatus 410 and actual articulation angle information from the attitude sensor system 34.multidot.3.
Further, as shown in FIG. 55, construction only from a simulation apparatus only of a ground system is possible.
In this instance, if an initial instruction value from the operator 31A is inputted, then an excessive instruction value suppressing index forecasting section 510A forecasts an excessive instruction value suppressing index based on the initial instruction value, and a scale conversion section 510B converts the initial instruction value by scale conversion based on the thus forecast excessive instruction value suppressing index and outputs a resulted value as an intermediate instruction value to a graphic display 52. The graphic display 52 displays an image of motion of the manipulator 11 based on the initial instruction value and the intermediate instruction value.
It is to be noted that, while, in the present embodiment, the present system is constituted separately from the simulation section 310 installed adjacent the operator 31A on the ground and the control section 300 installed on the spacecraft 12 on the orbit, according to the present invention, it is also possible to install the simulation section 310 on the spacecraft 12 on the orbit.
In this instance, the control section installed on the spacecraft 12 on the orbit is constituted from initial instruction value inputting means for inputting route instruction value information of a manipulator as an initial instruction value, excessive instruction value suppressing index forecasting means for receiving the initial instruction value from the initial instruction value inputting means and forecasting information of a disturbance, which is produced when the manipulator operates in accordance with the initial instruction value, on the real time basis as an excessive instruction value suppressing index based on a dynamic model regarding the manipulator 11, scale conversion means for performing scale conversion for the route instruction value information based on the excessive instruction value suppressing index forecast by the excessive instruction value suppressing index forecasting means to obtain an intermediate instruction value, and control means for controlling the manipulator in accordance with the intermediate instruction value obtained by the conversion of the scale conversion means. The route instruction value information of the manipulator 11 is inputted as an initial instruction value, and based on the dynamic model regarding the manipulator 11, information of a disturbance which is produced when the manipulator operates in accordance with the initial instruction value is forecast on the real time basis as an excessive instruction value suppressing index, and then scale conversion is performed for the initial instruction value based on the excessive instruction value suppressing index to obtain an intermediate instruction value, whereafter the manipulator 11 is controlled in accordance with the intermediate instruction value obtained by the scale conversion.
INDUSTRIAL APPLICABILITY OF THE INVENTION
The present system can be applied as an emulation apparatus in the procedure of development of onboard software (OBS) or an operator supporting apparatus in the procedure of application of a spacecraft when, in order to perform various operations on an orbit, which are forecast in construction and application of a space infrastructure such as a space station or a platform, efficiently with safety, various outside operations to be performed by an astronaut are performed by a spacecraft (a space robot, an artificial satellite or the like) having a manipulator for the astronaut or when a manipulator which is spaced dynamically from an operator therefor such as on the bottom of the sea or in an underground passage or a tunnel is operated by programmed operation or remote control.

Number	Date	Country	Kind
4-145981	Jun 1992	JPX
4-145982	Jun 1992	JPX

Number	Name	Date
4956790	Tsuchihashi et al.	Sep 1990
5046022	Conway et al.	Sep 1991
5231693	Backer et al.	Jul 1993
5303384	Rodriguez et al.	Apr 1994
5459659	Takenaka	Oct 1995

Number	Date	Country
61-79589	Apr 1986	JPX
61-239309	Oct 1986	JPX
62-199385	Sep 1987	JPX
63-66611	Mar 1988	JPX
63-95508	Apr 1988	JPX
63-196388	Aug 1988	JPX
1259401	Oct 1989	JPX
1289683	Nov 1989	JPX
2116494	May 1990	JPX
2176906	Jul 1990	JPX
2274484	Nov 1990	JPX
386484	Apr 1991	JPX
4129688	Apr 1992	JPX
4141388	May 1992	JPX
4269185	Sep 1992	JPX

Simulation method and apparatus for manipulator apparatus, simulation and control method and apparatus for manipulator apparatus, and control method and apparatus for manipulator apparatus

Information

Patent Number

Date Filed

Date Issued

Inventors

Original Assignees

Examiners

CPC

US Classifications

Field of Search

US

International Classifications

Abstract

Description

Claims

Priority Claims (2)

PCT Information

US Referenced Citations (5)

Foreign Referenced Citations (15)